Visual Servoing Platform version 3.5.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
64void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend);
65int 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
69void 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
85int 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
116int 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;
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;
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;
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;
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;
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;
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}
error that can be emited by ViSP classes.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:81
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:235
@ DEMENTHON
Definition: vpPose.h:86
@ LAGRANGE_LOWE
Definition: vpPose.h:91
@ RANSAC
Definition: vpPose.h:90
@ DEMENTHON_LOWE
Definition: vpPose.h:93
@ LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:99
@ VIRTUAL_VS
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:85
@ DEMENTHON_VIRTUAL_VS
Definition: vpPose.h:97
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
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
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
void setRansacThreshold(const double &t)
Definition: vpPose.h:236