ViSP  2.10.0
vpHomographyRansac.cpp
1 
2 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3  * Copyright Projet Lagadic / IRISA-INRIA Rennes, 2005
4  * www : http://www.irisa.fr/lagadic
5  *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6  *
7  * File: vpHomographyRansac.cpp
8  * Project: ViSP 2.0
9  * Author: Eric Marchand
10  * From: vpHomographyRansac.cpp, ViSP 2
11  *
12  * Version control
13  * ===============
14  *
15  * $Id: vpHomographyRansac.cpp 4649 2014-02-07 14:57:11Z fspindle $
16  * optimized by Tran to improve speed.
17  * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
18 
19 #include <visp/vpHomography.h>
20 #include <visp/vpColVector.h>
21 #include <visp/vpRansac.h>
22 
23 #include <visp/vpImage.h>
24 #include <visp/vpDisplay.h>
25 #include <visp/vpMeterPixelConversion.h>
26 
27 #define vpEps 1e-6
28 
34 #ifndef DOXYGEN_SHOULD_SKIP_THIS
35 
36 bool iscolinear(double *x1, double *x2, double *x3);
37 bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);
38 
39 bool
40 iscolinear(double *x1, double *x2, double *x3)
41 {
42  vpColVector p1(3), p2(3), p3(3);
43  p1 << x1 ;
44  p2 << x2 ;
45  p3 << x3 ;
46  //vpColVector v;
47  //vpColVector::cross(p2-p1, p3-p1, v);
48  //return (v.sumSquare() < vpEps);
49  // Assume inhomogeneous coords, or homogeneous coords with equal
50  // scale.
51  return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
52 }
53 bool
54 isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
55 {
56  return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
57 }
58 
59 
60 bool
61 vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind,
62  double threshold_area)
63 {
64 
65  unsigned int i, j, k;
66 
67  for (i=1 ; i < 4 ; i++)
68  for (j=0 ; j<i ; j++)
69  if (ind[i]==ind[j]) return true ;
70 
71  unsigned int n = x.getRows()/4 ;
72  double pa[4][3] ;
73  double pb[4][3] ;
74 
75 
76 
77  for(i = 0 ; i < 4 ; i++)
78  {
79  pb[i][0] = x[2*ind[i]] ;
80  pb[i][1] = x[2*ind[i]+1] ;
81  pb[i][2] = 1;
82 
83  pa[i][0] = x[2*n+2*ind[i]] ;
84  pa[i][1] = x[2*n+2*ind[i]+1] ;
85  pa[i][2] = 1;
86  }
87 
88  i = 0, j = 1, k = 2;
89 
90  double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
91  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
92  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
93 
94  i = 0; j = 1, k = 3;
95  double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
96  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
97  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
98 
99  i = 0; j = 2, k = 3;
100  double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
101  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
102  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
103 
104  i = 1; j = 2, k = 3;
105  double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
106  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
107  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
108 
109  double sum_area = area012 + area013 + area023 + area123;
110 
111  return ((sum_area < threshold_area) ||
112  (iscolinear(pa[0],pa[1],pa[2]) ||
113  iscolinear(pa[0],pa[1],pa[3]) ||
114  iscolinear(pa[0],pa[2],pa[3]) ||
115  iscolinear(pa[1],pa[2],pa[3]) ||
116  iscolinear(pb[0],pb[1],pb[2]) ||
117  iscolinear(pb[0],pb[1],pb[3]) ||
118  iscolinear(pb[0],pb[2],pb[3]) ||
119  iscolinear(pb[1],pb[2],pb[3])));
120 }
121  /*
122 \brief
123 Function to determine if a set of 4 pairs of matched points give rise
124 to a degeneracy in the calculation of a homography as needed by RANSAC.
125 This involves testing whether any 3 of the 4 points in each set is
126 colinear.
127 
128 point are coded this way
129 x1b,y1b, x2b, y2b, ... xnb, ynb
130 x1a,y1a, x2a, y2a, ... xna, yna
131 leading to 2*2*n
132 */
133 bool
134 vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
135 {
136  for (unsigned int i = 1; i < 4 ; i++)
137  for (unsigned int j = 0 ;j < i ; j++)
138  if (ind[i] == ind[j]) return true ;
139 
140  unsigned int n = x.getRows()/4;
141  double pa[4][3];
142  double pb[4][3];
143  unsigned int n2 = 2 * n;
144  unsigned int ind2;
145  for(unsigned int i = 0; i < 4 ;i++)
146  {
147  ind2 = 2 * ind[i];
148  pb[i][0] = x[ind2];
149  pb[i][1] = x[ind2+1];
150  pb[i][2] = 1;
151 
152  pa[i][0] = x[n2+ind2] ;
153  pa[i][1] = x[n2+ind2+1] ;
154  pa[i][2] = 1;
155  }
156  return ( iscolinear(pa[0],pa[1],pa[2]) ||
157  iscolinear(pa[0],pa[1],pa[3]) ||
158  iscolinear(pa[0],pa[2],pa[3]) ||
159  iscolinear(pa[1],pa[2],pa[3]) ||
160  iscolinear(pb[0],pb[1],pb[2]) ||
161  iscolinear(pb[0],pb[1],pb[3]) ||
162  iscolinear(pb[0],pb[2],pb[3]) ||
163  iscolinear(pb[1],pb[2],pb[3]));
164 }
165 bool
166 vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
167  const std::vector<double> &xa, const std::vector<double> &ya)
168 {
169  unsigned int n = (unsigned int)xb.size();
170  if (n < 4)
171  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
172 
173  std::vector<vpColVector> pa(n), pb(n);
174  for (unsigned i=0; i<n;i++) {
175  pa[i].resize(3);
176  pa[i][0] = xa[i];
177  pa[i][1] = ya[i];
178  pa[i][2] = 1;
179  pb[i].resize(3);
180  pb[i][0] = xb[i];
181  pb[i][1] = yb[i];
182  pb[i][2] = 1;
183  }
184 
185  for (unsigned int i = 0; i < n-2; i++) {
186  for (unsigned int j = i+1; j < n-1; j++) {
187  for (unsigned int k = j+1; k < n ; k++)
188  {
189  if (isColinear(pa[i], pa[j], pa[k])) {
190  return true;
191  }
192  if (isColinear(pb[i], pb[j], pb[k])){
193  return true;
194  }
195  }
196  }
197  }
198  return false;
199 }
200 // Fit model to this random selection of data points.
201 void
202 vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
203 {
204  unsigned int i ;
205  unsigned int n = x.getRows()/4 ;
206  std::vector<double> xa(4), xb(4);
207  std::vector<double> ya(4), yb(4);
208  unsigned int n2 = n * 2;
209  unsigned int ind2;
210  for(i=0 ; i < 4 ; i++)
211  {
212  ind2 = 2 * ind[i];
213  xb[i] = x[ind2] ;
214  yb[i] = x[ind2+1] ;
215 
216  xa[i] = x[n2+ind2] ;
217  ya[i] = x[n2+ind2+1] ;
218  }
219 
220  vpHomography aHb ;
221  try {
222  vpHomography::HLM(xb, yb, xa, ya, true, aHb);
223  }
224  catch(...)
225  {
226  aHb.setIdentity();
227  }
228 
229  M.resize(9);
230  for (i=0 ; i <9 ; i++)
231  {
232  M[i] = aHb.data[i] ;
233  }
234  aHb /= aHb[2][2] ;
235 }
236 
237 
238 // Evaluate distances between points and model.
239 double
240 vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
241 {
242  unsigned int i ;
243  unsigned int n = x.getRows()/4 ;
244  unsigned int n2 = n * 2;
245  unsigned int i2;
246  vpColVector *pa;
247  vpColVector *pb;
248 
249  pa = new vpColVector [n];
250  pb = new vpColVector [n];
251 
252  for( i=0 ; i < n ; i++)
253  {
254  i2 = 2 * i;
255  pb[i].resize(3) ;
256  pb[i][0] = x[i2] ;
257  pb[i][1] = x[i2+1] ;
258  pb[i][2] = 1;
259 
260  pa[i].resize(3) ;
261  pa[i][0] = x[n2+i2] ;
262  pa[i][1] = x[n2+i2+1] ;
263  pa[i][2] = 1;
264  }
265 
266  vpMatrix aHb(3,3) ;
267 
268  for (i=0 ; i <9 ; i++)
269  {
270  aHb.data[i] = M[i];
271  }
272 
273  aHb /= aHb[2][2];
274 
275  d.resize(n);
276 
277  vpColVector Hpb ;
278  for (i=0 ; i <n ; i++)
279  {
280  Hpb = aHb*pb[i] ;
281  Hpb /= Hpb[2] ;
282  d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
283  }
284 
285  delete [] pa;
286  delete [] pb;
287 
288  return 0 ;
289 }
290 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
291 
292 
293 void
294 vpHomography::initRansac(unsigned int n,
295  double *xb, double *yb,
296  double *xa, double *ya,
297  vpColVector &x)
298 {
299  x.resize(4*n) ;
300  unsigned int n2 = n * 2;
301  unsigned int i2;
302  for (unsigned int i=0 ; i < n ; i++)
303  {
304  i2 = 2 * i;
305  x[i2] = xb[i] ;
306  x[i2+1] = yb[i] ;
307  x[n2+i2] = xa[i] ;
308  x[n2+i2+1] = ya[i] ;
309  }
310 }
311 
312 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
313 
318 bool
319 vpHomography::ransac(unsigned int n,
320  double *xb, double *yb,
321  double *xa, double *ya ,
322  vpHomography &aHb,
323  int consensus,
324  double threshold
325  )
326 {
327  vpColVector x ;
328  vpHomography::initRansac(n, xb, yb, xa, ya, x) ;
329 
330  vpColVector M ;
331  vpColVector inliers(n) ;
332 
333 
334  bool ransacable = vpRansac<vpHomography>::ransac(n, x, 4, threshold, M, inliers, consensus);
335 
336  if(ransacable)
337  {
338  for (unsigned int i = 0 ;i < 9 ;i++)
339  {
340  aHb.data[i] = M[i];
341  }
342  aHb /= aHb[2][2];
343  }
344  return ransacable;
345 }
346 
379 bool vpHomography::ransac(unsigned int n,
380  double *xb, double *yb,
381  double *xa, double *ya ,
382  vpHomography &bHa,
383  vpColVector &inliers,
384  double /* residual */,
385  int consensus,
386  double threshold,
387  double areaThreshold)
388 {
389  vpColVector x ;
390  vpHomography::initRansac(n, xb, yb, xa, ya, x);
391 
392  vpColVector M ;
393 
394  bool ransacable= vpRansac<vpHomography>::ransac(n, x, 4,
395  threshold, M,
396  inliers, consensus,
397  areaThreshold);
398 
399  for (unsigned int i = 0 ;i < 9 ;i++)
400  {
401  bHa.data[i] = M[i];
402  }
403 
404  bHa /= bHa[2][2];
405  return ransacable;
406 }
407 #endif //#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
408 
435 bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<double> &yb,
436  const std::vector<double> &xa, const std::vector<double> &ya,
437  vpHomography &aHb,
438  std::vector<bool> &inliers,
439  double &residual,
440  unsigned int nbInliersConsensus,
441  double threshold,
442  bool normalization)
443 {
444  unsigned int n = (unsigned int)xb.size();
445  if (yb.size() != n || xa.size() != n || ya.size() != n)
447  "Bad dimension for robust homography estimation"));
448 
449  // 4 point are required
450  if(n<4)
451  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
452 
453  vpUniRand random((const long)time(NULL)) ;
454 
455  std::vector<unsigned int> best_consensus;
456  std::vector<unsigned int> cur_consensus;
457  std::vector<unsigned int> cur_outliers;
458  std::vector<unsigned int> cur_randoms;
459 
460  std::vector<unsigned int> rand_ind;
461 
462  unsigned int nbMinRandom = 4 ;
463  unsigned int ransacMaxTrials = 1000;
464  unsigned int maxDegenerateIter = 1000;
465 
466  unsigned int nbTrials = 0;
467  unsigned int nbDegenerateIter = 0;
468  unsigned int nbInliers = 0;
469 
470  bool foundSolution = false;
471 
472  std::vector<double> xa_rand(nbMinRandom);
473  std::vector<double> ya_rand(nbMinRandom);
474  std::vector<double> xb_rand(nbMinRandom);
475  std::vector<double> yb_rand(nbMinRandom);
476 
477  if (inliers.size() != n)
478  inliers.resize(n);
479 
480  while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus)
481  {
482  cur_outliers.clear();
483  cur_randoms.clear();
484 
485  bool degenerate = true;
486  while(degenerate == true){
487  std::vector<bool> usedPt(n, false);
488 
489  rand_ind.clear();
490  for(unsigned int i = 0; i < nbMinRandom; i++)
491  {
492  // Generate random indicies in the range 0..n
493  unsigned int r = (unsigned int)ceil(random()*n) -1;
494  while(usedPt[r]) {
495  r = (unsigned int)ceil(random()*n) -1;
496  }
497  usedPt[r] = true;
498  rand_ind.push_back(r);
499 
500  xa_rand[i] = xa[r];
501  ya_rand[i] = ya[r];
502  xb_rand[i] = xb[r];
503  yb_rand[i] = yb[r];
504  }
505 
506  try{
507  if (! vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
508  vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
509  degenerate = false;
510  }
511  }
512  catch(...){
513  degenerate = true;
514  }
515 
516  nbDegenerateIter ++;
517 
518  if (nbDegenerateIter > maxDegenerateIter){
519  vpERROR_TRACE("Unable to select a nondegenerate data set");
520  throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
521  }
522  }
523 
524  aHb /= aHb[2][2] ;
525 
526  // Computing Residual
527  double r = 0;
528  vpColVector a(3), b(3), c(3) ;
529  for (unsigned int i=0 ; i < nbMinRandom ; i++) {
530  a[0] = xa_rand[i] ; a[1] = ya_rand[i] ; a[2] = 1 ;
531  b[0] = xb_rand[i] ; b[1] = yb_rand[i] ; b[2] = 1 ;
532 
533  c = aHb*b; c /= c[2] ;
534  r += (a-c).sumSquare() ;
535  //cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
536  }
537 
538  // Finding inliers & ouliers
539  r = sqrt(r/nbMinRandom);
540  //std::cout << "Candidate residual: " << r << std::endl;
541  if (r < threshold)
542  {
543  unsigned int nbInliersCur = 0;
544  for (unsigned int i = 0; i < n ; i++)
545  {
546  a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
547  b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
548 
549  c = aHb*b ; c /= c[2] ;
550  double error = sqrt((a-c).sumSquare()) ;
551  if(error <= threshold){
552  nbInliersCur++;
553  cur_consensus.push_back(i);
554  inliers[i] = true;
555  }
556  else{
557  cur_outliers.push_back(i);
558  inliers[i] = false;
559  }
560  }
561  //std::cout << "nb inliers that matches: " << nbInliersCur << std::endl;
562  if(nbInliersCur > nbInliers)
563  {
564  foundSolution = true;
565  best_consensus = cur_consensus;
566  nbInliers = nbInliersCur;
567  }
568 
569  cur_consensus.clear();
570  }
571 
572  nbTrials++;
573  if(nbTrials >= ransacMaxTrials){
574  vpERROR_TRACE("Ransac reached the maximum number of trials");
575  foundSolution = true;
576  }
577  }
578 
579  if(foundSolution){
580  if(nbInliers >= nbInliersConsensus)
581  {
582  std::vector<double> xa_best(best_consensus.size());
583  std::vector<double> ya_best(best_consensus.size());
584  std::vector<double> xb_best(best_consensus.size());
585  std::vector<double> yb_best(best_consensus.size());
586 
587  for(unsigned i = 0 ; i < best_consensus.size(); i++)
588  {
589  xa_best[i] = xa[best_consensus[i]];
590  ya_best[i] = ya[best_consensus[i]];
591  xb_best[i] = xb[best_consensus[i]];
592  yb_best[i] = yb[best_consensus[i]];
593  }
594 
595  vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization) ;
596  aHb /= aHb[2][2];
597 
598  residual = 0 ;
599  vpColVector a(3), b(3), c(3);
600  for (unsigned int i=0 ; i < best_consensus.size() ; i++) {
601  a[0] = xa_best[i] ; a[1] = ya_best[i] ; a[2] = 1 ;
602  b[0] = xb_best[i] ; b[1] = yb_best[i] ; b[2] = 1 ;
603 
604  c = aHb*b ; c /= c[2] ;
605  residual += (a-c).sumSquare() ;
606  }
607 
608  residual = sqrt(residual/best_consensus.size());
609  return true;
610  }
611  else {
612  return false;
613  }
614  }
615  else {
616  return false;
617  }
618 }
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:158
#define vpERROR_TRACE
Definition: vpDebug.h:395
error that can be emited by ViSP classes.
Definition: vpException.h:76
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)
unsigned int size() const
Definition: vpColVector.h:204
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:178
static bool ransac(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, std::vector< bool > &inliers, double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization=true)
static void DLT(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, bool normalization=true)
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:124
Class for generating random numbers with uniform probability density.
Definition: vpNoise.h:68
double * data
Data array.
Definition: vpHomography.h:182
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
void setIdentity()
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98