ViSP  2.8.0
vpHomographyRansac.cpp
1
2 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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 3496 2011-11-22 15:14:32Z 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 #define vpEps 1e-6
24
31 bool
32 iscolinear(double *x1, double *x2, double *x3)
33 {
34  vpColVector p1(3), p2(3), p3(3);
35  p1 << x1 ;
36  p2 << x2 ;
37  p3 << x3 ;
38  //vpColVector v;
39  //vpColVector::cross(p2-p1, p3-p1, v);
40  //return (v.sumSquare() < vpEps);
41  // Assume inhomogeneous coords, or homogeneous coords with equal
42  // scale.
43  return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
44 }
45
46
47 bool
49  double threshold_area)
50 {
51
52  unsigned int i, j, k;
53
54  for (i=1 ; i < 4 ; i++)
55  for (j=0 ; j<i ; j++)
56  if (ind[i]==ind[j]) return true ;
57
58  unsigned int n = x.getRows()/4 ;
59  double pa[4][3] ;
60  double pb[4][3] ;
61
62
63
64  for(i = 0 ; i < 4 ; i++)
65  {
66  pb[i][0] = x[2*ind[i]] ;
67  pb[i][1] = x[2*ind[i]+1] ;
68  pb[i][2] = 1;
69
70  pa[i][0] = x[2*n+2*ind[i]] ;
71  pa[i][1] = x[2*n+2*ind[i]+1] ;
72  pa[i][2] = 1;
73  }
74
75  i = 0, j = 1, k = 2;
76
77  double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
78  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
79  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
80
81  i = 0; j = 1, k = 3;
82  double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
83  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
84  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
85
86  i = 0; j = 2, k = 3;
87  double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
88  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
89  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
90
91  i = 1; j = 2, k = 3;
92  double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
93  pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
94  -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
95
96  double sum_area = area012 + area013 + area023 + area123;
97
98  return ((sum_area < threshold_area) ||
99  (iscolinear(pa[0],pa[1],pa[2]) ||
100  iscolinear(pa[0],pa[1],pa[3]) ||
101  iscolinear(pa[0],pa[2],pa[3]) ||
102  iscolinear(pa[1],pa[2],pa[3]) ||
103  iscolinear(pb[0],pb[1],pb[2]) ||
104  iscolinear(pb[0],pb[1],pb[3]) ||
105  iscolinear(pb[0],pb[2],pb[3]) ||
106  iscolinear(pb[1],pb[2],pb[3])));
107 }
108  /*
109 \brief
110 Function to determine if a set of 4 pairs of matched points give rise
111 to a degeneracy in the calculation of a homography as needed by RANSAC.
112 This involves testing whether any 3 of the 4 points in each set is
113 colinear.
114
115 point are coded this way
116 x1b,y1b, x2b, y2b, ... xnb, ynb
117 x1a,y1a, x2a, y2a, ... xna, yna
119 */
120 bool
122 {
123  for (unsigned int i = 1; i < 4 ; i++)
124  for (unsigned int j = 0 ;j < i ; j++)
125  if (ind[i] == ind[j]) return true ;
126
127  unsigned int n = x.getRows()/4;
128  double pa[4][3];
129  double pb[4][3];
130  unsigned int n2 = 2 * n;
131  unsigned int ind2;
132  for(unsigned int i = 0; i < 4 ;i++)
133  {
134  ind2 = 2 * ind[i];
135  pb[i][0] = x[ind2];
136  pb[i][1] = x[ind2+1];
137  pb[i][2] = 1;
138
139  pa[i][0] = x[n2+ind2] ;
140  pa[i][1] = x[n2+ind2+1] ;
141  pa[i][2] = 1;
142  }
143  return ( iscolinear(pa[0],pa[1],pa[2]) ||
144  iscolinear(pa[0],pa[1],pa[3]) ||
145  iscolinear(pa[0],pa[2],pa[3]) ||
146  iscolinear(pa[1],pa[2],pa[3]) ||
147  iscolinear(pb[0],pb[1],pb[2]) ||
148  iscolinear(pb[0],pb[1],pb[3]) ||
149  iscolinear(pb[0],pb[2],pb[3]) ||
150  iscolinear(pb[1],pb[2],pb[3]));
151 }
152 // Fit model to this random selection of data points.
153 void
155 {
156  unsigned int i ;
157  unsigned int n = x.getRows()/4 ;
158  double xa[4], xb[4];
159  double ya[4], yb[4];
160  unsigned int n2 = n * 2;
161  unsigned int ind2;
162  for(i=0 ; i < 4 ; i++)
163  {
164  ind2 = 2 * ind[i];
165  xb[i] = x[ind2] ;
166  yb[i] = x[ind2+1] ;
167
168  xa[i] = x[n2+ind2] ;
169  ya[i] = x[n2+ind2+1] ;
170  }
171
172  vpHomography aHb ;
173  try {
174  vpHomography::HLM(4,xb, yb, xa, ya, true, aHb);
175  //vpHomography::HLM(8, xb, yb, xa, ya, false, aHb); //modified 13/09
176  }
177  catch(...)
178  {
179  aHb.setIdentity();
180  }
181
182  M.resize(9);
183  for (i=0 ; i <9 ; i++)
184  {
185  M[i] = aHb.data[i] ;
186  }
187  aHb /= aHb[2][2] ;
188 }
189
190
191 // Evaluate distances between points and model.
192 double
194 {
195  unsigned int i ;
196  unsigned int n = x.getRows()/4 ;
197  unsigned int n2 = n * 2;
198  unsigned int i2;
199  vpColVector *pa;
200  vpColVector *pb;
201
202  pa = new vpColVector [n];
203  pb = new vpColVector [n];
204
205  for( i=0 ; i < n ; i++)
206  {
207  i2 = 2 * i;
208  pb[i].resize(3) ;
209  pb[i][0] = x[i2] ;
210  pb[i][1] = x[i2+1] ;
211  pb[i][2] = 1;
212
213  pa[i].resize(3) ;
214  pa[i][0] = x[n2+i2] ;
215  pa[i][1] = x[n2+i2+1] ;
216  pa[i][2] = 1;
217  }
218
219  vpMatrix aHb(3,3) ;
220
221  for (i=0 ; i <9 ; i++)
222  {
223  aHb.data[i] = M[i];
224  }
225
226  aHb /= aHb[2][2];
227
228  d.resize(n);
229
230  vpColVector Hpb ;
231  for (i=0 ; i <n ; i++)
232  {
233  Hpb = aHb*pb[i] ;
234  Hpb /= Hpb[2] ;
235  d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
236  }
237
238  delete [] pa;
239  delete [] pb;
240
241  return 0 ;
242 }
243
244
245 void
246 vpHomography::initRansac(unsigned int n,
247  double *xb, double *yb,
248  double *xa, double *ya,
249  vpColVector &x)
250 {
251  x.resize(4*n) ;
252  unsigned int n2 = n * 2;
253  unsigned int i2;
254  for (unsigned int i=0 ; i < n ; i++)
255  {
256  i2 = 2 * i;
257  x[i2] = xb[i] ;
258  x[i2+1] = yb[i] ;
259  x[n2+i2] = xa[i] ;
260  x[n2+i2+1] = ya[i] ;
261  }
262 }
263
264 bool
265 vpHomography::ransac(unsigned int n,
266  double *xb, double *yb,
267  double *xa, double *ya ,
268  vpHomography &aHb,
269  int consensus,
270  double threshold
271  )
272 {
273  vpColVector x ;
274  vpHomography::initRansac(n, xb, yb, xa, ya, x) ;
275
276  vpColVector M ;
277  vpColVector inliers(n) ;
278
279
280  bool ransacable = vpRansac<vpHomography>::ransac(n, x, 4, threshold, M, inliers, consensus);
281
282  if(ransacable)
283  {
284  for (unsigned int i = 0 ;i < 9 ;i++)
285  {
286  aHb.data[i] = M[i];
287  }
288  aHb /= aHb[2][2];
289  }
290  return ransacable;
291 }
292
323 bool vpHomography::ransac(unsigned int n,
324  double *xb, double *yb,
325  double *xa, double *ya ,
326  vpHomography &bHa,
327  vpColVector &inliers,
328  double& /* residual */,
329  int consensus,
330  double threshold,
331  double areaThreshold)
332 {
333  vpColVector x ;
334  vpHomography::initRansac(n, xb, yb, xa, ya, x);
335
336  vpColVector M ;
337
338  bool ransacable= vpRansac<vpHomography>::ransac(n, x, 4,
339  threshold, M,
340  inliers, consensus,
341  areaThreshold);
342
343  for (unsigned int i = 0 ;i < 9 ;i++)
344  {
345  bHa.data[i] = M[i];
346  }
347
348  bHa /= bHa[2][2];
349  return ransacable;
350 }
351
352 /*
353  * Local variables:
354  * c-basic-offset: 2
355  * End:
356  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:153
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:760
static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
double * data
address of the first element of the data array
Definition: vpMatrix.h:116
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:173
void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:1110
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)...
static double computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
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
static bool ransac(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpHomography &aHb, int consensus=1000, double threshold=1e-6)
static bool degenerateConfiguration(vpColVector &x, unsigned int *ind)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94