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.),
56 iP1(), iP2(), alpha1(0), alpha2(2 * M_PI),
57 ce(0.), se(0.), angle(), m00(0.),
58 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
59 mu11(0.), mu20(0.), mu02(0.),
61 m11(0.), m02(0.), m20(0.),
64 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
67 m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.),
68 m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackArc(false), m_arcEpsilon(1e-6)
82 K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
83 iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2),
84 ce(me_ellipse.ce), se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
85 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
86 mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02),
87 m10(me_ellipse.m10), m01(me_ellipse.m01), m11(me_ellipse.m11),
88 m02(me_ellipse.m02), m20(me_ellipse.m20),
90 thresholdWeight(me_ellipse.thresholdWeight),
91 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
92 expecteddensity(me_ellipse.expecteddensity),
94 m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax),
95 m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
96 m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
97 m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints), m_trackArc(me_ellipse.m_trackArc)
119 double u = iP.
get_u();
120 double v = iP.
get_v();
134 double A =
K[0] * u +
K[2] * v +
K[3];
135 double B =
K[1] * v +
K[2] * u +
K[4];
137 double theta = atan2(B, A);
153 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
195 double u =
a * cos(
angle);
196 double v =
b * sin(
angle);
240 double co = (du *
ce + dv *
se)/
a;
241 double si = (- du *
se + dv *
ce)/
b;
242 double angle = atan2(si,co);
257 if (d <= std::numeric_limits<double>::epsilon()) {
264 e = atan2(2.0*
m_n11, num)/2.0;
270 a = sqrt(2.0*(num + d));
271 b = sqrt(2.0*(num - d));
316 double num =
K[0] *
K[1] -
K[2] *
K[2];
321 m_uc = (
K[2] *
K[4] -
K[1] *
K[3]) / num;
322 m_vc = (
K[2] *
K[3] -
K[0] *
K[4]) / num;
335 for (
unsigned int i=0; i < 6; i++) {
349 std::cout <<
"K :" <<
K.
t() << std::endl;
350 std::cout <<
"xc = " <<
m_uc <<
", yc = " <<
m_vc << std::endl;
351 std::cout <<
"n20 = " <<
m_n20 <<
", n11 = " <<
m_n11 <<
", n02 = " <<
m_n02 <<std::endl;
352 std::cout <<
"A = " <<
a <<
", B = " <<
b <<
", E (dg) " <<
vpMath::deg(
e) <<std::endl;
377 int nbrows =
static_cast<int>(I.
getHeight());
378 int nbcols =
static_cast<int>(I.
getWidth());
380 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
381 std::cout <<
"In vpMeEllipse::sample: ";
382 std::cout <<
"function called with sample step = 0, set to 10 dg";
388#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
410 angle.push_back(ang);
435 unsigned int nb_pts_added = 0;
436 int nbrows =
static_cast<int>(I.
getHeight());
437 int nbcols =
static_cast<int>(I.
getWidth());
451 std::list<double>::iterator angleList =
angle.begin();
452 double ang = *angleList;
453 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
456 double nextang = *angleList;
459 if ((nextang - ang) > 1.6 * incr) {
460 ang = (nextang + ang) / 2.0;
474 if ((new_ang - ang) > M_PI) {
475 new_ang -= 2.0 * M_PI;
477 else if ((ang - new_ang) > M_PI) {
478 new_ang += 2.0 * M_PI;
480 list.insert(meList,pix);
481 angle.insert(angleList,new_ang);
492 unsigned int nbpts =
static_cast<unsigned int>(floor((
m_alphamin-
alpha1)/incr));
494 for (
unsigned int i = 0; i < nbpts; i++) {
508 if ((new_ang - ang) > M_PI) {
509 new_ang -= 2.0 * M_PI;
511 else if ((ang - new_ang) > M_PI) {
512 new_ang += 2.0 * M_PI;
514 list.push_front(pix);
515 angle.push_front(new_ang);
527 for (
unsigned int i = 0; i < nbpts; i++) {
541 if ((new_ang - ang) > M_PI) {
542 new_ang -= 2.0 * M_PI;
544 else if ((ang - new_ang) > M_PI) {
545 new_ang += 2.0 * M_PI;
548 angle.push_back(new_ang);
561 printf(
"In plugHoles(): nb of added points : %d\n", nb_pts_added);
588 unsigned int n =
static_cast<unsigned int>(iP.size());
592 for (
unsigned int k = 0; k < n; k++) {
594 double u = (iP[k].get_u() - um) / um;
595 double v = (iP[k].get_v() - vm) / vm;
598 A[k][2] = 2.0 * u * v;
611 for (
unsigned int i = 0; i < 6 ; i++)
K[i] = um * vm * KerA[i][0];
617 K[3] =
K[3]/um -
K[0] * um -
K[2] * vm;
618 K[4] =
K[4]/vm -
K[1] * vm -
K[2] * um;
619 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;
656 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
660 double u = (p_me.
jfloat - um) / um;
661 double v = (p_me.
ifloat - vm) / vm;
664 A[k][2] = 2.0 * u * v;
686 unsigned int iter = 0;
693 while ((iter < 4) && (var > 0.001)) {
694 for (
unsigned int i = 0; i < k ; i++) {
695 for (
unsigned int j = 0; j < 6 ; j++) {
696 DA[i][j] = w[i] * A[i][j];
699 unsigned int dim = DA.
nullSpace(KerDA, 1);
705 for (
unsigned int i=0; i<6 ; i++)
K[i] = KerDA[i][0];
706 var = (
K-Kprev).frobeniusNorm();
718 K[3] =
K[3]/um -
K[0] * um -
K[2] * vm;
719 K[4] =
K[4]/vm -
K[1] * vm -
K[2] * um;
720 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;
724 for (
unsigned int i=0; i < k ; i++) {
746 double previous_ang = - 4.0 * M_PI;
750 std::list<double>::iterator angleList =
angle.begin();
751 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
757 double ang = *angleList;
761 if ((new_ang - ang) > M_PI) {
762 new_ang -= 2.0 * M_PI;
764 else if ((ang - new_ang) > M_PI) {
765 new_ang += 2.0 * M_PI;
770 if ((new_ang - previous_ang) >= (0.6 * incr)) {
771 *angleList = previous_ang = new_ang;
777 printf(
"In LQR: angle : %lf, i = %.0lf, j = %.0lf\n",
784 printf(
"too near : angle %lf, i %.0f , j : %0.f\n",
787 meList =
list.erase(meList);
788 angleList =
angle.erase(angleList);
794 printf(
"not in interval: angle : %lf, i %.0f , j : %0.f\n",
797 meList =
list.erase(meList);
798 angleList =
angle.erase(angleList);
805 printf(
"point %d outlier i : %.0f , j : %0.f, poids : %lf\n",
809 meList =
list.erase(meList);
810 angleList =
angle.erase(angleList);
815 meList =
list.erase(meList);
816 angleList =
angle.erase(angleList);
820 printf(
"point not me i : %.0f , j : %0.f\n", p_me.
ifloat, p_me.
jfloat);
863 const unsigned int n = 5;
864 std::vector<vpImagePoint> iP(n);
868 std::cout <<
"First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
870 for (
unsigned int k = 0; k < n; k++) {
871 std::cout <<
"Click point " << k + 1 <<
"/" << n <<
" on the ellipse " << std::endl;
875 std::cout << iP[k] << std::endl;
943 if (pt1 != NULL && pt2 != NULL) {
1002 printf(
"nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1014 printf(
"nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1025 printf(
"A click to continue \n");
1035 printf(
"nb of Good points %u, density %d, alphamax %lf, alphamin %lf\n",
1058#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1070#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1074void vpMeEllipse::computeMoments()
1104 double a_p,
double b_p,
double e_p,
double alpha1_p,
double alpha2_p)
1153 std::vector<vpImagePoint> v_iP(n);
1155 for (
unsigned int i = 0; i < n; i++) {
1166 std::vector<vpImagePoint> v_iP(n);
1168 for (
unsigned int k=0; k < n; k++) {
1169 v_iP[k].set_ij(i[k],j[k]);
1198 const vpImagePoint ¢er,
const double &A,
const double &B,
const double &E,
1199 const double &smallalpha,
const double &highalpha,
1200 const vpColor &color,
unsigned int thickness)
1202 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha,
false, color, thickness,
true,
true);
1229 const vpImagePoint ¢er,
const double &A,
const double &B,
const double &E,
1230 const double &smallalpha,
const double &highalpha,
1231 const vpColor &color,
unsigned int thickness)
1233 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)