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>
55 : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.),
56 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
57 mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
60 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
63 m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.), m_expectedDensity(0),
64 m_numberOfGoodPoints(0), m_trackArc(false), m_arcEpsilon(1e-6)
77 :
vpMeTracker(me_ellipse), K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
78 iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2), ce(me_ellipse.ce),
79 se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
80 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
81 mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02), m10(me_ellipse.m10), m01(me_ellipse.m01),
82 m11(me_ellipse.m11), m02(me_ellipse.m02), m20(me_ellipse.m20),
84 thresholdWeight(me_ellipse.thresholdWeight),
85 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
86 expecteddensity(me_ellipse.expecteddensity),
88 m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
89 m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
90 m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
91 m_trackArc(me_ellipse.m_trackArc)
113 double u = iP.
get_u();
114 double v = iP.
get_v();
128 double A =
K[0] * u +
K[2] * v +
K[3];
129 double B =
K[1] * v +
K[2] * u +
K[4];
131 double theta = atan2(B, A);
147 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
189 double u =
a * cos(
angle);
190 double v =
b * sin(
angle);
234 double co = (du *
ce + dv *
se) /
a;
235 double si = (-du *
se + dv *
ce) /
b;
236 double angle = atan2(si, co);
251 if (d <= std::numeric_limits<double>::epsilon()) {
257 e = atan2(2.0 *
m_n11, num) / 2.0;
263 a = sqrt(2.0 * (num + d));
264 b = sqrt(2.0 * (num - d));
308 double num =
K[0] *
K[1] -
K[2] *
K[2];
313 m_uc = (
K[2] *
K[4] -
K[1] *
K[3]) / num;
314 m_vc = (
K[2] *
K[3] -
K[0] *
K[4]) / num;
326 for (
unsigned int i = 0; i < 6; i++) {
340 std::cout <<
"K :" <<
K.
t() << std::endl;
341 std::cout <<
"xc = " <<
m_uc <<
", yc = " <<
m_vc << std::endl;
342 std::cout <<
"n20 = " <<
m_n20 <<
", n11 = " <<
m_n11 <<
", n02 = " <<
m_n02 << std::endl;
343 std::cout <<
"A = " <<
a <<
", B = " <<
b <<
", E (dg) " <<
vpMath::deg(
e) << std::endl;
368 int nbrows =
static_cast<int>(I.
getHeight());
369 int nbcols =
static_cast<int>(I.
getWidth());
371 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
372 std::cout <<
"In vpMeEllipse::sample: ";
373 std::cout <<
"function called with sample step = 0, set to 10 dg";
379 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
401 angle.push_back(ang);
426 unsigned int nb_pts_added = 0;
427 int nbrows =
static_cast<int>(I.
getHeight());
428 int nbcols =
static_cast<int>(I.
getWidth());
442 std::list<double>::iterator angleList =
angle.begin();
443 double ang = *angleList;
444 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
447 double nextang = *angleList;
450 if ((nextang - ang) > 1.6 * incr) {
451 ang = (nextang + ang) / 2.0;
465 if ((new_ang - ang) > M_PI) {
466 new_ang -= 2.0 * M_PI;
467 }
else if ((ang - new_ang) > M_PI) {
468 new_ang += 2.0 * M_PI;
470 list.insert(meList, pix);
471 angle.insert(angleList, new_ang);
482 unsigned int nbpts =
static_cast<unsigned int>(floor((
m_alphamin -
alpha1) / incr));
484 for (
unsigned int i = 0; i < nbpts; i++) {
498 if ((new_ang - ang) > M_PI) {
499 new_ang -= 2.0 * M_PI;
500 }
else if ((ang - new_ang) > M_PI) {
501 new_ang += 2.0 * M_PI;
503 list.push_front(pix);
504 angle.push_front(new_ang);
516 for (
unsigned int i = 0; i < nbpts; i++) {
530 if ((new_ang - ang) > M_PI) {
531 new_ang -= 2.0 * M_PI;
532 }
else if ((ang - new_ang) > M_PI) {
533 new_ang += 2.0 * M_PI;
536 angle.push_back(new_ang);
549 printf(
"In plugHoles(): nb of added points : %d\n", nb_pts_added);
576 unsigned int n =
static_cast<unsigned int>(iP.size());
580 for (
unsigned int k = 0; k < n; k++) {
582 double u = (iP[k].get_u() - um) / um;
583 double v = (iP[k].get_v() - vm) / vm;
586 A[k][2] = 2.0 * u * v;
599 for (
unsigned int i = 0; i < 6; i++)
600 K[i] = um * vm * KerA[i][0];
606 K[3] =
K[3] / um -
K[0] * um -
K[2] * vm;
607 K[4] =
K[4] / vm -
K[1] * vm -
K[2] * um;
608 K[5] =
K[5] -
K[0] * um * um -
K[1] * vm * vm - 2.0 *
K[2] * um * vm - 2.0 *
K[3] * um - 2.0 *
K[4] * vm;
645 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
649 double u = (p_me.
jfloat - um) / um;
650 double v = (p_me.
ifloat - vm) / vm;
653 A[k][2] = 2.0 * u * v;
675 unsigned int iter = 0;
682 while ((iter < 4) && (var > 0.001)) {
683 for (
unsigned int i = 0; i < k; i++) {
684 for (
unsigned int j = 0; j < 6; j++) {
685 DA[i][j] = w[i] * A[i][j];
688 unsigned int dim = DA.
nullSpace(KerDA, 1);
694 for (
unsigned int i = 0; i < 6; i++)
696 var = (
K - Kprev).frobeniusNorm();
708 K[3] =
K[3] / um -
K[0] * um -
K[2] * vm;
709 K[4] =
K[4] / vm -
K[1] * vm -
K[2] * um;
710 K[5] =
K[5] -
K[0] * um * um -
K[1] * vm * vm - 2.0 *
K[2] * um * vm - 2.0 *
K[3] * um - 2.0 *
K[4] * vm;
714 for (
unsigned int i = 0; i < k; i++) {
736 double previous_ang = -4.0 * M_PI;
740 std::list<double>::iterator angleList =
angle.begin();
741 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
747 double ang = *angleList;
751 if ((new_ang - ang) > M_PI) {
752 new_ang -= 2.0 * M_PI;
753 }
else if ((ang - new_ang) > M_PI) {
754 new_ang += 2.0 * M_PI;
759 if ((new_ang - previous_ang) >= (0.6 * incr)) {
760 *angleList = previous_ang = new_ang;
773 meList =
list.erase(meList);
774 angleList =
angle.erase(angleList);
781 meList =
list.erase(meList);
782 angleList =
angle.erase(angleList);
788 printf(
"point %d outlier i : %.0f , j : %0.f, poids : %lf\n", k, p_me.
ifloat, p_me.
jfloat, w[k]);
791 meList =
list.erase(meList);
792 angleList =
angle.erase(angleList);
796 meList =
list.erase(meList);
797 angleList =
angle.erase(angleList);
801 printf(
"point not me i : %.0f , j : %0.f\n", p_me.
ifloat, p_me.
jfloat);
844 const unsigned int n = 5;
845 std::vector<vpImagePoint> iP(n);
849 std::cout <<
"First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
851 for (
unsigned int k = 0; k < n; k++) {
852 std::cout <<
"Click point " << k + 1 <<
"/" << n <<
" on the ellipse " << std::endl;
856 std::cout << iP[k] << std::endl;
924 if (pt1 != NULL && pt2 != NULL) {
1005 printf(
"A click to continue \n");
1039 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1050 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1054 void vpMeEllipse::computeMoments()
1084 double e_p,
double alpha1_p,
double alpha2_p)
1133 std::vector<vpImagePoint> v_iP(n);
1135 for (
unsigned int i = 0; i < n; i++) {
1146 std::vector<vpImagePoint> v_iP(n);
1148 for (
unsigned int k = 0; k < n; k++) {
1149 v_iP[k].set_ij(i[k], j[k]);
1178 const double &E,
const double &smallalpha,
const double &highalpha,
const vpColor &color,
1179 unsigned int thickness)
1181 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha,
false, color, thickness,
true,
true);
1208 const double &E,
const double &smallalpha,
const double &highalpha,
const vpColor &color,
1209 unsigned int thickness)
1211 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha,
false, color, thickness,
true,
true);
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionnalities.
static const vpColor cyan
static const vpColor orange
static const vpColor blue
static const vpColor green
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
error that can be emited by ViSP classes.
@ dimensionError
Bad dimension.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
void set_uv(double u, double v)
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
static int round(double x)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Class that tracks an ellipse using moving edges.
double m_n20
Second order centered and normalized moments.
double m_arcEpsilon
Epsilon value used to check if arc angles are the same.
double a
is the semimajor axis of the ellipse.
double se
Value of sin(e).
double mu11
Second order centered moments.
double m10
First order raw moments.
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
void leastSquareRobust(const vpImage< unsigned char > &I)
void display(const vpImage< unsigned char > &I, vpColor col)
double m_vc
Value of v coordinate of iPc.
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
vpImagePoint iPc
The coordinates of the ellipse center.
unsigned int plugHoles(const vpImage< unsigned char > &I)
std::list< double > angle
Stores the value in increasing order of the angle on the ellipse for each vpMeSite .
double b
is the semiminor axis of the ellipse.
double expecteddensity
Expected number of points to track along the ellipse.
void printParameters() const
double m_uc
Value of u coordinate of iPc.
double ce
Value of cos(e).
void initTracking(const vpImage< unsigned char > &I, bool trackArc=false)
double m11
Second order raw moments.
bool m_trackArc
Track an arc of ellipse (true) or a complete one (false).
void computePointOnEllipse(const double ang, vpImagePoint &iP)
double computeTheta(const vpImagePoint &iP) const
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
void track(const vpImage< unsigned char > &I)
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false)
double thresholdWeight
Threshold on the weights for the robust least square.
double computeAngleOnEllipse(const vpImagePoint &pt) const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
@ NO_SUPPRESSION
Point used by the tracker.
void setDisplay(vpMeSiteDisplayType select)
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
vpMeSiteState getState() const
void setState(const vpMeSiteState &flag)
Contains abstract elements for a Distance to Feature type feature.
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
std::list< vpMeSite > list
vpMe * me
Moving edges initialisation parameters.
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
void setMu1(const double &mu_1)
void setSampleStep(const double &s)
void setRange(const unsigned int &r)
void setMu2(const double &mu_2)
double getSampleStep() const
unsigned int getRange() const
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
void setMinMedianAbsoluteDeviation(double mad_min)
#define vpDEBUG_ENABLE(level)