38 #include <visp3/vision/vpHomography.h>
39 #include <visp3/core/vpColVector.h>
40 #include <visp3/core/vpRansac.h>
42 #include <visp3/core/vpImage.h>
43 #include <visp3/core/vpDisplay.h>
44 #include <visp3/core/vpMeterPixelConversion.h>
53 #ifndef DOXYGEN_SHOULD_SKIP_THIS
55 bool iscolinear(
double *x1,
double *x2,
double *x3);
59 iscolinear(
double *x1,
double *x2,
double *x3)
80 vpHomography::degenerateConfiguration(
vpColVector &x,
unsigned int *ind,
81 double threshold_area)
86 for (i=1 ; i < 4 ; i++)
88 if (ind[i]==ind[j])
return true ;
90 unsigned int n = x.
getRows()/4 ;
96 for(i = 0 ; i < 4 ; i++)
98 pb[i][0] = x[2*ind[i]] ;
99 pb[i][1] = x[2*ind[i]+1] ;
102 pa[i][0] = x[2*n+2*ind[i]] ;
103 pa[i][1] = x[2*n+2*ind[i]+1] ;
109 double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
110 pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
111 -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
114 double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
115 pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
116 -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
119 double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
120 pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
121 -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
124 double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
125 pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
126 -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
128 double sum_area = area012 + area013 + area023 + area123;
130 return ((sum_area < threshold_area) ||
131 (iscolinear(pa[0],pa[1],pa[2]) ||
132 iscolinear(pa[0],pa[1],pa[3]) ||
133 iscolinear(pa[0],pa[2],pa[3]) ||
134 iscolinear(pa[1],pa[2],pa[3]) ||
135 iscolinear(pb[0],pb[1],pb[2]) ||
136 iscolinear(pb[0],pb[1],pb[3]) ||
137 iscolinear(pb[0],pb[2],pb[3]) ||
138 iscolinear(pb[1],pb[2],pb[3])));
153 vpHomography::degenerateConfiguration(
vpColVector &x,
unsigned int *ind)
155 for (
unsigned int i = 1; i < 4 ; i++)
156 for (
unsigned int j = 0 ;j < i ; j++)
157 if (ind[i] == ind[j])
return true ;
159 unsigned int n = x.
getRows()/4;
162 unsigned int n2 = 2 * n;
164 for(
unsigned int i = 0; i < 4 ;i++)
168 pb[i][1] = x[ind2+1];
171 pa[i][0] = x[n2+ind2] ;
172 pa[i][1] = x[n2+ind2+1] ;
175 return ( iscolinear(pa[0],pa[1],pa[2]) ||
176 iscolinear(pa[0],pa[1],pa[3]) ||
177 iscolinear(pa[0],pa[2],pa[3]) ||
178 iscolinear(pa[1],pa[2],pa[3]) ||
179 iscolinear(pb[0],pb[1],pb[2]) ||
180 iscolinear(pb[0],pb[1],pb[3]) ||
181 iscolinear(pb[0],pb[2],pb[3]) ||
182 iscolinear(pb[1],pb[2],pb[3]));
185 vpHomography::degenerateConfiguration(
const std::vector<double> &xb,
const std::vector<double> &yb,
186 const std::vector<double> &xa,
const std::vector<double> &ya)
188 unsigned int n = (
unsigned int)xb.
size();
192 std::vector<vpColVector> pa(n), pb(n);
193 for (
unsigned i=0; i<n;i++) {
204 for (
unsigned int i = 0; i < n-2; i++) {
205 for (
unsigned int j = i+1; j < n-1; j++) {
206 for (
unsigned int k = j+1; k < n ; k++)
208 if (isColinear(pa[i], pa[j], pa[k])) {
211 if (isColinear(pb[i], pb[j], pb[k])){
224 unsigned int n = x.
getRows()/4 ;
225 std::vector<double> xa(4), xb(4);
226 std::vector<double> ya(4), yb(4);
227 unsigned int n2 = n * 2;
229 for(i=0 ; i < 4 ; i++)
236 ya[i] = x[n2+ind2+1] ;
249 for (i=0 ; i <9 ; i++)
262 unsigned int n = x.
getRows()/4 ;
263 unsigned int n2 = n * 2;
271 for( i=0 ; i < n ; i++)
280 pa[i][0] = x[n2+i2] ;
281 pa[i][1] = x[n2+i2+1] ;
287 for (i=0 ; i <9 ; i++)
297 for (i=0 ; i <n ; i++)
301 d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
309 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
313 vpHomography::initRansac(
unsigned int n,
314 double *xb,
double *yb,
315 double *xa,
double *ya,
319 unsigned int n2 = n * 2;
321 for (
unsigned int i=0 ; i < n ; i++)
358 const std::vector<double> &xa,
const std::vector<double> &ya,
360 std::vector<bool> &inliers,
362 unsigned int nbInliersConsensus,
366 unsigned int n = (
unsigned int)xb.size();
367 if (yb.size() != n || xa.size() != n || ya.size() != n)
369 "Bad dimension for robust homography estimation"));
375 vpUniRand random((
const long)time(NULL)) ;
377 std::vector<unsigned int> best_consensus;
378 std::vector<unsigned int> cur_consensus;
379 std::vector<unsigned int> cur_outliers;
380 std::vector<unsigned int> cur_randoms;
382 std::vector<unsigned int> rand_ind;
384 unsigned int nbMinRandom = 4 ;
385 unsigned int ransacMaxTrials = 1000;
386 unsigned int maxDegenerateIter = 1000;
388 unsigned int nbTrials = 0;
389 unsigned int nbDegenerateIter = 0;
390 unsigned int nbInliers = 0;
392 bool foundSolution =
false;
394 std::vector<double> xa_rand(nbMinRandom);
395 std::vector<double> ya_rand(nbMinRandom);
396 std::vector<double> xb_rand(nbMinRandom);
397 std::vector<double> yb_rand(nbMinRandom);
399 if (inliers.size() != n)
402 while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus)
404 cur_outliers.clear();
407 bool degenerate =
true;
408 while(degenerate ==
true){
409 std::vector<bool> usedPt(n,
false);
412 for(
unsigned int i = 0; i < nbMinRandom; i++)
415 unsigned int r = (
unsigned int)ceil(random()*n) -1;
417 r = (
unsigned int)ceil(random()*n) -1;
420 rand_ind.push_back(r);
429 if (! vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
440 if (nbDegenerateIter > maxDegenerateIter){
451 for (
unsigned int i=0 ; i < nbMinRandom ; i++) {
452 a[0] = xa_rand[i] ; a[1] = ya_rand[i] ; a[2] = 1 ;
453 b[0] = xb_rand[i] ; b[1] = yb_rand[i] ; b[2] = 1 ;
455 c = aHb*b; c /= c[2] ;
456 r += (a-c).sumSquare() ;
461 r = sqrt(r/nbMinRandom);
465 unsigned int nbInliersCur = 0;
466 for (
unsigned int i = 0; i < n ; i++)
468 a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
469 b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
471 c = aHb*b ; c /= c[2] ;
472 double error = sqrt((a-c).sumSquare()) ;
473 if(error <= threshold){
475 cur_consensus.push_back(i);
479 cur_outliers.push_back(i);
484 if(nbInliersCur > nbInliers)
486 foundSolution =
true;
487 best_consensus = cur_consensus;
488 nbInliers = nbInliersCur;
491 cur_consensus.clear();
495 if(nbTrials >= ransacMaxTrials){
496 vpERROR_TRACE(
"Ransac reached the maximum number of trials");
497 foundSolution =
true;
502 if(nbInliers >= nbInliersConsensus)
504 std::vector<double> xa_best(best_consensus.size());
505 std::vector<double> ya_best(best_consensus.size());
506 std::vector<double> xb_best(best_consensus.size());
507 std::vector<double> yb_best(best_consensus.size());
509 for(
unsigned i = 0 ; i < best_consensus.size(); i++)
511 xa_best[i] = xa[best_consensus[i]];
512 ya_best[i] = ya[best_consensus[i]];
513 xb_best[i] = xb[best_consensus[i]];
514 yb_best[i] = yb[best_consensus[i]];
522 for (
unsigned int i=0 ; i < best_consensus.size() ; i++) {
523 a[0] = xa_best[i] ; a[1] = ya_best[i] ; a[2] = 1 ;
524 b[0] = xb_best[i] ; b[1] = yb_best[i] ; b[2] = 1 ;
526 c = aHb*b ; c /= c[2] ;
527 residual += (a-c).sumSquare() ;
530 residual = sqrt(residual/best_consensus.size());
Implementation of a matrix and operations on matrices.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
error that can be emited by ViSP classes.
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
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)
Implementation of an homography and operations on homographies.
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)
unsigned int getRows() const
Return the number of rows of the 2D array.
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)
Implementation of column vector and the associated operations.
Class for generating random numbers with uniform probability density.
void resize(const unsigned int i, const bool flagNullify=true)