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 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
159 "Moving edges not initialized")) ;
166 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
170 "sample step = 0")) ;
178 if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
183 double stepi = diffsi/(double)n_sample;
184 double stepj = diffsj/(double)n_sample;
203 pix.
init((
int)is, (
int)js, delta, 0,
sign) ;
220 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
221 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
256 std::cout <<
"Click on the line first point..." <<std::endl ;
260 std::cout <<
"Click on the line second point..." <<std::endl ;
304 unsigned int iter =0 ;
305 unsigned int nos_1 = 0 ;
306 double distance = 100;
313 "not enough point")) ;
322 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
333 while (iter < 4 && distance > 0.05)
344 for (i=0 ; i < nos_1 ; i++)
350 distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
355 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
387 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
398 while (iter < 4 && distance > 0.05)
409 for (i=0 ; i < nos_1 ; i++)
415 distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
421 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
447 normalizeAngle(delta) ;
466 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()"<<std::endl ;
468 int i1s, j1s, i2s, j2s;
485 double angle = atan2((
double)(i1s-i2s),(
double)(j1s-j2s)) ;
492 computeDelta(delta,i1s,j1s,i2s,j2s) ;
512 vpCDEBUG(1) <<
" end vpMeLine::initTracking()"<<std::endl ;
523 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ){
547 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
567 if (fabs(imin-imax) < 25)
569 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
603 vpCDEBUG(1) <<
"begin vpMeLine::sample() : "<<std::endl ;
606 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
608 "Moving edges not initialized")) ;
616 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
621 "sample step = 0")) ;
630 double di = diffsi/sqrt(s) ;
631 double dj = diffsj/sqrt(s) ;
648 for (
int i=0 ; i < 3 ; i++)
679 for (
int i=0 ; i < 3 ; i++)
709 vpCDEBUG(1) <<
"end vpMeLine::sample() : " ;
710 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
731 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
733 "Moving edges not initialized")) ;
736 project(a,b,c,
PExt[0].ifloat,
PExt[0].jfloat,i1,j1) ;
737 project(a,b,c,
PExt[1].ifloat,
PExt[1].jfloat,i2,j2) ;
750 if ((
double)n<0.9*expecteddensity)
752 double delta_new =
delta;
772 double angle = delta + M_PI/2;
775 while (angle<0) angle += M_PI;
776 while (angle>M_PI) angle -= M_PI;
781 if(std::fabs(std::fabs(angle) - 180) <= std::numeric_limits<double>::epsilon())
793 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
812 vpCDEBUG(1) <<
"begin vpMeLine::track()"<<std::endl ;
880 vpCDEBUG(1) <<
"end vpMeLine::track()"<<std::endl ;
883 void vpMeLine::update_indices(
double theta,
int i,
int j,
int incr,
int& i1,
int& i2,
int& j1,
int& j2){
884 i1 = (int)(i + cos(theta) *incr) ;
885 j1 = (int)(j + sin(theta) *incr) ;
887 i2 = (int)(i - cos(theta) *incr) ;
888 j2 = (int)(j - sin(theta) *incr) ;
905 while (theta >= M_PI) theta -=M_PI ;
906 while (theta < 0) theta +=M_PI ;
937 int i1=0,i2=0,j1=0,j2=0 ;
938 unsigned char v1=0,v2=0 ;
942 update_indices(theta,i,j,incr,i1,i2,j1,j2);
944 if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
945 j1<0 || j1>=width_ || j2<0 || j2>=width_){
946 double rho_lim1 = fabs((
double)i/cos(theta));
947 double rho_lim2 = fabs((
double)j/sin(theta));
949 double co_rho_lim1 = fabs(((
double)(height_-i))/cos(theta));
950 double co_rho_lim2 = fabs(((
double)(width_-j))/sin(theta));
952 double rho_lim = std::min(rho_lim1,rho_lim2);
953 double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
954 incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
958 "increment is too small")) ;
960 update_indices(theta,i,j,incr,i1,i2,j1,j2);
966 unsigned int i1_ =
static_cast<unsigned int>(i1);
967 unsigned int j1_ =
static_cast<unsigned int>(j1);
968 unsigned int i2_ =
static_cast<unsigned int>(i2);
969 unsigned int j2_ =
static_cast<unsigned int>(j2);
979 std::cout <<
"In CStraightLine::GetParameters() " ;
980 std::cout <<
" Error Tracking " << abs(v1-v2) << std::endl ;
983 update_indices(theta,i,j,incr,i1,i2,j1,j2);
986 if (theta >=0 && theta <= M_PI/2)
1095 double a1 = line1.
a;
1096 double b1 = line1.
b;
1097 double c1 = line1.
c;
1098 double a2 = line2.
a;
1099 double b2 = line2.
b;
1100 double c2 = line2.
c;
1107 denom = (-(a2/a1) * b1 + b2);
1110 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
1112 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1117 if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
1119 j = ( (a2/a1)*c1 - c2 ) / denom;
1120 i = (-b1*j - c1) / a1;
1126 denom = (-(b2/b1) * a1 + a2);
1129 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
1131 std::cout <<
"!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1136 if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
1138 i = ( (b2/b1)*c1 - c2 ) / denom;
1139 j = (-a1*i - c1) / b1;
1173 const double &A,
const double &B,
const double &C,
1174 const vpColor &color,
unsigned int thickness)
1178 if (fabs(A) < fabs(B)) {
1179 double i1, j1, i2, j2;
1181 j1 = (-A*i1 -C) / B;
1183 j2 = (-A*i2 -C) / B;
1194 double i1, j1, i2, j2;
1196 i1 = -(B * j1 + C) / A;
1198 i2 = -(B * j2 + C) / A;
1237 const double &A,
const double &B,
const double &C,
1238 const vpColor &color,
unsigned int thickness)
1242 if (fabs(A) < fabs(B)) {
1243 double i1, j1, i2, j2;
1245 j1 = (-A*i1 -C) / B;
1247 j2 = (-A*i2 -C) / B;
1258 double i1, j1, i2, j2;
1260 i1 = -(B * j1 + C) / A;
1262 i2 = -(B * j2 + C) / A;
1303 const std::list<vpMeSite> &site_list,
1304 const double &A,
const double &B,
const double &C,
1305 const vpColor &color,
unsigned int thickness)
1309 for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
1324 if (fabs(A) < fabs(B)) {
1325 double i1, j1, i2, j2;
1327 j1 = (-A*i1 -C) / B;
1329 j2 = (-A*i2 -C) / B;
1340 double i1, j1, i2, j2;
1342 i1 = -(B * j1 + C) / A;
1344 i2 = -(B * j2 + C) / A;
1385 const std::list<vpMeSite> &site_list,
1386 const double &A,
const double &B,
const double &C,
1387 const vpColor &color,
unsigned int thickness)
1391 for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
1406 if (fabs(A) < fabs(B)) {
1407 double i1, j1, i2, j2;
1409 j1 = (-A*i1 -C) / B;
1411 j2 = (-A*i2 -C) / B;
1422 double i1, j1, i2, j2;
1424 i1 = -(B * j1 + C) / A;
1426 i2 = -(B * j2 + C) / A;