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;
129 std::cout << std::endl << legend <<
"\n "
130 <<
"tx = " << cpo[0] <<
"\n "
131 <<
"ty = " << cpo[1] <<
"\n "
132 <<
"tz = " << cpo[2] <<
"\n "
133 <<
"tux = vpMath::rad(" <<
vpMath::deg(cpo[3]) <<
")\n "
134 <<
"tuy = vpMath::rad(" <<
vpMath::deg(cpo[4]) <<
")\n "
135 <<
"tuz = vpMath::rad(" <<
vpMath::deg(cpo[5]) <<
")\n"
148 for(
unsigned int i=0; i<6; i++) {
149 if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
152 std::cout <<
"Based on 3D parameters " << legend <<
" is " << (fail ?
"badly" :
"well") <<
" estimated" << std::endl;
156 r = sqrt(r)/pose.
listP.size();
158 fail = (r > 0.1) ? 1 : 0;
159 std::cout <<
"Based on 2D residual (" << r <<
") " << legend <<
" is " << (fail ?
"badly" :
"well") <<
" estimated" << std::endl;
164 main(
int argc,
const char ** argv)
167 if (getOptions(argc, argv) ==
false) {
182 int test_fail = 0, fail = 0;
187 for(
int i=0 ; i < 5 ; i++) {
194 print_pose(cMo_ref, std::string(
"Reference pose"));
196 std::cout <<
"-------------------------------------------------"<<std::endl ;
199 print_pose(cMo, std::string(
"Pose estimated by Lagrange"));
200 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Lagrange");
203 std::cout <<
"--------------------------------------------------"<<std::endl ;
206 print_pose(cMo, std::string(
"Pose estimated by Dementhon"));
207 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Dementhon");
210 std::cout <<
"--------------------------------------------------"<<std::endl ;
215 print_pose(cMo, std::string(
"Pose estimated by Ransac"));
216 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Ransac");
219 std::cout <<
"--------------------------------------------------"<<std::endl ;
222 print_pose(cMo, std::string(
"Pose estimated by Lagrange than Lowe"));
223 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Lagrange than Lowe");
226 std::cout <<
"--------------------------------------------------"<<std::endl ;
229 print_pose(cMo, std::string(
"Pose estimated by Dementhon than Lowe"));
230 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Dementhon than Lowe");
235 std::cout <<
"--------------------------------------------------"<<std::endl ;
238 print_pose(cMo, std::string(
"Pose estimated by VVS"));
239 fail = compare_pose(pose, cMo_ref, cMo,
"pose by VVS");
242 std::cout <<
"-------------------------------------------------"<<std::endl ;
245 print_pose(cMo, std::string(
"Pose estimated by Dementhon than by VVS"));
246 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Dementhon than by VVS");
249 std::cout <<
"-------------------------------------------------"<<std::endl ;
252 print_pose(cMo, std::string(
"Pose estimated by Lagrange than by VVS"));
253 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Lagrange than by VVS");
256 std::cout <<
"\nGlobal pose estimation test " << (test_fail ?
"fail" :
"is ok") << std::endl;