36 #include <visp3/core/vpHomogeneousMatrix.h>
37 #include <visp3/core/vpMath.h>
38 #include <visp3/core/vpPoint.h>
39 #include <visp3/vision/vpPose.h>
55 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
57 std::cout <<
"Find Matches using Ransac" << std::endl;
58 std::vector<vpPoint> P;
60 P.push_back(
vpPoint(-L, -L, 0));
64 P.push_back(
vpPoint(-0, L / 2., L));
68 std::vector<vpPoint> p(P.size());
69 for (
unsigned int i = 0; i < P.size(); i++) {
72 p[i].set_x(pt.
get_x());
73 p[i].set_y(pt.
get_y());
76 unsigned int ninliers;
77 std::vector<vpPoint> inliers;
78 double threshold = 1e-6;
79 unsigned int nbInlierToReachConsensus = (
unsigned int)(P.size());
83 vpPose::findMatch(p, P, nbInlierToReachConsensus, threshold, ninliers, inliers, cMo);
85 std::cout <<
"Inliers: " << std::endl;
86 for (
unsigned int i = 0; i < inliers.size(); i++) {
88 std::cout << std::endl;
91 std::cout <<
"cMo :\n" <<
vpPoseVector(cMo).
t() << std::endl << std::endl;
96 std::cout << std::endl;
97 std::cout <<
"reference cMo :\n" << pose_ref.
t() << std::endl << std::endl;
98 std::cout <<
"estimated cMo :\n" << pose_est.
t() << std::endl << std::endl;
101 for (
unsigned int i = 0; i < 6; i++) {
102 if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
106 std::cout <<
"Matching is " << (test_fail ?
"badly" :
"well") <<
" performed" << std::endl;
108 return (test_fail ? EXIT_FAILURE : EXIT_SUCCESS);
110 std::cout <<
"Catch an exception: " << e << std::endl;
114 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
error that can be emitted by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_y() const
Get the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
Implementation of a pose vector and operations on poses.
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, bool useParallelRansac=true, unsigned int nthreads=0, bool(*func)(const vpHomogeneousMatrix &)=nullptr)