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
53 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
60 std::cout <<
"begin vpPose::Init() " << std::endl;
69 computeCovariance =
false;
70 covarianceMatrix.
clear();
71 ransacNbInlierConsensus = 4;
72 ransacMaxTrials = 1000;
73 ransacInliers.clear();
74 ransacInlierIndex.clear();
75 ransacThreshold = 0.0001;
76 distanceToPlaneForCoplanarityTest = 0.001;
79 useParallelRansac =
false;
80 nbParallelRansacThreads = 0;
84 std::cout <<
"end vpPose::Init() " << std::endl;
91 : npt(0), listP(), residual(0), lambda(0.9), vvsIterMax(200), c3d(), computeCovariance(false), covarianceMatrix(),
92 ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001),
93 distanceToPlaneForCoplanarityTest(0.001), ransacFlag(
vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false),
94 nbParallelRansacThreads(0),
95 vvsEpsilon(1e-8), dementhonSvThresh(1e-6)
99 : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), lambda(0.9), vvsIterMax(200),
100 c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000),
101 ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001),
102 ransacFlag(
vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false),
103 nbParallelRansacThreads(0),
104 vvsEpsilon(1e-8), dementhonSvThresh(1e-6)
113 std::cout <<
"begin vpPose::~vpPose() " << std::endl;
119 std::cout <<
"end vpPose::~vpPose() " << std::endl;
128 listOfPoints.clear();
142 listP.push_back(newP);
143 listOfPoints.push_back(newP);
157 listP.insert(
listP.end(), lP.begin(), lP.end());
158 listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
187 bool vpPose::coplanar(
int &coplanar_plane_type,
double *p_a,
double *p_b,
double *p_c,
double *p_d)
189 coplanar_plane_type = 0;
199 std::vector<vpPoint> shuffled_listP = vpUniRand::shuffleVector<vpPoint>(listOfPoints);
201 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
203 std::vector<vpPoint>::const_iterator it = shuffled_listP.begin();
208 bool degenerate =
true;
209 bool not_on_origin =
true;
210 std::vector<vpPoint>::const_iterator it_tmp;
212 std::vector<vpPoint>::const_iterator it_i, it_j, it_k;
213 for (it_i = shuffled_listP.begin(); it_i != shuffled_listP.end(); ++it_i) {
214 if (degenerate ==
false) {
220 if ((std::fabs(P1.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
221 (std::fabs(P1.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
222 (std::fabs(P1.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
223 not_on_origin =
false;
226 not_on_origin =
true;
231 for (it_j = it_tmp; it_j != shuffled_listP.end(); ++it_j) {
232 if (degenerate ==
false) {
237 if ((std::fabs(P2.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
238 (std::fabs(P2.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
239 (std::fabs(P2.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
240 not_on_origin =
false;
243 not_on_origin =
true;
248 for (it_k = it_tmp; it_k != shuffled_listP.end(); ++it_k) {
250 if ((std::fabs(P3.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
251 (std::fabs(P3.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
252 (std::fabs(P3.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
253 not_on_origin =
false;
256 not_on_origin =
true;
280 if (cross_prod.
sumSquare() <= std::numeric_limits<double>::epsilon())
285 if (degenerate ==
false)
294 coplanar_plane_type = 4;
298 double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
299 double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
300 double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
301 double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
305 if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
306 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
307 coplanar_plane_type = 1;
309 else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
310 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
311 coplanar_plane_type = 2;
313 else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
314 std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
315 coplanar_plane_type = 3;
320 for (it = shuffled_listP.begin(); it != shuffled_listP.end(); ++it) {
325 if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
371 double squared_error = 0;
373 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
375 double x = P.
get_x();
376 double y = P.
get_y();
382 return (squared_error);
420 double squared_error = 0;
421 residuals.
resize(
static_cast<unsigned int>(
listP.size()));
424 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
426 double x = P.
get_x();
427 double y = P.
get_y();
429 double u_initial = 0., v_initial = 0.;
434 double u_moved = 0., v_moved = 0.;
438 residuals[i++] = squaredResidual;
439 squared_error += squaredResidual;
441 return (squared_error);
481 "Dementhon method cannot be used in that case "
482 "(at least 4 points are required)"
483 "Not enough point (%d) to compute the pose ",
488 int coplanar_plane_type = 0;
489 bool plan =
coplanar(coplanar_plane_type);
503 int coplanar_plane_type = 0;
504 bool plan =
coplanar(coplanar_plane_type, &a, &b, &c, &d);
508 if (coplanar_plane_type == 4) {
510 "(points are collinear)"));
514 "Lagrange method cannot be used in that case "
515 "(at least 4 points are required). "
516 "Not enough point (%d) to compute the pose ",
524 "Lagrange method cannot be used in that case "
525 "(at least 6 points are required when 3D points are non coplanar). "
526 "Not enough point (%d) to compute the pose ",
536 "Ransac method cannot be used in that case "
537 "(at least 4 points are required). "
538 "Not enough point (%d) to compute the pose ",
585 double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
588 int coplanar_plane_type = 0;
589 bool plan =
coplanar(coplanar_plane_type, &a, &b, &c, &d);
590 bool hasDementhonSucceeded(
false), hasLagrangeSucceeded(
false);
600 hasDementhonSucceeded =
true;
615 hasDementhonSucceeded =
true;
619 hasDementhonSucceeded =
false;
633 hasLagrangeSucceeded =
true;
652 hasLagrangeSucceeded =
true;
656 hasLagrangeSucceeded =
false;
660 if ((hasDementhonSucceeded || hasLagrangeSucceeded)) {
663 cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
676 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
679 std::cout <<
"3D oP " << P.
oP.
t();
680 std::cout <<
"3D cP " << P.
cP.
t();
681 std::cout <<
"2D " << P.
p.
t();
722 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
740 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
773 std::vector<double> rectx(4);
774 std::vector<double> recty(4);
783 std::vector<double> irectx(4);
784 std::vector<double> irecty(4);
785 irectx[0] = (p1.
get_x());
786 irecty[0] = (p1.
get_y());
787 irectx[1] = (p2.
get_x());
788 irecty[1] = (p2.
get_y());
789 irectx[2] = (p3.
get_x());
790 irecty[2] = (p3.
get_y());
791 irectx[3] = (p4.
get_x());
792 irecty[3] = (p4.
get_y());
800 for (
unsigned int i = 0; i < 3; i++)
801 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...
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.
@ 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)
double dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan=NULL, double *p_a=NULL, double *p_b=NULL, double *p_c=NULL, double *p_d=NULL)
Compute the pose of a planar object using Lagrange approach.
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 ...
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)
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 *p_a=NULL, double *p_b=NULL, double *p_c=NULL, double *p_d=NULL)
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...
void setDementhonSvThreshold(const double &svThresh)