44 #include <visp/vpPose.h>
45 #include <visp/vpPoint.h>
46 #include <visp/vpMath.h>
47 #include <visp/vpHomogeneousMatrix.h>
66 std::cout <<
"Pose computation with matched points" << std::endl;
84 for(
int i=0 ; i < size ; i++)
88 std::cout << std::endl;
93 P[3].
set_y(P[3].get_y() + 2*error);
94 P[6].
set_x(P[6].get_x() + error);
97 for(
int i=0 ; i < size ; i++)
100 unsigned int nbInlierToReachConsensus = (
unsigned int)(75.0 * (
double)size / 100.0);
101 double threshold = 0.001;
112 std::cout <<
"Inliers: " << std::endl;
113 for (
unsigned int i = 0; i < inliers.size() ; i++)
116 std::cout << std::endl;
122 std::cout << std::endl;
123 std::cout <<
"reference cMo :\n" << pose_ref.
t() << std::endl << std::endl;
124 std::cout <<
"estimated cMo :\n" << pose_est.
t() << std::endl << std::endl;
127 for(
unsigned int i=0; i<6; i++) {
128 if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
132 std::cout <<
"Pose is " << (test_fail ?
"badly" :
"well") <<
" estimated" << std::endl;
137 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.
void setRansacThreshold(const double &t)
void set_x(const double x)
Set the point x coordinate in the image plane.
Class that defines what is a point.
virtual void print() const
vpRowVector t() const
transpose of Vector
Class used for pose computation from N points (pose from point only).
void set_y(const double y)
Set the point y coordinate in the image plane.
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
The pose is a complete representation of every rigid motion in the euclidian space.
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
void addPoint(const vpPoint &P)
Add a new point in this array.
std::vector< vpPoint > getRansacInliers() const
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...