44 #include <visp/vpPose.h>
45 #include <visp/vpPoint.h>
46 #include <visp/vpMath.h>
47 #include <visp/vpTranslationVector.h>
48 #include <visp/vpRxyzVector.h>
49 #include <visp/vpRotationMatrix.h>
50 #include <visp/vpHomogeneousMatrix.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpParseArgv.h>
58 #define GETOPTARGS "h"
74 void usage(
const char *name,
const char *badparam)
77 Compute the pose of a 3D object using the Dementhon, Lagrange and\n\
78 Non-Linear approach.\n\
89 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
98 bool getOptions(
int argc,
const char **argv)
105 case 'h': usage(argv[0], NULL);
return false;
break;
108 usage(argv[0], optarg);
113 if ((c == 1) || (c == -1)) {
115 usage(argv[0], NULL);
116 std::cerr <<
"ERROR: " << std::endl;
117 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
126 main(
int argc,
const char ** argv)
129 if (getOptions(argc, argv) ==
false) {
149 for(i=0 ; i < 4 ; i++)
150 P[i].project(cMo_ref) ;
153 for (i=0 ; i < 4 ; i++)
161 std::cout <<
"-------------------------------------------------"<<std::endl ;
164 std::cout << cMo << std::endl ;
165 std::cout <<
"Lagrange residual term: " << pose.
computeResidual(cMo) <<std::endl ;
171 std::cout <<
"\nPose Lagrange "
172 <<
"(residual: " << residual <<
")\n "
173 <<
"cdto[0] = " << cto[0] <<
";\n "
174 <<
"cdto[1] = " << cto[1] <<
";\n "
175 <<
"cdto[2] = " << cto[2] <<
";\n "
176 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
177 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
178 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
182 std::cout <<
"--------------------------------------------------"<<std::endl ;
185 std::cout << cMo << std::endl ;
186 std::cout <<
"Lowe residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
193 std::cout <<
"\nPose Lowe "
194 <<
"(residual: " << residual <<
")\n "
195 <<
"cdto[0] = " << cto[0] <<
";\n "
196 <<
"cdto[1] = " << cto[1] <<
";\n "
197 <<
"cdto[2] = " << cto[2] <<
";\n "
198 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
199 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
200 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
203 std::cout <<
"--------------------------------------------------"<<std::endl ;
205 vpTRACE(
"DEMENTHON pose :" ) ;
206 std::cout << cMo << std::endl ;
215 std::cout <<
"\nPose Dementhon "
216 <<
"(residual: " << residual <<
")\n "
217 <<
"cdto[0] = " << cto[0] <<
";\n "
218 <<
"cdto[1] = " << cto[1] <<
";\n "
219 <<
"cdto[2] = " << cto[2] <<
";\n "
220 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
221 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
222 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
225 std::cout <<
"--------------------------------------------------"<<std::endl ;
227 std::cout <<
"Lowe residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
237 std::cout <<
"\nPose lowe "
238 <<
"(residual: " << residual <<
")\n "
239 <<
"cdto[0] = " << cto[0] <<
";\n "
240 <<
"cdto[1] = " << cto[1] <<
";\n "
241 <<
"cdto[2] = " << cto[2] <<
";\n "
242 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
243 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
244 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
247 std::cout <<std::endl << std::endl ;
248 std::cout <<
"--------------------------------------------------"<<std::endl ;
249 std::cout <<
"Virtual Visual servoing " << std::endl ;
251 std::cout <<
"--------------------------------------------------"<<std::endl ;
254 std::cout << cMo << std::endl ;
263 std::cout <<
"\nPose Lagrange "
264 <<
"(residual: " << residual <<
")\n "
265 <<
"cdto[0] = " << cto[0] <<
";\n "
266 <<
"cdto[1] = " << cto[1] <<
";\n "
267 <<
"cdto[2] = " << cto[2] <<
";\n "
268 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
269 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
270 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
274 std::cout <<
"--------------------------------------------------"<<std::endl ;
276 vpTRACE(
"VIRTUAL_VS pose :" ) ;
277 std::cout << cMo << std::endl ;
278 std::cout <<
"vvs residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
285 std::cout <<
"\nPose VVS "
286 <<
"(residual: " << residual <<
")\n "
287 <<
"cdto[0] = " << cto[0] <<
";\n "
288 <<
"cdto[1] = " << cto[1] <<
";\n "
289 <<
"cdto[2] = " << cto[2] <<
";\n "
290 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
291 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
292 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
295 std::cout <<
"-------------------------------------------------"<<std::endl ;
297 vpTRACE(
"DEMENTHON pose :" ) ;
298 std::cout << cMo << std::endl ;
307 std::cout <<
"\nPose Dementhon "
308 <<
"(residual: " << residual <<
")\n "
309 <<
"cdto[0] = " << cto[0] <<
";\n "
310 <<
"cdto[1] = " << cto[1] <<
";\n "
311 <<
"cdto[2] = " << cto[2] <<
";\n "
312 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
313 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
314 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
317 std::cout <<
"-------------------------------------------------"<<std::endl ;
319 std::cout <<
"vvs residual term: " <<pose.
computeResidual(cMo) <<std::endl ;
326 std::cout <<
"\nPose VVS "
327 <<
"(residual: " << residual <<
")\n "
328 <<
"cdto[0] = " << cto[0] <<
";\n "
329 <<
"cdto[1] = " << cto[1] <<
";\n "
330 <<
"cdto[2] = " << cto[2] <<
";\n "
331 <<
"cdro[0] = vpMath::rad(" <<
vpMath::deg(cro[0]) <<
");\n "
332 <<
"cdro[1] = vpMath::rad(" <<
vpMath::deg(cro[1]) <<
");\n "
333 <<
"cdro[2] = vpMath::rad(" <<
vpMath::deg(cro[2]) <<
");\n"
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
The vpRotationMatrix considers the particular case of a rotation matrix.
Class used for pose computation from N points (pose from point only).
double computeResidual(vpHomogeneousMatrix &cMo)
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
void extract(vpRotationMatrix &R) const
static double rad(double deg)
static double deg(double rad)
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
void addPoint(const vpPoint &P)
Add a new point in this array.
void buildFrom(const double phi, const double theta, const double psi)
Class that consider the case of a translation vector.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
void clearPoint()
suppress all the point in the array of point