Visual Servoing Platform  version 3.4.0
testPose.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Compute the pose of a 3D object using the Dementhon, Lagrange and
33  * Non-Linear approach.
34  *
35  * Authors:
36  * Eric Marchand
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpHomogeneousMatrix.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpPoint.h>
45 #include <visp3/core/vpRotationMatrix.h>
46 #include <visp3/core/vpRxyzVector.h>
47 #include <visp3/core/vpTranslationVector.h>
48 #include <visp3/vision/vpPose.h>
49 
50 #include <stdio.h>
51 #include <stdlib.h>
52 
53 #define L 0.035
54 #define L2 0.1
55 
64 void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend);
65 int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
66  const std::string &legend);
67 
68 // print the resulting estimated pose
69 void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend)
70 {
71  vpPoseVector cpo = vpPoseVector(cMo);
72 
73  std::cout << std::endl
74  << legend << "\n "
75  << "tx = " << cpo[0] << "\n "
76  << "ty = " << cpo[1] << "\n "
77  << "tz = " << cpo[2] << "\n "
78  << "tux = vpMath::rad(" << vpMath::deg(cpo[3]) << ")\n "
79  << "tuy = vpMath::rad(" << vpMath::deg(cpo[4]) << ")\n "
80  << "tuz = vpMath::rad(" << vpMath::deg(cpo[5]) << ")\n"
81  << std::endl;
82 }
83 
84 // test if pose is well estimated
85 int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
86  const std::string &legend)
87 {
88  vpPoseVector pose_ref = vpPoseVector(cMo_ref);
89  vpPoseVector pose_est = vpPoseVector(cMo_est);
90 
91  int fail = 0;
92 
93  // Test done on the 3D pose
94  for (unsigned int i = 0; i < 6; i++) {
95  if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
96  fail = 1;
97  }
98 
99  std::cout << "Based on 3D parameters " << legend << " is " << (fail ? "badly" : "well") << " estimated" << std::endl;
100 
101  // Test done on the residual
102  double r = pose.computeResidual(cMo_est);
103  if (pose.listP.size() < 4) {
104  fail = 1;
105  std::cout << "Not enough point" << std::endl;
106  return fail;
107  }
108  r = sqrt(r / pose.listP.size());
109  // std::cout << "Residual on each point (meter): " << r << std::endl;
110  fail = (r > 0.001) ? 1 : 0;
111  std::cout << "Based on 2D residual (" << r << ") " << legend << " is " << (fail ? "badly" : "well") << " estimated"
112  << std::endl;
113  return fail;
114 }
115 
116 int main()
117 {
118 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
119  try {
120  int test_planar_fail = 0, test_non_planar_fail = 0, fail = 0;
121 
122  vpHomogeneousMatrix cMo; // will contain the estimated pose
123 
124  {
125  //
126  // Test planar case with 4 points
127  //
128 
129  std::cout << "Start test considering planar case with 4 points..." << std::endl;
130  std::cout << "===================================================" << std::endl;
131 
132  //vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0, vpMath::rad(10));
133  vpPoseVector cpo_ref = vpPoseVector(-0.01, -0.02, 0.3, vpMath::rad(20), vpMath::rad(-20), vpMath::rad(10));
134  vpHomogeneousMatrix cMo_ref(cpo_ref);
135 
136  int npt = 4;
137  std::vector<vpPoint> P(npt); // Point to be tracked
138  double Z = 0.05; // FS: Dementhon estimation is not good when Z=0.3
139 
140  P[0].setWorldCoordinates(-L, -L, Z);
141  P[1].setWorldCoordinates( L, -L, Z);
142  P[2].setWorldCoordinates( L, L, Z);
143  P[3].setWorldCoordinates(-L, L, Z);
144 
145  vpPose pose;
146 
147  for (int i = 0; i < npt; i++) {
148  P[i].project(cMo_ref);
149  // P[i].print();
150  pose.addPoint(P[i]); // and added to the pose computation class
151  }
152 
153  // Let's go ...
154 
155  print_pose(cMo_ref, std::string("Reference pose"));
156 
157  std::cout << "-------------------------------------------------" << std::endl;
158  pose.computePose(vpPose::LAGRANGE, cMo);
159 
160  print_pose(cMo, std::string("Pose estimated by Lagrange"));
161  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange");
162  test_planar_fail |= fail;
163 
164  std::cout << "--------------------------------------------------" << std::endl;
165  pose.computePose(vpPose::DEMENTHON, cMo);
166 
167  print_pose(cMo, std::string("Pose estimated by Dementhon"));
168  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon");
169  test_planar_fail |= fail;
170 
171  std::cout << "--------------------------------------------------" << std::endl;
172 
174  pose.setRansacThreshold(0.01);
175  pose.computePose(vpPose::RANSAC, cMo);
176 
177  print_pose(cMo, std::string("Pose estimated by Ransac"));
178  fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac");
179  test_planar_fail |= fail;
180 
181  std::cout << "--------------------------------------------------" << std::endl;
183 
184  print_pose(cMo, std::string("Pose estimated by Lagrange then Lowe"));
185  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange then Lowe");
186  test_planar_fail |= fail;
187 
188  std::cout << "--------------------------------------------------" << std::endl;
190 
191  print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
192  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon then Lowe");
193  test_planar_fail |= fail;
194 
195  // Now Virtual Visual servoing
196  std::cout << "--------------------------------------------------" << std::endl;
197  pose.computePose(vpPose::VIRTUAL_VS, cMo);
198 
199  print_pose(cMo, std::string("Pose estimated by VVS"));
200  fail = compare_pose(pose, cMo_ref, cMo, "pose by VVS");
201  test_planar_fail |= fail;
202 
203  std::cout << "-------------------------------------------------" << std::endl;
205 
206  print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
207  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon then by VVS");
208  test_planar_fail |= fail;
209 
210  std::cout << "-------------------------------------------------" << std::endl;
212 
213  print_pose(cMo, std::string("Pose estimated by Lagrange then by VVS"));
214  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange then by VVS");
215  test_planar_fail |= fail;
216 
217  }
218 
219  {
220  //
221  // Test non-planar case with 6 points (at least 6 points for Lagrange non planar)
222  //
223 
224  std::cout << "\nStart test considering non-planar case with 6 points..." << std::endl;
225  std::cout << "=======================================================" << std::endl;
226 
227  vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0, vpMath::rad(10));
228  vpHomogeneousMatrix cMo_ref(cpo_ref);
229 
230  int npt = 6;
231  std::vector<vpPoint> P(npt); // Point to be tracked
232  P[0].setWorldCoordinates(-L, -L, 0); // Lagrange not accurate...
233  P[0].setWorldCoordinates(-L, -L, -0.02);
234  P[1].setWorldCoordinates( L, -L, 0);
235  P[2].setWorldCoordinates( L, L, 0);
236  P[3].setWorldCoordinates(-2 * L, 3 * L, 0);
237  P[4].setWorldCoordinates(-L, L, 0.01);
238  P[5].setWorldCoordinates( L, L/2., 0.03);
239 
240  vpPose pose;
241 
242  for (int i = 0; i < npt; i++) {
243  P[i].project(cMo_ref);
244  // P[i].print();
245  pose.addPoint(P[i]); // and added to the pose computation class
246  }
247 
248  // Let's go ...
249  print_pose(cMo_ref, std::string("Reference pose"));
250 
251  std::cout << "-------------------------------------------------" << std::endl;
252  pose.computePose(vpPose::LAGRANGE, cMo);
253 
254  print_pose(cMo, std::string("Pose estimated by Lagrange"));
255  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange");
256  test_non_planar_fail |= fail;
257 
258  std::cout << "--------------------------------------------------" << std::endl;
259  pose.computePose(vpPose::DEMENTHON, cMo);
260 
261  print_pose(cMo, std::string("Pose estimated by Dementhon"));
262  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon");
263  test_non_planar_fail |= fail;
264 
265  std::cout << "--------------------------------------------------" << std::endl;
267  pose.setRansacThreshold(0.01);
268  pose.computePose(vpPose::RANSAC, cMo);
269 
270  print_pose(cMo, std::string("Pose estimated by Ransac"));
271  fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac");
272  test_non_planar_fail |= fail;
273 
274  std::cout << "--------------------------------------------------" << std::endl;
276 
277  print_pose(cMo, std::string("Pose estimated by Lagrange then Lowe"));
278  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange then Lowe");
279  test_non_planar_fail |= fail;
280 
281  std::cout << "--------------------------------------------------" << std::endl;
283 
284  print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
285  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon then Lowe");
286  test_non_planar_fail |= fail;
287 
288  // Now Virtual Visual servoing
289 
290  std::cout << "--------------------------------------------------" << std::endl;
291  pose.computePose(vpPose::VIRTUAL_VS, cMo);
292 
293  print_pose(cMo, std::string("Pose estimated by VVS"));
294  fail = compare_pose(pose, cMo_ref, cMo, "pose by VVS");
295  test_non_planar_fail |= fail;
296 
297  std::cout << "-------------------------------------------------" << std::endl;
299 
300  print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
301  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon then by VVS");
302  test_non_planar_fail |= fail;
303 
304  std::cout << "-------------------------------------------------" << std::endl;
306 
307  print_pose(cMo, std::string("Pose estimated by Lagrange then by VVS"));
308  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange then by VVS");
309  test_non_planar_fail |= fail;
310  }
311 
312  //
313  // Test non-planar case with 4 points (Lagrange can not be used)
314  //
315 
316  std::cout << "\nStart test considering non-planar case with 4 points..." << std::endl;
317  std::cout << "=======================================================" << std::endl;
318 
319  {
320  int npt = 4;
321  std::vector<vpPoint> P(npt); // Point to be tracked
322  P[0].setWorldCoordinates(-L2, -L2, 0);
323  P[1].setWorldCoordinates( L2, -L2, 0.2);
324  P[2].setWorldCoordinates( L2, L2, -0.1);
325  P[3].setWorldCoordinates(-L2, L2, 0);
326 
327  vpPose pose;
328 
329  vpPoseVector cpo_ref = vpPoseVector(-0.1, -0.2, 0.8, vpMath::rad(10), vpMath::rad(-10), vpMath::rad(25));
330  vpHomogeneousMatrix cMo_ref(cpo_ref);
331 
332  for (int i = 0; i < npt; i++) {
333  P[i].project(cMo_ref);
334  // P[i].print(); printf("\n");
335  pose.addPoint(P[i]); // and added to the pose computation class
336  }
337 
338  // Let's go ...
339  print_pose(cMo_ref, std::string("Reference pose"));
340 
341  std::cout << "--------------------------------------------------" << std::endl;
342  pose.computePose(vpPose::DEMENTHON, cMo);
343 
344  print_pose(cMo, std::string("Pose estimated by Dementhon"));
345  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon");
346  test_non_planar_fail |= fail;
347 
348  std::cout << "--------------------------------------------------" << std::endl;
350  pose.setRansacThreshold(0.01);
351  pose.computePose(vpPose::RANSAC, cMo);
352 
353  print_pose(cMo, std::string("Pose estimated by Ransac"));
354  fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac");
355  test_non_planar_fail |= fail;
356 
357  std::cout << "--------------------------------------------------" << std::endl;
359 
360  print_pose(cMo, std::string("Pose estimated by Dementhon then Lowe"));
361  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon then Lowe");
362  test_non_planar_fail |= fail;
363 
364  // Now Virtual Visual servoing
365  std::cout << "--------------------------------------------------" << std::endl;
366  pose.computePose(vpPose::VIRTUAL_VS, cMo);
367 
368  print_pose(cMo, std::string("Pose estimated by VVS"));
369  fail = compare_pose(pose, cMo_ref, cMo, "pose by VVS");
370  test_non_planar_fail |= fail;
371 
372  std::cout << "-------------------------------------------------" << std::endl;
374 
375  print_pose(cMo, std::string("Pose estimated by Dementhon then by VVS"));
376  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon then by VVS");
377  test_non_planar_fail |= fail;
378 
379  std::cout << "-------------------------------------------------" << std::endl;
380  }
381 
382  std::cout << "=======================================================" << std::endl;
383  std::cout << "Pose estimation test from planar points: " << (test_planar_fail ? "fail" : "is ok") << std::endl;
384  std::cout << "Pose estimation test from non-planar points: " << (test_non_planar_fail ? "fail" : "is ok") << std::endl;
385  std::cout << "Global pose estimation test: " << ((test_planar_fail | test_non_planar_fail) ? "fail" : "is ok") << std::endl;
386 
387  return ((test_planar_fail | test_non_planar_fail) ? EXIT_FAILURE : EXIT_SUCCESS);
388  } catch (const vpException &e) {
389  std::cout << "Catch an exception: " << e << std::endl;
390  return EXIT_FAILURE;
391  }
392 #else
393  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
394  return EXIT_SUCCESS;
395 #endif
396 }
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
Definition: vpException.h:71
void setRansacThreshold(const double &t)
Definition: vpPose.h:238
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
static double rad(double deg)
Definition: vpMath.h:110
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:237
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
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:336