44 #include <visp/vpPose.h>
45 #include <visp/vpPoint.h>
46 #include <visp/vpMath.h>
47 #include <visp/vpHomogeneousMatrix.h>
66 std::cout <<
"Find Matches using Ransac" << std::endl;
67 unsigned int nb3D = 5;
68 unsigned int nb2D = 5;
69 std::vector<vpPoint> P(nb3D);
70 std::vector<vpPoint> p(nb2D);
72 P[0].setWorldCoordinates(-L,-L, 0 ) ;
73 P[1].setWorldCoordinates(L,-L, 0 ) ;
74 P[2].setWorldCoordinates(L,L, 0 ) ;
75 P[3].setWorldCoordinates(-L,L, 0 ) ;
76 P[4].setWorldCoordinates(-0,L/2., L ) ;
80 for(
unsigned int i=0 ; i < nb3D ; i++)
84 p[i].set_x(pt.
get_x());
85 p[i].set_y(pt.
get_y());
88 unsigned int ninliers ;
89 std::vector<vpPoint> inliers;
90 double threshold = 1e-6;
91 unsigned int nbInlierToReachConsensus = nb3D;
97 std::cout <<
"Inliers: " << std::endl;
98 for (
unsigned int i = 0; i < inliers.size() ; i++)
101 std::cout << std::endl;
104 std::cout <<
"cMo :\n" <<
vpPoseVector(cMo).
t() << std::endl << std::endl;
109 std::cout << std::endl;
110 std::cout <<
"reference cMo :\n" << pose_ref.
t() << std::endl << std::endl;
111 std::cout <<
"estimated cMo :\n" << pose_est.
t() << std::endl << std::endl;
114 for(
unsigned int i=0; i<6; i++) {
115 if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
119 std::cout <<
"Matching is " << (test_fail ?
"badly" :
"well") <<
" performed" << std::endl;
124 std::cout <<
"Catch an exception: " << e << std::endl;
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
error that can be emited by ViSP classes.
double get_y() const
Get the point y coordinate in the image plane.
Class that defines what is a point.
static void findMatch(std::vector< vpPoint > &p2D, std::vector< vpPoint > &p3D, const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector< vpPoint > &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials=10000)
vpRowVector t() const
transpose of Vector
double get_x() const
Get the point x coordinate in the image plane.
static double rad(double deg)
The pose is a complete representation of every rigid motion in the euclidian space.