/*!
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 ;
}
/*!
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 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 ;
}
/*!
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 ;
}