Visual Servoing Platform version 3.5.0
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
56bool iscolinear(double *x1, double *x2, double *x3);
57bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);
58
59bool 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}
72bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
73{
74 return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps);
75}
76
77bool 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
130Function to determine if a set of 4 pairs of matched points give rise
131to a degeneracy in the calculation of a homography as needed by RANSAC.
132This involves testing whether any 3 of the 4 points in each set is
133colinear.
134
135point are coded this way
136x1b,y1b, x2b, y2b, ... xnb, ynb
137x1a,y1a, x2a, y2a, ... xna, yna
138leading to 2*2*n
139*/
140bool 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}
165bool 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.
199void 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.
229double 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
276void 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
318bool 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}
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
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:351
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
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:154
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101
#define vpERROR_TRACE
Definition: vpDebug.h:393