Visual Servoing Platform  version 3.1.0
testPose.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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 
63 void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend);
64 int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
65  const std::string &legend);
66 
67 // print the resulting estimated pose
68 void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend)
69 {
70  vpPoseVector cpo = vpPoseVector(cMo);
71 
72  std::cout << std::endl
73  << legend << "\n "
74  << "tx = " << cpo[0] << "\n "
75  << "ty = " << cpo[1] << "\n "
76  << "tz = " << cpo[2] << "\n "
77  << "tux = vpMath::rad(" << vpMath::deg(cpo[3]) << ")\n "
78  << "tuy = vpMath::rad(" << vpMath::deg(cpo[4]) << ")\n "
79  << "tuz = vpMath::rad(" << vpMath::deg(cpo[5]) << ")\n"
80  << std::endl;
81 }
82 
83 // test if pose is well estimated
84 int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const vpHomogeneousMatrix &cMo_est,
85  const std::string &legend)
86 {
87  vpPoseVector pose_ref = vpPoseVector(cMo_ref);
88  vpPoseVector pose_est = vpPoseVector(cMo_est);
89 
90  int fail = 0;
91 
92  // Test done on the 3D pose
93  for (unsigned int i = 0; i < 6; i++) {
94  if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
95  fail = 1;
96  }
97 
98  std::cout << "Based on 3D parameters " << legend << " is " << (fail ? "badly" : "well") << " estimated" << std::endl;
99 
100  // Test done on the residual
101  double r = pose.computeResidual(cMo_est);
102  if (pose.listP.size() < 4) {
103  fail = 1;
104  std::cout << "Not enough point" << std::endl;
105  return fail;
106  }
107  r = sqrt(r) / pose.listP.size();
108  // std::cout << "Residual on each point (meter): " << r << std::endl;
109  fail = (r > 0.1) ? 1 : 0;
110  std::cout << "Based on 2D residual (" << r << ") " << legend << " is " << (fail ? "badly" : "well") << " estimated"
111  << std::endl;
112  return fail;
113 }
114 
115 int main()
116 {
117  try {
118  vpPoint P[5]; // Point to be tracked
119  vpPose pose;
120  pose.clearPoint();
121 
122  P[0].setWorldCoordinates(-L, -L, 0);
123  P[1].setWorldCoordinates(L, -L, 0);
124  P[2].setWorldCoordinates(L, L, 0);
125  P[3].setWorldCoordinates(-2 * L, 3 * L, 0);
126  P[4].setWorldCoordinates(-L, L, 0.01);
127 
128  int test_fail = 0, fail = 0;
129  vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0, vpMath::rad(10));
130  vpHomogeneousMatrix cMo_ref(cpo_ref);
131  vpHomogeneousMatrix cMo; // will contain the estimated pose
132 
133  for (int i = 0; i < 5; i++) {
134  P[i].project(cMo_ref);
135  // P[i].print();
136  pose.addPoint(P[i]); // and added to the pose computation class
137  }
138 
139  // Let's go ...
140  print_pose(cMo_ref,
141  std::string("Reference pose")); // print the reference pose
142 
143  std::cout << "-------------------------------------------------" << std::endl;
144  pose.computePose(vpPose::LAGRANGE, cMo);
145 
146  print_pose(cMo, std::string("Pose estimated by Lagrange"));
147  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange");
148  test_fail |= fail;
149 
150  std::cout << "--------------------------------------------------" << std::endl;
151  pose.computePose(vpPose::DEMENTHON, cMo);
152 
153  print_pose(cMo, std::string("Pose estimated by Dementhon"));
154  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon");
155  test_fail |= fail;
156 
157  std::cout << "--------------------------------------------------" << std::endl;
159  pose.setRansacThreshold(0.01);
160  pose.computePose(vpPose::RANSAC, cMo);
161 
162  print_pose(cMo, std::string("Pose estimated by Ransac"));
163  fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac");
164  test_fail |= fail;
165 
166  std::cout << "--------------------------------------------------" << std::endl;
168 
169  print_pose(cMo, std::string("Pose estimated by Lagrange than Lowe"));
170  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange than Lowe");
171  test_fail |= fail;
172 
173  std::cout << "--------------------------------------------------" << std::endl;
175 
176  print_pose(cMo, std::string("Pose estimated by Dementhon than Lowe"));
177  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than Lowe");
178  test_fail |= fail;
179 
180  // Now Virtual Visual servoing
181 
182  std::cout << "--------------------------------------------------" << std::endl;
183  pose.computePose(vpPose::VIRTUAL_VS, cMo);
184 
185  print_pose(cMo, std::string("Pose estimated by VVS"));
186  fail = compare_pose(pose, cMo_ref, cMo, "pose by VVS");
187  test_fail |= fail;
188 
189  std::cout << "-------------------------------------------------" << std::endl;
191 
192  print_pose(cMo, std::string("Pose estimated by Dementhon than by VVS"));
193  fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than by VVS");
194  test_fail |= fail;
195 
196  std::cout << "-------------------------------------------------" << std::endl;
198 
199  print_pose(cMo, std::string("Pose estimated by Lagrange than by VVS"));
200  fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange than by VVS");
201  test_fail |= fail;
202 
203  std::cout << "\nGlobal pose estimation test " << (test_fail ? "fail" : "is ok") << std::endl;
204 
205  return test_fail;
206  } catch (vpException &e) {
207  std::cout << "Catch an exception: " << e << std::endl;
208  return 1;
209  }
210 }
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:252
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:115
Class that defines what is a point.
Definition: vpPoint.h:58
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:79
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
Definition: vpPose.cpp:362
static double rad(double deg)
Definition: vpMath.h:102
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:251
static double deg(double rad)
Definition: vpMath.h:95
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix &#39;cMo&#39;.
Definition: vpPose.cpp:324
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:137
void clearPoint()
Definition: vpPose.cpp:122