Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
testPose.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Compute the pose of a 3D object using the Dementhon, Lagrange and
32 * Non-Linear approach.
33 */
34
35#include <visp3/core/vpCameraParameters.h>
36#include <visp3/core/vpMeterPixelConversion.h>
37#include <visp3/core/vpPixelMeterConversion.h>
38#include <visp3/core/vpDebug.h>
39#include <visp3/core/vpGaussRand.h>
40#include <visp3/core/vpHomogeneousMatrix.h>
41#include <visp3/core/vpMath.h>
42#include <visp3/core/vpPoint.h>
43#include <visp3/core/vpRotationMatrix.h>
44#include <visp3/core/vpRxyzVector.h>
45#include <visp3/core/vpTranslationVector.h>
46#include <visp3/vision/vpPose.h>
47
48#include <stdio.h>
49#include <stdlib.h>
50
51#define L 0.035
52#define L2 0.1
53
61
62#ifdef ENABLE_VISP_NAMESPACE
63using namespace VISP_NAMESPACE_NAME;
64#endif
65
66void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend);
67int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &cam,
68 const std::string &legend);
69int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &cam,
70 const std::string &legend, const double &translation3DThresh, const double &rotationRadian3DThresh, const double &pose2DThresh, const double &posePixThresh);
71
72int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &cam,
73 const std::string &legend)
74{
75 return compare_pose(pose, cMo_ref, cMo_est, cam,
76 legend, 0.001, 0.001, 0.001, 1.);
77}
78
79// print the resulting estimated pose
80void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend)
81{
82 vpPoseVector cpo = vpPoseVector(cMo);
83
84 std::cout << std::endl
85 << legend << "\n "
86 << "tx = " << cpo[0] << "\n "
87 << "ty = " << cpo[1] << "\n "
88 << "tz = " << cpo[2] << "\n "
89 << "tux = vpMath::rad(" << vpMath::deg(cpo[3]) << ")\n "
90 << "tuy = vpMath::rad(" << vpMath::deg(cpo[4]) << ")\n "
91 << "tuz = vpMath::rad(" << vpMath::deg(cpo[5]) << ")\n"
92 << std::endl;
93}
94
95// test if pose is well estimated
96int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &cam,
97 const std::string &legend, const double &translation3DThresh, const double &rotation3DThresh, const double &pose2DThresh, const double &posePixThresh)
98{
99 vpPoseVector pose_ref = vpPoseVector(cMo_ref);
100 vpPoseVector pose_est = vpPoseVector(cMo_est);
101
102 int fail_3d = 0;
103
104 // Test done on the 3D pose
105 for (unsigned int i = 0; i < 6; i++) {
106 double pose3DThresh = 0.;
107 if (i < 3) {
108 pose3DThresh = translation3DThresh;
109 }
110 else {
111 pose3DThresh = rotation3DThresh;
112 }
113 if (std::fabs(pose_ref[i] - pose_est[i]) > pose3DThresh) {
114 fail_3d = 1;
115 std::cout << "ref[" << i << "] - est[" << i << "] = " << pose_ref[i] - pose_est[i] << " > " << pose3DThresh << std::endl;
116 }
117 }
118
119 std::cout << "Based on 3D parameters " << legend << " is " << (fail_3d ? "badly" : "well") << " estimated" << std::endl;
120
121 // // Test done on the residual
122
123 // Residual expressed in meters
124 double r = pose.computeResidual(cMo_est);
125 if (pose.listP.size() < 4) {
126 fail_3d = 1;
127 std::cout << "Not enough point" << std::endl;
128 return fail_3d;
129 }
130 r = sqrt(r / pose.listP.size());
131 // std::cout << "Residual on each point (meter): " << r << std::endl;
132 int fail_2d = (r > pose2DThresh) ? 1 : 0;
133 std::cout << "Based on 2D residual (" << r << ") " << legend << " is " << (fail_2d ? "badly" : "well") << " estimated"
134 << std::endl;
135
136 // Residual expressed in pixels
137 double r_pix = pose.computeResidual(cMo_est, cam);
138 r_pix = sqrt(r_pix / pose.listP.size());
139 // std::cout << "Residual on each point (pixel): " << r << std::endl;
140 int fail_pix = (r_pix > posePixThresh) ? 1 : 0;
141 std::cout << "Based on pixel residual (" << r_pix << ") " << legend << " is " << (fail_pix ? "badly" : "well") << " estimated"
142 << std::endl;
143 return fail_3d + fail_2d + fail_pix;
144}
145
146int main()
147{
148#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
149 try {
150 int test_planar_fail = 0, test_non_planar_fail = 0, fail = 0;
151 const double translation3DthreshWhenNoise = 0.005;
152 const double rotation3DthreshWhenNoise = vpMath::rad(1.);
153 const double residual2DWhenNoise = 0.001;
154 const double residualPixelWhenNoise = 1.;
155
156 vpHomogeneousMatrix cMo; // will contain the estimated pose
157 vpCameraParameters cam; // Default camera parameters to compute the residual in terms of pixel
158
159 {
160 //
161 // Test planar case with 4 points
162 //
163
164 std::cout << "Start test considering planar case with 4 points..." << std::endl;
165 std::cout << "===================================================" << std::endl;
166
167 // vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0, vpMath::rad(10));
168 vpPoseVector cpo_ref = vpPoseVector(-0.01, -0.02, 0.3, vpMath::rad(20), vpMath::rad(-20), vpMath::rad(10));
169 vpHomogeneousMatrix cMo_ref(cpo_ref);
170
171 int npt = 4;
172 std::vector<vpPoint> P(npt); // Point to be tracked
173 double Z = 0.05; // FS: Dementhon estimation is not good when Z=0.3
174
175 P[0].setWorldCoordinates(-L, -L, Z);
176 P[1].setWorldCoordinates(L, -L, Z);
177 P[2].setWorldCoordinates(L, L, Z);
178 P[3].setWorldCoordinates(-L, L, Z);
179
180 vpPose pose;
181
182 for (int i = 0; i < npt; i++) {
183 P[i].project(cMo_ref);
184 // P[i].print();
185 pose.addPoint(P[i]); // and added to the pose computation class
186 }
187
188 // Let's go ...
189
190 print_pose(cMo_ref, std::string("Reference pose"));
191
192 std::cout << "-------------------------------------------------" << std::endl;
193 pose.computePose(vpPose::LAGRANGE, cMo);
194
195 print_pose(cMo, std::string("Pose estimated by Lagrange"));
196 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange");
197 test_planar_fail |= fail;
198
199 std::cout << "--------------------------------------------------" << std::endl;
200 pose.computePose(vpPose::DEMENTHON, cMo);
201
202 print_pose(cMo, std::string("Pose estimated by Dementhon"));
203 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon");
204 test_planar_fail |= fail;
205
206 std::cout << "--------------------------------------------------" << std::endl;
207
208 pose.setRansacNbInliersToReachConsensus(4);
209 pose.setRansacThreshold(0.01);
210 pose.computePose(vpPose::RANSAC, cMo);
211
212 print_pose(cMo, std::string("Pose estimated by Ransac"));
213 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Ransac");
214 test_planar_fail |= fail;
215
216 std::cout << "--------------------------------------------------" << std::endl;
217 pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
218
219 print_pose(cMo, std::string("Pose estimated by Lagrange then Lowe"));
220 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then Lowe");
221 test_planar_fail |= fail;
222
223 std::cout << "--------------------------------------------------" << std::endl;
224 pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
225
226 print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
227 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then Lowe");
228 test_planar_fail |= fail;
229
230 // Now Virtual Visual servoing
231 std::cout << "--------------------------------------------------" << std::endl;
232 pose.computePose(vpPose::VIRTUAL_VS, cMo);
233
234 print_pose(cMo, std::string("Pose estimated by VVS"));
235 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by VVS");
236 test_planar_fail |= fail;
237
238 std::cout << "-------------------------------------------------" << std::endl;
239 pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
240
241 print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
242 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then by VVS");
243 test_planar_fail |= fail;
244
245 std::cout << "-------------------------------------------------" << std::endl;
246 pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
247
248 print_pose(cMo, std::string("Pose estimated by Lagrange then by VVS"));
249 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then by VVS");
250 test_planar_fail |= fail;
251
252 std::cout << "-------------------------------------------------" << std::endl;
253 pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
254
255 print_pose(cMo, std::string("Pose estimated either by Dementhon or Lagrange then by VVS"));
256 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose either by Dementhon or Lagrange then by VVS");
257 test_planar_fail |= fail;
258 }
259
260 {
261 //
262 // Test non-planar case with 6 points (at least 6 points for Lagrange non planar)
263 //
264
265 std::cout << "\nStart test considering non-planar case with 6 points..." << std::endl;
266 std::cout << "=======================================================" << std::endl;
267
268 vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0, vpMath::rad(10));
269 vpHomogeneousMatrix cMo_ref(cpo_ref);
270
271 int npt = 6;
272 std::vector<vpPoint> P(npt); // Point to be tracked
273 P[0].setWorldCoordinates(-L, -L, 0); // Lagrange not accurate...
274 P[0].setWorldCoordinates(-L, -L, -0.02);
275 P[1].setWorldCoordinates(L, -L, 0);
276 P[2].setWorldCoordinates(L, L, 0);
277 P[3].setWorldCoordinates(-2 * L, 3 * L, 0);
278 P[4].setWorldCoordinates(-L, L, 0.01);
279 P[5].setWorldCoordinates(L, L / 2., 0.03);
280
281 vpPose pose;
282
283 for (int i = 0; i < npt; i++) {
284 P[i].project(cMo_ref);
285 // P[i].print();
286 pose.addPoint(P[i]); // and added to the pose computation class
287 }
288
289 // Let's go ...
290 print_pose(cMo_ref, std::string("Reference pose"));
291
292 std::cout << "-------------------------------------------------" << std::endl;
293 pose.computePose(vpPose::LAGRANGE, cMo);
294
295 print_pose(cMo, std::string("Pose estimated by Lagrange"));
296 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange");
297 test_non_planar_fail |= fail;
298
299 std::cout << "--------------------------------------------------" << std::endl;
300 pose.computePose(vpPose::DEMENTHON, cMo);
301
302 print_pose(cMo, std::string("Pose estimated by Dementhon"));
303 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon");
304 test_non_planar_fail |= fail;
305
306 std::cout << "--------------------------------------------------" << std::endl;
307 pose.setRansacNbInliersToReachConsensus(4);
308 pose.setRansacThreshold(0.01);
309 pose.computePose(vpPose::RANSAC, cMo);
310
311 print_pose(cMo, std::string("Pose estimated by Ransac"));
312 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Ransac");
313 test_non_planar_fail |= fail;
314
315 std::cout << "--------------------------------------------------" << std::endl;
316 pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
317
318 print_pose(cMo, std::string("Pose estimated by Lagrange then Lowe"));
319 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then Lowe");
320 test_non_planar_fail |= fail;
321
322 std::cout << "--------------------------------------------------" << std::endl;
323 pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
324
325 print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
326 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then Lowe");
327 test_non_planar_fail |= fail;
328
329 // Now Virtual Visual servoing
330
331 std::cout << "--------------------------------------------------" << std::endl;
332 pose.computePose(vpPose::VIRTUAL_VS, cMo);
333
334 print_pose(cMo, std::string("Pose estimated by VVS"));
335 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by VVS");
336 test_non_planar_fail |= fail;
337
338 std::cout << "-------------------------------------------------" << std::endl;
339 pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
340
341 print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
342 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then by VVS");
343 test_non_planar_fail |= fail;
344
345 std::cout << "-------------------------------------------------" << std::endl;
346 pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
347
348 print_pose(cMo, std::string("Pose estimated by Lagrange then by VVS"));
349 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then by VVS");
350 test_non_planar_fail |= fail;
351
352 std::cout << "-------------------------------------------------" << std::endl;
353 pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
354
355 print_pose(cMo, std::string("Pose estimated either by Dementhon or Lagrange then by VVS"));
356 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose either by Dementhon or Lagrange then by VVS");
357 test_non_planar_fail |= fail;
358 }
359
360 //
361 // Test non-planar case with 4 points (Lagrange can not be used)
362 //
363
364 std::cout << "\nStart test considering non-planar case with 4 points..." << std::endl;
365 std::cout << "=======================================================" << std::endl;
366
367 {
368 int npt = 4;
369 std::vector<vpPoint> P(npt); // Point to be tracked
370 P[0].setWorldCoordinates(-L2, -L2, 0);
371 P[1].setWorldCoordinates(L2, -L2, 0.2);
372 P[2].setWorldCoordinates(L2, L2, -0.1);
373 P[3].setWorldCoordinates(-L2, L2, 0);
374
375 vpPose pose;
376
377 vpPoseVector cpo_ref = vpPoseVector(-0.1, -0.2, 0.8, vpMath::rad(10), vpMath::rad(-10), vpMath::rad(25));
378 vpHomogeneousMatrix cMo_ref(cpo_ref);
379
380 for (int i = 0; i < npt; i++) {
381 P[i].project(cMo_ref);
382 // P[i].print(); printf("\n");
383 pose.addPoint(P[i]); // and added to the pose computation class
384 }
385
386 // Let's go ...
387 print_pose(cMo_ref, std::string("Reference pose"));
388
389 std::cout << "--------------------------------------------------" << std::endl;
390 pose.computePose(vpPose::DEMENTHON, cMo);
391
392 print_pose(cMo, std::string("Pose estimated by Dementhon"));
393 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon");
394 test_non_planar_fail |= fail;
395
396 std::cout << "--------------------------------------------------" << std::endl;
397 pose.setRansacNbInliersToReachConsensus(4);
398 pose.setRansacThreshold(0.01);
399 pose.computePose(vpPose::RANSAC, cMo);
400
401 print_pose(cMo, std::string("Pose estimated by Ransac"));
402 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Ransac");
403 test_non_planar_fail |= fail;
404
405 std::cout << "--------------------------------------------------" << std::endl;
406 pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
407
408 print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
409 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then Lowe");
410 test_non_planar_fail |= fail;
411
412 // Now Virtual Visual servoing
413 std::cout << "--------------------------------------------------" << std::endl;
414 pose.computePose(vpPose::VIRTUAL_VS, cMo);
415
416 print_pose(cMo, std::string("Pose estimated by VVS"));
417 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by VVS");
418 test_non_planar_fail |= fail;
419
420 std::cout << "-------------------------------------------------" << std::endl;
421 pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
422
423 print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
424 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then by VVS");
425 test_non_planar_fail |= fail;
426
427 std::cout << "-------------------------------------------------" << std::endl;
428
429 pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
430
431 print_pose(cMo, std::string("Pose estimated either by Dementhon or Lagrange then by VVS"));
432 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose either by Dementhon or Lagrange then by VVS");
433 test_non_planar_fail |= fail;
434
435 std::cout << "-------------------------------------------------" << std::endl;
436 }
437
438 //
439 // Test computeResidual with results expressed in pixel
440 //
441
442 std::cout << "Start test considering planar case with 4 points and noise on the projection..." << std::endl;
443 std::cout << "===================================================" << std::endl;
444 {
445 vpPoseVector cpo_ref = vpPoseVector(-0.01, -0.02, 0.3, vpMath::rad(20), vpMath::rad(-20), vpMath::rad(10));
446 vpHomogeneousMatrix cMo_ref(cpo_ref);
447
448 int npt = 4;
449 std::vector<vpPoint> P(npt); // Point to be tracked
450 double Z = 0.05; // FS: Dementhon estimation is not good when Z=0.3
451
452 P[0].setWorldCoordinates(-L, -L, Z);
453 P[1].setWorldCoordinates(L, -L, Z);
454 P[2].setWorldCoordinates(L, L, Z);
455 P[3].setWorldCoordinates(-L, L, Z);
456
457 vpPose pose;
458 vpGaussRand random(0.08, 0., 42); // Gaussian noise of mean = 0. and sigma = 1.
459
460 for (int i = 0; i < npt; i++) {
461 // Projecting point in camera frame
462 P[i].project(cMo_ref);
463
464 // Computing theoretical u and v based on the 2D coordinates
465 double x_theo = P[i].get_X() / P[i].get_Z();
466 double y_theo = P[i].get_Y() / P[i].get_Z();
467 double u_theo = 0., v_theo = 0.;
468 vpMeterPixelConversion::convertPoint(cam, x_theo, y_theo, u_theo, v_theo);
469
470 // Adding noise to u, v
471 double u_noisy = u_theo + random();
472 double v_noisy = v_theo + random();
473
474 // Computing corresponding x, y
475 double x_noisy = 0., y_noisy = 0.;
476 vpPixelMeterConversion::convertPoint(cam, u_noisy, v_noisy, x_noisy, y_noisy);
477
478 P[i].set_x(x_noisy);
479 P[i].set_y(y_noisy);
480
481 pose.addPoint(P[i]); // and added to the pose computation class
482 std::cout << "P[" << i << "]:\n\tu_theo = " << u_theo << "\tu_noisy = " << u_noisy << std::endl;
483 std::cout << "\tv_theo = " << v_theo << "\tv_noisy = " << v_noisy << std::endl;
484 std::cout << "\tx_theo = " << x_theo << "\ty_noisy = " << x_noisy << std::endl;
485 std::cout << "\ty_theo = " << y_theo << "\tx_noisy = " << y_noisy << std::endl;
486 }
487
488 print_pose(cMo_ref, std::string("Reference pose"));
489
490 std::cout << "-------------------------------------------------" << std::endl;
491 pose.computePose(vpPose::LAGRANGE, cMo);
492
493 print_pose(cMo, std::string("Pose estimated by Lagrange"));
494 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange"
495 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
496 test_planar_fail |= fail;
497
498 std::cout << "--------------------------------------------------" << std::endl;
499 pose.computePose(vpPose::DEMENTHON, cMo);
500
501 print_pose(cMo, std::string("Pose estimated by Dementhon"));
502 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon"
503 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
504 test_planar_fail |= fail;
505
506 std::cout << "--------------------------------------------------" << std::endl;
507
508 pose.setRansacNbInliersToReachConsensus(4);
509 pose.setRansacThreshold(0.01);
510 pose.computePose(vpPose::RANSAC, cMo);
511
512 print_pose(cMo, std::string("Pose estimated by Ransac"));
513 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Ransac"
514 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
515 test_planar_fail |= fail;
516
517 std::cout << "--------------------------------------------------" << std::endl;
518 pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
519
520 print_pose(cMo, std::string("Pose estimated by Lagrange then Lowe"));
521 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then Lowe"
522 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
523 test_planar_fail |= fail;
524
525 std::cout << "--------------------------------------------------" << std::endl;
526 pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
527
528 print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
529 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then Lowe"
530 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
531 test_planar_fail |= fail;
532
533 // Now Virtual Visual servoing
534 std::cout << "--------------------------------------------------" << std::endl;
535 pose.computePose(vpPose::VIRTUAL_VS, cMo);
536
537 print_pose(cMo, std::string("Pose estimated by VVS"));
538 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by VVS"
539 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
540 test_planar_fail |= fail;
541
542 std::cout << "-------------------------------------------------" << std::endl;
543 pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
544
545 print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
546 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then by VVS"
547 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
548 test_planar_fail |= fail;
549
550 std::cout << "-------------------------------------------------" << std::endl;
551 pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
552
553 print_pose(cMo, std::string("Pose estimated by Lagrange then by VVS"));
554 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then by VVS"
555 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
556 test_planar_fail |= fail;
557
558 std::cout << "-------------------------------------------------" << std::endl;
559 pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
560
561 print_pose(cMo, std::string("Pose estimated either by Dementhon or Lagrange then by VVS"));
562 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose either by Dementhon or Lagrange then by VVS"
563 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
564 test_planar_fail |= fail;
565 }
566
567 //
568 // Test non-planar case with 6 points (at least 6 points for Lagrange non planar)
569 //
570
571 std::cout << "\nStart test considering non-planar case with 6 points and noise on the projection..." << std::endl;
572 std::cout << "=======================================================" << std::endl;
573
574 {
575 vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0, vpMath::rad(10));
576 vpHomogeneousMatrix cMo_ref(cpo_ref);
577
578 int npt = 6;
579 std::vector<vpPoint> P(npt); // Point to be tracked
580 P[0].setWorldCoordinates(-L, -L, 0); // Lagrange not accurate...
581 P[0].setWorldCoordinates(-L, -L, -0.02);
582 P[1].setWorldCoordinates(L, -L, 0);
583 P[2].setWorldCoordinates(L, L, 0);
584 P[3].setWorldCoordinates(-2 * L, 3 * L, 0);
585 P[4].setWorldCoordinates(-L, L, 0.01);
586 P[5].setWorldCoordinates(L, L / 2., 0.03);
587
588 vpPose pose;
589 vpGaussRand random(0.08, 0., 42); // Gaussian noise of mean = 0. and sigma = 1.
590
591 for (int i = 0; i < npt; i++) {
592 // Projecting point in camera frame
593 P[i].project(cMo_ref);
594
595 // Computing theoretical u and v based on the 2D coordinates
596 double x_theo = P[i].get_X() / P[i].get_Z();
597 double y_theo = P[i].get_Y() / P[i].get_Z();
598 double u_theo = 0., v_theo = 0.;
599 vpMeterPixelConversion::convertPoint(cam, x_theo, y_theo, u_theo, v_theo);
600
601 // Adding noise to u, v
602 double u_noisy = u_theo + random();
603 double v_noisy = v_theo + random();
604
605 // Computing corresponding x, y
606 double x_noisy = 0., y_noisy = 0.;
607 vpPixelMeterConversion::convertPoint(cam, u_noisy, v_noisy, x_noisy, y_noisy);
608
609 P[i].set_x(x_noisy);
610 P[i].set_y(y_noisy);
611
612 pose.addPoint(P[i]); // and added to the pose computation class
613 std::cout << "P[" << i << "]:\n\tu_theo = " << u_theo << "\tu_noisy = " << u_noisy << std::endl;
614 std::cout << "\tv_theo = " << v_theo << "\tv_noisy = " << v_noisy << std::endl;
615 std::cout << "\tx_theo = " << x_theo << "\ty_noisy = " << x_noisy << std::endl;
616 std::cout << "\ty_theo = " << y_theo << "\tx_noisy = " << y_noisy << std::endl;
617 }
618
619 // Let's go ...
620 print_pose(cMo_ref, std::string("Reference pose"));
621
622 std::cout << "-------------------------------------------------" << std::endl;
623 pose.computePose(vpPose::LAGRANGE, cMo);
624
625 print_pose(cMo, std::string("Pose estimated by Lagrange"));
626 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange"
627 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
628 test_non_planar_fail |= fail;
629
630 std::cout << "--------------------------------------------------" << std::endl;
631 pose.computePose(vpPose::DEMENTHON, cMo);
632
633 print_pose(cMo, std::string("Pose estimated by Dementhon"));
634 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon"
635 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
636 test_non_planar_fail |= fail;
637
638 std::cout << "--------------------------------------------------" << std::endl;
639 pose.setRansacNbInliersToReachConsensus(4);
640 pose.setRansacThreshold(0.01);
641 pose.computePose(vpPose::RANSAC, cMo);
642
643 print_pose(cMo, std::string("Pose estimated by Ransac"));
644 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Ransac"
645 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
646 test_non_planar_fail |= fail;
647
648 std::cout << "--------------------------------------------------" << std::endl;
649 pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
650
651 print_pose(cMo, std::string("Pose estimated by Lagrange then Lowe"));
652 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then Lowe"
653 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
654 test_non_planar_fail |= fail;
655
656 std::cout << "--------------------------------------------------" << std::endl;
657 pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
658
659 print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
660 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then Lowe"
661 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
662 test_non_planar_fail |= fail;
663
664 // Now Virtual Visual servoing
665
666 std::cout << "--------------------------------------------------" << std::endl;
667 pose.computePose(vpPose::VIRTUAL_VS, cMo);
668
669 print_pose(cMo, std::string("Pose estimated by VVS"));
670 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by VVS"
671 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
672 test_non_planar_fail |= fail;
673
674 std::cout << "-------------------------------------------------" << std::endl;
675 pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
676
677 print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
678 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then by VVS"
679 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
680 test_non_planar_fail |= fail;
681
682 std::cout << "-------------------------------------------------" << std::endl;
683 pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
684
685 print_pose(cMo, std::string("Pose estimated by Lagrange then by VVS"));
686 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Lagrange then by VVS"
687 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
688 test_non_planar_fail |= fail;
689
690 std::cout << "-------------------------------------------------" << std::endl;
691 pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
692
693 print_pose(cMo, std::string("Pose estimated either by Dementhon or Lagrange then by VVS"));
694 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose either by Dementhon or Lagrange then by VVS"
695 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
696 test_non_planar_fail |= fail;
697 }
698
699 //
700 // Test non-planar case with 4 points (Lagrange can not be used)
701 //
702
703 std::cout << "\nStart test considering non-planar case with 4 points and noise on the projection..." << std::endl;
704 std::cout << "=======================================================" << std::endl;
705
706 {
707 int npt = 4;
708 std::vector<vpPoint> P(npt); // Point to be tracked
709 P[0].setWorldCoordinates(-L2, -L2, 0.2);
710 P[1].setWorldCoordinates(L2, -L2, 0.4);
711 P[2].setWorldCoordinates(L2, L2, 0.1);
712 P[3].setWorldCoordinates(-L2, L2, 0.4);
713
714 vpPose pose;
715
716 vpPoseVector cpo_ref = vpPoseVector(-0.1, -0.2, 0.8, vpMath::rad(10), vpMath::rad(-10), vpMath::rad(25));
717 vpHomogeneousMatrix cMo_ref(cpo_ref);
718
719 vpGaussRand random(0.08, 0., 42); // Gaussian noise of mean = 0. and sigma = 1.
720
721 for (int i = 0; i < npt; i++) {
722 // Projecting point in camera frame
723 P[i].project(cMo_ref);
724
725 // Computing theoretical u and v based on the 2D coordinates
726 double x_theo = P[i].get_X() / P[i].get_Z();
727 double y_theo = P[i].get_Y() / P[i].get_Z();
728 double u_theo = 0., v_theo = 0.;
729 vpMeterPixelConversion::convertPoint(cam, x_theo, y_theo, u_theo, v_theo);
730
731 // Adding noise to u, v
732 double u_noisy = u_theo + random();
733 double v_noisy = v_theo + random();
734
735 // Computing corresponding x, y
736 double x_noisy = 0., y_noisy = 0.;
737 vpPixelMeterConversion::convertPoint(cam, u_noisy, v_noisy, x_noisy, y_noisy);
738
739 P[i].set_x(x_noisy);
740 P[i].set_y(y_noisy);
741
742 pose.addPoint(P[i]); // and added to the pose computation class
743 std::cout << "P[" << i << "]:\n\tu_theo = " << u_theo << "\tu_noisy = " << u_noisy << std::endl;
744 std::cout << "\tv_theo = " << v_theo << "\tv_noisy = " << v_noisy << std::endl;
745 std::cout << "\tx_theo = " << x_theo << "\ty_noisy = " << x_noisy << std::endl;
746 std::cout << "\ty_theo = " << y_theo << "\tx_noisy = " << y_noisy << std::endl;
747 }
748
749 // Let's go ...
750 print_pose(cMo_ref, std::string("Reference pose"));
751
752 std::cout << "--------------------------------------------------" << std::endl;
753 pose.computePose(vpPose::DEMENTHON, cMo);
754
755 print_pose(cMo, std::string("Pose estimated by Dementhon"));
756 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon"
757 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
758 test_non_planar_fail |= fail;
759
760 std::cout << "--------------------------------------------------" << std::endl;
761 pose.setRansacNbInliersToReachConsensus(4);
762 pose.setRansacThreshold(0.01);
763 pose.computePose(vpPose::RANSAC, cMo);
764
765 print_pose(cMo, std::string("Pose estimated by Ransac"));
766 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Ransac"
767 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
768 test_non_planar_fail |= fail;
769
770 std::cout << "--------------------------------------------------" << std::endl;
771 pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
772
773 print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
774 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then Lowe"
775 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
776 test_non_planar_fail |= fail;
777
778 // Now Virtual Visual servoing
779 std::cout << "--------------------------------------------------" << std::endl;
780 pose.computePose(vpPose::VIRTUAL_VS, cMo);
781
782 print_pose(cMo, std::string("Pose estimated by VVS"));
783 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by VVS"
784 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
785 test_non_planar_fail |= fail;
786
787 std::cout << "-------------------------------------------------" << std::endl;
788 pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
789
790 print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
791 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose by Dementhon then by VVS"
792 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
793 test_non_planar_fail |= fail;
794
795 std::cout << "-------------------------------------------------" << std::endl;
796
797 pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
798
799 print_pose(cMo, std::string("Pose estimated either by Dementhon or Lagrange then by VVS"));
800 fail = compare_pose(pose, cMo_ref, cMo, cam, "pose either by Dementhon or Lagrange then by VVS"
801 , translation3DthreshWhenNoise, rotation3DthreshWhenNoise, residual2DWhenNoise, residualPixelWhenNoise);
802 test_non_planar_fail |= fail;
803
804 std::cout << "-------------------------------------------------" << std::endl;
805 }
806
807 std::cout << "=======================================================" << std::endl;
808 std::cout << "Pose estimation test from planar points: " << (test_planar_fail ? "fail" : "is ok") << std::endl;
809 std::cout << "Pose estimation test from non-planar points: " << (test_non_planar_fail ? "fail" : "is ok")
810 << std::endl;
811 std::cout << "Global pose estimation test: " << ((test_planar_fail | test_non_planar_fail) ? "fail" : "is ok")
812 << std::endl;
813
814 return ((test_planar_fail | test_non_planar_fail) ? EXIT_FAILURE : EXIT_SUCCESS);
815 }
816 catch (const vpException &e) {
817 std::cout << "Catch an exception: " << e << std::endl;
818 return EXIT_FAILURE;
819 }
820#else
821 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
822 return EXIT_SUCCESS;
823#endif
824}
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition vpCameraParameters.h:311
vpException
error that can be emitted by ViSP classes.
Definition vpException.h:60
vpGaussRand
Class for generating random number with normal probability density.
Definition vpGaussRand.h:117
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition vpHomogeneousMatrix.h:221
vpMath::rad
static double rad(double deg)
Definition vpMath.h:129
vpMath::deg
static double deg(double rad)
Definition vpMath.h:119
vpMeterPixelConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Definition vpMeterPixelConversion.h:105
vpPixelMeterConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Definition vpPixelMeterConversion.h:108
vpPoseVector
Implementation of a pose vector and operations on poses.
Definition vpPoseVector.h:203
vpPose
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:82
vpPose::addPoint
void addPoint(const vpPoint &P)
Definition vpPose.cpp:96
vpPose::setRansacNbInliersToReachConsensus
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition vpPose.h:397
vpPose::DEMENTHON
@ DEMENTHON
Definition vpPose.h:88
vpPose::LAGRANGE_LOWE
@ LAGRANGE_LOWE
Definition vpPose.h:93
vpPose::RANSAC
@ RANSAC
Definition vpPose.h:92
vpPose::DEMENTHON_LOWE
@ DEMENTHON_LOWE
Definition vpPose.h:95
vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:103
vpPose::LAGRANGE_VIRTUAL_VS
@ LAGRANGE_VIRTUAL_VS
Definition vpPose.h:101
vpPose::VIRTUAL_VS
@ VIRTUAL_VS
Definition vpPose.h:97
vpPose::LAGRANGE
@ LAGRANGE
Definition vpPose.h:87
vpPose::DEMENTHON_VIRTUAL_VS
@ DEMENTHON_VIRTUAL_VS
Definition vpPose.h:99
vpPose::computePose
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
Definition vpPose.cpp:385
vpPose::listP
std::list< vpPoint > listP
Array of point (use here class vpPoint).
Definition vpPose.h:119
vpPose::computeResidual
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition vpPose.cpp:298
vpPose::setRansacThreshold
void setRansacThreshold(const double &t)
Definition vpPose.h:402
VISP_NAMESPACE_NAME
Definition vpEigenConversion.h:44