39 #include <visp3/me/vpMeEllipse.h>
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpImagePoint.h>
44 #include <visp3/core/vpRobust.h>
45 #include <visp3/me/vpMe.h>
63 double i = iP.
get_i();
64 double j = iP.
get_j();
66 double A = 2 * i + 2 * K[1] * j + 2 * K[2];
67 double B = 2 * K[0] * j + 2 * K[1] * i + 2 * K[3];
71 while (theta > M_PI) {
83 : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.),
84 mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.), thresholdWeight(0.2), expecteddensity(0.)
102 :
vpMeTracker(meellipse), K(meellipse.K), iPc(meellipse.iPc), a(0.), b(0.), e(0.), iP1(meellipse.iP1),
103 iP2(meellipse.iP2), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(meellipse.angle), m00(0.), mu11(0.),
104 mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.), thresholdWeight(0.2), expecteddensity(0.)
161 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
162 std::cout <<
"In vpMeEllipse::sample: ";
163 std::cout <<
"function called with sample step = 0";
181 double j =
a * sin(k);
182 double i =
b * cos(k);
193 computeTheta(theta,
K, iP11);
251 void vpMeEllipse::getParameters()
254 for (
unsigned int i = 0; i < 5; i++)
258 double d = k[2] * k[2] - k[0] * k[1];
260 iPc.
set_i((k[1] * k[3] - k[2] * k[4]) / d);
261 iPc.
set_j((k[0] * k[4] - k[2] * k[3]) / d);
265 if (std::fabs(k[2]) <= std::numeric_limits<double>::epsilon()) {
268 e = (k[1] - k[0] + sq) / (2.0 * k[2]);
282 double a2 = num / (k[0] + k[1] + sq);
283 double b2 = num / (k[0] + k[1] - sq);
295 std::cout <<
"K" << std::endl;
297 std::cout <<
iPc << std::endl;
313 int number_of_points = 2000;
314 double incr = 2 * M_PI / number_of_points;
320 while (k < 2 * M_PI) {
322 double j1 =
a * sin(k);
323 double i1 =
b * cos(k);
348 else if (std::fabs(
alpha2 -
alpha1) < std::fabs(
alpha1) * std::numeric_limits<double>::epsilon())
357 void vpMeEllipse::updateTheta()
361 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
366 computeTheta(theta,
K, iP);
376 void vpMeEllipse::suppressPoints()
379 std::list<vpMeSite>::iterator itList =
list.begin();
380 for (std::list<double>::iterator it =
angle.begin(); it !=
angle.end();) {
383 itList =
list.erase(itList);
384 it =
angle.erase(it);
430 for (
unsigned int i = 0; i < 3; i++) {
465 for (
unsigned int i = 0; i < 3; i++) {
510 void vpMeEllipse::setExtremities()
512 double alphamin = +1e6;
513 double alphamax = -1e6;
520 std::list<double>::const_iterator itAngle =
angle.begin();
522 for (std::list<vpMeSite>::const_iterator itList =
list.begin(); itList !=
list.end(); ++itList) {
524 double alpha = *itAngle;
525 if (alpha < alphamin) {
531 if (alpha > alphamax) {
550 void vpMeEllipse::leastSquare()
559 unsigned int iter = 0;
572 if (
list.size() < 3) {
580 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
585 A[k][2] = 2 * p_me.
ifloat;
586 A[k][3] = 2 * p_me.
jfloat;
602 r.setIteration(iter);
606 for (i = 0; i < nos_1; i++) {
614 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
625 for (i = 0; i < 5; i++)
655 const unsigned int n = 5;
658 for (
unsigned int k = 0; k < n; k++) {
659 std::cout <<
"Click points " << k + 1 <<
"/" << n;
660 std::cout <<
" on the ellipse in the trigonometric order" << std::endl;
664 std::cout << iP[k] << std::endl;
695 for (
unsigned int k = 0; k < n; k++) {
698 A[k][2] = 2 * iP[k].
get_i();
699 A[k][3] = 2 * iP[k].
get_j();
741 unsigned int n = (
unsigned int)(iP.size());
750 for (
unsigned int k = 0; k < n; k++) {
752 A[k][1] = 2 * iP[k].get_i() * iP[k].get_j();
753 A[k][2] = 2 * iP[k].get_i();
754 A[k][3] = 2 * iP[k].get_j();
783 double e_p,
double low_alpha,
double high_alpha)
855 void vpMeEllipse::computeMoments()
857 double tane = tan(-1 /
e);
869 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
874 void vpMeEllipse::computeAngle(
int ip1,
int jp1,
double &_alpha1,
int ip2,
int jp2,
double &_alpha2)
878 int number_of_points = 2000;
879 double incr = 2 * M_PI / number_of_points;
887 double j1 =
a * cos(k);
888 double i1 =
b * sin(k);
920 void vpMeEllipse::computeAngle(
int ip1,
int jp1,
int ip2,
int jp2)
924 computeAngle(ip1, jp1, a1, ip2, jp2, a2);
937 for (
unsigned int k = 0; k < n; k++) {
939 A[k][1] = 2 * i[k] * j[k];
997 const double &E,
const double &smallalpha,
const double &highalpha,
const vpColor &color,
998 unsigned int thickness)
1004 j1 = j2 = i1 = i2 = 0;
1010 double k = smallalpha;
1011 while (k + incr < highalpha) {
1015 j2 = A * cos(k + incr);
1016 i2 = B * sin(k + incr);
1021 iP11.
set_j(center.
get_j() + cos(E) * j1 - sin(E) * i1);
1022 iP11.
set_i(center.
get_i() - (sin(E) * j1 + cos(E) * i1));
1024 iP22.
set_j(center.
get_j() + cos(E) * j2 - sin(E) * i2);
1025 iP22.
set_i(center.
get_i() - (sin(E) * j2 + cos(E) * i2));
1032 j1 = A * cos(smallalpha);
1033 i1 = B * sin(smallalpha);
1035 j2 = A * cos(highalpha);
1036 i2 = B * sin(highalpha);
1041 iP11.
set_j(center.
get_j() + cos(E) * j1 - sin(E) * i1);
1042 iP11.
set_i(center.
get_i() - (sin(E) * j1 + cos(E) * i1));
1044 iP22.
set_j(center.
get_j() + cos(E) * j2 - sin(E) * i2);
1045 iP22.
set_i(center.
get_i() - (sin(E) * j2 + cos(E) * i2));
1075 const double &E,
const double &smallalpha,
const double &highalpha,
const vpColor &color,
1076 unsigned int thickness)
1082 j1 = j2 = i1 = i2 = 0;
1088 double k = smallalpha;
1089 while (k + incr < highalpha) {
1093 j2 = A * cos(k + incr);
1094 i2 = B * sin(k + incr);
1099 iP11.
set_j(center.
get_j() + cos(E) * j1 - sin(E) * i1);
1100 iP11.
set_i(center.
get_i() - (sin(E) * j1 + cos(E) * i1));
1102 iP22.
set_j(center.
get_j() + cos(E) * j2 - sin(E) * i2);
1103 iP22.
set_i(center.
get_i() - (sin(E) * j2 + cos(E) * i2));
1110 j1 = A * cos(smallalpha);
1111 i1 = B * sin(smallalpha);
1113 j2 = A * cos(highalpha);
1114 i2 = B * sin(highalpha);
1119 iP11.
set_j(center.
get_j() + cos(E) * j1 - sin(E) * i1);
1120 iP11.
set_i(center.
get_i() - (sin(E) * j1 + cos(E) * i1));
1122 iP22.
set_j(center.
get_j() + cos(E) * j2 - sin(E) * i2);
1123 iP22.
set_i(center.
get_i() - (sin(E) * j2 + cos(E) * i2));