35#include <visp3/core/vpDebug.h>
36#include <visp3/core/vpMatrixException.h>
37#include <visp3/core/vpTrackingException.h>
38#include <visp3/core/vpImagePoint.h>
39#include <visp3/core/vpRobust.h>
40#include <visp3/me/vpMe.h>
41#include <visp3/me/vpMeEllipse.h>
45 : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.),
46#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
47 mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
49 thresholdWeight(0.2), m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.), m_expectedDensity(0),
50 m_numberOfGoodPoints(0), m_trackCircle(false), m_trackArc(false), m_arcEpsilon(1e-6)
60 :
vpMeTracker(me_ellipse), K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
61 iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2), ce(me_ellipse.ce),
62 se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
63#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
64 mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02), m10(me_ellipse.m10), m01(me_ellipse.m01),
65 m11(me_ellipse.m11), m02(me_ellipse.m02), m20(me_ellipse.m20),
67 thresholdWeight(me_ellipse.thresholdWeight),
69 m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
70 m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
71 m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
72 m_trackCircle(me_ellipse.m_trackCircle), m_trackArc(me_ellipse.m_trackArc)
83 double u = iP.
get_u();
84 double v = iP.
get_v();
91 double A =
K[0] * u +
K[2] * v +
K[3];
92 double B =
K[1] * v +
K[2] * u +
K[4];
94 double theta = atan2(B, A);
105 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
140 double u =
a * cos(
angle);
141 double v =
b * sin(
angle);
180 double co = (du *
ce + dv *
se) /
a;
181 double si = (-du *
se + dv *
ce) /
b;
182 double angle = atan2(si, co);
191 if (d <= std::numeric_limits<double>::epsilon()) {
198 e = atan2(2.0 *
m_n11, num) / 2.0;
204 a = sqrt(2.0 * (num + d));
205 b = sqrt(2.0 * (num - d));
229 double num =
K[0] *
K[1] -
K[2] *
K[2];
234 m_uc = (
K[2] *
K[4] -
K[1] *
K[3]) / num;
235 m_vc = (
K[2] *
K[3] -
K[0] *
K[4]) / num;
247 for (
unsigned int i = 0; i < 6; i++) {
257 std::cout <<
"K :" <<
K.
t() << std::endl;
258 std::cout <<
"xc = " <<
m_uc <<
", yc = " <<
m_vc << std::endl;
259 std::cout <<
"n20 = " <<
m_n20 <<
", n11 = " <<
m_n11 <<
", n02 = " <<
m_n02 << std::endl;
260 std::cout <<
"A = " <<
a <<
", B = " <<
b <<
", E (dg) " <<
vpMath::deg(
e) << std::endl;
273 int nbrows =
static_cast<int>(I.
getHeight());
274 int nbcols =
static_cast<int>(I.
getWidth());
276 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
277 std::cout <<
"Warning: In vpMeEllipse::sample() ";
278 std::cout <<
"function called with sample step = 0. We set it rather to 10 pixels";
283 double perim = M_PI * (3.0 * (
a +
b) - sqrt((3.0 *
a +
b) * (
a + 3.0 *
b)));
285 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
me->
getSampleStep()));
286 double incr = 2.0 * M_PI / nb_pt;
296 double ang =
alpha1 + incr / 2.0;
314 angle.push_back(ang);
329 unsigned int nb_pts_added = 0;
330 int nbrows =
static_cast<int>(I.
getHeight());
331 int nbcols =
static_cast<int>(I.
getWidth());
337 double perim = M_PI * (3.0 * (
a +
b) - sqrt((3.0 *
a +
b) * (
a + 3.0 *
b)));
339 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
me->
getSampleStep()));
340 double incr = 2.0 * M_PI / nb_pt;
344 std::list<double>::iterator angleList =
angle.begin();
345 std::list<vpMeSite>::iterator meList =
list.begin();
346 double ang = *angleList;
349 while (meList !=
list.end()) {
350 double nextang = *angleList;
351 if ((nextang - ang) > 2.0 * incr) {
354 while (ang < (nextang - incr)) {
368 if ((new_ang - ang) > M_PI) {
369 new_ang -= 2.0 * M_PI;
371 else if ((ang - new_ang) > M_PI) {
372 new_ang += 2.0 * M_PI;
374 list.insert(meList, pix);
375 angle.insert(angleList, new_ang);
390 if (nb_pts_added > 0) {
391 std::cout <<
"Number of added points from holes with angles: " << nb_pts_added << std::endl;
396 angleList =
angle.begin();
398 meList =
list.begin();
402 while (meList !=
list.end()) {
403 double nextang = *angleList;
409 ang = (nextang + ang) / 2.0;
423 if ((new_ang - ang) > M_PI) {
424 new_ang -= 2.0 * M_PI;
426 else if ((ang - new_ang) > M_PI) {
427 new_ang += 2.0 * M_PI;
429 list.insert(meList, pix);
430 angle.insert(angleList, new_ang);
444 if (nb_pts_added > 0) {
445 std::cout <<
"Number of added points from holes : " << nb_pts_added << std::endl;
446 angleList =
angle.begin();
447 while (angleList !=
angle.end()) {
449 std::cout <<
"ang = " <<
vpMath::deg(ang) << std::endl;
456 unsigned int nbpts = 0;
459 nbpts =
static_cast<unsigned int>(floor((
m_alphamin -
alpha1 - incr / 2.0) / incr));
462 for (
unsigned int i = 0; i < nbpts; i++) {
476 if ((new_ang - ang) > M_PI) {
477 new_ang -= 2.0 * M_PI;
479 else if ((ang - new_ang) > M_PI) {
480 new_ang += 2.0 * M_PI;
482 list.push_front(pix);
483 angle.push_front(new_ang);
486 std::cout <<
"Add extremity 1, ang = " <<
vpMath::deg(new_ang) << std::endl;
494 if (nb_pts_added > 0) std::cout <<
"Number of added points from holes and first extremity : " << nb_pts_added << std::endl;
500 nbpts =
static_cast<unsigned int>(floor((
alpha2 - incr / 2.0 -
m_alphamax) / incr));
503 for (
unsigned int i = 0; i < nbpts; i++) {
517 if ((new_ang - ang) > M_PI) {
518 new_ang -= 2.0 * M_PI;
520 else if ((ang - new_ang) > M_PI) {
521 new_ang += 2.0 * M_PI;
524 angle.push_back(new_ang);
527 std::cout <<
"Add extremity 2, ang = " <<
vpMath::deg(new_ang) << std::endl;
536 if (nb_pts_added > 0) std::cout <<
"In plugHoles(): nb of added points : " << nb_pts_added << std::endl;
545 unsigned int n =
static_cast<unsigned int>(iP.size());
557 for (
unsigned int k = 0; k < n; k++) {
559 double u = (iP[k].get_u() - um) / um;
560 double v = (iP[k].get_v() - vm) / um;
564 b[k] = u * u + v * v;
570 double ratio = vm / um;
571 K[0] =
K[1] = 1.0 / (um * um);
573 K[3] = -(1.0 + x[0] / 2.0) / um;
574 K[4] = -(ratio + x[1] / 2.0) / um;
575 K[5] = -x[2] + 1.0 + ratio * ratio + x[0] + ratio * x[1];
594 for (
unsigned int k = 0; k < n; k++) {
596 double u = (iP[k].get_u() - um) / um;
597 double v = (iP[k].get_v() - vm) / vm;
600 A[k][2] = 2.0 * u * v;
610 for (
unsigned int i = 0; i < 6; i++)
616 K[3] =
K[3] * vm -
K[0] * um -
K[2] * vm;
617 K[4] =
K[4] * um -
K[1] * vm -
K[2] * um;
618 K[5] =
K[5] * um * vm -
K[0] * um * um -
K[1] * vm * vm - 2.0 *
K[2] * um * vm - 2.0 *
K[3] * um - 2.0 *
K[4] * vm;
647 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
651 double u = (p_me.
jfloat - um) / um;
652 double v = (p_me.
ifloat - vm) / um;
656 b[k] = u * u + v * v;
671 unsigned int iter = 0;
680 while ((iter < 4) && (var > 0.1)) {
681 for (
unsigned int i = 0; i < k; i++) {
682 for (
unsigned int j = 0; j < 3; j++) {
683 DA[i][j] = w[i] * A[i][j];
691 double ratio = vm / um;
692 K[0] =
K[1] = 1.0 / (um * um);
694 K[3] = -(1.0 + x[0] / 2.0) / um;
695 K[4] = -(ratio + x[1] / 2.0) / um;
696 K[5] = -x[2] + 1.0 + ratio * ratio + x[0] + ratio * x[1];
702 var = (xg - xg_prev).frobeniusNorm();
706 for (
unsigned int i = 0; i < k; i++) {
709 double sign =
K[0] * x * x +
K[1] * y * y + 2. *
K[2] * x * y + 2. *
K[3] * x + 2. *
K[4] * y +
K[5];
740 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
744 double u = (p_me.
jfloat - um) / um;
745 double v = (p_me.
ifloat - vm) / vm;
748 A[k][2] = 2.0 * u * v;
766 unsigned int iter = 0;
774 while ((iter < 4) && (var > 0.1)) {
775 for (
unsigned int i = 0; i < k; i++) {
776 for (
unsigned int j = 0; j < 6; j++) {
777 DA[i][j] = w[i] * A[i][j];
780 unsigned int dim = DA.
nullSpace(KerDA, 1);
785 for (
unsigned int i = 0; i < 6; i++)
791 K[3] =
K[3] * vm -
K[0] * um -
K[2] * vm;
792 K[4] =
K[4] * um -
K[1] * vm -
K[2] * um;
793 K[5] =
K[5] * um * vm -
K[0] * um * um -
K[1] * vm * vm - 2.0 *
K[2] * um * vm - 2.0 *
K[3] * um - 2.0 *
K[4] * vm;
799 var = (xg - xg_prev).frobeniusNorm();
803 for (
unsigned int i = 0; i < k; i++) {
806 double sign =
K[0] * x * x +
K[1] * y * y + 2. *
K[2] * x * y + 2. *
K[3] * x + 2. *
K[4] * y +
K[5];
823 double previous_ang = -4.0 * M_PI;
825 std::list<double>::iterator angleList =
angle.begin();
826 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
830 double ang = *angleList;
831 meList =
list.erase(meList);
832 angleList =
angle.erase(angleList);
842 double ang = *angleList;
843 meList =
list.erase(meList);
844 angleList =
angle.erase(angleList);
848 printf(
"point %d outlier i : %.0f , j : %0.f, ang = %lf, poids : %lf\n",
854 double ang = *angleList;
858 if ((new_ang - ang) > M_PI) {
859 new_ang -= 2.0 * M_PI;
861 else if ((ang - new_ang) > M_PI) {
862 new_ang += 2.0 * M_PI;
864 *angleList = previous_ang = new_ang;
868 printf(
"point %d inlier i : %.0f , j : %0.f, poids : %lf\n", k, p_me.
ifloat, p_me.
jfloat, w[k]);
878 std::list<double> finAngle;
880 std::list<vpMeSite> finMe;
882 std::list<double>::iterator debutAngleList;
883 std::list<vpMeSite>::iterator debutMeList;
884 angleList =
angle.begin();
885 std::list<vpMeSite>::iterator meList;
886 for (meList =
list.begin(); meList !=
list.end();) {
888 double ang = *angleList;
893 angleList =
angle.erase(angleList);
894 finAngle.push_back(ang);
895 meList =
list.erase(meList);
896 finMe.push_back(p_me);
901 angleList =
angle.erase(angleList);
902 meList =
list.erase(meList);
904 angle.push_front(ang);
905 debutAngleList =
angle.begin();
908 list.push_front(p_me);
909 debutMeList =
list.begin();
915 debutAngleList =
angle.insert(debutAngleList, ang);
917 debutMeList =
list.insert(debutMeList, p_me);
927 angleList =
angle.end();
928 angle.splice(angleList, finAngle);
930 list.splice(meList, finMe);
932 unsigned int numberOfGoodPoints = 0;
933 previous_ang = -4.0 * M_PI;
936 double perim = M_PI * (3.0 * (
a +
b) - sqrt((3.0 *
a +
b) * (
a + 3.0 *
b)));
937 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
me->
getSampleStep()));
938 double incr = 2.0 * M_PI / nb_pt;
949 angleList =
angle.begin();
950 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
952 double new_ang = *angleList;
954 if ((new_ang - previous_ang) >= (0.6 * incr)) {
955 previous_ang = new_ang;
956 numberOfGoodPoints++;
967 meList =
list.erase(meList);
968 angleList =
angle.erase(angleList);
978 meList =
list.erase(meList);
979 angleList =
angle.erase(angleList);
994 printf(
"Fin leastSquareRobust : nb pts ok = %d \n", numberOfGoodPoints);
997 return(numberOfGoodPoints);
1011 std::vector<vpImagePoint> iP(n);
1018 std::cout <<
"First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
1020 for (
unsigned int k = 0; k < n; k++) {
1021 std::cout <<
"Click point " << k + 1 <<
"/" << n <<
" on the circle " << std::endl;
1025 std::cout << iP[k] << std::endl;
1030 std::cout <<
"First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
1032 for (
unsigned int k = 0; k < n; k++) {
1033 std::cout <<
"Click point " << k + 1 <<
"/" << n <<
" on the ellipse " << std::endl;
1037 std::cout << iP[k] << std::endl;
1044 bool trackCircle,
bool trackArc)
1080 if (pt1 != NULL && pt2 != NULL) {
1140 printf(
"1st step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1156 printf(
"A click to continue \n");
1165 printf(
"nb of Good points %u, density %d %lf, alphamin %lf, alphamax\n",
1189#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1200#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1204void vpMeEllipse::computeMoments()
1234 double b_p,
double e_p,
double alpha1_p,
double alpha2_p)
1283 std::vector<vpImagePoint> v_iP(n);
1285 for (
unsigned int i = 0; i < n; i++) {
1296 std::vector<vpImagePoint> v_iP(n);
1298 for (
unsigned int k = 0; k < n; k++) {
1299 v_iP[k].set_ij(i[k], j[k]);
1329 const double &B,
const double &E,
const double &smallalpha,
const double &highalpha,
1330 const vpColor &color,
unsigned int thickness)
1363 const double &E,
const double &smallalpha,
const double &highalpha,
1364 const vpColor &color,
unsigned int thickness)
1372 const double &B,
const double &E,
const double &smallalpha,
const double &highalpha,
1373 const vpColor &color,
unsigned int thickness)
1375 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha,
false, color, thickness,
true,
true);
1379 const double &E,
const double &smallalpha,
const double &highalpha,
1380 const vpColor &color,
unsigned int thickness)
1382 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 functionalities.
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 emitted 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)
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
static int round(double x)
static double deg(double rad)
@ rankDeficient
Rank deficient.
Implementation of a matrix and operations on matrices.
void solveBySVD(const vpColVector &B, vpColVector &x) const
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 semi major axis of the ellipse.
void computePointOnEllipse(const double angle, vpImagePoint &iP)
double se
Value of sin(e).
double mu11
Second order centered moments.
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) override
double m10
First order raw moments.
void display(const vpImage< unsigned char > &I, const vpColor &col, unsigned int thickness=1)
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
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 semi minor axis of the ellipse.
void printParameters() const
double m_uc
Value of u coordinate of iPc.
bool m_trackCircle
Track a circle (true) or an ellipse (false).
double ce
Value of cos(e).
double m11
Second order raw moments.
bool m_trackArc
Track an arc of ellipse/circle (true) or a complete one (false).
void initTracking(const vpImage< unsigned char > &I, bool trackCircle=false, bool trackArc=false)
unsigned int leastSquareRobust(const vpImage< unsigned char > &I)
double computeTheta(const vpImagePoint &iP) const
double m_n02
Second order centered and normalized moments .
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
double m_n11
Second order centered and normalized moments .
void track(const vpImage< unsigned char > &I)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &A, const double &B, const double &E, const double &smallalpha, const double &highalpha, const vpColor &color=vpColor::green, unsigned int thickness=1)
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)
double ifloat
Floating coordinates along i of a site.
double alpha
Angle of tangent at site.
double jfloat
Floating coordinates along j of a site.
vpMeSiteState getState() const
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
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)
std::list< vpMeSite > list
void display(const vpImage< unsigned char > &I)
vpMe * me
Moving edges initialisation parameters.
void setSampleStep(const double &s)
void setRange(const unsigned int &r)
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)
@ notEnoughPointError
Not enough point to track.
#define vpDEBUG_ENABLE(level)