Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpHomographyRansac.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Homography estimation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpColVector.h>
40 #include <visp3/core/vpRansac.h>
41 #include <visp3/vision/vpHomography.h>
42 
43 #include <visp3/core/vpDisplay.h>
44 #include <visp3/core/vpImage.h>
45 #include <visp3/core/vpMeterPixelConversion.h>
46 
47 #define vpEps 1e-6
48 
54 #ifndef DOXYGEN_SHOULD_SKIP_THIS
55 
56 bool iscolinear(double *x1, double *x2, double *x3);
57 bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);
58 
59 bool iscolinear(double *x1, double *x2, double *x3)
60 {
61  vpColVector p1(3), p2(3), p3(3);
62  p1 << x1;
63  p2 << x2;
64  p3 << x3;
65  // vpColVector v;
66  // vpColVector::cross(p2-p1, p3-p1, v);
67  // return (v.sumSquare() < vpEps);
68  // Assume inhomogeneous coords, or homogeneous coords with equal
69  // scale.
70  return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps);
71 }
72 bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
73 {
74  return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps);
75 }
76 
77 bool vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind, double threshold_area)
78 {
79 
80  unsigned int i, j, k;
81 
82  for (i = 1; i < 4; i++)
83  for (j = 0; j < i; j++)
84  if (ind[i] == ind[j])
85  return true;
86 
87  unsigned int n = x.getRows() / 4;
88  double pa[4][3];
89  double pb[4][3];
90 
91  for (i = 0; i < 4; i++) {
92  pb[i][0] = x[2 * ind[i]];
93  pb[i][1] = x[2 * ind[i] + 1];
94  pb[i][2] = 1;
95 
96  pa[i][0] = x[2 * n + 2 * ind[i]];
97  pa[i][1] = x[2 * n + 2 * ind[i] + 1];
98  pa[i][2] = 1;
99  }
100 
101  i = 0, j = 1, k = 2;
102 
103  double area012 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
104  -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
105 
106  i = 0;
107  j = 1, k = 3;
108  double area013 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
109  -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
110 
111  i = 0;
112  j = 2, k = 3;
113  double area023 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
114  -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
115 
116  i = 1;
117  j = 2, k = 3;
118  double area123 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
119  -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
120 
121  double sum_area = area012 + area013 + area023 + area123;
122 
123  return ((sum_area < threshold_area) ||
124  (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) ||
125  iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) ||
126  iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3])));
127 }
128 /*
129 \brief
130 Function to determine if a set of 4 pairs of matched points give rise
131 to a degeneracy in the calculation of a homography as needed by RANSAC.
132 This involves testing whether any 3 of the 4 points in each set is
133 colinear.
134 
135 point are coded this way
136 x1b,y1b, x2b, y2b, ... xnb, ynb
137 x1a,y1a, x2a, y2a, ... xna, yna
138 leading to 2*2*n
139 */
140 bool vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
141 {
142  for (unsigned int i = 1; i < 4; i++)
143  for (unsigned int j = 0; j < i; j++)
144  if (ind[i] == ind[j])
145  return true;
146 
147  unsigned int n = x.getRows() / 4;
148  double pa[4][3];
149  double pb[4][3];
150  unsigned int n2 = 2 * n;
151  for (unsigned int i = 0; i < 4; i++) {
152  unsigned int ind2 = 2 * ind[i];
153  pb[i][0] = x[ind2];
154  pb[i][1] = x[ind2 + 1];
155  pb[i][2] = 1;
156 
157  pa[i][0] = x[n2 + ind2];
158  pa[i][1] = x[n2 + ind2 + 1];
159  pa[i][2] = 1;
160  }
161  return (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) ||
162  iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) ||
163  iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3]));
164 }
165 bool vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
166  const std::vector<double> &xa, const std::vector<double> &ya)
167 {
168  unsigned int n = (unsigned int)xb.size();
169  if (n < 4)
170  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
171 
172  std::vector<vpColVector> pa(n), pb(n);
173  for (unsigned i = 0; i < n; i++) {
174  pa[i].resize(3);
175  pa[i][0] = xa[i];
176  pa[i][1] = ya[i];
177  pa[i][2] = 1;
178  pb[i].resize(3);
179  pb[i][0] = xb[i];
180  pb[i][1] = yb[i];
181  pb[i][2] = 1;
182  }
183 
184  for (unsigned int i = 0; i < n - 2; i++) {
185  for (unsigned int j = i + 1; j < n - 1; j++) {
186  for (unsigned int k = j + 1; k < n; k++) {
187  if (isColinear(pa[i], pa[j], pa[k])) {
188  return true;
189  }
190  if (isColinear(pb[i], pb[j], pb[k])) {
191  return true;
192  }
193  }
194  }
195  }
196  return false;
197 }
198 // Fit model to this random selection of data points.
199 void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
200 {
201  unsigned int n = x.getRows() / 4;
202  std::vector<double> xa(4), xb(4);
203  std::vector<double> ya(4), yb(4);
204  unsigned int n2 = n * 2;
205  for (unsigned int i = 0; i < 4; i++) {
206  unsigned int ind2 = 2 * ind[i];
207  xb[i] = x[ind2];
208  yb[i] = x[ind2 + 1];
209 
210  xa[i] = x[n2 + ind2];
211  ya[i] = x[n2 + ind2 + 1];
212  }
213 
214  vpHomography aHb;
215  try {
216  vpHomography::HLM(xb, yb, xa, ya, true, aHb);
217  } catch (...) {
218  aHb.eye();
219  }
220 
221  M.resize(9);
222  for (unsigned int i = 0; i < 9; i++) {
223  M[i] = aHb.data[i];
224  }
225  aHb /= aHb[2][2];
226 }
227 
228 // Evaluate distances between points and model.
229 double vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
230 {
231  unsigned int n = x.getRows() / 4;
232  unsigned int n2 = n * 2;
233  vpColVector *pa;
234  vpColVector *pb;
235 
236  pa = new vpColVector[n];
237  pb = new vpColVector[n];
238 
239  for (unsigned int i = 0; i < n; i++) {
240  unsigned int i2 = 2 * i;
241  pb[i].resize(3);
242  pb[i][0] = x[i2];
243  pb[i][1] = x[i2 + 1];
244  pb[i][2] = 1;
245 
246  pa[i].resize(3);
247  pa[i][0] = x[n2 + i2];
248  pa[i][1] = x[n2 + i2 + 1];
249  pa[i][2] = 1;
250  }
251 
252  vpMatrix aHb(3, 3);
253 
254  for (unsigned int i = 0; i < 9; i++) {
255  aHb.data[i] = M[i];
256  }
257 
258  aHb /= aHb[2][2];
259 
260  d.resize(n);
261 
262  vpColVector Hpb;
263  for (unsigned int i = 0; i < n; i++) {
264  Hpb = aHb * pb[i];
265  Hpb /= Hpb[2];
266  d[i] = sqrt((pa[i] - Hpb).sumSquare());
267  }
268 
269  delete[] pa;
270  delete[] pb;
271 
272  return 0;
273 }
274 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
275 
276 void vpHomography::initRansac(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpColVector &x)
277 {
278  x.resize(4 * n);
279  unsigned int n2 = n * 2;
280  for (unsigned int i = 0; i < n; i++) {
281  unsigned int i2 = 2 * i;
282  x[i2] = xb[i];
283  x[i2 + 1] = yb[i];
284  x[n2 + i2] = xa[i];
285  x[n2 + i2 + 1] = ya[i];
286  }
287 }
288 
318 bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
319  const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers,
320  double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization)
321 {
322  unsigned int n = (unsigned int)xb.size();
323  if (yb.size() != n || xa.size() != n || ya.size() != n)
324  throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation"));
325 
326  // 4 point are required
327  if (n < 4)
328  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
329 
330  vpUniRand random((long)time(NULL));
331 
332  std::vector<unsigned int> best_consensus;
333  std::vector<unsigned int> cur_consensus;
334  std::vector<unsigned int> cur_outliers;
335  std::vector<unsigned int> cur_randoms;
336 
337  std::vector<unsigned int> rand_ind;
338 
339  unsigned int nbMinRandom = 4;
340  unsigned int ransacMaxTrials = 1000;
341  unsigned int maxDegenerateIter = 1000;
342 
343  unsigned int nbTrials = 0;
344  unsigned int nbDegenerateIter = 0;
345  unsigned int nbInliers = 0;
346 
347  bool foundSolution = false;
348 
349  std::vector<double> xa_rand(nbMinRandom);
350  std::vector<double> ya_rand(nbMinRandom);
351  std::vector<double> xb_rand(nbMinRandom);
352  std::vector<double> yb_rand(nbMinRandom);
353 
354  if (inliers.size() != n)
355  inliers.resize(n);
356 
357  while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus) {
358  cur_outliers.clear();
359  cur_randoms.clear();
360 
361  bool degenerate = true;
362  while (degenerate == true) {
363  std::vector<bool> usedPt(n, false);
364 
365  rand_ind.clear();
366  for (unsigned int i = 0; i < nbMinRandom; i++) {
367  // Generate random indicies in the range 0..n
368  unsigned int r = (unsigned int)ceil(random() * n) - 1;
369  while (usedPt[r]) {
370  r = (unsigned int)ceil(random() * n) - 1;
371  }
372  usedPt[r] = true;
373  rand_ind.push_back(r);
374 
375  xa_rand[i] = xa[r];
376  ya_rand[i] = ya[r];
377  xb_rand[i] = xb[r];
378  yb_rand[i] = yb[r];
379  }
380 
381  try {
382  if (!vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
383  vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
384  degenerate = false;
385  }
386  } catch (...) {
387  degenerate = true;
388  }
389 
390  nbDegenerateIter++;
391 
392  if (nbDegenerateIter > maxDegenerateIter) {
393  vpERROR_TRACE("Unable to select a nondegenerate data set");
394  throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
395  }
396  }
397 
398  aHb /= aHb[2][2];
399 
400  // Computing Residual
401  double r = 0;
402  vpColVector a(3), b(3), c(3);
403  for (unsigned int i = 0; i < nbMinRandom; i++) {
404  a[0] = xa_rand[i];
405  a[1] = ya_rand[i];
406  a[2] = 1;
407  b[0] = xb_rand[i];
408  b[1] = yb_rand[i];
409  b[2] = 1;
410 
411  c = aHb * b;
412  c /= c[2];
413  r += (a - c).sumSquare();
414  // cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
415  }
416 
417  // Finding inliers & ouliers
418  r = sqrt(r / nbMinRandom);
419  // std::cout << "Candidate residual: " << r << std::endl;
420  if (r < threshold) {
421  unsigned int nbInliersCur = 0;
422  for (unsigned int i = 0; i < n; i++) {
423  a[0] = xa[i];
424  a[1] = ya[i];
425  a[2] = 1;
426  b[0] = xb[i];
427  b[1] = yb[i];
428  b[2] = 1;
429 
430  c = aHb * b;
431  c /= c[2];
432  double error = sqrt((a - c).sumSquare());
433  if (error <= threshold) {
434  nbInliersCur++;
435  cur_consensus.push_back(i);
436  inliers[i] = true;
437  } else {
438  cur_outliers.push_back(i);
439  inliers[i] = false;
440  }
441  }
442  // std::cout << "nb inliers that matches: " << nbInliersCur <<
443  // std::endl;
444  if (nbInliersCur > nbInliers) {
445  foundSolution = true;
446  best_consensus = cur_consensus;
447  nbInliers = nbInliersCur;
448  }
449 
450  cur_consensus.clear();
451  }
452 
453  nbTrials++;
454  if (nbTrials >= ransacMaxTrials) {
455  vpERROR_TRACE("Ransac reached the maximum number of trials");
456  foundSolution = true;
457  }
458  }
459 
460  if (foundSolution) {
461  if (nbInliers >= nbInliersConsensus) {
462  std::vector<double> xa_best(best_consensus.size());
463  std::vector<double> ya_best(best_consensus.size());
464  std::vector<double> xb_best(best_consensus.size());
465  std::vector<double> yb_best(best_consensus.size());
466 
467  for (unsigned i = 0; i < best_consensus.size(); i++) {
468  xa_best[i] = xa[best_consensus[i]];
469  ya_best[i] = ya[best_consensus[i]];
470  xb_best[i] = xb[best_consensus[i]];
471  yb_best[i] = yb[best_consensus[i]];
472  }
473 
474  vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization);
475  aHb /= aHb[2][2];
476 
477  residual = 0;
478  vpColVector a(3), b(3), c(3);
479  for (unsigned int i = 0; i < best_consensus.size(); i++) {
480  a[0] = xa_best[i];
481  a[1] = ya_best[i];
482  a[2] = 1;
483  b[0] = xb_best[i];
484  b[1] = yb_best[i];
485  b[2] = 1;
486 
487  c = aHb * b;
488  c /= c[2];
489  residual += (a - c).sumSquare();
490  }
491 
492  residual = sqrt(residual / best_consensus.size());
493  return true;
494  } else {
495  return false;
496  }
497  } else {
498  return false;
499  }
500 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:351
#define vpERROR_TRACE
Definition: vpDebug.h:393
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
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)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
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)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:100