ViSP  2.6.2
vpPoseRansac.cpp
1 /****************************************************************************
2  *
3  * $Id: vpPoseRansac.cpp 3765 2012-06-05 14:41:33Z 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  * Aurelien Yol
40  *
41  *****************************************************************************/
42 
43 
49 #include <visp/vpPose.h>
50 #include <visp/vpColVector.h>
51 #include <visp/vpRansac.h>
52 #include <visp/vpTime.h>
53 #include <visp/vpList.h>
54 #include <visp/vpPoseException.h>
55 
56 #include <iostream>
57 #include <cmath> // std::fabs
58 #include <limits> // numeric_limits
59 #include <stdlib.h>
60 
61 #define eps 1e-6
62 
63 
70 {
71  srand(0);
72  std::vector<unsigned int> best_consensus;
73  std::vector<unsigned int> cur_consensus;
74  std::vector<unsigned int> cur_outliers;
75  std::vector<unsigned int> cur_randoms;
76  unsigned int size = listP.size();
77  int nbTrials = 0;
78  unsigned int nbMinRandom = 4 ;
79  unsigned int nbInliers = 0;
80 
81  bool foundSolution = false;
82 
83  while (nbTrials < ransacMaxTrials && nbInliers < (unsigned)ransacNbInlierConsensus)
84  {
85  cur_outliers.clear();
86  cur_randoms.clear();
87 
88  std::vector<bool> usedPt(size, false);
89 
90  vpPose poseMin ;
91  for(unsigned int i = 0; i < nbMinRandom; i++)
92  {
93  int r = rand()%size;
94  while(usedPt[r] ) r = rand()%size;
95  usedPt[r] = true;
96 
97  std::list<vpPoint>::const_iterator iter = listP.begin();
98  std::advance(iter, r);
99  vpPoint pt = *iter;
100 
101  bool degenerate = false;
102  for(std::list<vpPoint>::const_iterator it = poseMin.listP.begin(); it != poseMin.listP.end(); ++it){
103  vpPoint ptdeg = *it;
104  if( ((fabs(pt.get_x() - ptdeg.get_x()) < 1e-6) && (fabs(pt.get_y() - ptdeg.get_y()) < 1e-6)) ||
105  ((fabs(pt.get_oX() - ptdeg.get_oX()) < 1e-6) && (fabs(pt.get_oY() - ptdeg.get_oY()) < 1e-6) && (fabs(pt.get_oZ() - ptdeg.get_oZ()) < 1e-6))){
106  degenerate = true;
107  break;
108  }
109  }
110 
111  if(!degenerate){
112  poseMin.addPoint(pt) ;
113  cur_randoms.push_back(r);
114  }
115  else
116  i--;
117  }
118 
119  poseMin.computePose(vpPose::DEMENTHON,cMo) ;
120  double r = poseMin.computeResidual(cMo) ;
121  r = sqrt(r)/(double)nbMinRandom;
122 
123  if (r < ransacThreshold)
124  {
125  unsigned int nbInliersCur = 0;
126  //std::cout << "R├ęsultat : " << r << " / " << vpPoseVector(cMo).sumSquare()<< std::endl ;
127  int iter = 0;
128  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
129  {
130  vpPoint pt = *it;
131  vpPoint p(pt) ;
132  p.track(cMo) ;
133 
134  double d = vpMath::sqr(p.get_x() - pt.get_x()) + vpMath::sqr(p.get_y() - pt.get_y()) ;
135  double error = sqrt(d) ;
136  if(error < ransacThreshold){ // the point is considered an inlier if the error is below the threshold
137  nbInliersCur++;
138  cur_consensus.push_back(iter);
139  }
140  else
141  cur_outliers.push_back(iter);
142 
143  iter++;
144  }
145  //std::cout << "Nombre d'inliers " << nbInliersCur << "/" << nbInliers << std::endl ;
146 
147  if(nbInliersCur > nbInliers)
148  {
149  foundSolution = true;
150  best_consensus = cur_consensus;
151  nbInliers = nbInliersCur;
152  }
153 
154  nbTrials++;
155  cur_consensus.clear();
156 
157  if(nbTrials >= ransacMaxTrials){
158  vpERROR_TRACE("Ransac reached the maximum number of trials");
159  foundSolution = true;
160  }
161  }
162  }
163 
164  if(foundSolution){
165  //std::cout << "Nombre d'inliers " << nbInliers << std::endl ;
166 
167  //Display the random picked points
168  /*
169  std::cout << "Randoms : ";
170  for(unsigned int i = 0 ; i < cur_randoms.size() ; i++)
171  std::cout << cur_randoms[i] << " ";
172  std::cout << std::endl;
173  */
174 
175  //Display the outliers
176  /*
177  std::cout << "Outliers : ";
178  for(unsigned int i = 0 ; i < cur_outliers.size() ; i++)
179  std::cout << cur_outliers[i] << " ";
180  std::cout << std::endl;
181  */
182 
183  if(nbInliers >= (unsigned)ransacNbInlierConsensus)
184  {
185  vpPose pose ;
186  for(unsigned i = 0 ; i < best_consensus.size(); i++)
187  {
188  std::list<vpPoint>::const_iterator iter = listP.begin();
189  std::advance(iter, best_consensus[i]);
190  vpPoint pt = *iter;
191 
192  pose.addPoint(pt) ;
193  ransacInliers.push_back(pt);
194  }
195 
197  //std::cout << "Residue finale "<< pose.computeResidual(cMo) << std::endl ;
198  }
199  }
200 }
201 
226 void vpPose::findMatch(std::vector<vpPoint> &p2D,
227  std::vector<vpPoint> &p3D,
228  const int &numberOfInlierToReachAConsensus,
229  const double &threshold,
230  unsigned int &ninliers,
231  std::vector<vpPoint> &listInliers,
232  vpHomogeneousMatrix &cMo,
233  const int &maxNbTrials )
234 {
235  vpPose pose;
236 
237  int nbPts = 0;
238  for(unsigned int i = 0 ; i < p2D.size() ; i++)
239  {
240  for(unsigned int j = 0 ; j < p3D.size() ; j++)
241  {
242  vpPoint pt;
243  pt.set_x(p2D[i].get_x());
244  pt.set_y(p2D[i].get_y());
245  pt.setWorldCoordinates(p3D[j].getWorldCoordinates());
246  pose.addPoint(pt);
247  nbPts++;
248  }
249  }
250 
251  if (pose.listP.size() < 4)
252  {
253  vpERROR_TRACE("Ransac method cannot be used in that case ") ;
254  vpERROR_TRACE("(at least 4 points are required)") ;
255  vpERROR_TRACE("Not enough point (%d) to compute the pose ",pose.listP.size()) ;
257  "Not enough points ")) ;
258  }
259  else
260  {
261  pose.setRansacMaxTrials(maxNbTrials);
262  pose.setRansacNbInliersToReachConsensus(numberOfInlierToReachAConsensus);
263  pose.setRansacThreshold(threshold);
264  pose.computePose(vpPose::RANSAC, cMo);
265  ninliers = pose.getRansacNbInliers();
266  listInliers = pose.getRansacInliers();
267  }
268 }
269 
270 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
271 
272 /*
273 \brief
274 
275 determines if there more than one the same point in the chosen set of point
276 
277 point are coded this way
278 x1,y1, X1, Y1, Z1
279 xnm, ynm, Xnm, Ynm, Znm
280 
281 leading to 5*n*m
282 */
283 bool
285 {
286 
287  // vpTRACE("%p %p %d",&x, ind, x.getRows()) ;
288  for (int i=1 ; i < 4 ; i++)
289  for (int j=0 ; j<i ; j++)
290  {
291  unsigned int indi = 5*ind[i] ;
292  unsigned int indj = 5*ind[j] ;
293 
294  if ((fabs(x[indi] - x[indj]) < 1e-6) && (fabs(x[indi+1] - x[indj+1]) < 1e-6))
295  { return true ; }
296 
297  if ((fabs(x[indi+2] - x[indj+2]) < 1e-6) && (fabs(x[indi+3] - x[indj+3]) < 1e-6) && (fabs(x[indi+4] - x[indj+4]) < 1e-6))
298  { return true ; }
299  }
300 
301  return false ;
302 }
308 void
310 {
311  unsigned int i ;
312 
313  vpPoint p[4] ;
314 
315  vpPose pose ;
316  pose.clearPoint() ;
317  for(i=0 ; i < 4 ; i++)
318  {
319 
320  unsigned int index = 5*ind[i] ;
321 
322  p[i].set_x(x[index]) ;
323  p[i].set_y(x[index+1]) ;
324 
325  p[i].setWorldCoordinates(x[index+2],x[index+3], x[index+4]) ;
326  pose.addPoint(p[i]) ;
327  }
328 
329 
330  // pose.printPoint() ;
331  vpHomogeneousMatrix cMo ;
332  try {
333  pose.computePose(vpPose::DEMENTHON, cMo) ;
334  // std::cout << cMo << std::endl ;
335  }
336  catch(...)
337  {
338  cMo.setIdentity() ;
339  }
340 
341  M.resize(16) ;
342  for (i=0 ; i <16 ; i++)
343  {
344  M[i] = cMo.data[i] ;
345  }
346 
347 }
348 
349 
356 double
358 {
359 
360  unsigned int i ;
361  unsigned int n = x.getRows()/5 ;
362 
363  vpPoint *p;
364  p = new vpPoint [n] ;
365  {
366  // firsttime=1 ;
367  for( i=0 ; i < n ; i++)
368  {
369  p[i].setWorldCoordinates(x[5*i+2],x[5*i+3], x[5*i+4]) ;
370  }
371  }
372 
373  vpHomogeneousMatrix cMo ;
374  for (i=0 ; i <16 ; i++)
375  {
376  cMo.data[i] = M[i];
377  }
378 
379 
380  d.resize(n) ;
381  vpColVector cP, xy ;
382 
383  for( i=0 ; i < n ; i++)
384  {
385  p[i].changeFrame(cMo,cP) ;
386  p[i].projection(cP,xy) ;
387  d[i] = sqrt(vpMath::sqr(x[5*i]-xy[0])+vpMath::sqr(x[5*i+1]-xy[1])) ;
388  }
389 
390  delete [] p;
391 
392  return 0 ;
393 }
394 
395 void
396 vpPose::initRansac(const unsigned int n,
397  const double *x, const double *y,
398  const unsigned int m,
399  const double *X, const double *Y, const double *Z,
400  vpColVector &data)
401 {
402  data.resize(5*n*m) ;
403  unsigned int k =0 ;
404  for (unsigned int i=0 ; i < n ; i++)
405  {
406  for (unsigned int j=0 ; j < m ; j++)
407  {
408  data[k] = x[i] ;
409  data[k+1] = y[i] ;
410  data[k+2] = X[j] ;
411  data[k+3] = Y[j] ;
412  data[k+4] = Z[j] ;
413 
414  k+=5 ;
415  }
416  }
417 }
418 
419 
454 void
455 vpPose::ransac(const unsigned int n,
456  const double *x, const double *y,
457  const unsigned int m,
458  const double *X, const double *Y, const double *Z,
459  const int numberOfInlierToReachAConsensus,
460  const double threshold,
461  unsigned int &ninliers,
462  vpColVector &xi, vpColVector &yi,
463  vpColVector &Xi, vpColVector &Yi, vpColVector &Zi,
464  vpHomogeneousMatrix &cMo,
465  const int maxNbTrials)
466 {
467 
468 
469  double tms = vpTime::measureTimeMs() ;
470  vpColVector data ;
471  unsigned int i;
472  vpPose::initRansac(n,x,y,m,X,Y,Z, data) ;
473 
474  vpColVector M(16) ;
475  vpColVector inliers(n*m) ;
476  vpRansac<vpPose>::ransac(n*m,data,4,
477  threshold, M,inliers,
478  numberOfInlierToReachAConsensus, 0.0, maxNbTrials) ;
479 
480 
481  // we count the number of inliers
482  ninliers = 0 ;
483  for(i=0 ; i < n*m ; i++)
484  {
485  //if (inliers[i]==1)
486  if (std::fabs(inliers[i]-1) <= std::fabs(vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
487  {
488  ninliers++ ;
489  }
490  }
491 
492  xi.resize(ninliers) ;
493  yi.resize(ninliers) ;
494  Xi.resize(ninliers) ;
495  Yi.resize(ninliers) ;
496  Zi.resize(ninliers) ;
497 
498  unsigned int k =0 ;
499  for(i=0 ; i < n*m ; i++)
500  {
501  //if (inliers[i]==1)
502  if (std::fabs(inliers[i]-1) <= std::fabs(vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
503  {
504  xi[k] = data[5*i] ;
505  yi[k] = data[5*i+1] ;
506  Xi[k] = data[5*i+2] ;
507  Yi[k] = data[5*i+3] ;
508  Zi[k] = data[5*i+4] ;
509  k++ ;
510  }
511  }
512 
513  for (i=0 ; i <16 ; i++)
514  {
515  cMo.data[i] = M[i];
516  }
517 
518  std::cout << vpTime::measureTimeMs() - tms << "ms" << std::endl ;
519 
520 }
521 
549 void
550 vpPose::ransac(const unsigned int n,
551  const vpPoint *p,
552  const unsigned int m,
553  const vpPoint *P,
554  const int numberOfInlierToReachAConsensus,
555  const double threshold,
556  unsigned int &ninliers,
557  std::list<vpPoint> &lPi,
558  vpHomogeneousMatrix &cMo,
559  const int maxNbTrials)
560 {
561  double *x, *y;
562  x = new double [n];
563  y = new double [n] ;
564  for (unsigned int i=0 ; i < n ; i++)
565  {
566  x[i] = p[i].get_x() ;
567  y[i] = p[i].get_y() ;
568  }
569  double *X, *Y, *Z;
570  X = new double [m];
571  Y = new double [m];
572  Z = new double [m];
573  for (unsigned int i=0 ; i < m ; i++)
574  {
575  X[i] = P[i].get_oX() ;
576  Y[i] = P[i].get_oY() ;
577  Z[i] = P[i].get_oZ() ;
578  }
579 
580  vpColVector xi,yi,Xi,Yi,Zi ;
581 
582  ransac(n,x,y,
583  m,X,Y,Z, numberOfInlierToReachAConsensus,
584  threshold,
585  ninliers,
586  xi,yi,Xi,Yi,Zi,
587  cMo, maxNbTrials) ;
588 
589  for(unsigned int i=0 ; i < ninliers ; i++)
590  {
591  vpPoint Pi ;
592  Pi.setWorldCoordinates(Xi[i], Yi[i], Zi[i]) ;
593  Pi.set_x(xi[i]) ;
594  Pi.set_y(yi[i]) ;
595  lPi.push_back(Pi) ;
596  }
597 
598  delete [] x;
599  delete [] y;
600  delete [] X;
601  delete [] Y;
602  delete [] Z;
603 }
604 
630 void
631 vpPose::ransac(std::list<vpPoint> &lp,
632  std::list<vpPoint> &lP,
633  const int numberOfInlierToReachAConsensus,
634  const double threshold,
635  unsigned int &ninliers,
636  std::list<vpPoint> &lPi,
637  vpHomogeneousMatrix &cMo,
638  const int maxNbTrials)
639 {
640  unsigned int i;
641  unsigned int n = lp.size() ;
642  unsigned int m = lP.size() ;
643 
644  double *x, *y;
645  x = new double [n];
646  y = new double [n];
647 
648  vpPoint pin ;
649  i = 0;
650  for (std::list<vpPoint>::const_iterator it = lp.begin(); it != lp.end(); ++it)
651  {
652  pin = *it;
653  x[i] = pin.get_x() ;
654  y[i] = pin.get_y() ;
655  ++ i;
656  }
657 
658  double *X, *Y, *Z;
659  X = new double [m];
660  Y = new double [m];
661  Z = new double [m];
662  i = 0;
663  for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
664  {
665  pin = *it;
666  X[i] = pin.get_oX() ;
667  Y[i] = pin.get_oY() ;
668  Z[i] = pin.get_oZ() ;
669  ++i;
670  }
671 
672  vpColVector xi,yi,Xi,Yi,Zi ;
673 
674  ransac(n,x,y,
675  m,X,Y,Z, numberOfInlierToReachAConsensus,
676  threshold,
677  ninliers,
678  xi,yi,Xi,Yi,Zi,
679  cMo, maxNbTrials) ;
680 
681  for( i=0 ; i < ninliers ; i++)
682  {
683  vpPoint Pi ;
684  Pi.setWorldCoordinates(Xi[i],Yi[i], Zi[i]) ;
685  Pi.set_x(xi[i]) ;
686  Pi.set_y(yi[i]) ;
687  lPi.push_back(Pi);
688  }
689 
690  delete [] x;
691  delete [] y;
692  delete [] X;
693  delete [] Y;
694  delete [] Z;
695 
696 }
697 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS
698 
699 /*
700  * Local variables:
701  * c-basic-offset: 2
702  * End:
703  */
void projection(const vpColVector &_cP, vpColVector &_p)
Projection onto the image plane of a point. Input: the 3D coordinates in the camera frame _cP...
Definition: vpPoint.cpp:132
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 setIdentity()
Basic initialisation (identity)
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:136
void setRansacThreshold(const double &t)
Definition: vpPose.h:174
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.h:183
void track(const vpHomogeneousMatrix &cMo)
static double measureTimeMs()
Definition: vpTime.cpp:86
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:145
static void findMatch(std::vector< vpPoint > &p2D, std::vector< vpPoint > &p3D, const int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector< vpPoint > &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials=10000)
static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
double computeResidual()
compute the residual (in meter)
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:97
Class that defines what is a point.
Definition: vpPoint.h:65
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
double * data
address of the first element of the data array
Definition: vpMatrix.h:116
std::vector< vpPoint > getRansacInliers()
Definition: vpPose.h:177
static double sqr(double x)
Definition: vpMath.h:106
int getRansacNbInliers()
Definition: vpPose.h:176
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:80
double computeResidual(vpHomogeneousMatrix &cMo)
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
Definition: vpPose.cpp:255
void setRansacNbInliersToReachConsensus(const int &nbC)
Definition: vpPose.h:173
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:138
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.h:185
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
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:175
static void ransac(const unsigned int n, const double *x, const double *y, const unsigned int m, const double *X, const double *Y, const double *Z, const int numberOfInlierToReachAConsensus, const double threshold, unsigned int &ninliers, vpColVector &xi, vpColVector &yi, vpColVector &Xi, vpColVector &Yi, vpColVector &Zi, vpHomogeneousMatrix &cMo, const int maxNbTrials=10000)
static bool degenerateConfiguration(vpColVector &x, unsigned int *ind)
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:134
Error that can be emited by the vpPose class and its derivates.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
static bool ransac(unsigned int npts, vpColVector &x, unsigned int s, double t, vpColVector &model, vpColVector &inliers, int consensus=1000, double areaThreshold=0.0, const int maxNbumbersOfTrials=10000)
RANSAC - Robustly fits a model to data with the RANSAC algorithm.
Definition: vpRansac.h:138
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Definition: vpPose.cpp:298
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:150
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:148
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 resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94
void clearPoint()
suppress all the point in the array of point
Definition: vpPose.cpp:126