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 ;
163 vpCDEBUG(1) <<
"begin vpMeEllipse::sample() : "<<std::endl ;
171 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon())
173 std::cout <<
"In vpMeEllipse::sample: " ;
174 std::cout <<
"function called with sample step = 0" ;
216 computeTheta(theta, K, iP11) ;
238 n_sample =
list.size() ;
240 vpCDEBUG(1) <<
"end vpMeEllipse::sample() : " ;
241 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl ;
261 if ((
double)n<0.9*expecteddensity){
274 vpMeEllipse::getParameters()
278 for (
unsigned int i=0 ; i < 5 ; i++)
282 double d = k[2]*k[2] - k[0]*k[1];
285 iPc.
set_i( (k[1] * k[3] - k[2] * k[4]) / d );
286 iPc.
set_j( (k[0] * k[4] - k[2] * k[3]) / d );
295 e = (k[1] - k[0] + sq) / (2.0*k[2]);
301 if(
e < 0.0)
e += M_PI ;
307 double a2 = num / (k[0] + k[1] + sq ) ;
308 double b2 = num / (k[0] + k[1] - sq ) ;
321 std::cout <<
"K" << std::endl ;
323 std::cout <<
iPc << std::endl ;
339 double j1, i1, j11, i11;
342 int number_of_points = 2000 ;
343 double incr = 2 * M_PI / number_of_points ;
402 vpMeEllipse::updateTheta()
406 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
411 computeTheta(theta, K, iP) ;
421 vpMeEllipse::suppressPoints()
424 std::list<vpMeSite>::iterator itList =
list.begin();
425 for(std::list<double>::iterator it=
angle.begin(); it!=
angle.end(); ){
429 itList =
list.erase(itList) ;
430 it =
angle.erase(it);
472 for (
unsigned int i=0 ; i < 3 ; i++)
509 for (
unsigned int i=0 ; i < 3 ; i++)
557 vpMeEllipse::setExtremities()
559 double alphamin = +1e6;
560 double alphamax = -1e6;
567 std::list<double>::const_iterator itAngle =
angle.begin();
569 for(std::list<vpMeSite>::const_iterator itList=
list.begin(); itList!=
list.end(); ++itList){
571 double alpha = *itAngle;
572 if (alpha < alphamin)
579 if (alpha > alphamax)
601 vpMeEllipse::leastSquare()
610 unsigned int iter =0 ;
627 "not enough point")) ;
636 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
661 r.setIteration(iter) ;
665 for (i=0 ; i < nos_1 ; i++)
674 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
687 for(i = 0; i < 5; i ++)
697 for(std::list<vpMeSite>::const_iterator it=
list.begin(); it!=
list.end(); ++it){
720 r.setIteration(iter) ;
724 for (i=0 ; i < nos_1 ; i++)
733 for(std::list<vpMeSite>::iterator it=
list.begin(); it!=
list.end(); ++it){
746 for(i = 0; i < 3; i ++)
778 vpCDEBUG(1) <<
" begin vpMeEllipse::initTracking()"<<std::endl ;
780 const unsigned int n=5 ;
783 for (
unsigned int k =0 ; k < n ; k++)
785 std::cout <<
"Click points "<< k+1 <<
"/" << n ;
786 std::cout <<
" on the ellipse in the trigonometric order" <<std::endl ;
788 std::cout << iP[k] << std::endl;
812 vpCDEBUG(1) <<
" begin vpMeEllipse::initTracking()"<<std::endl ;
824 for (
unsigned int k =0 ; k < n ; k++)
828 A[k][2] = 2* iP[k].
get_i() ;
829 A[k][3] = 2* iP[k].
get_j() ;
836 std::cout << K << std::endl;
845 for (
unsigned int k =0 ; k < n ; k++)
847 A[k][0] = 2* iP[k].
get_i() ;
848 A[k][1] = 2* iP[k].
get_j() ;
861 std::cout << K << std::endl;
900 vpCDEBUG(1) <<
"begin vpMeEllipse::track()"<<std::endl ;
998 vpCDEBUG(1) <<
"end vpMeEllipse::track()"<<std::endl ;
1009 vpMeEllipse::computeMoments()
1011 double tane = tan(-1/
e);
1030 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1036 vpMeEllipse::computeAngle(
int ip1,
int jp1,
double &_alpha1,
1037 int ip2,
int jp2,
double &_alpha2)
1041 double j1, i1, j11, i11;
1044 int number_of_points = 2000 ;
1045 double incr = 2 * M_PI / number_of_points ;
1047 double dmin1 = 1e6 ;
1048 double dmin2 = 1e6 ;
1090 vpMeEllipse::computeAngle(
int ip1,
int jp1,
int ip2,
int jp2)
1094 computeAngle(ip1,jp1,a1, ip2, jp2,a2) ;
1100 unsigned *i,
unsigned *j)
1102 vpCDEBUG(1) <<
" begin vpMeEllipse::initTracking()"<<std::endl ;
1114 for (
unsigned int k =0 ; k < n ; k++)
1117 A[k][1] = 2* i[k] * j[k] ;
1126 std::cout << K << std::endl;
1135 for (
unsigned int k =0 ; k < n ; k++)
1151 std::cout << K << std::endl;
1182 #endif // Deprecated
1205 const double &A,
const double &B,
const double &E,
1206 const double & smallalpha,
const double &highalpha,
1213 j1 = j2 = i1 = i2 = 0 ;
1219 double k = smallalpha ;
1220 while (k+incr<highalpha)
1225 j2 = A *cos(k+incr) ;
1226 i2 = B *sin(k+incr) ;
1231 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1232 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1234 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1235 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );
1242 j1 = A *cos(smallalpha) ;
1243 i1 = B *sin(smallalpha) ;
1245 j2 = A *cos(highalpha) ;
1246 i2 = B *sin(highalpha) ;
1251 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1252 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1254 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1255 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );
1282 const double &A,
const double &B,
const double &E,
1283 const double & smallalpha,
const double &highalpha,
1290 j1 = j2 = i1 = i2 = 0 ;
1296 double k = smallalpha ;
1297 while (k+incr<highalpha)
1302 j2 = A *cos(k+incr) ;
1303 i2 = B *sin(k+incr) ;
1308 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1309 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1311 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1312 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );
1319 j1 = A *cos(smallalpha) ;
1320 i1 = B *sin(smallalpha) ;
1322 j2 = A *cos(highalpha) ;
1323 i2 = B *sin(highalpha) ;
1328 iP11.
set_j ( center.
get_j() + cos(E) *j1 - sin(E) *i1 );
1329 iP11.
set_i ( center.
get_i() -( sin(E) *j1 + cos(E) *i1) );
1331 iP22.
set_j ( center.
get_j() + cos(E) *j2 - sin(E) *i2 );
1332 iP22.
set_i ( center.
get_i() -( sin(E) *j2 + cos(E) *i2) );