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;
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
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.
std::vector< vpPoint > getRansacInliers()
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.
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...