/*!
Compute the pose from a list lp of 2D point (x,y) and a list lP 3D points
(X,Y,Z) in P using the Ransac algorithm. It is not assumed that
the 2D and 3D points are registred
at least numberOfInlierToReachAConsensus of true correspondance are required
to validate the pose
the inliers are given in a list of lPi vpPoint
the pose is return in cMo
*/
void
vpPose::ransac(vpList<vpPoint> &lp,
vpList<vpPoint> &lP,
const int numberOfInlierToReachAConsensus,
const double threshold,
int &ninliers,
vpList<vpPoint> &lPi,
vpHomogeneousMatrix &cMo)
{
int n = lp.nbElement() ;
int m = lP.nbElement() ;
double *x, *y;
x = new double [n];
y = new double [n];
vpPoint pin ;
lp.front() ;
int i = 0 ;
while(!lp.outside())
{
pin = lp.value() ; lp.next() ;
x[i] = pin.get_x() ;
y[i] = pin.get_y() ;
i++ ;
}
double *X, *Y, *Z;
X = new double [m];
Y = new double [m];
Z = new double [m];
lP.front() ;
i = 0 ;
while(!lP.outside())
{
pin = lP.value() ; lP.next() ;
X[i] = pin.get_oX() ;
Y[i] = pin.get_oY() ;
Z[i] = pin.get_oZ() ;
i++ ;
}
vpColVector xi,yi,Xi,Yi,Zi ;
ransac(n,x,y,
m,X,Y,Z, numberOfInlierToReachAConsensus,
threshold,
ninliers,
xi,yi,Xi,Yi,Zi,
cMo) ;
for( i=0 ; i < ninliers ; i++)
{
vpPoint Pi ;
Pi.setWorldCoordinates(Xi[i],Yi[i], Zi[i]) ;
Pi.set_x(xi[i]) ;
Pi.set_y(yi[i]) ;
lPi += Pi ;
}
delete [] x;
delete [] y;
delete [] X;
delete [] Y;
delete [] Z;
}
