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 thresholdWeight(0.2), m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.),
47 m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackCircle(false), m_trackArc(false), m_arcEpsilon(1e-6)
57 :
vpMeTracker(me_ellipse), K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
58 iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2), ce(me_ellipse.ce),
59 se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
60 thresholdWeight(me_ellipse.thresholdWeight),
61 m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
62 m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
63 m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
64 m_trackCircle(me_ellipse.m_trackCircle), m_trackArc(me_ellipse.m_trackArc)
75 double u = iP.
get_u();
76 double v = iP.
get_v();
83 double A =
K[0] * u +
K[2] * v +
K[3];
84 double B =
K[1] * v +
K[2] * u +
K[4];
86 double theta = atan2(B, A);
97 for (std::list<vpMeSite>::iterator it =
list.begin(); it !=
list.end(); ++it) {
132 double u =
a * cos(
angle);
133 double v =
b * sin(
angle);
172 double co = (du *
ce + dv *
se) /
a;
173 double si = (-du *
se + dv *
ce) /
b;
174 double angle = atan2(si, co);
183 if (d <= std::numeric_limits<double>::epsilon()) {
190 e = atan2(2.0 *
m_n11, num) / 2.0;
196 a = sqrt(2.0 * (num + d));
197 b = sqrt(2.0 * (num - d));
221 double num =
K[0] *
K[1] -
K[2] *
K[2];
226 m_uc = (
K[2] *
K[4] -
K[1] *
K[3]) / num;
227 m_vc = (
K[2] *
K[3] -
K[0] *
K[4]) / num;
239 for (
unsigned int i = 0; i < 6; i++) {
249 std::cout <<
"K :" <<
K.
t() << std::endl;
250 std::cout <<
"xc = " <<
m_uc <<
", yc = " <<
m_vc << std::endl;
251 std::cout <<
"n20 = " <<
m_n20 <<
", n11 = " <<
m_n11 <<
", n02 = " <<
m_n02 << std::endl;
252 std::cout <<
"A = " <<
a <<
", B = " <<
b <<
", E (dg) " <<
vpMath::deg(
e) << std::endl;
265 int nbrows =
static_cast<int>(I.
getHeight());
266 int nbcols =
static_cast<int>(I.
getWidth());
268 if (std::fabs(
me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
269 std::cout <<
"Warning: In vpMeEllipse::sample() ";
270 std::cout <<
"function called with sample step = 0. We set it rather to 10 pixels";
275 double perim = M_PI * (3.0 * (
a +
b) - sqrt((3.0 *
a +
b) * (
a + 3.0 *
b)));
277 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
me->
getSampleStep()));
278 double incr = 2.0 * M_PI / nb_pt;
288 double ang =
alpha1 + incr / 2.0;
306 angle.push_back(ang);
321 unsigned int nb_pts_added = 0;
322 int nbrows =
static_cast<int>(I.
getHeight());
323 int nbcols =
static_cast<int>(I.
getWidth());
329 double perim = M_PI * (3.0 * (
a +
b) - sqrt((3.0 *
a +
b) * (
a + 3.0 *
b)));
331 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
me->
getSampleStep()));
332 double incr = 2.0 * M_PI / nb_pt;
336 std::list<double>::iterator angleList =
angle.begin();
337 std::list<vpMeSite>::iterator meList =
list.begin();
338 double ang = *angleList;
341 while (meList !=
list.end()) {
342 double nextang = *angleList;
343 if ((nextang - ang) > 2.0 * incr) {
346 while (ang < (nextang - incr)) {
360 if ((new_ang - ang) > M_PI) {
361 new_ang -= 2.0 * M_PI;
363 else if ((ang - new_ang) > M_PI) {
364 new_ang += 2.0 * M_PI;
366 list.insert(meList, pix);
367 angle.insert(angleList, new_ang);
382 if (nb_pts_added > 0) {
383 std::cout <<
"Number of added points from holes with angles: " << nb_pts_added << std::endl;
388 angleList =
angle.begin();
390 meList =
list.begin();
394 while (meList !=
list.end()) {
395 double nextang = *angleList;
401 ang = (nextang + ang) / 2.0;
415 if ((new_ang - ang) > M_PI) {
416 new_ang -= 2.0 * M_PI;
418 else if ((ang - new_ang) > M_PI) {
419 new_ang += 2.0 * M_PI;
421 list.insert(meList, pix);
422 angle.insert(angleList, new_ang);
436 if (nb_pts_added > 0) {
437 std::cout <<
"Number of added points from holes : " << nb_pts_added << std::endl;
438 angleList =
angle.begin();
439 while (angleList !=
angle.end()) {
441 std::cout <<
"ang = " <<
vpMath::deg(ang) << std::endl;
448 unsigned int nbpts = 0;
451 nbpts =
static_cast<unsigned int>(floor((
m_alphamin -
alpha1 - incr / 2.0) / incr));
454 for (
unsigned int i = 0; i < nbpts; i++) {
468 if ((new_ang - ang) > M_PI) {
469 new_ang -= 2.0 * M_PI;
471 else if ((ang - new_ang) > M_PI) {
472 new_ang += 2.0 * M_PI;
474 list.push_front(pix);
475 angle.push_front(new_ang);
478 std::cout <<
"Add extremity 1, ang = " <<
vpMath::deg(new_ang) << std::endl;
486 if (nb_pts_added > 0) {
487 std::cout <<
"Number of added points from holes and first extremity : " << nb_pts_added << std::endl;
494 nbpts =
static_cast<unsigned int>(floor((
alpha2 - incr / 2.0 -
m_alphamax) / incr));
497 for (
unsigned int i = 0; i < nbpts; i++) {
511 if ((new_ang - ang) > M_PI) {
512 new_ang -= 2.0 * M_PI;
514 else if ((ang - new_ang) > M_PI) {
515 new_ang += 2.0 * M_PI;
518 angle.push_back(new_ang);
521 std::cout <<
"Add extremity 2, ang = " <<
vpMath::deg(new_ang) << std::endl;
530 if (nb_pts_added > 0) {
531 std::cout <<
"In plugHoles(): nb of added points : " << nb_pts_added << std::endl;
541 unsigned int n =
static_cast<unsigned int>(iP.size());
553 for (
unsigned int k = 0; k < n; k++) {
555 double u = (iP[k].get_u() - um) / um;
556 double v = (iP[k].get_v() - vm) / um;
560 b[k] = u * u + v * v;
566 double ratio = vm / um;
567 K[0] =
K[1] = 1.0 / (um * um);
569 K[3] = -(1.0 + x[0] / 2.0) / um;
570 K[4] = -(ratio + x[1] / 2.0) / um;
571 K[5] = -x[2] + 1.0 + ratio * ratio + x[0] + ratio * x[1];
590 for (
unsigned int k = 0; k < n; k++) {
592 double u = (iP[k].get_u() - um) / um;
593 double v = (iP[k].get_v() - vm) / vm;
596 A[k][2] = 2.0 * u * v;
606 for (
unsigned int i = 0; i < 6; i++)
612 K[3] =
K[3] * vm -
K[0] * um -
K[2] * vm;
613 K[4] =
K[4] * um -
K[1] * vm -
K[2] * um;
614 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;
643 for (std::list<vpMeSite>::const_iterator it =
list.begin(); it !=
list.end(); ++it) {
647 double u = (p_me.
jfloat - um) / um;
648 double v = (p_me.
ifloat - vm) / um;
652 b[k] = u * u + v * v;
667 unsigned int iter = 0;
676 while ((iter < 4) && (var > 0.1)) {
677 for (
unsigned int i = 0; i < k; i++) {
678 for (
unsigned int j = 0; j < 3; j++) {
679 DA[i][j] = w[i] * A[i][j];
687 double ratio = vm / um;
688 K[0] =
K[1] = 1.0 / (um * um);
690 K[3] = -(1.0 + x[0] / 2.0) / um;
691 K[4] = -(ratio + x[1] / 2.0) / um;
692 K[5] = -x[2] + 1.0 + ratio * ratio + x[0] + ratio * x[1];
698 var = (xg - xg_prev).frobeniusNorm();
702 for (
unsigned int i = 0; i < k; i++) {
705 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];
827 double previous_ang = -4.0 * M_PI;
829 std::list<double>::iterator angleList =
angle.begin();
830 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
834 double ang = *angleList;
835 meList =
list.erase(meList);
836 angleList =
angle.erase(angleList);
846 double ang = *angleList;
847 meList =
list.erase(meList);
848 angleList =
angle.erase(angleList);
852 printf(
"point %d outlier i : %.0f , j : %0.f, ang = %lf, poids : %lf\n",
858 double ang = *angleList;
862 if ((new_ang - ang) > M_PI) {
863 new_ang -= 2.0 * M_PI;
865 else if ((ang - new_ang) > M_PI) {
866 new_ang += 2.0 * M_PI;
868 *angleList = previous_ang = new_ang;
872 printf(
"point %d inlier i : %.0f , j : %0.f, poids : %lf\n", k, p_me.
ifloat, p_me.
jfloat, w[k]);
882 std::list<double> finAngle;
884 std::list<vpMeSite> finMe;
886 std::list<double>::iterator debutAngleList;
887 std::list<vpMeSite>::iterator debutMeList;
888 angleList =
angle.begin();
889 std::list<vpMeSite>::iterator meList;
890 for (meList =
list.begin(); meList !=
list.end();) {
892 double ang = *angleList;
897 angleList =
angle.erase(angleList);
898 finAngle.push_back(ang);
899 meList =
list.erase(meList);
900 finMe.push_back(p_me);
905 angleList =
angle.erase(angleList);
906 meList =
list.erase(meList);
908 angle.push_front(ang);
909 debutAngleList =
angle.begin();
912 list.push_front(p_me);
913 debutMeList =
list.begin();
919 debutAngleList =
angle.insert(debutAngleList, ang);
921 debutMeList =
list.insert(debutMeList, p_me);
931 angleList =
angle.end();
932 angle.splice(angleList, finAngle);
934 list.splice(meList, finMe);
936 unsigned int numberOfGoodPoints = 0;
937 previous_ang = -4.0 * M_PI;
940 double perim = M_PI * (3.0 * (
a +
b) - sqrt((3.0 *
a +
b) * (
a + 3.0 *
b)));
941 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
me->
getSampleStep()));
942 double incr = 2.0 * M_PI / nb_pt;
953 angleList =
angle.begin();
954 for (std::list<vpMeSite>::iterator meList =
list.begin(); meList !=
list.end();) {
956 double new_ang = *angleList;
958 if ((new_ang - previous_ang) >= (0.6 * incr)) {
959 previous_ang = new_ang;
960 numberOfGoodPoints++;
971 meList =
list.erase(meList);
972 angleList =
angle.erase(angleList);
982 meList =
list.erase(meList);
983 angleList =
angle.erase(angleList);
998 printf(
"Fin leastSquareRobust : nb pts ok = %d \n", numberOfGoodPoints);
1001 return(numberOfGoodPoints);
1015 std::vector<vpImagePoint> iP(n);
1022 std::cout <<
"First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
1024 for (
unsigned int k = 0; k < n; k++) {
1025 std::cout <<
"Click point " << k + 1 <<
"/" << n <<
" on the circle " << std::endl;
1029 std::cout << iP[k] << std::endl;
1034 std::cout <<
"First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
1036 for (
unsigned int k = 0; k < n; k++) {
1037 std::cout <<
"Click point " << k + 1 <<
"/" << n <<
" on the ellipse " << std::endl;
1041 std::cout << iP[k] << std::endl;
1048 bool trackCircle,
bool trackArc)
1084 if (pt1 !=
nullptr && pt2 !=
nullptr) {
1144 printf(
"1st step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1160 printf(
"A click to continue \n");
1169 printf(
"nb of Good points %u, density %d %lf, alphamin %lf, alphamax\n",
1200 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1227 const double &B,
const double &E,
const double &smallalpha,
const double &highalpha,
1228 const vpColor &color,
unsigned int thickness)
1261 const double &E,
const double &smallalpha,
const double &highalpha,
1262 const vpColor &color,
unsigned int thickness)
1270 const double &B,
const double &E,
const double &smallalpha,
const double &highalpha,
1271 const vpColor &color,
unsigned int thickness)
1273 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha,
false, color, thickness,
true,
true);
1277 const double &E,
const double &smallalpha,
const double &highalpha,
1278 const vpColor &color,
unsigned int thickness)
1280 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)
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).
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) override
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).
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)
virtual ~vpMeEllipse() override
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 setRange(const unsigned int &range)
void setSampleStep(const double &sample_step)
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)