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;
93 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
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++)