50 #include <visp/vpMeTracker.h>
51 #include <visp/vpMe.h>
52 #include <visp/vpMeSite.h>
53 #include <visp/vpMeLine.h>
54 #include <visp/vpRobust.h>
55 #include <visp/vpTrackingException.h>
56 #include <visp/vpImagePoint.h>
57 #include <visp/vpMath.h>
64 normalizeAngle(
double &delta)
66 while (delta > M_PI) { delta -= M_PI ; }
67 while (delta < -M_PI) { delta += M_PI ; }
71 computeDelta(
double &delta,
int i1,
int j1,
int i2,
int j2)
74 double B = double(i1-i2) ;
75 double A = double(j1-j2) ;
79 normalizeAngle(delta) ;
84 project(
double a,
double b,
double c,
85 double i,
double j,
double &ip,
double &jp)
119 delta = meline.
delta;
157 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
161 "sample step = 0")) ;
169 if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
174 double stepi = diffsi/(double)n_sample;
175 double stepj = diffsj/(double)n_sample;
194 pix.
init((
int)is, (
int)js, delta, 0,
sign) ;
211 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
212 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
247 std::cout <<
"Click on the line first point..." <<std::endl ;
249 std::cout <<
"Click on the line second point..." <<std::endl ;
291 unsigned int iter =0 ;
292 unsigned int nos_1 = 0 ;
293 double distance = 100;
300 "not enough point")) ;
309 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
320 while (iter < 4 && distance > 0.05)
331 for (i=0 ; i < nos_1 ; i++)
337 distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
342 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
374 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
385 while (iter < 4 && distance > 0.05)
396 for (i=0 ; i < nos_1 ; i++)
402 distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
408 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
434 normalizeAngle(delta) ;
453 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()"<<std::endl ;
455 int i1s, j1s, i2s, j2s;
472 double angle = atan2((
double)(i1s-i2s),(
double)(j1s-j2s)) ;
479 computeDelta(delta,i1s,j1s,i2s,j2s) ;
499 vpCDEBUG(1) <<
" end vpMeLine::initTracking()"<<std::endl ;
510 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ){
534 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
554 if (fabs(imin-imax) < 25)
556 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
588 vpCDEBUG(1) <<
"begin vpMeLine::sample() : "<<std::endl ;
595 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
600 "sample step = 0")) ;
609 double di = diffsi/sqrt(s) ;
610 double dj = diffsj/sqrt(s) ;
627 for (
int i=0 ; i < 3 ; i++)
658 for (
int i=0 ; i < 3 ; i++)
688 vpCDEBUG(1) <<
"end vpMeLine::sample() : " ;
689 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
710 project(a,b,c,
PExt[0].ifloat,
PExt[0].jfloat,i1,j1) ;
711 project(a,b,c,
PExt[1].ifloat,
PExt[1].jfloat,i2,j2) ;
724 if ((
double)n<0.9*expecteddensity)
726 double delta_new =
delta;
746 double angle = delta + M_PI/2;
749 while (angle<0) angle += M_PI;
750 while (angle>M_PI) angle -= M_PI;
755 if(std::fabs(std::fabs(angle) - 180) <= std::numeric_limits<double>::epsilon())
767 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
786 vpCDEBUG(1) <<
"begin vpMeLine::track()"<<std::endl ;
854 vpCDEBUG(1) <<
"end vpMeLine::track()"<<std::endl ;
857 void vpMeLine::update_indices(
double theta,
int i,
int j,
int incr,
int& i1,
int& i2,
int& j1,
int& j2){
858 i1 = (int)(i + cos(theta) *incr) ;
859 j1 = (int)(j + sin(theta) *incr) ;
861 i2 = (int)(i - cos(theta) *incr) ;
862 j2 = (int)(j - sin(theta) *incr) ;
879 while (theta >= M_PI) theta -=M_PI ;
880 while (theta < 0) theta +=M_PI ;
911 int i1=0,i2=0,j1=0,j2=0 ;
912 unsigned char v1=0,v2=0 ;
916 update_indices(theta,i,j,incr,i1,i2,j1,j2);
918 if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
919 j1<0 || j1>=width_ || j2<0 || j2>=width_){
920 double rho_lim1 = fabs((
double)i/cos(theta));
921 double rho_lim2 = fabs((
double)j/sin(theta));
923 double co_rho_lim1 = fabs(((
double)(height_-i))/cos(theta));
924 double co_rho_lim2 = fabs(((
double)(width_-j))/sin(theta));
926 double rho_lim = std::min(rho_lim1,rho_lim2);
927 double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
928 incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
932 "increment is too small")) ;
934 update_indices(theta,i,j,incr,i1,i2,j1,j2);
940 unsigned int i1_ =
static_cast<unsigned int>(i1);
941 unsigned int j1_ =
static_cast<unsigned int>(j1);
942 unsigned int i2_ =
static_cast<unsigned int>(i2);
943 unsigned int j2_ =
static_cast<unsigned int>(j2);
953 std::cout <<
"In CStraightLine::GetParameters() " ;
954 std::cout <<
" Error Tracking " << abs(v1-v2) << std::endl ;
957 update_indices(theta,i,j,incr,i1,i2,j1,j2);
960 if (theta >=0 && theta <= M_PI/2)
1069 double a1 = line1.
a;
1070 double b1 = line1.
b;
1071 double c1 = line1.
c;
1072 double a2 = line2.
a;
1073 double b2 = line2.
b;
1074 double c2 = line2.
c;
1081 denom = (-(a2/a1) * b1 + b2);
1084 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
1086 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1091 if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
1093 j = ( (a2/a1)*c1 - c2 ) / denom;
1094 i = (-b1*j - c1) / a1;
1100 denom = (-(b2/b1) * a1 + a2);
1103 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
1105 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1110 if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
1112 i = ( (b2/b1)*c1 - c2 ) / denom;
1113 j = (-a1*i - c1) / b1;
1147 const double &A,
const double &B,
const double &C,
1148 const vpColor &color,
unsigned int thickness)
1152 if (fabs(A) < fabs(B)) {
1153 double i1, j1, i2, j2;
1155 j1 = (-A*i1 -C) / B;
1157 j2 = (-A*i2 -C) / B;
1168 double i1, j1, i2, j2;
1170 i1 = -(B * j1 + C) / A;
1172 i2 = -(B * j2 + C) / A;
1211 const double &A,
const double &B,
const double &C,
1212 const vpColor &color,
unsigned int thickness)
1216 if (fabs(A) < fabs(B)) {
1217 double i1, j1, i2, j2;
1219 j1 = (-A*i1 -C) / B;
1221 j2 = (-A*i2 -C) / B;
1232 double i1, j1, i2, j2;
1234 i1 = -(B * j1 + C) / A;
1236 i2 = -(B * j2 + C) / A;
1277 const std::list<vpMeSite> &site_list,
1278 const double &A,
const double &B,
const double &C,
1279 const vpColor &color,
unsigned int thickness)
1283 for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
1298 if (fabs(A) < fabs(B)) {
1299 double i1, j1, i2, j2;
1301 j1 = (-A*i1 -C) / B;
1303 j2 = (-A*i2 -C) / B;
1314 double i1, j1, i2, j2;
1316 i1 = -(B * j1 + C) / A;
1318 i2 = -(B * j2 + C) / A;
1359 const std::list<vpMeSite> &site_list,
1360 const double &A,
const double &B,
const double &C,
1361 const vpColor &color,
unsigned int thickness)
1365 for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
1380 if (fabs(A) < fabs(B)) {
1381 double i1, j1, i2, j2;
1383 j1 = (-A*i1 -C) / B;
1385 j2 = (-A*i2 -C) / B;
1396 double i1, j1, i2, j2;
1398 i1 = -(B * j1 + C) / A;
1400 i2 = -(B * j2 + C) / A;