39 #include <visp3/core/vpCameraParameters.h>
40 #include <visp3/core/vpDisplay.h>
41 #include <visp3/core/vpException.h>
42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 #include <visp3/core/vpUniRand.h>
45 #include <visp3/vision/vpPose.h>
46 #include <visp3/vision/vpPoseException.h>
47 #ifdef VISP_HAVE_HOMOGRAPHY
48 #include <visp3/vision/vpHomography.h>
56 #ifndef DOXYGEN_SHOULD_SKIP_THIS
59 const int def_vvsIterMax = 200;
60 const unsigned int def_ransacNbInlier = 4;
61 const int def_ransacMaxTrials = 1000;
66 : npt(0), listP(), residual(0), m_lambda(0.9), m_dementhonSvThresh(1e-6), vvsIterMax(def_vvsIterMax), c3d(),
67 computeCovariance(false), covarianceMatrix(),
68 ransacNbInlierConsensus(def_ransacNbInlier), ransacMaxTrials(def_ransacMaxTrials), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
69 distToPlaneForCoplanarityTest(0.001), ransacFlag(
vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false),
70 nbParallelRansacThreads(0),
75 : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), m_lambda(0.9),
76 m_dementhonSvThresh(1e-6), vvsIterMax(def_vvsIterMax),
77 c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(def_ransacNbInlier), ransacMaxTrials(def_ransacMaxTrials),
78 ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distToPlaneForCoplanarityTest(0.001),
79 ransacFlag(
vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false),
80 nbParallelRansacThreads(0),
98 listP.push_back(newP);
99 listOfPoints.push_back(newP);
105 listP.insert(
listP.end(), lP.begin(), lP.end());
106 listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
107 npt =
static_cast<unsigned int>(
listP.size());
120 bool vpPose::coplanar(
int &coplanar_plane_type,
double *p_a,
double *p_b,
double *p_c,
double *p_d)
122 coplanar_plane_type = 0;
123 const unsigned int nbMinPt = 2;
128 const unsigned int nbPtPlan = 3;
129 if (
npt == nbPtPlan) {
134 std::vector<vpPoint> shuffled_listP = vpUniRand::shuffleVector<vpPoint>(listOfPoints);
136 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
138 std::vector<vpPoint>::const_iterator it = shuffled_listP.begin();
143 bool degenerate =
true;
144 bool not_on_origin =
true;
145 std::vector<vpPoint>::const_iterator it_tmp;
147 std::vector<vpPoint>::const_iterator it_i, it_j, it_k;
148 std::vector<vpPoint>::const_iterator shuffled_listp_end = shuffled_listP.end();
149 for (it_i = shuffled_listP.begin(); it_i != shuffled_listp_end; ++it_i) {
150 if (degenerate ==
false) {
156 if ((std::fabs(P1.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
157 (std::fabs(P1.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
158 (std::fabs(P1.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
159 not_on_origin =
false;
162 not_on_origin =
true;
167 std::vector<vpPoint>::const_iterator shuffled_listp_end_1 = shuffled_listP.end();
168 for (it_j = it_tmp; it_j != shuffled_listp_end_1; ++it_j) {
169 if (degenerate ==
false) {
174 if ((std::fabs(P2.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
175 (std::fabs(P2.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
176 (std::fabs(P2.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
177 not_on_origin =
false;
180 not_on_origin =
true;
185 std::vector<vpPoint>::const_iterator shuffled_listp_end_2 = shuffled_listP.end();
186 for (it_k = it_tmp; it_k != shuffled_listp_end_2; ++it_k) {
188 if ((std::fabs(P3.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
189 (std::fabs(P3.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
190 (std::fabs(P3.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
191 not_on_origin =
false;
194 not_on_origin =
true;
209 const int idX = 0, idY = 1, idZ = 2;
219 if (cross_prod.
sumSquare() <= std::numeric_limits<double>::epsilon()) {
226 if (degenerate ==
false) {
236 const int typeCollinear = 4;
237 coplanar_plane_type = typeCollinear;
241 double a = (((y1 * z2) - (y1 * z3) - (y2 * z1)) + (y2 * z3) + (y3 * z1)) - (y3 * z2);
242 double b = ((((-x1 * z2) + (x1 * z3) + (x2 * z1)) - (x2 * z3)) - (x3 * z1)) + (x3 * z2);
243 double c = (((x1 * y2) - (x1 * y3) - (x2 * y1)) + (x2 * y3) + (x3 * y1)) - (x3 * y2);
244 double d = (((-x1 * y2 * z3) + (x1 * y3 * z2) + (x2 * y1 * z3)) - (x2 * y3 * z1) - (x3 * y1 * z2)) + (x3 * y2 * z1);
246 if ((std::fabs(b) <= std::numeric_limits<double>::epsilon()) &&
247 (std::fabs(c) <= std::numeric_limits<double>::epsilon())) {
248 const int typeAxD = 1;
249 coplanar_plane_type = typeAxD;
251 else if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
252 (std::fabs(c) <= std::numeric_limits<double>::epsilon())) {
253 const int typeByD = 2;
254 coplanar_plane_type = typeByD;
256 else if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
257 (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
258 const int typeCzD = 3;
259 coplanar_plane_type = typeCzD;
264 std::vector<vpPoint>::const_iterator shuffled_listp_end_decl2 = shuffled_listP.end();
265 for (it = shuffled_listP.begin(); it != shuffled_listp_end_decl2; ++it) {
269 if (fabs(dist) > distToPlaneForCoplanarityTest) {
279 if (p_a !=
nullptr) {
283 if (p_b !=
nullptr) {
287 if (p_c !=
nullptr) {
291 if (p_d !=
nullptr) {
300 double squared_error = 0;
302 std::list<vpPoint>::const_iterator listp_end =
listP.end();
303 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
305 double x = P.
get_x();
306 double y = P.
get_y();
312 return squared_error;
323 double squared_error = 0;
324 residuals.
resize(
static_cast<unsigned int>(
listP.size()));
327 std::list<vpPoint>::const_iterator listp_end =
listP.end();
328 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
330 double x = P.
get_x();
331 double y = P.
get_y();
333 double u_initial = 0., v_initial = 0.;
338 double u_moved = 0., v_moved = 0.;
342 residuals[i] = squaredResidual;
344 squared_error += squaredResidual;
346 return squared_error;
351 const unsigned int minNbPtLagrangePlan = 4;
352 const unsigned int minNbPtLagrangeNoPlan = 6;
355 int coplanar_plane_type = 0;
356 bool plan =
coplanar(coplanar_plane_type, &a, &b, &c, &d);
359 const int typeCollinear = 4;
360 if (coplanar_plane_type == typeCollinear) {
362 "(points are collinear)"));
364 if (
npt < minNbPtLagrangePlan) {
366 "Lagrange method cannot be used in that case "
367 "(at least 4 points are required). "
368 "Not enough point (%d) to compute the pose ",
374 if (
npt < minNbPtLagrangeNoPlan) {
376 "Lagrange method cannot be used in that case "
377 "(at least 6 points are required when 3D points are non coplanar). "
378 "Not enough point (%d) to compute the pose ",
387 const unsigned int minNbPtDementhon = 4;
388 const unsigned int minNbPtRansac = 4;
389 std::stringstream errMsgDementhon;
390 errMsgDementhon <<
"Dementhon method cannot be used in that case "
391 <<
"(at least " << minNbPtDementhon <<
" points are required)"
392 <<
"Not enough point (" <<
npt <<
") to compute the pose ";
398 if (
npt < minNbPtDementhon) {
402 int coplanar_plane_type = 0;
403 bool plan =
coplanar(coplanar_plane_type);
410 callLagrangePose(cMo);
414 if (
npt < minNbPtRansac) {
416 "Ransac method cannot be used in that case "
417 "(at least 4 points are required). "
418 "Not enough point (%d) to compute the pose ",
431 std::cout <<
"method not identified" << std::endl;
456 std::cout <<
"method not identified" << std::endl;
466 double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
469 int coplanar_plane_type = 0;
470 bool plan =
coplanar(coplanar_plane_type, &a, &b, &c, &d);
471 bool hasDementhonSucceeded(
false), hasLagrangeSucceeded(
false);
481 hasDementhonSucceeded =
true;
496 hasDementhonSucceeded =
true;
500 hasDementhonSucceeded =
false;
514 hasLagrangeSucceeded =
true;
533 hasLagrangeSucceeded =
true;
537 hasLagrangeSucceeded =
false;
541 if (hasDementhonSucceeded || hasLagrangeSucceeded) {
544 cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
557 std::list<vpPoint>::const_iterator listp_end =
listP.end();
558 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
561 std::cout <<
"3D oP " << P.
oP.
t();
562 std::cout <<
"3D cP " << P.
cP.
t();
563 std::cout <<
"2D " << P.
p.
t();
582 std::list<vpPoint>::const_iterator listp_end =
listP.end();
583 const unsigned int sizeCross = 5;
584 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
595 std::list<vpPoint>::const_iterator listp_end =
listP.end();
596 const unsigned int sizeCross = 5;
598 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
605 #ifdef VISP_HAVE_HOMOGRAPHY
609 const unsigned int id0 = 0, id1 = 1, id2 = 2, id3 = 3;
610 std::vector<double> rectx(4);
611 std::vector<double> recty(4);
620 std::vector<double> irectx(4);
621 std::vector<double> irecty(4);
622 irectx[id0] = (p1.
get_x());
623 irecty[id0] = (p1.
get_y());
624 irectx[id1] = (p2.
get_x());
625 irecty[id1] = (p2.
get_y());
626 irectx[id2] = (p3.
get_x());
627 irecty[id2] = (p3.
get_y());
628 irectx[id3] = (p4.
get_x());
629 irecty[id3] = (p4.
get_y());
636 const unsigned int val_3 = 3;
637 for (
unsigned int i = 0; i < val_3; ++i) {
638 for (
unsigned int j = 0; j < val_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)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
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)
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
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 emitted by the vpPose class and its derivatives.
@ notEnoughPointError
Not enough points to compute the pose.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void addPoint(const vpPoint &P)
vpPoseMethodType
Methods that could be used to estimate the pose from points.
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
unsigned int npt
Number of point used in pose computation.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
void setDistToPlaneForCoplanTest(double d)
void addPoints(const std::vector< vpPoint > &lP)
void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan=nullptr, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
double m_dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
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 coplanar(int &coplanar_plane_type, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo)
Method that first computes the pose cMo using the linear approaches of Dementhon and Lagrange and the...
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)
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
bool poseRansac(vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
void poseVirtualVS(vpHomogeneousMatrix &cMo)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
void setDementhonSvThreshold(const double &svThresh)