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