44 #include <visp/vpPose.h>
45 #include <visp/vpPoint.h>
46 #include <visp/vpMath.h>
47 #include <visp/vpHomogeneousMatrix.h>
65 std::cout <<
"Pose computation with matched points" << std::endl;
83 for(
int i=0 ; i < size ; i++)
87 std::cout << std::endl;
92 P[3].
set_y(P[3].get_y() + 2*error);
93 P[6].
set_x(P[6].get_x() + error);
96 for(
int i=0 ; i < size ; i++)
99 unsigned int nbInlierToReachConsensus = (
unsigned int)(75.0 * (
double)size / 100.0);
100 double threshold = 0.001;
111 std::cout <<
"Inliers: " << std::endl;
112 for (
unsigned int i = 0; i < inliers.size() ; i++)
115 std::cout << std::endl;
121 std::cout << std::endl;
122 std::cout <<
"reference cMo :\n" << pose_ref.
t() << std::endl << std::endl;
123 std::cout <<
"estimated cMo :\n" << pose_est.
t() << std::endl << std::endl;
126 for(
unsigned int i=0; i<6; i++) {
127 if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
131 std::cout <<
"Pose is " << (test_fail ?
"badly" :
"well") <<
" estimated" << std::endl;