44 #include <visp/vpMeEllipse.h>
46 #include <visp/vpMe.h>
47 #include <visp/vpRobust.h>
48 #include <visp/vpTrackingException.h>
49 #include <visp/vpDebug.h>
50 #include <visp/vpImagePoint.h>
65 double i = iP.
get_i();
66 double j = iP.
get_j();
68 double A = 2*i+2*K[1]*j + 2*K[2] ;
69 double B = 2*K[0]*j + 2*K[1]*i + 2*K[3];
73 while (theta > M_PI) { theta -= M_PI ; }
74 while (theta < 0) { theta += M_PI ; }
83 vpCDEBUG(1) <<
"begin vpMeEllipse::vpMeEllipse() " << std::endl ;
104 vpCDEBUG(1) <<
"end vpMeEllipse::vpMeEllipse() " << std::endl ;
144 vpCDEBUG(1) <<
"begin vpMeEllipse::~vpMeEllipse() " << std::endl ;
149 vpCDEBUG(1) <<
"end vpMeEllipse::~vpMeEllipse() " << std::endl ;
166 vpCDEBUG(1) <<
"begin vpMeEllipse::sample() : "<<std::endl ;
169 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
171 "Moving edges not initialized")) ;
180 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
182 std::cout <<
"In vpMeEllipse::sample: " ;
183 std::cout <<
"function called with sample step = 0" ;
222 computeTheta(theta, K, iP11) ;
244 n_sample = (
unsigned int)
list.size() ;
246 vpCDEBUG(1) <<
"end vpMeEllipse::sample() : " ;
247 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
269 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
271 "Moving edges not initialized")) ;
276 if ((
double)n<0.9*expecteddensity){
289 vpMeEllipse::getParameters()
293 for (
unsigned int i=0 ; i < 5 ; i++)
297 double d = k[2]*k[2] - k[0]*k[1];
300 iPc.
set_i( (k[1] * k[3] - k[2] * k[4]) / d );
301 iPc.
set_j( (k[0] * k[4] - k[2] * k[3]) / d );
310 e = (k[1] - k[0] + sq) / (2.0*k[2]);
316 if(
e < 0.0)
e += M_PI ;
322 double a2 = num / (k[0] + k[1] + sq ) ;
323 double b2 = num / (k[0] + k[1] - sq ) ;
336 std::cout <<
"K" << std::endl ;
338 std::cout <<
iPc << std::endl ;
354 double j1, i1, j11, i11;
357 int number_of_points = 2000 ;
358 double incr = 2 * M_PI / number_of_points ;
417 vpMeEllipse::updateTheta()
421 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
426 computeTheta(theta, K, iP) ;
436 vpMeEllipse::suppressPoints()
439 std::list<vpMeSite>::iterator itList =
list.begin();
440 for(std::list<double>::iterator it=
angle.begin(); it!=
angle.end(); ){
444 itList =
list.erase(itList) ;
445 it =
angle.erase(it);
469 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
471 "Moving edges not initialized")) ;
496 for (
unsigned int i=0 ; i < 3 ; i++)
533 for (
unsigned int i=0 ; i < 3 ; i++)
581 vpMeEllipse::setExtremities()
583 double alphamin = +1e6;
584 double alphamax = -1e6;
591 std::list<double>::const_iterator itAngle =
angle.begin();
593 for(std::list<vpMeSite>::const_iterator itList=
list.begin(); itList!=
list.end(); ++itList){
595 double alpha = *itAngle;
596 if (alpha < alphamin)
603 if (alpha > alphamax)
625 vpMeEllipse::leastSquare()
634 unsigned int iter =0 ;
651 "not enough point")) ;
660 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
685 r.setIteration(iter) ;
689 for (i=0 ; i < nos_1 ; i++)
698 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
711 for(i = 0; i < 5; i ++)
721 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
744 r.setIteration(iter) ;
748 for (i=0 ; i < nos_1 ; i++)
757 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
770 for(i = 0; i < 3; i ++)
802 vpCDEBUG(1) <<
" begin vpMeEllipse::initTracking()"<<std::endl ;
804 const unsigned int n=5 ;
807 for (
unsigned int k =0 ; k < n ; k++)
809 std::cout <<
"Click points "<< k+1 <<
"/" << n ;
810 std::cout <<
" on the ellipse in the trigonometric order" <<std::endl ;
814 std::cout << iP[k] << std::endl;
838 vpCDEBUG(1) <<
" begin vpMeEllipse::initTracking()"<<std::endl ;
850 for (
unsigned int k =0 ; k < n ; k++)
854 A[k][2] = 2* iP[k].
get_i() ;
855 A[k][3] = 2* iP[k].
get_j() ;
862 std::cout << K << std::endl;
871 for (
unsigned int k =0 ; k < n ; k++)
873 A[k][0] = 2* iP[k].
get_i() ;
874 A[k][1] = 2* iP[k].
get_j() ;
887 std::cout << K << std::endl;
926 vpCDEBUG(1) <<
"begin vpMeEllipse::track()"<<std::endl ;
1024 vpCDEBUG(1) <<
"end vpMeEllipse::track()"<<std::endl ;
1035 vpMeEllipse::computeMoments()
1037 double tane = tan(-1/
e);
1056 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1062 vpMeEllipse::computeAngle(
int ip1,
int jp1,
double &_alpha1,
1063 int ip2,
int jp2,
double &_alpha2)
1067 double j1, i1, j11, i11;
1070 int number_of_points = 2000 ;
1071 double incr = 2 * M_PI / number_of_points ;
1073 double dmin1 = 1e6 ;
1074 double dmin2 = 1e6 ;
1116 vpMeEllipse::computeAngle(
int ip1,
int jp1,
int ip2,
int jp2)
1120 computeAngle(ip1,jp1,a1, ip2, jp2,a2) ;
1126 unsigned *i,
unsigned *j)
1128 vpCDEBUG(1) <<
" begin vpMeEllipse::initTracking()"<<std::endl ;
1140 for (
unsigned int k =0 ; k < n ; k++)
1143 A[k][1] = 2* i[k] * j[k] ;
1152 std::cout << K << std::endl;
1161 for (
unsigned int k =0 ; k < n ; k++)
1177 std::cout << K << std::endl;
1208 #endif // Deprecated
1231 const double &A,
const double &B,
const double &E,
1232 const double & smallalpha,
const double &highalpha,
1239 j1 = j2 = i1 = i2 = 0 ;
1245 double k = smallalpha ;
1246 while (k+incr<highalpha)
1251 j2 = A *cos(k+incr) ;
1252 i2 = B *sin(k+incr) ;
1257 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1258 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1260 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1261 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );
1268 j1 = A *cos(smallalpha) ;
1269 i1 = B *sin(smallalpha) ;
1271 j2 = A *cos(highalpha) ;
1272 i2 = B *sin(highalpha) ;
1277 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1278 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1280 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1281 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );
1308 const double &A,
const double &B,
const double &E,
1309 const double & smallalpha,
const double &highalpha,
1316 j1 = j2 = i1 = i2 = 0 ;
1322 double k = smallalpha ;
1323 while (k+incr<highalpha)
1328 j2 = A *cos(k+incr) ;
1329 i2 = B *sin(k+incr) ;
1334 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1335 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1337 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1338 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );
1345 j1 = A *cos(smallalpha) ;
1346 i1 = B *sin(smallalpha) ;
1348 j2 = A *cos(highalpha) ;
1349 i2 = B *sin(highalpha) ;
1354 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1355 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1357 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1358 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );