Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
51bool iscolinear(double *x1, double *x2, double *x3);
52bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);
53
54bool 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 // vpColVector v;
61 // vpColVector::cross(p2-p1, p3-p1, v);
62 // return (v.sumSquare() < vpEps);
63 // Assume inhomogeneous coords, or homogeneous coords with equal
64 // scale.
65 return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps);
66}
67bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
68{
69 return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps);
70}
71
72bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *ind, double threshold_area)
73{
74
75 unsigned int i, j, k;
76
77 for (i = 1; i < 4; i++)
78 for (j = 0; j < i; j++)
79 if (ind[i] == ind[j])
80 return true;
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 return ((sum_area < threshold_area) ||
119 (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) ||
120 iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) ||
121 iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3])));
122}
123/*
124\brief
125Function to determine if a set of 4 pairs of matched points give rise
126to a degeneracy in the calculation of a homography as needed by RANSAC.
127This involves testing whether any 3 of the 4 points in each set is
128colinear.
129
130point are coded this way
131x1b,y1b, x2b, y2b, ... xnb, ynb
132x1a,y1a, x2a, y2a, ... xna, yna
133leading to 2*2*n
134*/
135bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *ind)
136{
137 for (unsigned int i = 1; i < 4; i++)
138 for (unsigned int j = 0; j < i; j++)
139 if (ind[i] == ind[j])
140 return true;
141
142 unsigned int n = x.getRows() / 4;
143 double pa[4][3];
144 double pb[4][3];
145 unsigned int n2 = 2 * n;
146 for (unsigned int i = 0; i < 4; i++) {
147 unsigned int 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]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) ||
157 iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) ||
158 iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3]));
159}
160bool vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
161 const std::vector<double> &xa, const std::vector<double> &ya)
162{
163 unsigned int n = (unsigned int)xb.size();
164 if (n < 4)
165 throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
166
167 std::vector<vpColVector> pa(n), pb(n);
168 for (unsigned i = 0; i < n; i++) {
169 pa[i].resize(3);
170 pa[i][0] = xa[i];
171 pa[i][1] = ya[i];
172 pa[i][2] = 1;
173 pb[i].resize(3);
174 pb[i][0] = xb[i];
175 pb[i][1] = yb[i];
176 pb[i][2] = 1;
177 }
178
179 for (unsigned int i = 0; i < n - 2; i++) {
180 for (unsigned int j = i + 1; j < n - 1; j++) {
181 for (unsigned int k = j + 1; k < n; k++) {
182 if (isColinear(pa[i], pa[j], pa[k])) {
183 return true;
184 }
185 if (isColinear(pb[i], pb[j], pb[k])) {
186 return true;
187 }
188 }
189 }
190 }
191 return false;
192}
193// Fit model to this random selection of data points.
194void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
195{
196 unsigned int n = x.getRows() / 4;
197 std::vector<double> xa(4), xb(4);
198 std::vector<double> ya(4), yb(4);
199 unsigned int n2 = n * 2;
200 for (unsigned int i = 0; i < 4; i++) {
201 unsigned int ind2 = 2 * ind[i];
202 xb[i] = x[ind2];
203 yb[i] = x[ind2 + 1];
204
205 xa[i] = x[n2 + ind2];
206 ya[i] = x[n2 + ind2 + 1];
207 }
208
209 vpHomography aHb;
210 try {
211 vpHomography::HLM(xb, yb, xa, ya, true, aHb);
212 } catch (...) {
213 aHb.eye();
214 }
215
216 M.resize(9);
217 for (unsigned int i = 0; i < 9; i++) {
218 M[i] = aHb.data[i];
219 }
220 aHb /= aHb[2][2];
221}
222
223// Evaluate distances between points and model.
224double vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
225{
226 unsigned int n = x.getRows() / 4;
227 unsigned int n2 = n * 2;
228 vpColVector *pa;
229 vpColVector *pb;
230
231 pa = new vpColVector[n];
232 pb = new vpColVector[n];
233
234 for (unsigned int i = 0; i < n; i++) {
235 unsigned int i2 = 2 * i;
236 pb[i].resize(3);
237 pb[i][0] = x[i2];
238 pb[i][1] = x[i2 + 1];
239 pb[i][2] = 1;
240
241 pa[i].resize(3);
242 pa[i][0] = x[n2 + i2];
243 pa[i][1] = x[n2 + i2 + 1];
244 pa[i][2] = 1;
245 }
246
247 vpMatrix aHb(3, 3);
248
249 for (unsigned int i = 0; i < 9; i++) {
250 aHb.data[i] = M[i];
251 }
252
253 aHb /= aHb[2][2];
254
255 d.resize(n);
256
257 vpColVector Hpb;
258 for (unsigned int i = 0; i < n; i++) {
259 Hpb = aHb * pb[i];
260 Hpb /= Hpb[2];
261 d[i] = sqrt((pa[i] - Hpb).sumSquare());
262 }
263
264 delete[] pa;
265 delete[] pb;
266
267 return 0;
268}
269#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
270
271void vpHomography::initRansac(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpColVector &x)
272{
273 x.resize(4 * n);
274 unsigned int n2 = n * 2;
275 for (unsigned int i = 0; i < n; i++) {
276 unsigned int i2 = 2 * i;
277 x[i2] = xb[i];
278 x[i2 + 1] = yb[i];
279 x[n2 + i2] = xa[i];
280 x[n2 + i2 + 1] = ya[i];
281 }
282}
283
284bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
285 const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers,
286 double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization)
287{
288 unsigned int n = (unsigned int)xb.size();
289 if (yb.size() != n || xa.size() != n || ya.size() != n)
290 throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation"));
291
292 // 4 point are required
293 if (n < 4)
294 throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
295
296 vpUniRand random((long)time(NULL));
297
298 std::vector<unsigned int> best_consensus;
299 std::vector<unsigned int> cur_consensus;
300 std::vector<unsigned int> cur_outliers;
301 std::vector<unsigned int> cur_randoms;
302
303 std::vector<unsigned int> rand_ind;
304
305 unsigned int nbMinRandom = 4;
306 unsigned int ransacMaxTrials = 1000;
307 unsigned int maxDegenerateIter = 1000;
308
309 unsigned int nbTrials = 0;
310 unsigned int nbDegenerateIter = 0;
311 unsigned int nbInliers = 0;
312
313 bool foundSolution = false;
314
315 std::vector<double> xa_rand(nbMinRandom);
316 std::vector<double> ya_rand(nbMinRandom);
317 std::vector<double> xb_rand(nbMinRandom);
318 std::vector<double> yb_rand(nbMinRandom);
319
320 if (inliers.size() != n)
321 inliers.resize(n);
322
323 while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus) {
324 cur_outliers.clear();
325 cur_randoms.clear();
326
327 bool degenerate = true;
328 while (degenerate == true) {
329 std::vector<bool> usedPt(n, false);
330
331 rand_ind.clear();
332 for (unsigned int i = 0; i < nbMinRandom; i++) {
333 // Generate random indicies in the range 0..n
334 unsigned int r = (unsigned int)ceil(random() * n) - 1;
335 while (usedPt[r]) {
336 r = (unsigned int)ceil(random() * n) - 1;
337 }
338 usedPt[r] = true;
339 rand_ind.push_back(r);
340
341 xa_rand[i] = xa[r];
342 ya_rand[i] = ya[r];
343 xb_rand[i] = xb[r];
344 yb_rand[i] = yb[r];
345 }
346
347 try {
348 if (!vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
349 vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
350 degenerate = false;
351 }
352 } catch (...) {
353 degenerate = true;
354 }
355
356 nbDegenerateIter++;
357
358 if (nbDegenerateIter > maxDegenerateIter) {
359 vpERROR_TRACE("Unable to select a nondegenerate data set");
360 throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
361 }
362 }
363
364 aHb /= aHb[2][2];
365
366 // Computing Residual
367 double r = 0;
368 vpColVector a(3), b(3), c(3);
369 for (unsigned int i = 0; i < nbMinRandom; i++) {
370 a[0] = xa_rand[i];
371 a[1] = ya_rand[i];
372 a[2] = 1;
373 b[0] = xb_rand[i];
374 b[1] = yb_rand[i];
375 b[2] = 1;
376
377 c = aHb * b;
378 c /= c[2];
379 r += (a - c).sumSquare();
380 // cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
381 }
382
383 // Finding inliers & ouliers
384 r = sqrt(r / nbMinRandom);
385 // std::cout << "Candidate residual: " << r << std::endl;
386 if (r < threshold) {
387 unsigned int nbInliersCur = 0;
388 for (unsigned int i = 0; i < n; i++) {
389 a[0] = xa[i];
390 a[1] = ya[i];
391 a[2] = 1;
392 b[0] = xb[i];
393 b[1] = yb[i];
394 b[2] = 1;
395
396 c = aHb * b;
397 c /= c[2];
398 double error = sqrt((a - c).sumSquare());
399 if (error <= threshold) {
400 nbInliersCur++;
401 cur_consensus.push_back(i);
402 inliers[i] = true;
403 } else {
404 cur_outliers.push_back(i);
405 inliers[i] = false;
406 }
407 }
408 // std::cout << "nb inliers that matches: " << nbInliersCur <<
409 // std::endl;
410 if (nbInliersCur > nbInliers) {
411 foundSolution = true;
412 best_consensus = cur_consensus;
413 nbInliers = nbInliersCur;
414 }
415
416 cur_consensus.clear();
417 }
418
419 nbTrials++;
420 if (nbTrials >= ransacMaxTrials) {
421 vpERROR_TRACE("Ransac reached the maximum number of trials");
422 foundSolution = true;
423 }
424 }
425
426 if (foundSolution) {
427 if (nbInliers >= nbInliersConsensus) {
428 std::vector<double> xa_best(best_consensus.size());
429 std::vector<double> ya_best(best_consensus.size());
430 std::vector<double> xb_best(best_consensus.size());
431 std::vector<double> yb_best(best_consensus.size());
432
433 for (unsigned i = 0; i < best_consensus.size(); i++) {
434 xa_best[i] = xa[best_consensus[i]];
435 ya_best[i] = ya[best_consensus[i]];
436 xb_best[i] = xb[best_consensus[i]];
437 yb_best[i] = yb[best_consensus[i]];
438 }
439
440 vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization);
441 aHb /= aHb[2][2];
442
443 residual = 0;
444 vpColVector a(3), b(3), c(3);
445 for (unsigned int i = 0; i < best_consensus.size(); i++) {
446 a[0] = xa_best[i];
447 a[1] = ya_best[i];
448 a[2] = 1;
449 b[0] = xb_best[i];
450 b[1] = yb_best[i];
451 b[2] = 1;
452
453 c = aHb * b;
454 c /= c[2];
455 residual += (a - c).sumSquare();
456 }
457
458 residual = sqrt(residual / best_consensus.size());
459 return true;
460 } else {
461 return false;
462 }
463 } else {
464 return false;
465 }
466}
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
void resize(unsigned int i, bool flagNullify=true)
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.
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:152
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:122
#define vpERROR_TRACE
Definition vpDebug.h:388