34#include <visp3/core/vpColVector.h>
35#include <visp3/core/vpRansac.h>
36#include <visp3/vision/vpHomography.h>
38#include <visp3/core/vpDisplay.h>
39#include <visp3/core/vpImage.h>
40#include <visp3/core/vpMeterPixelConversion.h>
49#ifndef DOXYGEN_SHOULD_SKIP_THIS
51bool iscolinear(
double *x1,
double *x2,
double *x3);
54bool iscolinear(
double *x1,
double *x2,
double *x3)
72bool vpHomography::degenerateConfiguration(
const vpColVector &x,
unsigned int *ind,
double threshold_area)
77 for (i = 1; i < 4; i++)
78 for (j = 0; j < i; j++)
82 unsigned int n = x.
getRows() / 4;
86 for (i = 0; i < 4; i++) {
87 pb[i][0] = x[2 * ind[i]];
88 pb[i][1] = x[2 * ind[i] + 1];
91 pa[i][0] = x[2 * n + 2 * ind[i]];
92 pa[i][1] = x[2 * n + 2 * ind[i] + 1];
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]);
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]);
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]);
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]);
116 double sum_area = area012 + area013 + area023 + area123;
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])));
135bool vpHomography::degenerateConfiguration(
const vpColVector &x,
unsigned int *ind)
137 for (
unsigned int i = 1; i < 4; i++)
138 for (
unsigned int j = 0; j < i; j++)
139 if (ind[i] == ind[j])
142 unsigned int n = x.
getRows() / 4;
145 unsigned int n2 = 2 * n;
146 for (
unsigned int i = 0; i < 4; i++) {
147 unsigned int ind2 = 2 * ind[i];
149 pb[i][1] = x[ind2 + 1];
152 pa[i][0] = x[n2 + ind2];
153 pa[i][1] = x[n2 + ind2 + 1];
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]));
160bool vpHomography::degenerateConfiguration(
const std::vector<double> &xb,
const std::vector<double> &yb,
161 const std::vector<double> &xa,
const std::vector<double> &ya)
163 unsigned int n = (
unsigned int)xb.size();
167 std::vector<vpColVector> pa(n), pb(n);
168 for (
unsigned i = 0; i < n; i++) {
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])) {
185 if (isColinear(pb[i], pb[j], pb[k])) {
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];
205 xa[i] = x[n2 + ind2];
206 ya[i] = x[n2 + ind2 + 1];
217 for (
unsigned int i = 0; i < 9; i++) {
226 unsigned int n = x.
getRows() / 4;
227 unsigned int n2 = n * 2;
234 for (
unsigned int i = 0; i < n; i++) {
235 unsigned int i2 = 2 * i;
238 pb[i][1] = x[i2 + 1];
242 pa[i][0] = x[n2 + i2];
243 pa[i][1] = x[n2 + i2 + 1];
249 for (
unsigned int i = 0; i < 9; i++) {
258 for (
unsigned int i = 0; i < n; i++) {
261 d[i] = sqrt((pa[i] - Hpb).sumSquare());
271void vpHomography::initRansac(
unsigned int n,
double *xb,
double *yb,
double *xa,
double *ya,
vpColVector &x)
274 unsigned int n2 = n * 2;
275 for (
unsigned int i = 0; i < n; i++) {
276 unsigned int i2 = 2 * i;
280 x[n2 + i2 + 1] = ya[i];
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)
288 unsigned int n = (
unsigned int)xb.size();
289 if (yb.size() != n || xa.size() != n || ya.size() != n)
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;
303 std::vector<unsigned int> rand_ind;
305 unsigned int nbMinRandom = 4;
306 unsigned int ransacMaxTrials = 1000;
307 unsigned int maxDegenerateIter = 1000;
309 unsigned int nbTrials = 0;
310 unsigned int nbDegenerateIter = 0;
311 unsigned int nbInliers = 0;
313 bool foundSolution =
false;
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);
320 if (inliers.size() != n)
323 while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus) {
324 cur_outliers.clear();
327 bool degenerate =
true;
328 while (degenerate ==
true) {
329 std::vector<bool> usedPt(n,
false);
332 for (
unsigned int i = 0; i < nbMinRandom; i++) {
334 unsigned int r = (
unsigned int)ceil(random() * n) - 1;
336 r = (
unsigned int)ceil(random() * n) - 1;
339 rand_ind.push_back(r);
348 if (!vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
358 if (nbDegenerateIter > maxDegenerateIter) {
369 for (
unsigned int i = 0; i < nbMinRandom; i++) {
379 r += (a - c).sumSquare();
384 r = sqrt(r / nbMinRandom);
387 unsigned int nbInliersCur = 0;
388 for (
unsigned int i = 0; i < n; i++) {
398 double error = sqrt((a - c).sumSquare());
399 if (error <= threshold) {
401 cur_consensus.push_back(i);
404 cur_outliers.push_back(i);
410 if (nbInliersCur > nbInliers) {
411 foundSolution =
true;
412 best_consensus = cur_consensus;
413 nbInliers = nbInliersCur;
416 cur_consensus.clear();
420 if (nbTrials >= ransacMaxTrials) {
421 vpERROR_TRACE(
"Ransac reached the maximum number of trials");
422 foundSolution =
true;
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());
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]];
445 for (
unsigned int i = 0; i < best_consensus.size(); i++) {
455 residual += (a - c).sumSquare();
458 residual = sqrt(residual / best_consensus.size());
Type * data
Address of the first element of the data array.
unsigned int getRows() const
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.
@ dimensionError
Bad dimension.
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.
Class for generating random numbers with uniform probability density.