/*!
Compute the pose using the Ransac approach.
\param cMo : Computed pose
*/
void vpPose::poseRansac(vpHomogeneousMatrix & cMo)
{
srand(0);
std::vector<unsigned int> best_consensus;
std::vector<unsigned int> cur_consensus;
std::vector<unsigned int> cur_outliers;
std::vector<unsigned int> cur_randoms;
unsigned int size = listP.size();
int nbTrials = 0;
unsigned int nbMinRandom = 4 ;
unsigned int nbInliers = 0;
bool foundSolution = false;
while (nbTrials < ransacMaxTrials && nbInliers < (unsigned)ransacNbInlierConsensus)
{
cur_outliers.clear();
cur_randoms.clear();
std::vector<bool> usedPt(size, false);
vpPose poseMin ;
for(unsigned int i = 0; i < nbMinRandom; i++)
{
int r = rand()%size;
while(usedPt[r] ) r = rand()%size;
usedPt[r] = true;
std::list<vpPoint>::const_iterator iter = listP.begin();
std::advance(iter, r);
vpPoint pt = *iter;
bool degenerate = false;
for(std::list<vpPoint>::const_iterator it = poseMin.listP.begin(); it != poseMin.listP.end(); ++it){
vpPoint ptdeg = *it;
if( ((fabs(pt.get_x() - ptdeg.get_x()) < 1e-6) && (fabs(pt.get_y() - ptdeg.get_y()) < 1e-6)) ||
((fabs(pt.get_oX() - ptdeg.get_oX()) < 1e-6) && (fabs(pt.get_oY() - ptdeg.get_oY()) < 1e-6) && (fabs(pt.get_oZ() - ptdeg.get_oZ()) < 1e-6))){
degenerate = true;
break;
}
}
if(!degenerate){
poseMin.addPoint(pt) ;
cur_randoms.push_back(r);
}
else
i--;
}
poseMin.computePose(vpPose::DEMENTHON,cMo) ;
double r = poseMin.computeResidual(cMo) ;
r = sqrt(r)/(double)nbMinRandom;
if (r < ransacThreshold)
{
unsigned int nbInliersCur = 0;
//std::cout << "Résultat : " << r << " / " << vpPoseVector(cMo).sumSquare()<< std::endl ;
int iter = 0;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
vpPoint pt = *it;
vpPoint p(pt) ;
p.track(cMo) ;
double d = vpMath::sqr(p.get_x() - pt.get_x()) + vpMath::sqr(p.get_y() - pt.get_y()) ;
double error = sqrt(d) ;
if(error < ransacThreshold){ // the point is considered an inlier if the error is below the threshold
nbInliersCur++;
cur_consensus.push_back(iter);
}
else
cur_outliers.push_back(iter);
iter++;
}
//std::cout << "Nombre d'inliers " << nbInliersCur << "/" << nbInliers << std::endl ;
if(nbInliersCur > nbInliers)
{
foundSolution = true;
best_consensus = cur_consensus;
nbInliers = nbInliersCur;
}
nbTrials++;
cur_consensus.clear();
if(nbTrials >= ransacMaxTrials){
vpERROR_TRACE("Ransac reached the maximum number of trials");
foundSolution = true;
}
}
}
if(foundSolution){
//std::cout << "Nombre d'inliers " << nbInliers << std::endl ;
//Display the random picked points
/*
std::cout << "Randoms : ";
for(unsigned int i = 0 ; i < cur_randoms.size() ; i++)
std::cout << cur_randoms[i] << " ";
std::cout << std::endl;
*/
//Display the outliers
/*
std::cout << "Outliers : ";
for(unsigned int i = 0 ; i < cur_outliers.size() ; i++)
std::cout << cur_outliers[i] << " ";
std::cout << std::endl;
*/
if(nbInliers >= (unsigned)ransacNbInlierConsensus)
{
vpPose pose ;
for(unsigned i = 0 ; i < best_consensus.size(); i++)
{
std::list<vpPoint>::const_iterator iter = listP.begin();
std::advance(iter, best_consensus[i]);
vpPoint pt = *iter;
pose.addPoint(pt) ;
ransacInliers.push_back(pt);
}
pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS,cMo) ;
//std::cout << "Residue finale "<< pose.computeResidual(cMo) << std::endl ;
}
}
}
/*!
From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a
and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, computes the
homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}\f$
using Ransac algorithm.
\param xb, yb : Coordinates vector of matched points in image b. These coordinates are expressed in meters.
\param xa, ya : Coordinates vector of matched points in image a. These coordinates are expressed in meters.
\param aHb : Estimated homography that relies the transformation from image a to image b.
\param inliers : Vector that indicates if a matched point is an inlier (true) or an outlier (false).
\param residual : Global residual computed as
\f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf H}_b}} {^b{\bf p}}} \|}^{2}}\f$ with \f$n\f$ the
number of inliers.
\param nbInliersConsensus : Minimal number of points requested to fit the estimated homography.
\param threshold : Threshold for outlier removing. A point is considered as an outlier if the reprojection error
\f$\| {^a{\bf p} - {\hat{^a{\bf H}_b}} {^b{\bf p}}} \|\f$ is greater than this threshold.
\param normalization : When set to true, the coordinates of the points are normalized. The normalization
carried out is the one preconized by Hartley.
\return true if the homography could be computed, false otherwise.
*/
bool vpHomography::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)
{
unsigned int n = (unsigned int)xb.size();
if (yb.size() != n || xa.size() != n || ya.size() != n)
throw(vpException(vpException::dimensionError,
"Bad dimension for robust homography estimation"));
// 4 point are required
if(n<4)
throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
vpUniRand random((const long)time(NULL)) ;
std::vector<unsigned int> best_consensus;
std::vector<unsigned int> cur_consensus;
std::vector<unsigned int> cur_outliers;
std::vector<unsigned int> cur_randoms;
std::vector<unsigned int> rand_ind;
unsigned int nbMinRandom = 4 ;
unsigned int ransacMaxTrials = 1000;
unsigned int maxDegenerateIter = 1000;
unsigned int nbTrials = 0;
unsigned int nbDegenerateIter = 0;
unsigned int nbInliers = 0;
bool foundSolution = false;
std::vector<double> xa_rand(nbMinRandom);
std::vector<double> ya_rand(nbMinRandom);
std::vector<double> xb_rand(nbMinRandom);
std::vector<double> yb_rand(nbMinRandom);
if (inliers.size() != n)
inliers.resize(n);
while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus)
{
cur_outliers.clear();
cur_randoms.clear();
bool degenerate = true;
while(degenerate == true){
std::vector<bool> usedPt(n, false);
rand_ind.clear();
for(unsigned int i = 0; i < nbMinRandom; i++)
{
// Generate random indicies in the range 0..n
unsigned int r = (unsigned int)ceil(random()*n) -1;
while(usedPt[r]) {
r = (unsigned int)ceil(random()*n) -1;
}
usedPt[r] = true;
rand_ind.push_back(r);
xa_rand[i] = xa[r];
ya_rand[i] = ya[r];
xb_rand[i] = xb[r];
yb_rand[i] = yb[r];
}
try{
if (! vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
degenerate = false;
}
}
catch(...){
degenerate = true;
}
nbDegenerateIter ++;
if (nbDegenerateIter > maxDegenerateIter){
vpERROR_TRACE("Unable to select a nondegenerate data set");
throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
}
}
aHb /= aHb[2][2] ;
// Computing Residual
double r = 0;
vpColVector a(3), b(3), c(3) ;
for (unsigned int i=0 ; i < nbMinRandom ; i++) {
a[0] = xa_rand[i] ; a[1] = ya_rand[i] ; a[2] = 1 ;
b[0] = xb_rand[i] ; b[1] = yb_rand[i] ; b[2] = 1 ;
c = aHb*b; c /= c[2] ;
r += (a-c).sumSquare() ;
//cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
}
// Finding inliers & ouliers
r = sqrt(r/nbMinRandom);
//std::cout << "Candidate residual: " << r << std::endl;
if (r < threshold)
{
unsigned int nbInliersCur = 0;
for (unsigned int i = 0; i < n ; i++)
{
a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
c = aHb*b ; c /= c[2] ;
double error = sqrt((a-c).sumSquare()) ;
if(error <= threshold){
nbInliersCur++;
cur_consensus.push_back(i);
inliers[i] = true;
}
else{
cur_outliers.push_back(i);
inliers[i] = false;
}
}
//std::cout << "nb inliers that matches: " << nbInliersCur << std::endl;
if(nbInliersCur > nbInliers)
{
foundSolution = true;
best_consensus = cur_consensus;
nbInliers = nbInliersCur;
}
cur_consensus.clear();
}
nbTrials++;
if(nbTrials >= ransacMaxTrials){
vpERROR_TRACE("Ransac reached the maximum number of trials");
foundSolution = true;
}
}
if(foundSolution){
if(nbInliers >= nbInliersConsensus)
{
std::vector<double> xa_best(best_consensus.size());
std::vector<double> ya_best(best_consensus.size());
std::vector<double> xb_best(best_consensus.size());
std::vector<double> yb_best(best_consensus.size());
for(unsigned i = 0 ; i < best_consensus.size(); i++)
{
xa_best[i] = xa[best_consensus[i]];
ya_best[i] = ya[best_consensus[i]];
xb_best[i] = xb[best_consensus[i]];
yb_best[i] = yb[best_consensus[i]];
}
vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization) ;
aHb /= aHb[2][2];
residual = 0 ;
vpColVector a(3), b(3), c(3);
for (unsigned int i=0 ; i < best_consensus.size() ; i++) {
a[0] = xa_best[i] ; a[1] = ya_best[i] ; a[2] = 1 ;
b[0] = xb_best[i] ; b[1] = yb_best[i] ; b[2] = 1 ;
c = aHb*b ; c /= c[2] ;
residual += (a-c).sumSquare() ;
}
residual = sqrt(residual/best_consensus.size());
return true;
}
else {
return false;
}
}
else {
return false;
}
}
