39 #include <visp3/core/vpColVector.h>
40 #include <visp3/core/vpRansac.h>
41 #include <visp3/vision/vpHomography.h>
43 #include <visp3/core/vpDisplay.h>
44 #include <visp3/core/vpImage.h>
45 #include <visp3/core/vpMeterPixelConversion.h>
54 #ifndef DOXYGEN_SHOULD_SKIP_THIS
56 bool iscolinear(
double *x1,
double *x2,
double *x3);
59 bool iscolinear(
double *x1,
double *x2,
double *x3)
77 bool vpHomography::degenerateConfiguration(
vpColVector &x,
unsigned int *ind,
double threshold_area)
82 for (i = 1; i < 4; i++)
83 for (j = 0; j < i; j++)
87 unsigned int n = x.
getRows() / 4;
91 for (i = 0; i < 4; i++) {
92 pb[i][0] = x[2 * ind[i]];
93 pb[i][1] = x[2 * ind[i] + 1];
96 pa[i][0] = x[2 * n + 2 * ind[i]];
97 pa[i][1] = x[2 * n + 2 * ind[i] + 1];
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]);
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]);
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]);
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]);
121 double sum_area = area012 + area013 + area023 + area123;
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])));
140 bool vpHomography::degenerateConfiguration(
vpColVector &x,
unsigned int *ind)
142 for (
unsigned int i = 1; i < 4; i++)
143 for (
unsigned int j = 0; j < i; j++)
144 if (ind[i] == ind[j])
147 unsigned int n = x.
getRows() / 4;
150 unsigned int n2 = 2 * n;
151 for (
unsigned int i = 0; i < 4; i++) {
152 unsigned int ind2 = 2 * ind[i];
154 pb[i][1] = x[ind2 + 1];
157 pa[i][0] = x[n2 + ind2];
158 pa[i][1] = x[n2 + ind2 + 1];
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]));
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)
168 unsigned int n = (
unsigned int)xb.
size();
172 std::vector<vpColVector> pa(n), pb(n);
173 for (
unsigned i = 0; i < n; i++) {
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])) {
190 if (isColinear(pb[i], pb[j], pb[k])) {
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];
210 xa[i] = x[n2 + ind2];
211 ya[i] = x[n2 + ind2 + 1];
222 for (
unsigned int i = 0; i < 9; i++) {
231 unsigned int n = x.
getRows() / 4;
232 unsigned int n2 = n * 2;
239 for (
unsigned int i = 0; i < n; i++) {
240 unsigned int i2 = 2 * i;
243 pb[i][1] = x[i2 + 1];
247 pa[i][0] = x[n2 + i2];
248 pa[i][1] = x[n2 + i2 + 1];
254 for (
unsigned int i = 0; i < 9; i++) {
263 for (
unsigned int i = 0; i < n; i++) {
266 d[i] = sqrt((pa[i] - Hpb).sumSquare());
276 void vpHomography::initRansac(
unsigned int n,
double *xb,
double *yb,
double *xa,
double *ya,
vpColVector &x)
279 unsigned int n2 = n * 2;
280 for (
unsigned int i = 0; i < n; i++) {
281 unsigned int i2 = 2 * i;
285 x[n2 + i2 + 1] = ya[i];
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)
322 unsigned int n = (
unsigned int)xb.size();
323 if (yb.size() != n || xa.size() != n || ya.size() != n)
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;
337 std::vector<unsigned int> rand_ind;
339 unsigned int nbMinRandom = 4;
340 unsigned int ransacMaxTrials = 1000;
341 unsigned int maxDegenerateIter = 1000;
343 unsigned int nbTrials = 0;
344 unsigned int nbDegenerateIter = 0;
345 unsigned int nbInliers = 0;
347 bool foundSolution =
false;
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);
354 if (inliers.size() != n)
357 while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus) {
358 cur_outliers.clear();
361 bool degenerate =
true;
362 while (degenerate ==
true) {
363 std::vector<bool> usedPt(n,
false);
366 for (
unsigned int i = 0; i < nbMinRandom; i++) {
368 unsigned int r = (
unsigned int)ceil(random() * n) - 1;
370 r = (
unsigned int)ceil(random() * n) - 1;
373 rand_ind.push_back(r);
382 if (!vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
392 if (nbDegenerateIter > maxDegenerateIter) {
403 for (
unsigned int i = 0; i < nbMinRandom; i++) {
413 r += (a - c).sumSquare();
418 r = sqrt(r / nbMinRandom);
421 unsigned int nbInliersCur = 0;
422 for (
unsigned int i = 0; i < n; i++) {
432 double error = sqrt((a - c).sumSquare());
433 if (error <= threshold) {
435 cur_consensus.push_back(i);
438 cur_outliers.push_back(i);
444 if (nbInliersCur > nbInliers) {
445 foundSolution =
true;
446 best_consensus = cur_consensus;
447 nbInliers = nbInliersCur;
450 cur_consensus.clear();
454 if (nbTrials >= ransacMaxTrials) {
455 vpERROR_TRACE(
"Ransac reached the maximum number of trials");
456 foundSolution =
true;
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());
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]];
479 for (
unsigned int i = 0; i < best_consensus.size(); i++) {
489 residual += (a - c).sumSquare();
492 residual = sqrt(residual / best_consensus.size());
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D 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 emited 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.