Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPose.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Pose computation.
32 */
33
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>
48
49#include <cmath> // std::fabs
50#include <limits> // numeric_limits
51
52#define DEBUG_LEVEL1 0
53#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
58{
59#if (DEBUG_LEVEL1)
60 std::cout << "begin vpPose::Init() " << std::endl;
61#endif
62
63 npt = 0;
64 listP.clear();
65 residual = 0;
66 lambda = 0.9;
67 vvsIterMax = 200;
68 c3d.clear();
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;
77 ransacFlag = NO_FILTER;
78 listOfPoints.clear();
79 useParallelRansac = false;
80 nbParallelRansacThreads = 0;
81 vvsEpsilon = 1e-8;
82
83#if (DEBUG_LEVEL1)
84 std::cout << "end vpPose::Init() " << std::endl;
85#endif
86}
87#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
88
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), // 0 means that we use C++11 (if available) to get the number of threads
95 vvsEpsilon(1e-8), dementhonSvThresh(1e-6)
96{ }
97
98vpPose::vpPose(const std::vector<vpPoint> &lP)
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), // 0 means that we use C++11 (if available) to get the number of threads
104 vvsEpsilon(1e-8), dementhonSvThresh(1e-6)
105{ }
106
111{
112#if (DEBUG_LEVEL1)
113 std::cout << "begin vpPose::~vpPose() " << std::endl;
114#endif
115
116 listP.clear();
117
118#if (DEBUG_LEVEL1)
119 std::cout << "end vpPose::~vpPose() " << std::endl;
120#endif
121}
126{
127 listP.clear();
128 listOfPoints.clear();
129 npt = 0;
130}
131
140void vpPose::addPoint(const vpPoint &newP)
141{
142 listP.push_back(newP);
143 listOfPoints.push_back(newP);
144 npt++;
145}
146
155void vpPose::addPoints(const std::vector<vpPoint> &lP)
156{
157 listP.insert(listP.end(), lP.begin(), lP.end());
158 listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
159 npt = (unsigned int)listP.size();
160}
161
162void vpPose::setDistanceToPlaneForCoplanarityTest(double d) { distanceToPlaneForCoplanarityTest = d; }
163
164void vpPose::setDementhonSvThreshold(const double &svThresh)
165{
166 if (svThresh < 0) {
167 throw vpException(vpException::badValue, "The svd threshold must be positive");
168 }
169 dementhonSvThresh = svThresh;
170}
171
187bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double *p_c, double *p_d)
188{
189 coplanar_plane_type = 0;
190 if (npt < 2) {
191 vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
192 throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
193 }
194
195 if (npt == 3)
196 return true;
197
198 // Shuffling the points to limit the risk of using points close to each other
199 std::vector<vpPoint> shuffled_listP = vpUniRand::shuffleVector<vpPoint>(listOfPoints);
200
201 double x1 = 0, x2 = 0, x3 = 0, y1 = 0, y2 = 0, y3 = 0, z1 = 0, z2 = 0, z3 = 0;
202
203 std::vector<vpPoint>::const_iterator it = shuffled_listP.begin();
204
205 vpPoint P1, P2, P3;
206
207 // Get three 3D points that are not collinear and that is not at origin
208 bool degenerate = true;
209 bool not_on_origin = true;
210 std::vector<vpPoint>::const_iterator it_tmp;
211
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) {
215 // std::cout << "Found a non degenerate configuration" << std::endl;
216 break;
217 }
218 P1 = *it_i;
219 // Test if point is on origin
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;
224 }
225 else {
226 not_on_origin = true;
227 }
228 if (not_on_origin) {
229 it_tmp = it_i;
230 ++it_tmp; // j = i+1
231 for (it_j = it_tmp; it_j != shuffled_listP.end(); ++it_j) {
232 if (degenerate == false) {
233 // std::cout << "Found a non degenerate configuration" << std::endl;
234 break;
235 }
236 P2 = *it_j;
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;
241 }
242 else {
243 not_on_origin = true;
244 }
245 if (not_on_origin) {
246 it_tmp = it_j;
247 ++it_tmp; // k = j+1
248 for (it_k = it_tmp; it_k != shuffled_listP.end(); ++it_k) {
249 P3 = *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;
254 }
255 else {
256 not_on_origin = true;
257 }
258 if (not_on_origin) {
259 x1 = P1.get_oX();
260 x2 = P2.get_oX();
261 x3 = P3.get_oX();
262
263 y1 = P1.get_oY();
264 y2 = P2.get_oY();
265 y3 = P3.get_oY();
266
267 z1 = P1.get_oZ();
268 z2 = P2.get_oZ();
269 z3 = P3.get_oZ();
270
271 vpColVector a_b(3), b_c(3), cross_prod;
272 a_b[0] = x1 - x2;
273 a_b[1] = y1 - y2;
274 a_b[2] = z1 - z2;
275 b_c[0] = x2 - x3;
276 b_c[1] = y2 - y3;
277 b_c[2] = z2 - z3;
278
279 cross_prod = vpColVector::crossProd(a_b, b_c);
280 if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
281 degenerate = true; // points are collinear
282 else
283 degenerate = false;
284 }
285 if (degenerate == false)
286 break;
287 }
288 }
289 }
290 }
291 }
292
293 if (degenerate) {
294 coplanar_plane_type = 4; // points are collinear
295 return true;
296 }
297
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;
302
303 // std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d <<
304 // std::endl;
305 if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
306 std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
307 coplanar_plane_type = 1; // ax=d
308 }
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; // by=d
312 }
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; // cz=d
316 }
317
318 double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
319
320 for (it = shuffled_listP.begin(); it != shuffled_listP.end(); ++it) {
321 P1 = *it;
322 double dist = (a * P1.get_oX() + b * P1.get_oY() + c * P1.get_oZ() + d) / D;
323 // std::cout << "dist= " << dist << std::endl;
324
325 if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
326 vpDEBUG_TRACE(10, " points are not coplanar ");
327 return false;
328 }
329 }
330
331 vpDEBUG_TRACE(10, " points are coplanar ");
332 // vpTRACE(" points are coplanar ") ;
333
334 // If the points are coplanar and the input/output parameters are different from NULL,
335 // getting the values of the plan coefficient and storing in the input/output parameters
336 if (p_a != NULL) {
337 *p_a = a;
338 }
339
340 if (p_b != NULL) {
341 *p_b = b;
342 }
343
344 if (p_c != NULL) {
345 *p_c = c;
346 }
347
348 if (p_d != NULL) {
349 *p_d = d;
350 }
351
352 return true;
353}
354
370{
371 double squared_error = 0;
372 vpPoint P;
373 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
374 P = *it;
375 double x = P.get_x();
376 double y = P.get_y();
377
378 P.track(cMo);
379
380 squared_error += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
381 }
382 return (squared_error);
383}
384
399{
400 vpColVector residuals;
401 return computeResidual(cMo, cam, residuals);
402}
403
418double vpPose::computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &residuals) const
419{
420 double squared_error = 0;
421 residuals.resize(static_cast<unsigned int>(listP.size()));
422 vpPoint P;
423 unsigned int i = 0;
424 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
425 P = *it;
426 double x = P.get_x();
427 double y = P.get_y();
428
429 double u_initial = 0., v_initial = 0.;
430 vpMeterPixelConversion::convertPoint(cam, x, y, u_initial, v_initial);
431
432 P.track(cMo);
433
434 double u_moved = 0., v_moved = 0.;
435 vpMeterPixelConversion::convertPoint(cam, P.get_x(), P.get_y(), u_moved, v_moved);
436
437 double squaredResidual = vpMath::sqr(u_moved - u_initial) + vpMath::sqr(v_moved - v_initial);
438 residuals[i++] = squaredResidual;
439 squared_error += squaredResidual;
440 }
441 return (squared_error);
442}
443
470{
471 if (npt < 4) {
472 throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose ", npt));
473 }
474
475 switch (method) {
476 case DEMENTHON:
478 case DEMENTHON_LOWE: {
479 if (npt < 4) {
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 ",
484 npt));
485 }
486
487 // test if the 3D points are coplanar
488 int coplanar_plane_type = 0;
489 bool plan = coplanar(coplanar_plane_type);
490 if (plan == true) {
492 }
493 else {
495 }
496 break;
497 }
498 case LAGRANGE:
500 case LAGRANGE_LOWE: {
501 // test if the 3D points are coplanar
502 double a, b, c, d; // To get the plan coefficients if the points are coplanar
503 int coplanar_plane_type = 0;
504 bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d);
505
506 if (plan == true) {
507
508 if (coplanar_plane_type == 4) {
509 throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case "
510 "(points are collinear)"));
511 }
512 if (npt < 4) {
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 ",
517 npt));
518 }
519 poseLagrangePlan(cMo, &plan, &a, &b, &c, &d);
520 }
521 else {
522 if (npt < 6) {
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 ",
527 npt));
528 }
530 }
531 break;
532 }
533 case RANSAC: {
534 if (npt < 4) {
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 ",
539 npt));
540 }
541 return poseRansac(cMo, func);
542 }
543 case LOWE:
544 case VIRTUAL_VS:
545 break;
548 }
549 }
550
551 switch (method) {
552 case LAGRANGE:
553 case DEMENTHON:
555 case RANSAC:
556 break;
557 case VIRTUAL_VS:
560 poseVirtualVS(cMo);
561 break;
562 }
563 case LOWE:
564 case LAGRANGE_LOWE:
565 case DEMENTHON_LOWE: {
566 poseLowe(cMo);
567 } break;
568 }
569
570 return true;
571}
572
583{
584 vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
585 double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
586 // test if the 3D points are coplanar
587 double a, b, c, d; // To get the plan coefficients if the points are coplanar
588 int coplanar_plane_type = 0;
589 bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d);
590 bool hasDementhonSucceeded(false), hasLagrangeSucceeded(false);
591 try {
592 if (plan) {
593 poseDementhonPlan(cMo_dementhon);
594 }
595 else {
596 poseDementhonNonPlan(cMo_dementhon);
597 }
598
599 r_dementhon = computeResidual(cMo_dementhon);
600 hasDementhonSucceeded = true; // We reached this point => no exception was thrown = method succeeded
601 }
602 catch (...) {
603 // An exception was thrown using the original assumption, trying we the other one
604 try {
605 if (plan) {
606 // Already tested poseDementhonPlan, now trying poseDementhonNonPlan
607 poseDementhonNonPlan(cMo_dementhon);
608 }
609 else {
610 // Already tested poseDementhonNonPlan, now trying poseDementhonPlan
611 poseDementhonPlan(cMo_dementhon);
612 }
613
614 r_dementhon = computeResidual(cMo_dementhon);
615 hasDementhonSucceeded = true; // We reached this point => no exception was thrown = method succeeded
616 }
617 catch (...) {
618 // The Dementhon method failed both with the planar and non-planar assumptions.
619 hasDementhonSucceeded = false;
620 }
621 }
622
623 try {
624 if (plan) {
625 // If plan is true, then a, b, c, d will have been set when we called coplanar.
626 poseLagrangePlan(cMo_lagrange, &plan, &a, &b, &c, &d);
627 }
628 else {
629 poseLagrangeNonPlan(cMo_lagrange);
630 }
631
632 r_lagrange = computeResidual(cMo_lagrange);
633 hasLagrangeSucceeded = true; // We reached this point => no exception was thrown = method succeeded
634 }
635 catch (...) {
636 // An exception was thrown using the original assumption, trying we the other one
637 try {
638 if (plan) {
639 // Already tested poseLagrangePlan, now trying poseLagrangeNonPlan
640 poseLagrangeNonPlan(cMo_lagrange);
641 }
642 else {
643 // Already tested poseLagrangeNonPlan, now trying poseLagrangePlan
644 // Because plan is false, then a, b, c, d will not have
645 // been initialized when calling coplanar
646 // We are expecting that the call to poseLagrangePlan will throw an exception
647 // because coplanar return false.
648 poseLagrangePlan(cMo_lagrange, &plan, &a, &b, &c, &d);
649 }
650
651 r_lagrange = computeResidual(cMo_lagrange);
652 hasLagrangeSucceeded = true; // We reached this point => no exception was thrown = method succeeded
653 }
654 catch (...) {
655 // The Lagrange method both failed with the planar and non-planar assumptions.
656 hasLagrangeSucceeded = false;
657 }
658 }
659
660 if ((hasDementhonSucceeded || hasLagrangeSucceeded)) {
661 // At least one of the linear methods managed to compute an initial pose.
662 // We initialize cMo with the method that had the lowest residual
663 cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
664 // We now use the non-linear Virtual Visual Servoing method to improve the estimated cMo
665 return computePose(vpPose::VIRTUAL_VS, cMo);
666 }
667 else {
668 // None of the linear methods manage to compute an initial pose
669 return false;
670 }
671}
672
674{
675 vpPoint P;
676 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
677 P = *it;
678
679 std::cout << "3D oP " << P.oP.t();
680 std::cout << "3D cP " << P.cP.t();
681 std::cout << "2D " << P.p.t();
682 }
683}
684
695 vpColor col)
696{
697 vpDisplay::displayFrame(I, cMo, cam, size, col);
698}
699
710{
711 vpDisplay::displayFrame(I, cMo, cam, size, col);
712}
713
719{
720 vpPoint P;
721 vpImagePoint ip;
722 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
723 P = *it;
724 vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
725 vpDisplay::displayCross(I, ip, 5, col);
726 // std::cout << "3D oP " << P.oP.t() ;
727 // std::cout << "3D cP " << P.cP.t() ;
728 // std::cout << "2D " << P.p.t() ;
729 }
730}
731
737{
738 vpPoint P;
739 vpImagePoint ip;
740 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
741 P = *it;
742 vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
743 vpDisplay::displayCross(I, ip, 5, col);
744 // std::cout << "3D oP " << P.oP.t() ;
745 // std::cout << "3D cP " << P.cP.t() ;
746 // std::cout << "2D " << P.p.t() ;
747 }
748}
749
771{
772
773 std::vector<double> rectx(4);
774 std::vector<double> recty(4);
775 rectx[0] = 0;
776 recty[0] = 0;
777 rectx[1] = 1;
778 recty[1] = 0;
779 rectx[2] = 1;
780 recty[2] = 1;
781 rectx[3] = 0;
782 recty[3] = 1;
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());
793
794 // calcul de l'homographie
795 vpMatrix H(3, 3);
796 vpHomography hom;
797
798 // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
799 vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom);
800 for (unsigned int i = 0; i < 3; i++)
801 for (unsigned int j = 0; j < 3; j++)
802 H[i][j] = hom[i][j];
803 // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y
804 // axis)
805 vpColVector kh1(3);
806 vpColVector kh2(3);
807 vpMatrix K(3, 3);
808 K = cam.get_K();
809 K.eye();
810 vpMatrix Kinv = K.pseudoInverse();
811
812 vpMatrix KinvH = Kinv * H;
813 kh1 = KinvH.getCol(0);
814 kh2 = KinvH.getCol(1);
815
816 double s = sqrt(kh1.sumSquare()) / sqrt(kh2.sumSquare());
817
818 vpMatrix D(3, 3);
819 D.eye();
820 D[1][1] = 1 / s;
821 vpMatrix cHo = H * D;
822
823 // Calcul de la rotation et de la translation
824 // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
825 p1.setWorldCoordinates(0, 0, 0);
826 p2.setWorldCoordinates(lx, 0, 0);
827 p3.setWorldCoordinates(lx, lx / s, 0);
828 p4.setWorldCoordinates(0, lx / s, 0);
829
830 vpPose P;
831 P.addPoint(p1);
832 P.addPoint(p2);
833 P.addPoint(p3);
834 P.addPoint(p4);
835
837 return lx / s;
838}
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
double sumSquare() const
vpRowVector t() const
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.
Definition vpColor.h:152
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.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
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 ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void eye()
Definition vpMatrix.cpp:446
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpColVector getCol(unsigned int j) const
void clear()
Definition vpMatrix.h:218
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:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
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...
Definition vpPose.h:81
vp_deprecated void init()
Definition vpPose.cpp:57
vpPose()
Definition vpPose.cpp:90
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
void printPoint()
Definition vpPose.cpp:673
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition vpPose.h:85
@ DEMENTHON
Definition vpPose.h:87
@ LAGRANGE_LOWE
Definition vpPose.h:92
@ RANSAC
Definition vpPose.h:91
@ DEMENTHON_LOWE
Definition vpPose.h:94
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
@ LAGRANGE_VIRTUAL_VS
Definition vpPose.h:100
@ VIRTUAL_VS
Definition vpPose.h:96
@ LAGRANGE
Definition vpPose.h:86
@ DEMENTHON_VIRTUAL_VS
Definition vpPose.h:98
@ LOWE
Definition vpPose.h:88
unsigned int npt
Number of point used in pose computation.
Definition vpPose.h:114
void setDistanceToPlaneForCoplanarityTest(double d)
Definition vpPose.cpp:162
void addPoints(const std::vector< vpPoint > &lP)
Definition vpPose.cpp:155
double dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition vpPose.h:199
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)
Definition vpPose.h:115
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:369
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
void clearPoint()
Definition vpPose.cpp:125
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
@ NO_FILTER
Definition vpPose.h:109
bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo)
Method that first computes the pose cMo using the linear approaches of Dementhon and Lagrange and the...
Definition vpPose.cpp:582
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition vpPose.cpp:694
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:769
double lambda
parameters use for the virtual visual servoing approach
Definition vpPose.h:120
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition vpPose.cpp:718
bool coplanar(int &coplanar_plane_type, double *p_a=NULL, double *p_b=NULL, double *p_c=NULL, double *p_d=NULL)
Definition vpPose.cpp:187
double residual
Residual in meter.
Definition vpPose.h:117
virtual ~vpPose()
Definition vpPose.cpp:110
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)
Definition vpPose.cpp:164
vpColVector cP
Definition vpTracker.h:72
vpColVector p
Definition vpTracker.h:68
static std::vector< T > shuffleVector(const std::vector< T > &inputVector)
Create a new vector that is a shuffled version of the inputVector.
Definition vpUniRand.h:156
#define vpDEBUG_TRACE
Definition vpDebug.h:482
#define vpERROR_TRACE
Definition vpDebug.h:388