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(),
100 useParallelRansac(false),
101 nbParallelRansacThreads(0),
107 : npt(static_cast<unsigned int>(lP.size())), listP(lP.begin(), lP.end()), residual(0), lambda(0.9), vvsIterMax(200), c3d(),
108 computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(),
109 ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001), ransacFlag(
vpPose::NO_FILTER),
110 listOfPoints(lP), useParallelRansac(false),
111 nbParallelRansacThreads(0),
122 std::cout <<
"begin vpPose::~vpPose() " << std::endl;
128 std::cout <<
"end vpPose::~vpPose() " << std::endl;
137 listOfPoints.clear();
151 listP.push_back(newP);
152 listOfPoints.push_back(newP);
166 listP.insert(
listP.end(), lP.begin(), lP.end());
167 listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
186 coplanar_plane_type = 0;
195 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
197 std::list<vpPoint>::const_iterator it =
listP.begin();
202 bool degenerate =
true;
203 bool not_on_origin =
true;
204 std::list<vpPoint>::const_iterator it_tmp;
206 std::list<vpPoint>::const_iterator it_i, it_j, it_k;
207 for (it_i =
listP.begin(); it_i !=
listP.end(); ++it_i) {
208 if (degenerate ==
false) {
214 if ((std::fabs(P1.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
215 (std::fabs(P1.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
216 (std::fabs(P1.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
217 not_on_origin =
false;
219 not_on_origin =
true;
224 for (it_j = it_tmp; it_j !=
listP.end(); ++it_j) {
225 if (degenerate ==
false) {
230 if ((std::fabs(P2.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
231 (std::fabs(P2.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
232 (std::fabs(P2.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
233 not_on_origin =
false;
235 not_on_origin =
true;
240 for (it_k = it_tmp; it_k !=
listP.end(); ++it_k) {
242 if ((std::fabs(P3.
get_oX()) <= std::numeric_limits<double>::epsilon()) &&
243 (std::fabs(P3.
get_oY()) <= std::numeric_limits<double>::epsilon()) &&
244 (std::fabs(P3.
get_oZ()) <= std::numeric_limits<double>::epsilon())) {
245 not_on_origin =
false;
247 not_on_origin =
true;
271 if (cross_prod.
sumSquare() <= std::numeric_limits<double>::epsilon())
276 if (degenerate ==
false)
285 coplanar_plane_type = 4;
289 double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
290 double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
291 double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
292 double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
296 if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
297 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
298 coplanar_plane_type = 1;
299 }
else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
300 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
301 coplanar_plane_type = 2;
302 }
else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
303 std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
304 coplanar_plane_type = 3;
309 for (it =
listP.begin(); it !=
listP.end(); ++it) {
314 if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
338 double squared_error = 0;
340 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
342 double x = P.
get_x();
343 double y = P.
get_y();
349 return (squared_error);
378 "Not enough point (%d) to compute the pose ",
npt));
387 "Dementhon method cannot be used in that case "
388 "(at least 4 points are required)"
389 "Not enough point (%d) to compute the pose ",
npt));
393 int coplanar_plane_type = 0;
394 bool plan =
coplanar(coplanar_plane_type);
405 int coplanar_plane_type;
406 bool plan =
coplanar(coplanar_plane_type);
411 if (coplanar_plane_type == 4) {
413 "Lagrange method cannot be used in that case "
414 "(points are collinear)"));
418 "Lagrange method cannot be used in that case "
419 "(at least 4 points are required). "
420 "Not enough point (%d) to compute the pose ",
npt));
426 "Lagrange method cannot be used in that case "
427 "(at least 6 points are required when 3D points are non coplanar). "
428 "Not enough point (%d) to compute the pose ",
npt));
436 "Ransac method cannot be used in that case "
437 "(at least 4 points are required). "
438 "Not enough point (%d) to compute the pose ",
npt));
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 displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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))
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...