39 #include <visp3/core/vpConfig.h> 40 #ifndef DOXYGEN_SHOULD_SKIP_THIS 50 #include <visp3/mbt/vpMbtMeLine.h> 51 #include <visp3/core/vpTrackingException.h> 52 #include <visp3/core/vpRobust.h> 56 normalizeAngle(
double &delta)
58 while (delta > M_PI) { delta -= M_PI ; }
59 while (delta < -M_PI) { delta += M_PI ; }
66 vpMbtMeLine::vpMbtMeLine()
67 : rho(0.), theta(0.), theta_1(M_PI/2), delta(0.), delta_1(0), sign(1),
68 a(0.), b(0.), c(0.), imin(0), imax(0), jmin(0), jmax(0),
76 vpMbtMeLine::~vpMbtMeLine()
95 double rho_,
double theta_)
97 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()"<<std::endl ;
103 PExt[0].ifloat = (float)ip1.
get_i() ;
104 PExt[0].jfloat = (float)ip1.
get_j() ;
105 PExt[1].ifloat = (float)ip2.
get_i() ;
106 PExt[1].jfloat = (float)ip2.
get_j() ;
109 this->theta = theta_;
116 delta = - theta + M_PI/2.0;
117 normalizeAngle(delta);
121 expecteddensity = (double)list.size();
129 vpCDEBUG(1) <<
" end vpMeLine::initTracking()"<<std::endl ;
147 if (std::fabs(me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
150 "Function vpMbtMeLine::sample() called with moving-edges sample step = 0")) ;
154 double diffsi = PExt[0].ifloat-PExt[1].ifloat;
155 double diffsj = PExt[0].jfloat-PExt[1].jfloat;
162 double stepi = diffsi/(double)n_sample;
163 double stepj = diffsj/(double)n_sample;
166 double is = PExt[1].ifloat;
167 double js = PExt[1].jfloat;
182 pix.
init((
int)is, (
int)js, delta, 0, sign) ;
184 pix.
track(I, me,
false);
202 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
203 vpCDEBUG(1) << list.size() <<
" point inserted in the list " << std::endl ;
215 for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
218 if (fabs(sin(theta)) > 0.9)
220 if ((s.
i < imin) ||(s.
i > imax))
226 else if (fabs(cos(theta)) > 0.9)
228 if ((s.
j < jmin) || (s.
j > jmax))
236 if ((s.
i < imin) ||(s.
i > imax) || (s.
j < jmin) || (s.
j > jmax) )
264 vpCDEBUG(1) <<
"begin vpMeLine::sample() : "<<std::endl ;
271 if (std::fabs(me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
274 "Function called with sample step = 0")) ;
278 double diffsi = PExt[0].ifloat-PExt[1].ifloat;
279 double diffsj = PExt[0].jfloat-PExt[1].jfloat;
283 double di = diffsi/sqrt(s) ;
284 double dj = diffsj/sqrt(s) ;
286 double length_p = sqrt(s);
293 P.
init((
int) PExt[0].ifloat, (
int)PExt[0].jfloat, delta_1, 0, sign) ;
296 unsigned int memory_range = me->
getRange() ;
299 for (
int i=0 ; i < 3 ; i++)
304 if ((P.
i < imin) ||(P.
i > imax) || (P.
j < jmin) || (P.
j > jmax) )
311 P.
track(I,me,
false) ;
323 P.
init((
int) PExt[1].ifloat, (
int)PExt[1].jfloat, delta_1, 0, sign) ;
325 for (
int i=0 ; i < 3 ; i++)
330 if ((P.
i < imin) ||(P.
i > imax) || (P.
j < jmin) || (P.
j > jmax) )
338 P.
track(I,me,
false) ;
352 vpCDEBUG(1) <<
"end vpMeLine::sample() : " ;
353 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
366 vpMbtMeLine::computeProjectionError(
const vpImage<unsigned char>& _I,
double &_sumErrorRad,
unsigned int &_nbFeatures)
370 double deltaNormalized = theta;
371 unsigned int iter = 0;
373 while (deltaNormalized<0) deltaNormalized += M_PI;
374 while (deltaNormalized>M_PI) deltaNormalized -= M_PI;
377 vecLine[0] = cos(deltaNormalized);
378 vecLine[1] = sin(deltaNormalized);
469 double offset = std::floor(filterX.getRows() / 2.0f);
471 for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
472 if(iter != 0 && iter+1 != list.size()){
473 double gradientX = 0;
474 double gradientY = 0;
476 double iSite = it->ifloat;
477 double jSite = it->jfloat;
479 for(
unsigned int i = 0; i<filterX.getRows() ; i++){
480 double iImg = iSite + (i - offset);
481 for (
unsigned int j = 0; j< filterX.getCols(); j++){
482 double jImg = jSite + (j-offset);
484 if(iImg < 0) iImg = 0.0;
485 if(jImg < 0) jImg = 0.0;
490 gradientX += filterX[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
494 for(
unsigned int i = 0; i<filterY.getRows() ; i++){
495 double iImg = iSite + (i - offset);
496 for (
unsigned int j = 0; j< filterY.getCols(); j++){
497 double jImg = jSite + (j-offset);
499 if(iImg < 0) iImg = 0.0;
500 if(jImg < 0) jImg = 0.0;
505 gradientY += filterY[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
509 double angle = atan2(gradientX,gradientY);
510 while (angle<0) angle += M_PI;
511 while (angle>M_PI) angle -= M_PI;
514 vecGrad[0] = cos(angle);
515 vecGrad[1] = sin(angle);
518 double angle1 = acos(vecLine * vecGrad);
519 double angle2 = acos(vecLine * (-vecGrad));
524 _sumErrorRad += std::min(angle1,angle2);
552 unsigned int n = numberOfSignal() ;
554 if ((
double)n<0.5*expecteddensity && n > 0)
556 double delta_new = delta;
559 expecteddensity = (double)list.size();
584 size_t n = list.size();
586 if ((
double)n<0.5*expecteddensity /*&& n > 0*/)
588 double delta_new = delta;
590 PExt[0].ifloat = (float)ip1.
get_i() ;
591 PExt[0].jfloat = (float)ip1.
get_j() ;
592 PExt[1].ifloat = (float)ip2.
get_i() ;
593 PExt[1].jfloat = (float)ip2.
get_j() ;
595 expecteddensity = (double)list.size();
605 vpMbtMeLine::updateDelta()
612 if(std::fabs(std::fabs(theta) - M_PI) <=
vpMath::maximum(std::fabs(theta), (
double)M_PI)*std::numeric_limits<double>::epsilon() )
617 diff = fabs(theta - theta_1);
623 delta = - theta + M_PI/2.0;
624 normalizeAngle(delta);
626 for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
670 this->theta = theta_;
699 double rho_,
double theta_)
702 this->theta = theta_;
724 vpMbtMeLine::setExtremities()
726 double i_min = +1e6 ;
732 for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
747 if ( ! list.empty() )
749 PExt[0].ifloat = i_min ;
750 PExt[0].jfloat = j_min ;
751 PExt[1].ifloat = i_max ;
752 PExt[1].jfloat = j_max ;
755 if (fabs(i_min-i_max) < 25)
757 for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
774 PExt[0].ifloat = i_min ;
775 PExt[0].jfloat = j_min ;
776 PExt[1].ifloat = i_max ;
777 PExt[1].jfloat = j_max ;
792 vpMbtMeLine::bubbleSortI()
795 unsigned int nbElmt = list.size();
796 for (
unsigned int pass = 1; pass < nbElmt; pass++)
799 for (
unsigned int i=0; i < nbElmt-pass; i++)
819 vpMbtMeLine::bubbleSortJ()
822 unsigned int nbElmt = list.size();
823 for(
unsigned int pass=1; pass < nbElmt; pass++)
826 for (
unsigned int i=0; i < nbElmt-pass; i++)
844 vpImagePoint itest(PExt[0].ifloat+(PExt[1].ifloat-PExt[0].ifloat)/2, PExt[0].jfloat+(PExt[1].jfloat-PExt[0].jfloat)/2);
847 pix.
init(itest.get_i(), itest.get_j(), delta, 0, sign);
851 unsigned int range = p_me->
getRange();
859 for(
unsigned int n = 0 ; n < 2 * range + 1 ; n++)
861 conv[n] = list_query_pixels[n].
convolution(I, p_me);
863 delete [] list_query_pixels;
Implementation of a matrix and operations on matrices.
vpMeSiteState getState() const
#define vpDEBUG_ENABLE(level)
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
static const vpColor green
static int round(const double x)
unsigned int getMaskSize() const
static Type maximum(const Type &a, const Type &b)
static const vpColor orange
void set_i(const double ii)
Error that can be emited by the vpTracker class and its derivates.
static const vpColor cyan
double getSampleStep() const
static double sqr(double x)
void setDisplay(vpMeSiteDisplayType select)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
void setState(const vpMeSiteState &flag)
void set_j(const double jj)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
unsigned int getHeight() const
Implementation of column vector and the associated operations.
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
void initTracking(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void setRange(const unsigned int &r)
unsigned int getWidth() const
unsigned int getRange() const
static const vpColor blue