43 #include <visp/vpConfig.h>
44 #ifndef DOXYGEN_SHOULD_SKIP_THIS
53 #include <visp/vpMbtMeLine.h>
54 #include <visp/vpTrackingException.h>
55 #include <visp/vpRobust.h>
59 normalizeAngle(
double &delta)
61 while (delta > M_PI) { delta -= M_PI ; }
62 while (delta < -M_PI) { delta += M_PI ; }
98 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()"<<std::endl ;
120 delta = - theta + M_PI/2.0;
121 normalizeAngle(delta);
133 vpCDEBUG(1) <<
" end vpMeLine::initTracking()"<<std::endl ;
151 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
155 "sample step = 0")) ;
167 double stepi = diffsi/(double)n_sample;
168 double stepj = diffsj/(double)n_sample;
171 double is = PExt[1].
ifloat;
172 double js = PExt[1].
jfloat;
187 pix.
init((
int)is, (
int)js, delta, 0, sign) ;
207 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
208 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
220 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ){
223 if (fabs(sin(theta)) > 0.9)
231 else if (fabs(cos(theta)) > 0.9)
263 vpCDEBUG(1) <<
"begin vpMeLine::sample() : "<<std::endl ;
270 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
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) ;
299 for (
int i=0 ; i < 3 ; i++)
324 P.
init((
int) PExt[1].ifloat, (
int)PExt[1].jfloat, delta_1, 0, sign) ;
326 for (
int i=0 ; i < 3 ; i++)
354 vpCDEBUG(1) <<
"end vpMeLine::sample() : " ;
355 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
376 if ((
double)n<0.5*expecteddensity && n > 0)
378 double delta_new = delta;
407 size_t n =
list.size();
410 if ((
double)n<0.5*expecteddensity && n > 0)
412 double delta_new = delta;
428 vpMbtMeLine::updateDelta()
435 if(std::fabs(std::fabs(theta) - M_PI) <=
vpMath::maximum(std::fabs(theta), (
double)M_PI)*std::numeric_limits<double>::epsilon() )
440 diff = fabs(theta - theta_1);
446 delta = - theta + M_PI/2.0;
447 normalizeAngle(delta);
449 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
546 vpMbtMeLine::setExtremities()
554 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
569 if ( !
list.empty() )
577 if (fabs(imin-imax) < 25)
579 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
614 vpMbtMeLine::bubbleSortI()
617 unsigned int nbElmt =
list.size();
618 for (
unsigned int pass = 1; pass < nbElmt; pass++)
621 for (
unsigned int i=0; i < nbElmt-pass; i++)
641 vpMbtMeLine::bubbleSortJ()
644 unsigned int nbElmt =
list.size();
645 for(
unsigned int pass=1; pass < nbElmt; pass++)
648 for (
unsigned int i=0; i < nbElmt-pass; i++)
666 vpImagePoint itest(PExt[0].ifloat+(PExt[1].ifloat-PExt[0].ifloat)/2, PExt[0].jfloat+(PExt[1].jfloat-PExt[0].jfloat)/2);
669 pix.
init(itest.get_i(), itest.get_j(), delta, 0, sign);
673 unsigned int range = me->
getRange();
681 for(
unsigned int n = 0 ; n < 2 * range + 1 ; n++)
685 delete [] list_query_pixels;