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;
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setRansacThreshold(const double &t)
std::list< vpPoint > listP
array of point (use here class vpPoint)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
Class used for pose computation from N points (pose from point only).
static double rad(double deg)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
static double deg(double rad)
The pose is a complete representation of every rigid motion in the euclidian space.
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.
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
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