ViSP  2.9.0
vpPose.cpp
1 /****************************************************************************
2 *
3 * $Id: vpPose.cpp 4649 2014-02-07 14:57:11Z fspindle $
4 *
5 * This file is part of the ViSP software.
6 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7 *
8 * This software is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU General Public License
10 * ("GPL") version 2 as published by the Free Software Foundation.
11 * See the file LICENSE.txt at the root directory of this source
12 * distribution for additional information about the GNU GPL.
13 *
14 * For using ViSP with software that can not be combined with the GNU
15 * GPL, please contact INRIA about acquiring a ViSP Professional
16 * Edition License.
17 *
18 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19 *
20 * This software was developed at:
21 * INRIA Rennes - Bretagne Atlantique
22 * Campus Universitaire de Beaulieu
23 * 35042 Rennes Cedex
24 * France
25 * http://www.irisa.fr/lagadic
26 *
27 * If you have questions regarding the use of this file, please contact
28 * INRIA at visp@inria.fr
29 *
30 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32 *
33 *
34 * Description:
35 * Pose computation.
36 *
37 * Authors:
38 * Eric Marchand
39 * Francois Chaumette
40 *
41 *****************************************************************************/
42 
43 
50 #include <visp/vpPose.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpException.h>
53 #include <visp/vpPoseException.h>
54 #include <visp/vpMeterPixelConversion.h>
55 #include <visp/vpCameraParameters.h>
56 #include <visp/vpDisplay.h>
57 #include <visp/vpMath.h>
58 
59 #include <cmath> // std::fabs
60 #include <limits> // numeric_limits
61 
62 #define DEBUG_LEVEL1 0
63 
66 void
68 {
69 #if (DEBUG_LEVEL1)
70  std::cout << "begin vpPose::Init() " << std::endl ;
71 #endif
72  npt = 0 ;
73  listP.clear() ;
74  c3d.clear();
75 
76  lambda = 0.25 ;
77 
78  vvsIterMax = 200 ;
79 
81 
82  computeCovariance = false;
83 
84  ransacMaxTrials = 1000;
85  ransacThreshold = 0.0001;
86  ransacNbInlierConsensus = 4;
87 
88  residual = 0;
89 #if (DEBUG_LEVEL1)
90  std::cout << "end vpPose::Init() " << std::endl ;
91 #endif
92 
93 }
94 
97  : npt(0), listP(), residual(0), lambda(0.25), vvsIterMax(200), c3d(),
98  computeCovariance(false), covarianceMatrix(),
99  ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacThreshold(0.0001),
100  distanceToPlaneForCoplanarityTest(0.001)
101 {
102 #if (DEBUG_LEVEL1)
103  std::cout << "begin vpPose::vpPose() " << std::endl ;
104 #endif
105 
106  init() ;
107 
108 #if (DEBUG_LEVEL1)
109  std::cout << "end vpPose::vpPose() " << std::endl ;
110 #endif
111 
112 }
113 
118 {
119 #if (DEBUG_LEVEL1)
120  std::cout << "begin vpPose::~vpPose() " << std::endl ;
121 #endif
122 
123  listP.clear() ;
124 
125 #if (DEBUG_LEVEL1)
126  std::cout << "end vpPose::~vpPose() " << std::endl ;
127 #endif
128 }
132 void
134 {
135 #if (DEBUG_LEVEL1)
136  std::cout << "begin vpPose::ClearPoint() " << std::endl ;
137 #endif
138 
139  listP.clear() ;
140  npt = 0 ;
141 
142 #if (DEBUG_LEVEL1)
143  std::cout << "end vpPose::ClearPoint() " << std::endl ;
144 #endif
145 }
146 
154 void
156 {
157 
158 #if (DEBUG_LEVEL1)
159  std::cout << "begin vpPose::AddPoint(Dot) " << std::endl ;
160 #endif
161 
162  listP.push_back(newP);
163  npt ++ ;
164 
165 #if (DEBUG_LEVEL1)
166  std::cout << "end vpPose::AddPoint(Dot) " << std::endl ;
167 #endif
168 }
169 
170 
171 void
173 {
175 }
176 
189 bool
190 vpPose::coplanar(int &coplanar_plane_type)
191 {
192  coplanar_plane_type = 0;
193  if (npt <2)
194  {
195  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
197  "Not enough points ")) ;
198  }
199 
200  if (npt ==3) return true ;
201 
202  double x1=0,x2=0,x3=0,y1=0,y2=0,y3=0,z1=0,z2=0,z3=0 ;
203 
204  std::list<vpPoint>::const_iterator it = listP.begin();
205 
206  vpPoint P1, P2, P3 ;
207 
208  // Get three 3D points that are not collinear and that is not at origin
209  bool degenerate = true;
210  bool not_on_origin = true;
211  std::list<vpPoint>::const_iterator it_tmp;
212 
213  std::list<vpPoint>::const_iterator it_i, it_j, it_k;
214  for (it_i=listP.begin(); it_i != listP.end(); ++it_i) {
215  if (degenerate == false) {
216  //std::cout << "Found a non degenerate configuration" << std::endl;
217  break;
218  }
219  P1 = *it_i;
220  // Test if point is on origin
221  if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon())
222  && (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon())
223  && (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
224  not_on_origin = false;
225  }
226  else {
227  not_on_origin = true;
228  }
229  if (not_on_origin) {
230  it_tmp = it_i; it_tmp ++; // j = i+1
231  for (it_j=it_tmp; it_j != 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; it_tmp ++; // k = j+1
247  for (it_k=it_tmp; it_k != listP.end(); ++it_k) {
248  P3 = *it_k;
249  if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon())
250  && (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon())
251  && (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
252  not_on_origin = false;
253  }
254  else {
255  not_on_origin = true;
256  }
257  if (not_on_origin) {
258  x1 = P1.get_oX() ;
259  x2 = P2.get_oX() ;
260  x3 = P3.get_oX() ;
261 
262  y1 = P1.get_oY() ;
263  y2 = P2.get_oY() ;
264  y3 = P3.get_oY() ;
265 
266  z1 = P1.get_oZ() ;
267  z2 = P2.get_oZ() ;
268  z3 = P3.get_oZ() ;
269 
270  vpColVector a_b(3), b_c(3), cross_prod;
271  a_b[0] = x1-x2; a_b[1] = y1-y2; a_b[2] = z1-z2;
272  b_c[0] = x2-x3; b_c[1] = y2-y3; b_c[2] = z2-z3;
273 
274  cross_prod = vpColVector::crossProd(a_b, b_c);
275  if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
276  degenerate = true; // points are collinear
277  else
278  degenerate = false;
279  }
280  if (degenerate == false)
281  break;
282  }
283  }
284  }
285  }
286  }
287 
288  if (degenerate) {
289  coplanar_plane_type = 4; // points are collinear
290  return true;
291  }
292 
293  double a = y1*z2 - y1*z3 - y2*z1 + y2*z3 + y3*z1 - y3*z2 ;
294  double b = -x1*z2 + x1*z3 + x2*z1 - x2*z3 - x3*z1 + x3*z2 ;
295  double c = x1*y2 - x1*y3 - x2*y1 + x2*y3 + x3*y1 - x3*y2 ;
296  double d = -x1*y2*z3 + x1*y3*z2 +x2*y1*z3 - x2*y3*z1 - x3*y1*z2 + x3*y2*z1 ;
297 
298  //std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d << std::endl;
299  if (std::fabs(b) <= std::numeric_limits<double>::epsilon()
300  && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
301  coplanar_plane_type = 1; // ax=d
302  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
303  && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
304  coplanar_plane_type = 2; // by=d
305  } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
306  && std::fabs(b) <= std::numeric_limits<double>::epsilon() ) {
307  coplanar_plane_type = 3; // cz=d
308  }
309 
310  double D = sqrt(vpMath::sqr(a)+vpMath::sqr(b)+vpMath::sqr(c)) ;
311 
312  double dist;
313 
314  for(it=listP.begin(); it != listP.end(); ++it)
315  {
316  P1 = *it ;
317  dist = (a*P1.get_oX() + b*P1.get_oY()+c*P1.get_oZ()+d)/D ;
318  //std::cout << "dist= " << dist << std::endl;
319 
320  if (fabs(dist) > distanceToPlaneForCoplanarityTest)
321  {
322  vpDEBUG_TRACE(10," points are not coplanar ") ;
323  // TRACE(" points are not coplanar ") ;
324  return false ;
325  }
326  }
327 
328  vpDEBUG_TRACE(10," points are coplanar ") ;
329  // vpTRACE(" points are coplanar ") ;
330 
331  return true ;
332 }
333 
343 double
345 {
346  double residual_ = 0 ;
347  vpPoint P ;
348  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
349  {
350  P = *it;
351  double x = P.get_x() ;
352  double y = P.get_y() ;
353 
354  P.track(cMo) ;
355 
356  residual_ += vpMath::sqr(x-P.get_x()) + vpMath::sqr(y-P.get_y()) ;
357  }
358  return residual_ ;
359 }
360 
361 
362 
385 void
387 {
388 #if (DEBUG_LEVEL1)
389  std::cout << "begin vpPose::ComputePose()" << std::endl ;
390 #endif
391 
392  if (npt <4)
393  {
394  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
396  "No enough point ")) ;
397  }
398 
399  switch (methode)
400  {
401  case DEMENTHON :
402  case DEMENTHON_VIRTUAL_VS :
403  case DEMENTHON_LOWE :
404  {
405 
406  if (npt <4)
407  {
408  vpERROR_TRACE("Dementhon method cannot be used in that case ") ;
409  vpERROR_TRACE("(at least 4 points are required)") ;
410  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
412  "Not enough points ")) ;
413  }
414 
415  // test si les point 3D sont coplanaires
416  int coplanar_plane_type = 0;
417  bool plan = coplanar(coplanar_plane_type) ;
418  if (plan == true)
419  {
420  //std::cout << "Plan" << std::endl;
421  try{
422  poseDementhonPlan(cMo);
423  }
424  catch(...)
425  {
426  vpERROR_TRACE(" ") ;
427  throw ;
428  }
429  //std::cout << "Fin Plan" << std::endl;
430  }
431  else
432  {
433  //std::cout << "No Plan" << std::endl;
434  try{
435  poseDementhonNonPlan(cMo) ;
436  }
437  catch(...)
438  {
439  vpERROR_TRACE(" ") ;
440  throw ;
441  }
442  //std::cout << "Fin No Plan" << std::endl;
443  }
444  }
445  break ;
446  case LAGRANGE :
447  case LAGRANGE_VIRTUAL_VS :
448  case LAGRANGE_LOWE :
449  {
450  // test si les point 3D sont coplanaires
451  int coplanar_plane_type;
452  bool plan = coplanar(coplanar_plane_type) ;
453 
454  if (plan == true && coplanar_plane_type > 0) // only plane oX=d, oY=d or oZ=d
455  {
456 
457  if (coplanar_plane_type == 4)
458  {
459  vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
460  vpERROR_TRACE("(points are collinear)") ;
462  "Points are collinear ")) ;
463  }
464  if (npt <4)
465  {
466  vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
467  vpERROR_TRACE("(at least 4 points are required)") ;
468  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
470  "Not enough points ")) ;
471  }
472  try {
473  poseLagrangePlan(cMo, coplanar_plane_type);
474  }
475  catch(...)
476  {
477  vpERROR_TRACE(" ") ;
478  throw ;
479  }
480  }
481  else
482  {
483  if (npt <4)
484  {
485  vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
486  vpERROR_TRACE("(at least 4 points are required)") ;
487  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
489  "Not enough points ")) ;
490  }
491  try {
492  poseLagrangeNonPlan(cMo);
493  }
494  catch(...)
495  {
496  vpERROR_TRACE(" ") ;
497  throw ;
498  }
499  }
500  }
501  break;
502  case RANSAC:
503  if (npt <4)
504  {
505  vpERROR_TRACE("Ransac method cannot be used in that case ") ;
506  vpERROR_TRACE("(at least 4 points are required)") ;
507  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
509  "Not enough points ")) ;
510  }
511  try {
512  poseRansac(cMo);
513  }
514  catch(...)
515  {
516  vpERROR_TRACE(" ") ;
517  throw ;
518  }
519  break;
520  case LOWE :
521  case VIRTUAL_VS:
522  break ;
523  }
524 
525  switch (methode)
526  {
527  case LAGRANGE :
528  case DEMENTHON :
529  case RANSAC :
530  break ;
531  case VIRTUAL_VS:
532  case LAGRANGE_VIRTUAL_VS:
534  {
535  try
536  {
537  poseVirtualVS(cMo);
538  }
539  catch(...)
540  {
541  vpERROR_TRACE(" ") ;
542  throw ;
543  }
544  }
545  break ;
546  case LOWE:
547  case LAGRANGE_LOWE:
548  case DEMENTHON_LOWE:
549  {
550  try
551  {
552  poseLowe(cMo);
553  }
554  catch(...)
555  {
556  vpERROR_TRACE(" ") ;
557  throw ;
558  }
559  }
560  break ;
561  }
562 
563 #if (DEBUG_LEVEL1)
564  std::cout << "end vpPose::ComputePose()" << std::endl ;
565 #endif
566 }
567 
568 
569 
570 void
572 {
573  vpPoint P;
574  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
575  {
576  P = *it ;
577 
578  std::cout << "3D oP " << P.oP.t() ;
579  std::cout << "3D cP " << P.cP.t() ;
580  std::cout << "2D " << P.p.t() ;
581  }
582 }
583 
584 
585 void
587  vpHomogeneousMatrix &cMo,
588  vpCameraParameters &cam,
589  double size,
590  vpColor col)
591 {
592  vpDisplay::displayFrame(I, cMo, cam, size, col);
593 }
594 
595 
596 void
598  vpHomogeneousMatrix &cMo,
599  vpCameraParameters &cam,
600  double size,
601  vpColor col)
602 {
603  vpDisplay::displayFrame(I,cMo,cam, size,col);
604 }
605 
606 /*
607 \brief display the 3D model in image I
608 \warning the 2D coordinate of the point have to be initialized
609 (lispP has to be modified)
610 */
611 void
613  vpCameraParameters &cam,
614  vpColor col)
615 {
616  vpPoint P ;
617  vpImagePoint ip;
618  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
619  {
620  P = *it;
621  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
622  vpDisplay::displayCross(I, ip, 5, col) ;
623  // std::cout << "3D oP " << P.oP.t() ;
624  // std::cout << "3D cP " << P.cP.t() ;
625  // std::cout << "2D " << P.p.t() ;
626  }
627 }
628 
629 /*
630 \brief display the 3D model in image I
631 \warning the 2D coordinate of the point have to be initialized
632 (lispP has to be modified)
633 */
634 void
636  vpCameraParameters &cam,
637  vpColor col)
638 {
639  vpPoint P ;
640  vpImagePoint ip;
641  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
642  {
643  P = *it;
644  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
645  vpDisplay::displayCross(I, ip, 5, col) ;
646  // std::cout << "3D oP " << P.oP.t() ;
647  // std::cout << "3D cP " << P.cP.t() ;
648  // std::cout << "2D " << P.p.t() ;
649  }
650 }
651 
652 
672 double
674  vpPoint &p3,vpPoint &p4,
675  double lx, vpCameraParameters & cam,
676  vpHomogeneousMatrix & cMo)
677 {
678 
679  std::vector<double> rectx(4) ;
680  std::vector<double> recty(4) ;
681  rectx[0]= 0 ;
682  recty[0]=0 ;
683  rectx[1]=1 ;
684  recty[1]=0 ;
685  rectx[2]=1 ;
686  recty[2]=1 ;
687  rectx[3]=0 ;
688  recty[3]=1 ;
689  std::vector<double> irectx(4) ;
690  std::vector<double> irecty(4) ;
691  irectx[0]=(p1.get_x()) ;
692  irecty[0]=(p1.get_y()) ;
693  irectx[1]=(p2.get_x()) ;
694  irecty[1]=(p2.get_y()) ;
695  irectx[2]=(p3.get_x()) ;
696  irecty[2]=(p3.get_y()) ;
697  irectx[3]=(p4.get_x()) ;
698  irecty[3]=(p4.get_y()) ;
699 
700  //calcul de l'homographie
701  vpMatrix H(3,3);
702  vpHomography hom;
703 
704  // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
705  vpHomography::HLM(rectx,recty,irectx,irecty,1,hom);
706  for (unsigned int i=0 ; i < 3 ; i++)
707  for(unsigned int j=0 ; j < 3 ; j++)
708  H[i][j] = hom[i][j] ;
709  //calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y axis)
710  vpColVector kh1(3);
711  vpColVector kh2(3);
712  vpMatrix K(3,3);
713  K = cam.get_K();
714  K.setIdentity();
715  vpMatrix Kinv =K.pseudoInverse();
716 
717  vpMatrix KinvH =Kinv*H;
718  kh1=KinvH.column(1);
719  kh2=KinvH.column(2);
720 
721 
722  double s= sqrt(kh1.sumSquare())/sqrt(kh2.sumSquare());
723 
724 
725  vpMatrix D(3,3);
726  D.setIdentity();
727  D[1][1]=1/s;
728  vpMatrix cHo=H*D;
729 
730  //Calcul de la rotation et de la translation
731  // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
732  p1.setWorldCoordinates(0,0,0) ;
733  p2.setWorldCoordinates(lx,0,0) ;
734  p3.setWorldCoordinates(lx,lx/s,0) ;
735  p4.setWorldCoordinates(0,lx/s,0) ;
736 
737 
738  vpPose P ;
739  P.addPoint(p1) ;
740  P.addPoint(p2) ;
741  P.addPoint(p3) ;
742  P.addPoint(p4) ;
743 
744 
746  return lx/s ;
747 
748 }
#define vpDEBUG_TRACE
Definition: vpDebug.h:482
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
virtual ~vpPose()
destructor
Definition: vpPose.cpp:117
double residual
compute the residual in meter
Definition: vpPose.h:97
void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0)
compute the pose using Lagrange approach (planar object)
bool coplanar(int &coplanar_plane_type)
test the coplanarity of the points
Definition: vpPose.cpp:190
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Lagrange approach (non planar object)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
void init()
basic initialisation (called by the constructors)
Definition: vpPose.cpp:67
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:612
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:129
void track(const vpHomogeneousMatrix &cMo)
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:809
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:95
vpColVector column(const unsigned int j)
Column extraction.
Definition: vpMatrix.cpp:2289
void poseVirtualVS(vpHomogeneousMatrix &cMo)
compute the pose using virtual visual servoing approach
Class that defines what is a point.
Definition: vpPoint.h:65
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)
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:100
vpColVector cP
Definition: vpTracker.h:82
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:178
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition: vpPose.cpp:586
void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:1159
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, the length on x axis is known but the proprtion of the rectangle are unknown.
Definition: vpPose.cpp:673
static double sqr(double x)
Definition: vpMath.h:106
vpRowVector t() const
transpose of Vector
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:78
Generic class defining intrinsic camera parameters.
double distanceToPlaneForCoplanarityTest
Definition: vpPose.h:145
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:131
void printPoint()
Definition: vpPose.cpp:571
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:290
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
void poseRansac(vpHomogeneousMatrix &cMo)
compute the pose using the Ransac approach
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay.cpp:371
vpPose()
Definition: vpPose.cpp:96
unsigned int npt
number of point used in pose computation
Definition: vpPose.h:94
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:127
vpMatrix get_K() const
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Dementhon approach (planar object)
Error that can be emited by the vpPose class and its derivates.
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
compute the pose using Dementhon approach (non planar object)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpPoseMethodType
Definition: vpPose.h:81
void setDistanceToPlaneForCoplanarityTest(double d)
Definition: vpPose.cpp:172
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1861
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Definition: vpPose.cpp:386
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
normalise the vector
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:155
vpColVector p
Definition: vpTracker.h:78
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
Definition: vpPose.cpp:344
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74
void clearPoint()
suppress all the point in the array of point
Definition: vpPose.cpp:133