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