49 #include <visp/vpPose.h>
50 #include <visp/vpColVector.h>
51 #include <visp/vpRansac.h>
52 #include <visp/vpTime.h>
53 #include <visp/vpList.h>
54 #include <visp/vpPoseException.h>
72 std::vector<unsigned int> best_consensus;
73 std::vector<unsigned int> cur_consensus;
74 std::vector<unsigned int> cur_outliers;
75 std::vector<unsigned int> cur_randoms;
76 unsigned int size =
listP.size();
78 unsigned int nbMinRandom = 4 ;
79 unsigned int nbInliers = 0;
82 double r, r_lagrange, r_dementhon;
84 bool foundSolution =
false;
86 while (nbTrials < ransacMaxTrials && nbInliers < (
unsigned)ransacNbInlierConsensus)
91 std::vector<bool> usedPt(size,
false);
94 for(
unsigned int i = 0; i < nbMinRandom; i++)
96 unsigned int r = (
unsigned int)rand()%size;
97 while(usedPt[r] ) r = (
unsigned int)rand()%size;
100 std::list<vpPoint>::const_iterator iter =
listP.begin();
101 std::advance(iter, r);
104 bool degenerate =
false;
105 for(std::list<vpPoint>::const_iterator it = poseMin.
listP.begin(); it != poseMin.
listP.end(); ++it){
115 cur_randoms.push_back(r);
126 if (r_lagrange < r_dementhon) {
134 r = sqrt(r)/(double)nbMinRandom;
136 if (r < ransacThreshold)
138 unsigned int nbInliersCur = 0;
139 unsigned int iter = 0;
140 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
147 double error = sqrt(d) ;
148 if(error < ransacThreshold){
151 bool degenerate =
false;
153 for(
unsigned int it_inlier_index = 0; it_inlier_index< cur_consensus.size(); it_inlier_index++){
154 std::list<vpPoint>::const_iterator it_point =
listP.begin();
155 std::advance(it_point, cur_consensus[it_inlier_index]);
168 cur_consensus.push_back(iter);
171 cur_outliers.push_back(iter);
175 cur_outliers.push_back(iter);
180 if(nbInliersCur > nbInliers)
182 foundSolution =
true;
183 best_consensus = cur_consensus;
184 nbInliers = nbInliersCur;
188 cur_consensus.clear();
190 if(nbTrials >= ransacMaxTrials){
191 vpERROR_TRACE(
"Ransac reached the maximum number of trials");
192 foundSolution =
true;
216 if(nbInliers >= (
unsigned)ransacNbInlierConsensus)
219 for(
unsigned i = 0 ; i < best_consensus.size(); i++)
221 std::list<vpPoint>::const_iterator iter =
listP.begin();
222 std::advance(iter, best_consensus[i]);
226 ransacInliers.push_back(pt);
234 if (r_lagrange < r_dementhon) {
271 std::vector<vpPoint> &p3D,
272 const unsigned int &numberOfInlierToReachAConsensus,
273 const double &threshold,
274 unsigned int &ninliers,
275 std::vector<vpPoint> &listInliers,
277 const int &maxNbTrials )
282 for(
unsigned int i = 0 ; i < p2D.size() ; i++)
284 for(
unsigned int j = 0 ; j < p3D.size() ; j++)
287 pt.
set_x(p2D[i].get_x());
288 pt.
set_y(p2D[i].get_y());
295 if (pose.
listP.size() < 4)
297 vpERROR_TRACE(
"Ransac method cannot be used in that case ") ;
301 "Not enough points ")) ;
314 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
332 for (
int i=1 ; i < 4 ; i++)
333 for (
int j=0 ; j<i ; j++)
335 unsigned int indi = 5*ind[i] ;
336 unsigned int indj = 5*ind[j] ;
338 if ((fabs(x[indi] - x[indj]) < 1e-6) && (fabs(x[indi+1] - x[indj+1]) < 1e-6))
341 if ((fabs(x[indi+2] - x[indj+2]) < 1e-6) && (fabs(x[indi+3] - x[indj+3]) < 1e-6) && (fabs(x[indi+4] - x[indj+4]) < 1e-6))
360 for(i=0 ; i < 4 ; i++)
363 unsigned int index = 5*ind[i] ;
365 p[i].
set_x(x[index]) ;
366 p[i].
set_y(x[index+1]) ;
385 for (i=0 ; i <16 ; i++)
403 unsigned int n = x.
getRows()/5 ;
409 for( i=0 ; i < n ; i++)
416 for (i=0 ; i <16 ; i++)
425 for( i=0 ; i < n ; i++)
438 vpPose::initRansac(
const unsigned int n,
439 const double *x,
const double *y,
440 const unsigned int m,
441 const double *X,
const double *Y,
const double *Z,
446 for (
unsigned int i=0 ; i < n ; i++)
448 for (
unsigned int j=0 ; j < m ; j++)
496 const double *x,
const double *y,
497 const unsigned int m,
498 const double *X,
const double *Y,
const double *Z,
499 const int numberOfInlierToReachAConsensus,
500 const double threshold,
501 unsigned int &ninliers,
505 const int maxNbTrials)
512 vpPose::initRansac(n,x,y,m,X,Y,Z, data) ;
517 threshold, M,inliers,
518 numberOfInlierToReachAConsensus, 0.0, maxNbTrials) ;
523 for(i=0 ; i < n*m ; i++)
526 if (std::fabs(inliers[i]-1) <= std::fabs(
vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
539 for(i=0 ; i < n*m ; i++)
542 if (std::fabs(inliers[i]-1) <= std::fabs(
vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
545 yi[k] = data[5*i+1] ;
546 Xi[k] = data[5*i+2] ;
547 Yi[k] = data[5*i+3] ;
548 Zi[k] = data[5*i+4] ;
553 for (i=0 ; i <16 ; i++)
590 const unsigned int m,
592 const int numberOfInlierToReachAConsensus,
593 const double threshold,
594 unsigned int &ninliers,
595 std::list<vpPoint> &lPi,
597 const int maxNbTrials)
602 for (
unsigned int i=0 ; i < n ; i++)
604 x[i] = p[i].
get_x() ;
605 y[i] = p[i].
get_y() ;
611 for (
unsigned int i=0 ; i < m ; i++)
621 m,X,Y,Z, numberOfInlierToReachAConsensus,
627 for(
unsigned int i=0 ; i < ninliers ; i++)
668 std::list<vpPoint> &lP,
669 const int numberOfInlierToReachAConsensus,
670 const double threshold,
671 unsigned int &ninliers,
672 std::list<vpPoint> &lPi,
674 const int maxNbTrials)
677 unsigned int n = lp.size() ;
678 unsigned int m = lP.size() ;
686 for (std::list<vpPoint>::const_iterator it = lp.begin(); it != lp.end(); ++it)
699 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
711 m,X,Y,Z, numberOfInlierToReachAConsensus,
717 for( i=0 ; i < ninliers ; i++)
733 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS