ViSP  2.6.2
vpPose.cpp
1 /****************************************************************************
2 *
3 * $Id: vpPose.cpp 3764 2012-06-05 14:34:06Z ayol $
4 *
5 * This file is part of the ViSP software.
6 * Copyright (C) 2005 - 2012 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 
75  lambda = 0.25 ;
76 
77  c3d = NULL ;
78 
79  vvsIterMax = 200 ;
80 
82 
83  computeCovariance = false;
84 
85  ransacMaxTrials = 1000;
86 
87 #if (DEBUG_LEVEL1)
88  std::cout << "end vpPose::Init() " << std::endl ;
89 #endif
90 
91 }
92 
94 {
95 #if (DEBUG_LEVEL1)
96  std::cout << "begin vpPose::vpPose() " << std::endl ;
97 #endif
98 
99  init() ;
100 
101 #if (DEBUG_LEVEL1)
102  std::cout << "end vpPose::vpPose() " << std::endl ;
103 #endif
104 
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 }
125 void
127 {
128 #if (DEBUG_LEVEL1)
129  std::cout << "begin vpPose::ClearPoint() " << std::endl ;
130 #endif
131 
132  listP.clear() ;
133  npt = 0 ;
134 
135 #if (DEBUG_LEVEL1)
136  std::cout << "end vpPose::ClearPoint() " << std::endl ;
137 #endif
138 }
139 
147 void
149 {
150 
151 #if (DEBUG_LEVEL1)
152  std::cout << "begin vpPose::AddPoint(Dot) " << std::endl ;
153 #endif
154 
155  listP.push_back(newP);
156  npt ++ ;
157 
158 #if (DEBUG_LEVEL1)
159  std::cout << "end vpPose::AddPoint(Dot) " << std::endl ;
160 #endif
161 }
162 
163 
164 void
166 {
168 }
169 
175 bool
177 {
178 
179  if (npt <2)
180  {
181  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
183  "Not enough points ")) ;
184  }
185 
186  if (npt ==3) return true ;
187 
188  double x1,x2,x3,y1,y2,y3,z1,z2,z3 ;
189 
190  std::list<vpPoint>::const_iterator it = listP.begin();
191 
192  vpPoint P1,P2, P3 ;
193  P1 = *it; ++it;
194  //if ((P1.get_oX() ==0) && (P1.get_oY() ==0) && (P1.get_oZ() ==0))
195  if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon())
196  && (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon())
197  && (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon()))
198  {
199  P1 = *it; ++it;
200  }
201  P2 = *it; ++it;
202  P3 = *it;
203 
204  x1 = P1.get_oX() ;
205  x2 = P2.get_oX() ;
206  x3 = P3.get_oX() ;
207 
208  y1 = P1.get_oY() ;
209  y2 = P2.get_oY() ;
210  y3 = P3.get_oY() ;
211 
212  z1 = P1.get_oZ() ;
213  z2 = P2.get_oZ() ;
214  z3 = P3.get_oZ() ;
215 
216  double a = y1*z2 - y1*z3 - y2*z1 + y2*z3 + y3*z1 - y3*z2 ;
217  double b = -x1*z2 + x1*z3 + x2*z1 - x2*z3 - x3*z1 + x3*z2 ;
218  double c = x1*y2 - x1*y3 - x2*y1 + x2*y3 + x3*y1 - x3*y2 ;
219  double d = -x1*y2*z3 + x1*y3*z2 +x2*y1*z3 - x2*y3*z1 - x3*y1*z2 + x3*y2*z1 ;
220 
221 
222  double D = sqrt(vpMath::sqr(a)+vpMath::sqr(b)+vpMath::sqr(c)) ;
223 
224  double dist;
225 
226  for(it=listP.begin(); it != listP.end(); ++it)
227  {
228  P1 = *it ;
229  dist = (a*P1.get_oX() + b*P1.get_oY()+c*P1.get_oZ()+d)/D ;
230 
231  if (fabs(dist) > distanceToPlaneForCoplanarityTest)
232  {
233  vpDEBUG_TRACE(10," points are not coplanar ") ;
234  // TRACE(" points are not coplanar ") ;
235  return false ;
236  }
237  }
238 
239  vpDEBUG_TRACE(10," points are coplanar ") ;
240  // vpTRACE(" points are coplanar ") ;
241 
242  return true ;
243 }
244 
254 double
256 {
257 
258  double residual = 0 ;
259  vpPoint P ;
260  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
261  {
262  P = *it;
263  double x = P.get_x() ;
264  double y = P.get_y() ;
265 
266  P.track(cMo) ;
267 
268  residual += vpMath::sqr(x-P.get_x()) + vpMath::sqr(y-P.get_y()) ;
269  }
270  return residual ;
271 }
272 
273 
274 
297 void
299 {
300 #if (DEBUG_LEVEL1)
301  std::cout << "begin vpPose::ComputePose()" << std::endl ;
302 #endif
303 
304  if (npt <4)
305  {
306  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
308  "No enough point ")) ;
309  }
310 
311  switch (methode)
312  {
313  case DEMENTHON :
314  case DEMENTHON_VIRTUAL_VS :
315  case DEMENTHON_LOWE :
316  {
317 
318  if (npt <4)
319  {
320  vpERROR_TRACE("Dementhon method cannot be used in that case ") ;
321  vpERROR_TRACE("(at least 4 points are required)") ;
322  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
324  "Not enough points ")) ;
325  }
326 
327  // test si les point 3D sont coplanaires
328  int plan = coplanar() ;
329  if (plan == 1)
330  {
331  //std::cout << "Plan" << std::endl;
332  try{
333  poseDementhonPlan(cMo);
334  }
335  catch(...)
336  {
337  vpERROR_TRACE(" ") ;
338  throw ;
339  }
340  //std::cout << "Fin Plan" << std::endl;
341  }
342  else
343  {
344  //std::cout << "No Plan" << std::endl;
345  try{
346  poseDementhonNonPlan(cMo) ;
347  }
348  catch(...)
349  {
350  vpERROR_TRACE(" ") ;
351  throw ;
352  }
353  //std::cout << "Fin No Plan" << std::endl;
354  }
355  }
356  break ;
357  case LAGRANGE :
358  case LAGRANGE_VIRTUAL_VS :
359  case LAGRANGE_LOWE :
360  {
361  // test si les point 3D sont coplanaires
362 
363  int plan = coplanar() ;
364 
365  if (plan == 1)
366  {
367 
368  if (npt <4)
369  {
370  vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
371  vpERROR_TRACE("(at least 4 points are required)") ;
372  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
374  "Not enough points ")) ;
375  }
376  try {
377  poseLagrangePlan(cMo);
378  }
379  catch(...)
380  {
381  vpERROR_TRACE(" ") ;
382  throw ;
383  }
384  }
385  else
386  {
387  if (npt <4)
388  {
389  vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
390  vpERROR_TRACE("(at least 4 points are required)") ;
391  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
393  "Not enough points ")) ;
394  }
395  try {
396  poseLagrangeNonPlan(cMo);
397  }
398  catch(...)
399  {
400  vpERROR_TRACE(" ") ;
401  throw ;
402  }
403  }
404  }
405  break;
406  case RANSAC:
407  if (npt <4)
408  {
409  vpERROR_TRACE("Ransac method cannot be used in that case ") ;
410  vpERROR_TRACE("(at least 4 points are required)") ;
411  vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
413  "Not enough points ")) ;
414  }
415  try {
416  poseRansac(cMo);
417  }
418  catch(...)
419  {
420  vpERROR_TRACE(" ") ;
421  throw ;
422  }
423  break;
424  case LOWE :
425  case VIRTUAL_VS:
426  break ;
427  }
428 
429  switch (methode)
430  {
431  case LAGRANGE :
432  case DEMENTHON :
433  case RANSAC :
434  break ;
435  case VIRTUAL_VS:
436  case LAGRANGE_VIRTUAL_VS:
438  {
439  try
440  {
441  poseVirtualVS(cMo);
442  }
443  catch(...)
444  {
445  vpERROR_TRACE(" ") ;
446  throw ;
447  }
448  }
449  break ;
450  case LOWE:
451  case LAGRANGE_LOWE:
452  case DEMENTHON_LOWE:
453  {
454  try
455  {
456  poseLowe(cMo);
457  }
458  catch(...)
459  {
460  vpERROR_TRACE(" ") ;
461  throw ;
462  }
463  }
464  break ;
465  }
466 
467 #if (DEBUG_LEVEL1)
468  std::cout << "end vpPose::ComputePose()" << std::endl ;
469 #endif
470 }
471 
472 
473 
474 void
476 {
477  vpPoint P;
478  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
479  {
480  P = *it ;
481 
482  std::cout << "3D oP " << P.oP.t() ;
483  std::cout << "3D cP " << P.cP.t() ;
484  std::cout << "2D " << P.p.t() ;
485  }
486 }
487 
488 
489 void
491  vpHomogeneousMatrix &cMo,
492  vpCameraParameters &cam,
493  double size,
494  vpColor col)
495 {
496  vpDisplay::displayFrame(I, cMo, cam, size, col);
497 }
498 
499 
500 void
502  vpHomogeneousMatrix &cMo,
503  vpCameraParameters &cam,
504  double size,
505  vpColor col)
506 {
507  vpDisplay::displayFrame(I,cMo,cam, size,col);
508 }
509 
510 /*
511 \brief display the 3D model in image I
512 \warning the 2D coordinate of the point have to be initialized
513 (lispP has to be modified)
514 */
515 void
517  vpCameraParameters &cam,
518  vpColor col)
519 {
520  vpPoint P ;
521  vpImagePoint ip;
522  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
523  {
524  P = *it;
525  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
526  vpDisplay::displayCross(I, ip, 5, col) ;
527  // std::cout << "3D oP " << P.oP.t() ;
528  // std::cout << "3D cP " << P.cP.t() ;
529  // std::cout << "2D " << P.p.t() ;
530  }
531 }
532 
533 /*
534 \brief display the 3D model in image I
535 \warning the 2D coordinate of the point have to be initialized
536 (lispP has to be modified)
537 */
538 void
540  vpCameraParameters &cam,
541  vpColor col)
542 {
543  vpPoint P ;
544  vpImagePoint ip;
545  for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
546  {
547  P = *it;
548  vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
549  vpDisplay::displayCross(I, ip, 5, col) ;
550  // std::cout << "3D oP " << P.oP.t() ;
551  // std::cout << "3D cP " << P.cP.t() ;
552  // std::cout << "2D " << P.p.t() ;
553  }
554 }
555 
556 
576 double
578  vpPoint &p3,vpPoint &p4,
579  double lx, vpCameraParameters & cam,
580  vpHomogeneousMatrix & cMo)
581 {
582 
583  double rectx[4] ;
584  double recty[4] ;
585  rectx[0]= 0 ;
586  recty[0]=0 ;
587  rectx[1]=1 ;
588  recty[1]=0 ;
589  rectx[2]=1 ;
590  recty[2]=1 ;
591  rectx[3]=0 ;
592  recty[3]=1 ;
593  double irectx[4] ;
594  double irecty[4] ;
595  irectx[0]=(p1.get_x()) ;
596  irecty[0]=(p1.get_y()) ;
597  irectx[1]=(p2.get_x()) ;
598  irecty[1]=(p2.get_y()) ;
599  irectx[2]=(p3.get_x()) ;
600  irecty[2]=(p3.get_y()) ;
601  irectx[3]=(p4.get_x()) ;
602  irecty[3]=(p4.get_y()) ;
603 
604  //calcul de l'homographie
605  vpMatrix H(3,3);
606  vpHomography hom;
607 
608  // vpHomography::HartleyDLT(4,rectx,recty,irectx,irecty,hom);
609  vpHomography::HLM(4,rectx,recty,irectx,irecty,1,hom);
610  for (unsigned int i=0 ; i < 3 ; i++)
611  for(unsigned int j=0 ; j < 3 ; j++)
612  H[i][j] = hom[i][j] ;
613  //calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y axis)
614  vpColVector kh1(3);
615  vpColVector kh2(3);
616  vpMatrix K(3,3);
617  K = cam.get_K();
618  K.setIdentity();
619  vpMatrix Kinv =K.pseudoInverse();
620 
621  vpMatrix KinvH =Kinv*H;
622  kh1=KinvH.column(1);
623  kh2=KinvH.column(2);
624 
625 
626  double s= sqrt(kh1.sumSquare())/sqrt(kh2.sumSquare());
627 
628 
629  vpMatrix D(3,3);
630  D.setIdentity();
631  D[1][1]=1/s;
632  vpMatrix cHo=H*D;
633 
634  //Calcul de la rotation et de la translation
635  // PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
636  p1.setWorldCoordinates(0,0,0) ;
637  p2.setWorldCoordinates(lx,0,0) ;
638  p3.setWorldCoordinates(lx,lx/s,0) ;
639  p4.setWorldCoordinates(0,lx/s,0) ;
640 
641 
642  vpPose P ;
643  P.addPoint(p1) ;
644  P.addPoint(p2) ;
645  P.addPoint(p3) ;
646  P.addPoint(p4) ;
647 
648 
650  return lx/s ;
651 
652 }
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
virtual ~vpPose()
destructor
Definition: vpPose.cpp:110
double residual
compute the residual in meter
Definition: vpPose.h:99
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:379
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:123
void displayModel(vpImage< unsigned char > &I, vpCameraParameters &cam, vpColor col=vpColor::none)
Definition: vpPose.cpp:516
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:136
void track(const vpHomogeneousMatrix &cMo)
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:145
double computeResidual()
compute the residual (in meter)
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:758
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:97
vpColVector column(const unsigned int j)
Column extraction.
Definition: vpMatrix.cpp:2238
void poseVirtualVS(vpHomogeneousMatrix &cMo)
compute the pose using virtual visual servoing approach
Class that defines what is a point.
Definition: vpPoint.h:65
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:102
vpColVector cP
Definition: vpTracker.h:82
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:174
static void display(vpImage< unsigned char > &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col=vpColor::none)
Definition: vpPose.cpp:490
void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:1108
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:577
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:80
static void HLM(unsigned int n, double *xb, double *yb, double *xa, double *ya, bool isplan, vpHomography &aHb)
Computes the homography matrix from planar or non planar points using Ezio Malis linear method (HLM)...
Generic class defining intrinsic camera parameters.
double distanceToPlaneForCoplanarityTest
Definition: vpPose.h:149
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:138
bool coplanar()
test the coplanarity of the points
Definition: vpPose.cpp:176
void printPoint()
Definition: vpPose.cpp:475
void poseLagrangePlan(vpHomogeneousMatrix &cMo)
compute the pose using Lagrange approach (planar object)
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:291
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:143
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:346
vpPose()
constructor
Definition: vpPose.cpp:93
unsigned int npt
number of point used in pose computation
Definition: vpPose.h:96
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:134
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:83
void setDistanceToPlaneForCoplanarityTest(double d)
Definition: vpPose.cpp:165
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1810
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Definition: vpPose.cpp:298
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:148
vpColVector p
Definition: vpTracker.h:78
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:126