/*!
Enable to display the points along the ellipse with a color corresponding to their state.
- If green : The vpMeSite is a good point.
- If blue : The point is removed because of the vpMeSite tracking phase (constrast problem).
- If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- If blue : The point is removed because of the robust method in the virtual visual servoing.
\param I : The image.
*/
void
vpMbtDistanceSphere::displayMovingEdges(const vpImage<unsigned char> &I)
{
if (meEllipse != NULL)
{
meEllipse->display(I); // display the me
if (vpDEBUG_ENABLE(3))
vpDisplay::flush(I);
}
}
/*!
Compute the two parameters \f$(\rho, \theta)\f$ of the line.
\param I : Image in which the line appears.
*/
void
vpMeLine::computeRhoTheta(const vpImage<unsigned char>& I)
{
//rho = -c ;
//theta = atan2(a,b) ;
rho = fabs(c);
theta = atan2(b,a) ;
while (theta >= M_PI) theta -=M_PI ;
while (theta < 0) theta +=M_PI ;
if(_useIntensityForRho){
/* while(theta < -M_PI) theta += 2*M_PI ;
while(theta >= M_PI) theta -= 2*M_PI ;
// If theta is between -90 and -180 get the equivalent
// between 0 and 90
if(theta <-M_PI/2)
{
theta += M_PI ;
rho *= -1 ;
}
// If theta is between 90 and 180 get the equivalent
// between 0 and -90
if(theta >M_PI/2)
{
theta -= M_PI ;
rho *= -1 ;
}
*/
// convention pour choisir le signe de rho
int i,j ;
i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat )/2) ;
j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat )/2) ;
int end = false ;
int incr = 10 ;
int i1=0,i2=0,j1=0,j2=0 ;
unsigned char v1=0,v2=0 ;
int width_ = (int)I.getWidth();
int height_ = (int)I.getHeight();
update_indices(theta,i,j,incr,i1,i2,j1,j2);
if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
j1<0 || j1>=width_ || j2<0 || j2>=width_){
double rho_lim1 = fabs((double)i/cos(theta));
double rho_lim2 = fabs((double)j/sin(theta));
double co_rho_lim1 = fabs(((double)(height_-i))/cos(theta));
double co_rho_lim2 = fabs(((double)(width_-j))/sin(theta));
double rho_lim = std::min(rho_lim1,rho_lim2);
double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
if(incr<INCR_MIN){
vpERROR_TRACE("increment is too small") ;
throw(vpTrackingException(vpTrackingException::fatalError,
"increment is too small")) ;
}
update_indices(theta,i,j,incr,i1,i2,j1,j2);
}
while (!end)
{
end = true;
unsigned int i1_ = static_cast<unsigned int>(i1);
unsigned int j1_ = static_cast<unsigned int>(j1);
unsigned int i2_ = static_cast<unsigned int>(i2);
unsigned int j2_ = static_cast<unsigned int>(j2);
v1=I[i1_][j1_];
v2=I[i2_][j2_];
if (abs(v1-v2) < 1)
{
incr-- ;
end = false ;
if (incr==1)
{
std::cout << "In CStraightLine::GetParameters() " ;
std::cout << " Error Tracking " << abs(v1-v2) << std::endl ;
}
}
update_indices(theta,i,j,incr,i1,i2,j1,j2);
}
if (theta >=0 && theta <= M_PI/2)
{
if (v2 < v1)
{
theta += M_PI;
rho *= -1;
}
}
else
{
double jinter;
jinter = -c/b;
if (v2 < v1)
{
theta += M_PI;
if (jinter > 0)
{
rho *= -1;
}
}
else
{
if (jinter < 0)
{
rho *= -1;
}
}
}
if (vpDEBUG_ENABLE(2))
{
vpImagePoint ip, ip1, ip2;
ip.set_i( i );
ip.set_j( j );
ip1.set_i( i1 );
ip1.set_j( j1 );
ip2.set_i( i2 );
ip2.set_j( j2 );
vpDisplay::displayArrow(I, ip, ip1, vpColor::green) ;
vpDisplay::displayArrow(I, ip, ip2, vpColor::red) ;
}
}
}
/*!
Track the line in the image I.
\param I : Image in which the line appears.
*/
void
vpMeLine::track(const vpImage<unsigned char> &I)
{
vpCDEBUG(1) <<"begin vpMeLine::track()"<<std::endl ;
// 1. On fait ce qui concerne les droites (peut etre vide)
{
}
// 2. On appelle ce qui n'est pas specifique
{
vpMeTracker::track(I) ;
}
// 3. On revient aux droites
{
// supression des points rejetes par les ME
suppressPoints() ;
setExtremities() ;
// Estimation des parametres de la droite aux moindres carre
try
{
leastSquare() ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
// recherche de point aux extremite de la droites
// dans le cas d'un glissement
seekExtremities(I) ;
setExtremities() ;
try
{
leastSquare() ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
// suppression des points rejetes par la regression robuste
suppressPoints() ;
setExtremities() ;
//reechantillonage si necessaire
reSample(I) ;
// remet a jour l'angle delta pour chaque point de la liste
updateDelta() ;
// Remise a jour de delta dans la liste de site me
if (vpDEBUG_ENABLE(2))
{
display(I,vpColor::red) ;
vpMeTracker::display(I) ;
vpDisplay::flush(I) ;
}
}
computeRhoTheta(I) ;
vpCDEBUG(1) <<"end vpMeLine::track()"<<std::endl ;
}
/*!
Seek along the line defined by its equation, the two extremities of
the line. This function is useful in case of translation of the
line.
\param I : Image in which the line appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeLine::seekExtremities(const vpImage<unsigned char> &I)
{
vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int rows = (int)I.getHeight() ;
int cols = (int)I.getWidth() ;
double n_sample;
//if (me->getSampleStep()==0)
if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
{
vpERROR_TRACE("function called with sample step = 0") ;
throw(vpTrackingException(vpTrackingException::fatalError,
"sample step = 0")) ;
}
// i, j portions of the line_p
double diffsi = PExt[0].ifloat-PExt[1].ifloat;
double diffsj = PExt[0].jfloat-PExt[1].jfloat;
double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
double dj = diffsj/sqrt(s) ;
double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
// number of samples along line_p
n_sample = length_p/(double)me->getSampleStep();
double sample_step = (double)me->getSampleStep();
vpMeSite P ;
P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
P.setDisplay(selectDisplay) ;
unsigned int memory_range = me->getRange() ;
me->setRange(1);
vpImagePoint ip;
for (int i=0 ; i < 3 ; i++)
{
P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
P.setDisplay(selectDisplay) ;
for (int i=0 ; i < 3 ; i++)
{
P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
me->setRange(memory_range);
vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
}
/*!
Construct a list of vpMeSite moving edges at a particular sampling
step between the two extremities of the line.
\param I : Image in which the line appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeLine::sample(const vpImage<unsigned char>& I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int rows = (int)I.getHeight() ;
int cols = (int)I.getWidth() ;
double n_sample;
if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
{
vpERROR_TRACE("function called with sample step = 0") ;
throw(vpTrackingException(vpTrackingException::fatalError,
"sample step = 0")) ;
}
// i, j portions of the line_p
double diffsi = PExt[0].ifloat-PExt[1].ifloat;
double diffsj = PExt[0].jfloat-PExt[1].jfloat;
double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
throw(vpTrackingException(vpTrackingException::fatalError,"points too close of each other to define a line")) ;
// number of samples along line_p
n_sample = length_p/(double)me->getSampleStep();
double stepi = diffsi/(double)n_sample;
double stepj = diffsj/(double)n_sample;
// Choose starting point
double is = PExt[1].ifloat;
double js = PExt[1].jfloat;
// Delete old list
list.clear();
// sample positions at i*me->getSampleStep() interval along the
// line_p, starting at PSiteExt[0]
vpImagePoint ip;
for(int i=0; i<=vpMath::round(n_sample); i++)
{
// If point is in the image, add to the sample list
if(!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols))
{
vpMeSite pix ; //= list.value();
pix.init((int)is, (int)js, delta, 0, sign) ;
pix.setDisplay(selectDisplay) ;
if(vpDEBUG_ENABLE(3))
{
ip.set_i( is );
ip.set_j( js );
vpDisplay::displayCross(I, ip, 2, vpColor::blue);
}
list.push_back(pix);
}
is += stepi;
js += stepj;
}
vpCDEBUG(1) << "end vpMeLine::sample() : ";
vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
}
/*!
Track the ellipse in the image I.
\param I : Image in which the ellipse appears.
*/
void
vpMeEllipse::track(const vpImage<unsigned char> &I)
{
vpCDEBUG(1) <<"begin vpMeEllipse::track()"<<std::endl ;
static int iter =0 ;
// 1. On fait ce qui concerne les ellipse (peut etre vide)
{
}
//vpDisplay::display(I) ;
// 2. On appelle ce qui n'est pas specifique
{
try{
vpMeTracker::track(I) ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
// std::cout << "number of signals " << numberOfSignal() << std::endl ;
}
// 3. On revient aux ellipses
{
// Estimation des parametres de la droite aux moindres carre
suppressPoints() ;
setExtremities() ;
try{
leastSquare() ; }
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
seekExtremities(I) ;
setExtremities() ;
try
{
leastSquare() ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
// suppression des points rejetes par la regression robuste
suppressPoints() ;
setExtremities() ;
//reechantillonage si necessaire
reSample(I) ;
// remet a jour l'angle delta pour chaque point de la liste
updateTheta() ;
computeMoments();
// Remise a jour de delta dans la liste de site me
if (vpDEBUG_ENABLE(2))
{
display(I,vpColor::red) ;
vpMeTracker::display(I) ;
vpDisplay::flush(I) ;
}
// computeAngle(iP1, iP2) ;
//
// if (iter%5==0)
// {
// sample(I) ;
// try{
// leastSquare() ; }
// catch(...)
// {
// vpERROR_TRACE("Error caught") ;
// throw ;
// }
// computeAngle(iP1, iP2) ;
// }
// seekExtremities(I) ;
//
// vpMeTracker::display(I) ;
// // vpDisplay::flush(I) ;
//
// // remet a jour l'angle theta pour chaque point de la liste
// updateTheta() ;
}
iter++ ;
vpCDEBUG(1) << "end vpMeEllipse::track()"<<std::endl ;
}
/*!
Seek along the ellipse edge defined by its equation, the two extremities of
the ellipse (ie the two points with the smallest and the biggest \f$ \alpha \f$ angle.
\param I : Image in which the ellipse appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int rows = (int)I.getHeight() ;
int cols = (int)I.getWidth() ;
vpImagePoint ip;
unsigned int memory_range = me->getRange() ;
me->setRange(2);
double memory_mu1 = me->getMu1();
me->setMu1(0.5);
double memory_mu2 = me->getMu2();
me->setMu2(0.5);
double incr = vpMath::rad(2.0) ;
if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
{
vpMeSite P;
double k = alpha1;
double i1,j1;
for (unsigned int i=0 ; i < 3 ; i++)
{
k -= incr;
//while ( k < -M_PI ) { k+=2*M_PI; }
i1 = b *cos(k) ; // equation of an ellipse
j1 = a *sin(k) ; // equation of an ellipse
P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
angle.push_back(k);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
k = alpha2;
for (unsigned int i=0 ; i < 3 ; i++)
{
k += incr;
//while ( k > M_PI ) { k-=2*M_PI; }
i1 = b *cos(k) ; // equation of an ellipse
j1 = a *sin(k) ; // equation of an ellipse
P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
angle.push_back(k);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
}
suppressPoints() ;
me->setRange(memory_range);
me->setMu1(memory_mu1);
me->setMu2(memory_mu2);
}
/*!
Construct a list of vpMeSite moving edges at a particular sampling
step between the two extremities. The two extremities are defined by
the points with the smallest and the biggest \f$ alpha \f$ angle.
\param I : Image in which the ellipse appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeEllipse::sample(const vpImage<unsigned char> & I)
{
vpCDEBUG(1) <<"begin vpMeEllipse::sample() : "<<std::endl ;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int height = (int)I.getHeight() ;
int width = (int)I.getWidth() ;
double n_sample;
//if (me->getSampleStep()==0)
if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
{
std::cout << "In vpMeEllipse::sample: " ;
std::cout << "function called with sample step = 0" ;
//return fatalError ;
}
double j, i;//, j11, i11;
vpImagePoint iP11;
j = i = 0.0 ;
double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
vpColor col = vpColor::red ;
getParameters() ;
// Delete old list
list.clear();
angle.clear();
// sample positions
double k = alpha1 ;
while (k<alpha2)
{
// j = a *cos(k) ; // equation of an ellipse
// i = b *sin(k) ; // equation of an ellipse
j = a *sin(k) ; // equation of an ellipse
i = b *cos(k) ; // equation of an ellipse
// (i,j) are the coordinates on the origin centered ellipse ;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
// iP11.set_j( iPc.get_j() + ce *j - se *i );
// iP11.set_i( iPc.get_i() -( se *j + ce *i) );
iP11.set_j( iPc.get_j() + ce *j + se *i );
iP11.set_i( iPc.get_i() - se *j + ce *i );
vpDisplay::displayCross(I, iP11, 5, col) ;
double theta ;
computeTheta(theta, K, iP11) ;
// If point is in the image, add to the sample list
if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
{
vpMeSite pix ;
pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
pix.setDisplay(selectDisplay) ;
pix.setState(vpMeSite::NO_SUPPRESSION);
if(vpDEBUG_ENABLE(3))
{
vpDisplay::displayCross(I,iP11, 5, vpColor::blue);
}
list.push_back(pix);
angle.push_back(k);
}
k += incr ;
}
vpMeTracker::initTracking(I) ;
n_sample = (unsigned int)list.size() ;
vpCDEBUG(1) << "end vpMeEllipse::sample() : " ;
vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
}
