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