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;
81 bool foundSolution =
false;
83 while (nbTrials < ransacMaxTrials && nbInliers < (
unsigned)ransacNbInlierConsensus)
88 std::vector<bool> usedPt(size,
false);
91 for(
unsigned int i = 0; i < nbMinRandom; i++)
94 while(usedPt[r] ) r = rand()%size;
97 std::list<vpPoint>::const_iterator iter =
listP.begin();
98 std::advance(iter, r);
101 bool degenerate =
false;
102 for(std::list<vpPoint>::const_iterator it = poseMin.
listP.begin(); it != poseMin.
listP.end(); ++it){
113 cur_randoms.push_back(r);
121 r = sqrt(r)/(double)nbMinRandom;
123 if (r < ransacThreshold)
125 unsigned int nbInliersCur = 0;
128 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it)
135 double error = sqrt(d) ;
136 if(error < ransacThreshold){
138 cur_consensus.push_back(iter);
141 cur_outliers.push_back(iter);
147 if(nbInliersCur > nbInliers)
149 foundSolution =
true;
150 best_consensus = cur_consensus;
151 nbInliers = nbInliersCur;
155 cur_consensus.clear();
157 if(nbTrials >= ransacMaxTrials){
158 vpERROR_TRACE(
"Ransac reached the maximum number of trials");
159 foundSolution =
true;
183 if(nbInliers >= (
unsigned)ransacNbInlierConsensus)
186 for(
unsigned i = 0 ; i < best_consensus.size(); i++)
188 std::list<vpPoint>::const_iterator iter =
listP.begin();
189 std::advance(iter, best_consensus[i]);
193 ransacInliers.push_back(pt);
227 std::vector<vpPoint> &p3D,
228 const int &numberOfInlierToReachAConsensus,
229 const double &threshold,
230 unsigned int &ninliers,
231 std::vector<vpPoint> &listInliers,
233 const int &maxNbTrials )
238 for(
unsigned int i = 0 ; i < p2D.size() ; i++)
240 for(
unsigned int j = 0 ; j < p3D.size() ; j++)
243 pt.
set_x(p2D[i].get_x());
244 pt.
set_y(p2D[i].get_y());
251 if (pose.
listP.size() < 4)
253 vpERROR_TRACE(
"Ransac method cannot be used in that case ") ;
257 "Not enough points ")) ;
270 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
288 for (
int i=1 ; i < 4 ; i++)
289 for (
int j=0 ; j<i ; j++)
291 unsigned int indi = 5*ind[i] ;
292 unsigned int indj = 5*ind[j] ;
294 if ((fabs(x[indi] - x[indj]) < 1e-6) && (fabs(x[indi+1] - x[indj+1]) < 1e-6))
297 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))
317 for(i=0 ; i < 4 ; i++)
320 unsigned int index = 5*ind[i] ;
322 p[i].
set_x(x[index]) ;
323 p[i].
set_y(x[index+1]) ;
342 for (i=0 ; i <16 ; i++)
361 unsigned int n = x.
getRows()/5 ;
367 for( i=0 ; i < n ; i++)
374 for (i=0 ; i <16 ; i++)
383 for( i=0 ; i < n ; i++)
396 vpPose::initRansac(
const unsigned int n,
397 const double *x,
const double *y,
398 const unsigned int m,
399 const double *X,
const double *Y,
const double *Z,
404 for (
unsigned int i=0 ; i < n ; i++)
406 for (
unsigned int j=0 ; j < m ; j++)
456 const double *x,
const double *y,
457 const unsigned int m,
458 const double *X,
const double *Y,
const double *Z,
459 const int numberOfInlierToReachAConsensus,
460 const double threshold,
461 unsigned int &ninliers,
465 const int maxNbTrials)
472 vpPose::initRansac(n,x,y,m,X,Y,Z, data) ;
477 threshold, M,inliers,
478 numberOfInlierToReachAConsensus, 0.0, maxNbTrials) ;
483 for(i=0 ; i < n*m ; i++)
486 if (std::fabs(inliers[i]-1) <= std::fabs(
vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
499 for(i=0 ; i < n*m ; i++)
502 if (std::fabs(inliers[i]-1) <= std::fabs(
vpMath::maximum(inliers[i], 1.)) * std::numeric_limits<double>::epsilon())
505 yi[k] = data[5*i+1] ;
506 Xi[k] = data[5*i+2] ;
507 Yi[k] = data[5*i+3] ;
508 Zi[k] = data[5*i+4] ;
513 for (i=0 ; i <16 ; i++)
552 const unsigned int m,
554 const int numberOfInlierToReachAConsensus,
555 const double threshold,
556 unsigned int &ninliers,
557 std::list<vpPoint> &lPi,
559 const int maxNbTrials)
564 for (
unsigned int i=0 ; i < n ; i++)
566 x[i] = p[i].
get_x() ;
567 y[i] = p[i].
get_y() ;
573 for (
unsigned int i=0 ; i < m ; i++)
583 m,X,Y,Z, numberOfInlierToReachAConsensus,
589 for(
unsigned int i=0 ; i < ninliers ; i++)
632 std::list<vpPoint> &lP,
633 const int numberOfInlierToReachAConsensus,
634 const double threshold,
635 unsigned int &ninliers,
636 std::list<vpPoint> &lPi,
638 const int maxNbTrials)
641 unsigned int n = lp.size() ;
642 unsigned int m = lP.size() ;
650 for (std::list<vpPoint>::const_iterator it = lp.begin(); it != lp.end(); ++it)
663 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
675 m,X,Y,Z, numberOfInlierToReachAConsensus,
681 for( i=0 ; i < ninliers ; i++)
697 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS
void projection(const vpColVector &_cP, vpColVector &_p)
Projection onto the image plane of a point. Input: the 3D coordinates in the camera frame _cP...
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setIdentity()
Basic initialisation (identity)
double get_oY() const
Get the point Y coordinate in the object frame.
void setRansacThreshold(const double &t)
void set_x(const double x)
Set the point x coordinate in the image plane.
void track(const vpHomogeneousMatrix &cMo)
static double measureTimeMs()
double get_y() const
Get the point y coordinate in the image plane.
static void findMatch(std::vector< vpPoint > &p2D, std::vector< vpPoint > &p3D, const int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector< vpPoint > &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials=10000)
static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
double computeResidual()
compute the residual (in meter)
std::list< vpPoint > listP
array of point (use here class vpPoint)
Class that defines what is a point.
static Type maximum(const Type &a, const Type &b)
double * data
address of the first element of the data array
std::vector< vpPoint > getRansacInliers()
static double sqr(double x)
Class used for pose computation from N points (pose from point only).
double computeResidual(vpHomogeneousMatrix &cMo)
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
void setRansacNbInliersToReachConsensus(const int &nbC)
double get_oZ() const
Get the point Z coordinate in the object frame.
void set_y(const double y)
Set the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
void poseRansac(vpHomogeneousMatrix &cMo)
compute the pose using the Ransac approach
void setRansacMaxTrials(const int &rM)
static void ransac(const unsigned int n, const double *x, const double *y, const unsigned int m, const double *X, const double *Y, const double *Z, const int numberOfInlierToReachAConsensus, const double threshold, unsigned int &ninliers, vpColVector &xi, vpColVector &yi, vpColVector &Xi, vpColVector &Yi, vpColVector &Zi, vpHomogeneousMatrix &cMo, const int maxNbTrials=10000)
static bool degenerateConfiguration(vpColVector &x, unsigned int *ind)
double get_oX() const
Get the point X coordinate in the object frame.
Error that can be emited by the vpPose class and its derivates.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
static bool ransac(unsigned int npts, vpColVector &x, unsigned int s, double t, vpColVector &model, vpColVector &inliers, int consensus=1000, double areaThreshold=0.0, const int maxNbumbersOfTrials=10000)
RANSAC - Robustly fits a model to data with the RANSAC algorithm.
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
unsigned int getRows() const
Return the number of rows of the matrix.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
void addPoint(const vpPoint &P)
Add a new point in this array.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
void resize(const unsigned int i, const bool flagNullify=true)
void clearPoint()
suppress all the point in the array of point