46 #include <visp3/core/vpCameraParameters.h>
47 #include <visp3/core/vpDebug.h>
48 #include <visp3/core/vpDisplay.h>
49 #include <visp3/core/vpException.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/vision/vpPose.h>
53 #include <visp3/vision/vpPoseException.h>
58 #define DEBUG_LEVEL1 0
59 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
66 std::cout <<
"begin vpPose::Init() " << std::endl;
75 computeCovariance =
false;
76 covarianceMatrix.
clear();
77 ransacNbInlierConsensus = 4;
78 ransacMaxTrials = 1000;
79 ransacInliers.clear();
80 ransacInlierIndex.clear();
81 ransacThreshold = 0.0001;
82 distanceToPlaneForCoplanarityTest = 0.001;
85 useParallelRansac =
false;
86 nbParallelRansacThreads = 0;
90 std::cout <<
"end vpPose::Init() " << std::endl;
97 : npt(0), listP(), residual(0), lambda(0.9), vvsIterMax(200), c3d(), computeCovariance(false), covarianceMatrix(),
98 ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
99 distanceToPlaneForCoplanarityTest(0.001), ransacFlag(
vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false),
100 nbParallelRansacThreads(0),
106 : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), lambda(0.9), vvsIterMax(200),
107 c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000),
108 ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001),
109 ransacFlag(
vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false),
110 nbParallelRansacThreads(0),
121 std::cout <<
"begin vpPose::~vpPose() " << std::endl;
127 std::cout <<
"end vpPose::~vpPose() " << std::endl;
136 listOfPoints.clear();
150 listP.push_back(newP);
151 listOfPoints.push_back(newP);
165 listP.insert(
listP.end(), lP.begin(), lP.end());
166 listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
185 coplanar_plane_type = 0;
194 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
196 std::list<vpPoint>::const_iterator it =
listP.begin();
201 bool degenerate =
true;
202 bool not_on_origin =
true;
203 std::list<vpPoint>::const_iterator it_tmp;
205 std::list<vpPoint>::const_iterator it_i, it_j, it_k;
206 for (it_i =
listP.begin(); it_i !=
listP.end(); ++it_i) {
207 if (degenerate ==
false) {
213 if ((std::fabs(P1.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
214 (std::fabs(P1.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
215 (std::fabs(P1.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
216 not_on_origin =
false;
218 not_on_origin =
true;
223 for (it_j = it_tmp; it_j !=
listP.end(); ++it_j) {
224 if (degenerate ==
false) {
229 if ((std::fabs(P2.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
230 (std::fabs(P2.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
231 (std::fabs(P2.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
232 not_on_origin =
false;
234 not_on_origin =
true;
239 for (it_k = it_tmp; it_k !=
listP.end(); ++it_k) {
241 if ((std::fabs(P3.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
242 (std::fabs(P3.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
243 (std::fabs(P3.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
244 not_on_origin =
false;
246 not_on_origin =
true;
270 if (cross_prod.
sumSquare() <= std::numeric_limits<double>::epsilon())
275 if (degenerate ==
false)
284 coplanar_plane_type = 4;
288 double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
289 double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
290 double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
291 double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
295 if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
296 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
297 coplanar_plane_type = 1;
298 }
else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
299 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
300 coplanar_plane_type = 2;
301 }
else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
302 std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
303 coplanar_plane_type = 3;
308 for (it =
listP.begin(); it !=
listP.end(); ++it) {
313 if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
337 double squared_error = 0;
339 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
341 double x = P.
get_x();
342 double y = P.
get_y();
348 return (squared_error);
385 "Dementhon method cannot be used in that case "
386 "(at least 4 points are required)"
387 "Not enough point (%d) to compute the pose ",
392 int coplanar_plane_type = 0;
393 bool plan =
coplanar(coplanar_plane_type);
404 int coplanar_plane_type;
405 bool plan =
coplanar(coplanar_plane_type);
409 if (coplanar_plane_type == 4) {
411 "(points are collinear)"));
415 "Lagrange method cannot be used in that case "
416 "(at least 4 points are required). "
417 "Not enough point (%d) to compute the pose ",
424 "Lagrange method cannot be used in that case "
425 "(at least 6 points are required when 3D points are non coplanar). "
426 "Not enough point (%d) to compute the pose ",
435 "Ransac method cannot be used in that case "
436 "(at least 4 points are required). "
437 "Not enough point (%d) to compute the pose ",
471 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
474 std::cout <<
"3D oP " << P.
oP.
t();
475 std::cout <<
"3D cP " << P.
cP.
t();
476 std::cout <<
"2D " << P.
p.
t();
517 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
535 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
568 std::vector<double> rectx(4);
569 std::vector<double> recty(4);
578 std::vector<double> irectx(4);
579 std::vector<double> irecty(4);
580 irectx[0] = (p1.
get_x());
581 irecty[0] = (p1.
get_y());
582 irectx[1] = (p2.
get_x());
583 irecty[1] = (p2.
get_y());
584 irectx[2] = (p3.
get_x());
585 irecty[2] = (p3.
get_y());
586 irectx[3] = (p4.
get_x());
587 irecty[3] = (p4.
get_y());
595 for (
unsigned int i = 0; i < 3; i++)
596 for (
unsigned int j = 0; j < 3; j++)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
Class to define RGB colors available for display functionnalities.
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
static void HLM(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, bool isplanar, vpHomography &aHb)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double sqr(double x)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpColVector getCol(unsigned int j) const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
double get_y() const
Get the point y coordinate in the image plane.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
double get_oY() const
Get the point oY coordinate in the object frame.
void setWorldCoordinates(double oX, double oY, double oZ)
Error that can be emited by the vpPose class and its derivates.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
vp_deprecated void init()
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
void addPoint(const vpPoint &P)
vpPoseMethodType
Methods that could be used to estimate the pose from points.
unsigned int npt
Number of point used in pose computation.
void setDistanceToPlaneForCoplanarityTest(double d)
void addPoints(const std::vector< vpPoint > &lP)
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
void poseLagrangePlan(vpHomogeneousMatrix &cMo)
Compute the pose of a planar object using Lagrange approach.
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo)
Carries out the camera pose the image of a rectangle and the intrinsec parameters,...
double lambda
parameters use for the virtual visual servoing approach
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
bool coplanar(int &coplanar_plane_type)
double residual
Residual in meter.
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...