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 <<
"Pose computation with matched points" << std::endl;
58 std::vector<vpPoint> P;
60 P.push_back(
vpPoint(-L, -L, 0));
66 P.push_back(
vpPoint(0, -L2, 0));
69 P.push_back(
vpPoint(-L2, 0, 0));
72 for (
size_t i = 0; i < P.size(); i++) {
73 P[i].project(cMo_ref);
75 std::cout << std::endl;
80 P[3].set_y(P[3].get_y() + 2 * error);
81 P[6].set_x(P[6].get_x() + error);
84 for (
size_t i = 0; i < P.size(); i++)
87 unsigned int nbInlierToReachConsensus = (
unsigned int)(75.0 * (
double)(P.size()) / 100.0);
88 double threshold = 0.001;
99 std::cout <<
"Inliers: " << std::endl;
100 for (
unsigned int i = 0; i < inliers.size(); i++) {
102 std::cout << std::endl;
108 std::cout << std::endl;
109 std::cout <<
"reference cMo :\n" << pose_ref.
t() << std::endl << std::endl;
110 std::cout <<
"estimated cMo :\n" << pose_est.
t() << std::endl << std::endl;
113 for (
unsigned int i = 0; i < 6; i++) {
114 if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
118 std::cout <<
"Pose is " << (test_fail ?
"badly" :
"well") <<
" estimated" << std::endl;
119 return (test_fail ? EXIT_FAILURE : EXIT_SUCCESS);
121 std::cout <<
"Catch an exception: " << e << std::endl;
125 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.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Implementation of a pose vector and operations on poses.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void addPoint(const vpPoint &P)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
std::vector< vpPoint > getRansacInliers() const
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
void setRansacThreshold(const double &t)