/*!
Resample the ellipse if the number of sample is less than 90% of the
expected value.
\note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
and the parameter which indicates the number of degrees between
two points (vpMe::sample_step).
\param I : Image in which the ellipse appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeEllipse::reSample(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
unsigned int n = numberOfSignal() ;
expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
if ((double)n<0.9*expecteddensity){
sample(I) ;
}
}
/*!
Resample the line if the number of sample is less than 80% of the
expected value.
\note The expected value is computed thanks to the length of the
line and the parameter which indicates the number of pixel between
two points (vpMe::sample_step).
\param I : Image in which the line appears.
*/
void
vpMeLine::reSample(const vpImage<unsigned char> &I)
{
double i1,j1,i2,j2 ;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
project(a,b,c,PExt[0].ifloat,PExt[0].jfloat,i1,j1) ;
project(a,b,c,PExt[1].ifloat,PExt[1].jfloat,i2,j2) ;
// Points extremites
PExt[0].ifloat = i1 ;
PExt[0].jfloat = j1 ;
PExt[1].ifloat = i2 ;
PExt[1].jfloat = j2 ;
double d = sqrt(vpMath::sqr(i1-i2)+vpMath::sqr(j1-j2)) ;
unsigned int n = numberOfSignal() ;
double expecteddensity = d / (double)me->getSampleStep();
if ((double)n<0.9*expecteddensity)
{
double delta_new = delta;
delta = delta_1;
sample(I) ;
delta = delta_new;
// 2. On appelle ce qui n'est pas specifique
{
vpMeTracker::initTracking(I) ;
}
}
}
/*!
Least squares method used to make the tracking more robust. It
ensures that the points taken into account to compute the right
equation belong to the line.
*/
void
vpMeLine::leastSquare()
{
vpMatrix A(numberOfSignal(),2) ;
vpColVector x(2), x_1(2) ;
x_1 = 0;
unsigned int i ;
vpRobust r(numberOfSignal()) ;
r.setThreshold(2);
r.setIteration(0) ;
vpMatrix D(numberOfSignal(),numberOfSignal()) ;
D.eye() ;
vpMatrix DA, DAmemory ;
vpColVector DAx ;
vpColVector w(numberOfSignal()) ;
vpColVector B(numberOfSignal()) ;
w =1 ;
vpMeSite p_me ;
unsigned int iter =0 ;
unsigned int nos_1 = 0 ;
double distance = 100;
if (list.size() <= 2 || numberOfSignal() <= 2)
{
//vpERROR_TRACE("Not enough point") ;
vpCDEBUG(1) << "Not enough point";
throw(vpTrackingException(vpTrackingException::notEnoughPointError,
"not enough point")) ;
}
if ((fabs(b) >=0.9)) // Construction du systeme Ax=B
// a i + j + c = 0
// A = (i 1) B = (-j)
{
nos_1 = numberOfSignal() ;
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = p_me.ifloat ;
A[k][1] = 1 ;
B[k] = -p_me.jfloat ;
k++ ;
}
}
while (iter < 4 && distance > 0.05)
{
DA = D*A ;
x = DA.pseudoInverse(1e-26) *D*B ;
vpColVector residu(nos_1);
residu = B - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++ ;
distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
x_1 = x;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < 0.2)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
// mise a jour de l'equation de la droite
a = x[0] ;
b = 1 ;
c = x[1] ;
double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
a /= s ;
b /= s ;
c /= s ;
}
else // Construction du systeme Ax=B
// i + bj + c = 0
// A = (j 1) B = (-i)
{
nos_1 = numberOfSignal() ;
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = p_me.jfloat ;
A[k][1] = 1 ;
B[k] = -p_me.ifloat ;
k++ ;
}
}
while (iter < 4 && distance > 0.05)
{
DA = D*A ;
x = DA.pseudoInverse(1e-26) *D*B ;
vpColVector residu(nos_1);
residu = B - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++ ;
distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
x_1 = x;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < 0.2)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
a = 1 ;
b = x[0] ;
c = x[1] ;
double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
a /= s ;
b /= s ;
c /= s ;
}
// mise a jour du delta
delta = atan2(a,b) ;
normalizeAngle(delta) ;
}
/*!
Least squares method used to make the tracking more robust. It
ensures that the points taken into account to compute the right
equation belong to the ellipse.
*/
void
vpMeEllipse::leastSquare()
{
// Construction du systeme Ax=b
//! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
// A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
unsigned int i ;
vpMeSite p_me ;
unsigned int iter =0 ;
vpColVector b_(numberOfSignal()) ;
vpRobust r(numberOfSignal()) ;
r.setThreshold(2);
r.setIteration(0) ;
vpMatrix D(numberOfSignal(),numberOfSignal()) ;
D.setIdentity() ;
vpMatrix DA, DAmemory ;
vpColVector DAx ;
vpColVector w(numberOfSignal()) ;
w =1 ;
unsigned int nos_1 = numberOfSignal() ;
if (list.size() < 3)
{
vpERROR_TRACE("Not enough point") ;
throw(vpTrackingException(vpTrackingException::notEnoughPointError,
"not enough point")) ;
}
if (circle ==false)
{
vpMatrix A(numberOfSignal(),5) ;
vpColVector x(5);
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = vpMath::sqr(p_me.jfloat) ;
A[k][1] = 2 * p_me.ifloat * p_me.jfloat ;
A[k][2] = 2 * p_me.ifloat ;
A[k][3] = 2 * p_me.jfloat ;
A[k][4] = 1 ;
b_[k] = - vpMath::sqr(p_me.ifloat) ;
k++ ;
}
}
while (iter < 4 )
{
DA = D*A ;
vpMatrix DAp ;
x = DA.pseudoInverse(1e-26) *D*b_ ;
vpColVector residu(nos_1);
residu = b_ - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < thresholdWeight)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
for(i = 0; i < 5; i ++)
K[i] = x[i];
}
else
{
vpMatrix A(numberOfSignal(),3) ;
vpColVector x(3);
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = 2* p_me.ifloat ;
A[k][1] = 2 * p_me.jfloat ;
A[k][2] = 1 ;
b_[k] = - vpMath::sqr(p_me.ifloat) - vpMath::sqr(p_me.jfloat) ;
k++ ;
}
}
while (iter < 4 )
{
DA = D*A ;
vpMatrix DAp ;
x = DA.pseudoInverse(1e-26) *D*b_ ;
vpColVector residu(nos_1);
residu = b_ - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k];
k++;
}
iter++;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < thresholdWeight)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
for(i = 0; i < 3; i ++)
K[i+2] = x[i];
}
getParameters() ;
}
