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> 65 const std::string &legend);
72 std::cout << std::endl
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" 85 const std::string &legend)
93 for (
unsigned int i = 0; i < 6; i++) {
94 if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
98 std::cout <<
"Based on 3D parameters " << legend <<
" is " << (fail ?
"badly" :
"well") <<
" estimated" << std::endl;
102 if (pose.
listP.size() < 4) {
104 std::cout <<
"Not enough point" << std::endl;
107 r = sqrt(r) / pose.
listP.size();
109 fail = (r > 0.1) ? 1 : 0;
110 std::cout <<
"Based on 2D residual (" << r <<
") " << legend <<
" is " << (fail ?
"badly" :
"well") <<
" estimated" 128 int test_fail = 0, fail = 0;
133 for (
int i = 0; i < 5; i++) {
141 std::string(
"Reference pose"));
143 std::cout <<
"-------------------------------------------------" << std::endl;
146 print_pose(cMo, std::string(
"Pose estimated by Lagrange"));
147 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Lagrange");
150 std::cout <<
"--------------------------------------------------" << std::endl;
153 print_pose(cMo, std::string(
"Pose estimated by Dementhon"));
154 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Dementhon");
157 std::cout <<
"--------------------------------------------------" << std::endl;
162 print_pose(cMo, std::string(
"Pose estimated by Ransac"));
163 fail = compare_pose(pose, cMo_ref, cMo,
"pose by Ransac");
166 std::cout <<
"--------------------------------------------------" << std::endl;
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");
173 std::cout <<
"--------------------------------------------------" << std::endl;
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");
182 std::cout <<
"--------------------------------------------------" << std::endl;
185 print_pose(cMo, std::string(
"Pose estimated by VVS"));
186 fail = compare_pose(pose, cMo_ref, cMo,
"pose by VVS");
189 std::cout <<
"-------------------------------------------------" << std::endl;
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");
196 std::cout <<
"-------------------------------------------------" << std::endl;
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");
203 std::cout <<
"\nGlobal pose estimation test " << (test_fail ?
"fail" :
"is ok") << std::endl;
207 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
void setRansacThreshold(const double &t)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Class that defines what is a point.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
static double rad(double deg)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
static double deg(double rad)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of a pose vector and operations on poses.
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
void addPoint(const vpPoint &P)