39 #include <visp3/core/vpCameraParameters.h>
40 #include <visp3/core/vpDebug.h>
41 #include <visp3/core/vpDisplay.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpMeterPixelConversion.h>
45 #include <visp3/core/vpUniRand.h>
46 #include <visp3/vision/vpPose.h>
47 #include <visp3/vision/vpPoseException.h>
52 #define DEBUG_LEVEL1 0
55 : npt(0), listP(), residual(0), m_lambda(0.9), m_dementhonSvThresh(1e-6), vvsIterMax(200), c3d(),
56 computeCovariance(false), covarianceMatrix(),
57 ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
58 distanceToPlaneForCoplanarityTest(0.001), ransacFlag(
vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false),
59 nbParallelRansacThreads(0),
64 : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), m_lambda(0.9),
65 m_dementhonSvThresh(1e-6), vvsIterMax(200),
66 c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000),
67 ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001),
68 ransacFlag(
vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false),
69 nbParallelRansacThreads(0),
76 std::cout <<
"begin vpPose::~vpPose() " << std::endl;
82 std::cout <<
"end vpPose::~vpPose() " << std::endl;
95 listP.push_back(newP);
96 listOfPoints.push_back(newP);
102 listP.insert(
listP.end(), lP.begin(), lP.end());
103 listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
117 bool vpPose::coplanar(
int &coplanar_plane_type,
double *p_a,
double *p_b,
double *p_c,
double *p_d)
119 coplanar_plane_type = 0;
129 std::vector<vpPoint> shuffled_listP = vpUniRand::shuffleVector<vpPoint>(listOfPoints);
131 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
133 std::vector<vpPoint>::const_iterator it = shuffled_listP.begin();
138 bool degenerate =
true;
139 bool not_on_origin =
true;
140 std::vector<vpPoint>::const_iterator it_tmp;
142 std::vector<vpPoint>::const_iterator it_i, it_j, it_k;
143 for (it_i = shuffled_listP.begin(); it_i != shuffled_listP.end(); ++it_i) {
144 if (degenerate ==
false) {
150 if ((std::fabs(P1.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
151 (std::fabs(P1.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
152 (std::fabs(P1.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
153 not_on_origin =
false;
156 not_on_origin =
true;
161 for (it_j = it_tmp; it_j != shuffled_listP.end(); ++it_j) {
162 if (degenerate ==
false) {
167 if ((std::fabs(P2.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
168 (std::fabs(P2.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
169 (std::fabs(P2.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
170 not_on_origin =
false;
173 not_on_origin =
true;
178 for (it_k = it_tmp; it_k != shuffled_listP.end(); ++it_k) {
180 if ((std::fabs(P3.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
181 (std::fabs(P3.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
182 (std::fabs(P3.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
183 not_on_origin =
false;
186 not_on_origin =
true;
210 if (cross_prod.
sumSquare() <= std::numeric_limits<double>::epsilon())
215 if (degenerate ==
false)
224 coplanar_plane_type = 4;
228 double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
229 double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
230 double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
231 double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
235 if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
236 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
237 coplanar_plane_type = 1;
239 else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
240 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
241 coplanar_plane_type = 2;
243 else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
244 std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
245 coplanar_plane_type = 3;
250 for (it = shuffled_listP.begin(); it != shuffled_listP.end(); ++it) {
255 if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
266 if (p_a !=
nullptr) {
270 if (p_b !=
nullptr) {
274 if (p_c !=
nullptr) {
278 if (p_d !=
nullptr) {
287 double squared_error = 0;
289 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
291 double x = P.
get_x();
292 double y = P.
get_y();
298 return (squared_error);
309 double squared_error = 0;
310 residuals.
resize(
static_cast<unsigned int>(
listP.size()));
313 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
315 double x = P.
get_x();
316 double y = P.
get_y();
318 double u_initial = 0., v_initial = 0.;
323 double u_moved = 0., v_moved = 0.;
327 residuals[i++] = squaredResidual;
328 squared_error += squaredResidual;
330 return (squared_error);
345 "Dementhon method cannot be used in that case "
346 "(at least 4 points are required)"
347 "Not enough point (%d) to compute the pose ",
352 int coplanar_plane_type = 0;
353 bool plan =
coplanar(coplanar_plane_type);
367 int coplanar_plane_type = 0;
368 bool plan =
coplanar(coplanar_plane_type, &a, &b, &c, &d);
372 if (coplanar_plane_type == 4) {
374 "(points are collinear)"));
378 "Lagrange method cannot be used in that case "
379 "(at least 4 points are required). "
380 "Not enough point (%d) to compute the pose ",
388 "Lagrange method cannot be used in that case "
389 "(at least 6 points are required when 3D points are non coplanar). "
390 "Not enough point (%d) to compute the pose ",
400 "Ransac method cannot be used in that case "
401 "(at least 4 points are required). "
402 "Not enough point (%d) to compute the pose ",
440 double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
443 int coplanar_plane_type = 0;
444 bool plan =
coplanar(coplanar_plane_type, &a, &b, &c, &d);
445 bool hasDementhonSucceeded(
false), hasLagrangeSucceeded(
false);
455 hasDementhonSucceeded =
true;
470 hasDementhonSucceeded =
true;
474 hasDementhonSucceeded =
false;
488 hasLagrangeSucceeded =
true;
507 hasLagrangeSucceeded =
true;
511 hasLagrangeSucceeded =
false;
515 if ((hasDementhonSucceeded || hasLagrangeSucceeded)) {
518 cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
531 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
534 std::cout <<
"3D oP " << P.
oP.
t();
535 std::cout <<
"3D cP " << P.
cP.
t();
536 std::cout <<
"2D " << P.
p.
t();
555 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
569 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
583 std::vector<double> rectx(4);
584 std::vector<double> recty(4);
593 std::vector<double> irectx(4);
594 std::vector<double> irecty(4);
595 irectx[0] = (p1.
get_x());
596 irecty[0] = (p1.
get_y());
597 irectx[1] = (p2.
get_x());
598 irecty[1] = (p2.
get_y());
599 irectx[2] = (p3.
get_x());
600 irecty[2] = (p3.
get_y());
601 irectx[3] = (p4.
get_x());
602 irecty[3] = (p4.
get_y());
610 for (
unsigned int i = 0; i < 3; i++)
611 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)
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 poseVirtualVS(vpHomogeneousMatrix &cMo)
void addPoint(const vpPoint &P)
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
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.
void setDistanceToPlaneForCoplanarityTest(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)
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
void setDementhonSvThreshold(const double &svThresh)