Visual Servoing Platform  version 3.6.1 under development (2024-05-04)
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 
62 void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend);
63 int 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 
66 int 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
74 void 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
90 int 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 
140 int 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;
194  pose.computePose(vpPose::DEMENTHON, cMo);
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 
202  pose.setRansacNbInliersToReachConsensus(4);
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;
211  pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
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;
218  pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
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;
226  pose.computePose(vpPose::VIRTUAL_VS, cMo);
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;
233  pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
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;
240  pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
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;
247  pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
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;
294  pose.computePose(vpPose::DEMENTHON, cMo);
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;
301  pose.setRansacNbInliersToReachConsensus(4);
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;
310  pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
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;
317  pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
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;
326  pose.computePose(vpPose::VIRTUAL_VS, cMo);
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;
333  pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
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;
340  pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
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;
347  pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
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;
384  pose.computePose(vpPose::DEMENTHON, cMo);
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;
391  pose.setRansacNbInliersToReachConsensus(4);
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;
400  pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
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;
408  pose.computePose(vpPose::VIRTUAL_VS, cMo);
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;
415  pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
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 
423  pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
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;
493  pose.computePose(vpPose::DEMENTHON, cMo);
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 
502  pose.setRansacNbInliersToReachConsensus(4);
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;
512  pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
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;
520  pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
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;
529  pose.computePose(vpPose::VIRTUAL_VS, cMo);
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;
537  pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
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;
545  pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
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;
553  pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
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;
625  pose.computePose(vpPose::DEMENTHON, cMo);
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;
633  pose.setRansacNbInliersToReachConsensus(4);
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;
643  pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
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;
651  pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
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;
661  pose.computePose(vpPose::VIRTUAL_VS, cMo);
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;
669  pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
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;
677  pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
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;
685  pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
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;
747  pose.computePose(vpPose::DEMENTHON, cMo);
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;
755  pose.setRansacNbInliersToReachConsensus(4);
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;
765  pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
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;
774  pose.computePose(vpPose::VIRTUAL_VS, cMo);
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;
782  pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
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 
791  pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
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 }
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:304
vpException
error that can be emitted by ViSP classes.
Definition: vpException.h:59
vpGaussRand
Class for generating random number with normal probability density.
Definition: vpGaussRand.h:116
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:199
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:127
vpMath::deg
static double deg(double rad)
Definition: vpMath.h:117
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:189
vpPose
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
vpPose::addPoint
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:93
vpPose::setRansacNbInliersToReachConsensus
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:385
vpPose::DEMENTHON
@ DEMENTHON
Definition: vpPose.h:84
vpPose::LAGRANGE_LOWE
@ LAGRANGE_LOWE
Definition: vpPose.h:89
vpPose::RANSAC
@ RANSAC
Definition: vpPose.h:88
vpPose::DEMENTHON_LOWE
@ DEMENTHON_LOWE
Definition: vpPose.h:91
vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:99
vpPose::LAGRANGE_VIRTUAL_VS
@ LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:97
vpPose::VIRTUAL_VS
@ VIRTUAL_VS
Definition: vpPose.h:93
vpPose::LAGRANGE
@ LAGRANGE
Definition: vpPose.h:83
vpPose::DEMENTHON_VIRTUAL_VS
@ DEMENTHON_VIRTUAL_VS
Definition: vpPose.h:95
vpPose::listP
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:115
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:289
vpPose::computePose
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
Definition: vpPose.cpp:339
vpPose::setRansacThreshold
void setRansacThreshold(const double &t)
Definition: vpPose.h:390