Visual Servoing Platform version 3.5.0
vpPose.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Pose computation.
33 *
34 * Authors:
35 * Eric Marchand
36 * Francois Chaumette
37 *
38 *****************************************************************************/
39
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>
54
55#include <cmath> // std::fabs
56#include <limits> // numeric_limits
57
58#define DEBUG_LEVEL1 0
59#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
64{
65#if (DEBUG_LEVEL1)
66 std::cout << "begin vpPose::Init() " << std::endl;
67#endif
68
69 npt = 0;
70 listP.clear();
71 residual = 0;
72 lambda = 0.9;
73 vvsIterMax = 200;
74 c3d.clear();
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;
83 ransacFlag = NO_FILTER;
84 listOfPoints.clear();
85 useParallelRansac = false;
86 nbParallelRansacThreads = 0;
87 vvsEpsilon = 1e-8;
88
89#if (DEBUG_LEVEL1)
90 std::cout << "end vpPose::Init() " << std::endl;
91#endif
92}
93#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
94
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), // 0 means that we use C++11 (if available) to get the number of threads
102 vvsEpsilon(1e-8)
103{
104}
105
106vpPose::vpPose(const std::vector<vpPoint>& lP)
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), // 0 means that we use C++11 (if available) to get the number of threads
112 vvsEpsilon(1e-8)
113{
114}
115
120{
121#if (DEBUG_LEVEL1)
122 std::cout << "begin vpPose::~vpPose() " << std::endl;
123#endif
124
125 listP.clear();
126
127#if (DEBUG_LEVEL1)
128 std::cout << "end vpPose::~vpPose() " << std::endl;
129#endif
130}
135{
136 listP.clear();
137 listOfPoints.clear();
138 npt = 0;
139}
140
149void vpPose::addPoint(const vpPoint &newP)
150{
151 listP.push_back(newP);
152 listOfPoints.push_back(newP);
153 npt++;
154}
155
164void vpPose::addPoints(const std::vector<vpPoint> &lP)
165{
166 listP.insert(listP.end(), lP.begin(), lP.end());
167 listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
168 npt = (unsigned int)listP.size();
169}
170
171void vpPose::setDistanceToPlaneForCoplanarityTest(double d) { distanceToPlaneForCoplanarityTest = d; }
172
184bool vpPose::coplanar(int &coplanar_plane_type)
185{
186 coplanar_plane_type = 0;
187 if (npt < 2) {
188 vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
189 throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
190 }
191
192 if (npt == 3)
193 return true;
194
195 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
196
197 std::list<vpPoint>::const_iterator it = listP.begin();
198
199 vpPoint P1, P2, P3;
200
201 // Get three 3D points that are not collinear and that is not at origin
202 bool degenerate = true;
203 bool not_on_origin = true;
204 std::list<vpPoint>::const_iterator it_tmp;
205
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) {
209 // std::cout << "Found a non degenerate configuration" << std::endl;
210 break;
211 }
212 P1 = *it_i;
213 // Test if point is on origin
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;
218 } else {
219 not_on_origin = true;
220 }
221 if (not_on_origin) {
222 it_tmp = it_i;
223 ++it_tmp; // j = i+1
224 for (it_j = it_tmp; it_j != listP.end(); ++it_j) {
225 if (degenerate == false) {
226 // std::cout << "Found a non degenerate configuration" << std::endl;
227 break;
228 }
229 P2 = *it_j;
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;
234 } else {
235 not_on_origin = true;
236 }
237 if (not_on_origin) {
238 it_tmp = it_j;
239 ++it_tmp; // k = j+1
240 for (it_k = it_tmp; it_k != listP.end(); ++it_k) {
241 P3 = *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;
246 } else {
247 not_on_origin = true;
248 }
249 if (not_on_origin) {
250 x1 = P1.get_oX();
251 x2 = P2.get_oX();
252 x3 = P3.get_oX();
253
254 y1 = P1.get_oY();
255 y2 = P2.get_oY();
256 y3 = P3.get_oY();
257
258 z1 = P1.get_oZ();
259 z2 = P2.get_oZ();
260 z3 = P3.get_oZ();
261
262 vpColVector a_b(3), b_c(3), cross_prod;
263 a_b[0] = x1 - x2;
264 a_b[1] = y1 - y2;
265 a_b[2] = z1 - z2;
266 b_c[0] = x2 - x3;
267 b_c[1] = y2 - y3;
268 b_c[2] = z2 - z3;
269
270 cross_prod = vpColVector::crossProd(a_b, b_c);
271 if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
272 degenerate = true; // points are collinear
273 else
274 degenerate = false;
275 }
276 if (degenerate == false)
277 break;
278 }
279 }
280 }
281 }
282 }
283
284 if (degenerate) {
285 coplanar_plane_type = 4; // points are collinear
286 return true;
287 }
288
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;
293
294 // std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d <<
295 // std::endl;
296 if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
297 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
298 coplanar_plane_type = 1; // ax=d
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; // by=d
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; // cz=d
305 }
306
307 double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
308
309 for (it = listP.begin(); it != listP.end(); ++it) {
310 P1 = *it;
311 double dist = (a * P1.get_oX() + b * P1.get_oY() + c * P1.get_oZ() + d) / D;
312 // std::cout << "dist= " << dist << std::endl;
313
314 if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
315 vpDEBUG_TRACE(10, " points are not coplanar ");
316 // TRACE(" points are not coplanar ") ;
317 return false;
318 }
319 }
320
321 vpDEBUG_TRACE(10, " points are coplanar ");
322 // vpTRACE(" points are coplanar ") ;
323
324 return true;
325}
326
337{
338 double squared_error = 0;
339 vpPoint P;
340 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
341 P = *it;
342 double x = P.get_x();
343 double y = P.get_y();
344
345 P.track(cMo);
346
347 squared_error += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
348 }
349 return (squared_error);
350}
351
375{
376 if (npt < 4) {
378 "Not enough point (%d) to compute the pose ", npt));
379 }
380
381 switch (method) {
382 case DEMENTHON:
384 case DEMENTHON_LOWE: {
385 if (npt < 4) {
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));
390 }
391
392 // test si les point 3D sont coplanaires
393 int coplanar_plane_type = 0;
394 bool plan = coplanar(coplanar_plane_type);
395 if (plan == true) {
397 } else {
399 }
400 } break;
401 case LAGRANGE:
403 case LAGRANGE_LOWE: {
404 // test si les point 3D sont coplanaires
405 int coplanar_plane_type;
406 bool plan = coplanar(coplanar_plane_type);
407
408 if (plan == true)
409 {
410
411 if (coplanar_plane_type == 4) {
413 "Lagrange method cannot be used in that case "
414 "(points are collinear)"));
415 }
416 if (npt < 4) {
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));
421 }
422 poseLagrangePlan(cMo);
423 } else {
424 if (npt < 6) {
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));
429 }
431 }
432 } break;
433 case RANSAC:
434 if (npt < 4) {
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));
439 }
440 return poseRansac(cMo, func);
441 break;
442 case LOWE:
443 case VIRTUAL_VS:
444 break;
445 }
446
447 switch (method) {
448 case LAGRANGE:
449 case DEMENTHON:
450 case RANSAC:
451 break;
452 case VIRTUAL_VS:
455 poseVirtualVS(cMo);
456 } break;
457 case LOWE:
458 case LAGRANGE_LOWE:
459 case DEMENTHON_LOWE: {
460 poseLowe(cMo);
461 } break;
462 }
463
464 // If here, there was no exception thrown so return true
465 return true;
466}
467
469{
470 vpPoint P;
471 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
472 P = *it;
473
474 std::cout << "3D oP " << P.oP.t();
475 std::cout << "3D cP " << P.cP.t();
476 std::cout << "2D " << P.p.t();
477 }
478}
479
490 vpColor col)
491{
492 vpDisplay::displayFrame(I, cMo, cam, size, col);
493}
494
505{
506 vpDisplay::displayFrame(I, cMo, cam, size, col);
507}
508
514{
515 vpPoint P;
516 vpImagePoint ip;
517 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
518 P = *it;
519 vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
520 vpDisplay::displayCross(I, ip, 5, col);
521 // std::cout << "3D oP " << P.oP.t() ;
522 // std::cout << "3D cP " << P.cP.t() ;
523 // std::cout << "2D " << P.p.t() ;
524 }
525}
526
532{
533 vpPoint P;
534 vpImagePoint ip;
535 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
536 P = *it;
537 vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
538 vpDisplay::displayCross(I, ip, 5, col);
539 // std::cout << "3D oP " << P.oP.t() ;
540 // std::cout << "3D cP " << P.cP.t() ;
541 // std::cout << "2D " << P.p.t() ;
542 }
543}
544
566{
567
568 std::vector<double> rectx(4);
569 std::vector<double> recty(4);
570 rectx[0] = 0;
571 recty[0] = 0;
572 rectx[1] = 1;
573 recty[1] = 0;
574 rectx[2] = 1;
575 recty[2] = 1;
576 rectx[3] = 0;
577 recty[3] = 1;
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());
588
589 // calcul de l'homographie
590 vpMatrix H(3, 3);
591 vpHomography hom;
592
593 // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
594 vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom);
595 for (unsigned int i = 0; i < 3; i++)
596 for (unsigned int j = 0; j < 3; j++)
597 H[i][j] = hom[i][j];
598 // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y
599 // axis)
600 vpColVector kh1(3);
601 vpColVector kh2(3);
602 vpMatrix K(3, 3);
603 K = cam.get_K();
604 K.eye();
605 vpMatrix Kinv = K.pseudoInverse();
606
607 vpMatrix KinvH = Kinv * H;
608 kh1 = KinvH.getCol(0);
609 kh2 = KinvH.getCol(1);
610
611 double s = sqrt(kh1.sumSquare()) / sqrt(kh2.sumSquare());
612
613 vpMatrix D(3, 3);
614 D.eye();
615 D[1][1] = 1 / s;
616 vpMatrix cHo = H * D;
617
618 // Calcul de la rotation et de la translation
619 // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
620 p1.setWorldCoordinates(0, 0, 0);
621 p2.setWorldCoordinates(lx, 0, 0);
622 p3.setWorldCoordinates(lx, lx / s, 0);
623 p4.setWorldCoordinates(0, lx / s, 0);
624
625 vpPose P;
626 P.addPoint(p1);
627 P.addPoint(p2);
628 P.addPoint(p3);
629 P.addPoint(p4);
630
632 return lx / s;
633}
Generic class defining intrinsic camera parameters.
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
vpRowVector t() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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.
Definition: vpHomography.h:175
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 ...
Definition: vpImagePoint.h:88
static double sqr(double x)
Definition: vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:449
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
void clear()
Definition: vpMatrix.h:219
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 ...
Definition: vpPoint.h:82
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
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...
Definition: vpPose.h:81
vp_deprecated void init()
Definition: vpPose.cpp:63
vpPose()
Definition: vpPose.cpp:96
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
void printPoint()
Definition: vpPose.cpp:468
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:84
@ DEMENTHON
Definition: vpPose.h:86
@ LAGRANGE_LOWE
Definition: vpPose.h:91
@ RANSAC
Definition: vpPose.h:90
@ DEMENTHON_LOWE
Definition: vpPose.h:93
@ LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:99
@ VIRTUAL_VS
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:85
@ DEMENTHON_VIRTUAL_VS
Definition: vpPose.h:97
@ LOWE
Definition: vpPose.h:87
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:109
void setDistanceToPlaneForCoplanarityTest(double d)
Definition: vpPose.cpp:171
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:164
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:336
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
void clearPoint()
Definition: vpPose.cpp:134
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:262
@ NO_FILTER
Definition: vpPose.h:104
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)
Definition: vpPose.cpp:489
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,...
Definition: vpPose.cpp:564
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:115
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:513
bool coplanar(int &coplanar_plane_type)
Definition: vpPose.cpp:184
double residual
Residual in meter.
Definition: vpPose.h:112
virtual ~vpPose()
Definition: vpPose.cpp:119
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
vpColVector cP
Definition: vpTracker.h:77
vpColVector p
Definition: vpTracker.h:73
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393