35 #include <visp3/core/vpMatrixException.h>
36 #include <visp3/core/vpTrackingException.h>
37 #include <visp3/core/vpImagePoint.h>
38 #include <visp3/me/vpMe.h>
39 #include <visp3/me/vpMeEllipse.h>
41 #ifndef VP_ME_ELLIPSE_REGULAR_SAMPLING
42 #define VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES
48 : m_K(), m_iPc(), m_a(0.), m_b(0.), m_e(0.), m_iP1(), m_iP2(), m_alpha1(0), m_ce(0.), m_se(0.), m_angleList(), m_m00(0.),
49 m_thresholdWeight(0.2), m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.),
50 m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackCircle(false), m_trackArc(false), m_arcEpsilon(1e-6)
52 const unsigned int val_2 = 2;
53 const unsigned int val_6 = 6;
63 :
vpMeTracker(me_ellipse), m_K(me_ellipse.m_K), m_iPc(me_ellipse.m_iPc), m_a(me_ellipse.m_a), m_b(me_ellipse.m_b), m_e(me_ellipse.m_e),
64 m_iP1(me_ellipse.m_iP1), m_iP2(me_ellipse.m_iP2), m_alpha1(me_ellipse.m_alpha1), m_alpha2(me_ellipse.m_alpha2), m_ce(me_ellipse.m_ce),
65 m_se(me_ellipse.m_se), m_angleList(me_ellipse.m_angleList), m_m00(me_ellipse.m_m00),
66 m_thresholdWeight(me_ellipse.m_thresholdWeight),
67 m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
68 m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
69 m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
70 m_trackCircle(me_ellipse.m_trackCircle), m_trackArc(me_ellipse.m_trackArc), m_arcEpsilon(me_ellipse.m_arcEpsilon)
81 double u = iP.
get_u();
82 double v = iP.
get_v();
89 double A = (
m_K[0] * u) + (
m_K[2] * v) +
m_K[3];
90 double B = (
m_K[1] * v) + (
m_K[2] * u) +
m_K[4];
92 double theta = atan2(B, A);
103 std::list<vpMeSite>::iterator end =
m_meList.end();
104 for (std::list<vpMeSite>::iterator it =
m_meList.begin(); it != end; ++it) {
121 #ifdef VP_ME_ELLIPSE_REGULAR_SAMPLING
124 double co = cos(angle);
125 double si = sin(angle);
127 double u = co * coef;
128 double v = si * coef;
131 #elif defined(VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES)
137 double u =
m_a * cos(angle);
138 double v =
m_b * sin(angle);
152 #ifdef VP_ME_ELLIPSE_REGULAR_SAMPLING
154 double du = pt.
get_u() - uc;
155 double dv = pt.
get_v() - vc;
156 double ang = atan2(dv, du) -
m_e;
160 else if (ang < -M_PI) {
163 #ifdef VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES
168 double co = cos(ang);
169 double si = sin(ang);
170 double coeff = 1.0 / sqrt((
m_b *
m_b * co * co) + (
m_a *
m_a * si * si));
175 #elif defined(VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES)
181 double angle = atan2(si, co);
190 double d = (num * num) + (4.0 *
m_n11 *
m_n11);
191 if (d <= std::numeric_limits<double>::epsilon()) {
198 m_e = atan2(2.0 *
m_n11, num) / 2.0;
204 m_a = sqrt(2.0 * (num + d));
205 m_b = sqrt(2.0 * (num - d));
211 const unsigned int index_0 = 0;
212 const unsigned int index_1 = 1;
213 const unsigned int index_2 = 2;
214 const unsigned int index_3 = 3;
215 const unsigned int index_4 = 4;
216 const unsigned int index_5 = 5;
234 const unsigned int index_0 = 0;
235 const unsigned int index_1 = 1;
236 const unsigned int index_2 = 2;
237 const unsigned int index_3 = 3;
238 const unsigned int index_4 = 4;
239 const unsigned int index_5 = 5;
241 double num = (
m_K[index_0] *
m_K[index_1]) - (
m_K[index_2] *
m_K[index_2]);
246 m_uc = ((
m_K[index_2] *
m_K[index_4]) - (
m_K[index_1] *
m_K[index_3])) / num;
247 m_vc = ((
m_K[index_2] *
m_K[index_3]) - (
m_K[index_0] *
m_K[index_4])) / num;
259 unsigned int Ksize =
m_K.
size();
260 for (
unsigned int i = 0; i < Ksize; ++i) {
267 std::cout <<
"K :" <<
m_K.
t() << std::endl;
268 std::cout <<
"xc = " <<
m_uc <<
", yc = " <<
m_vc << std::endl;
269 std::cout <<
"n20 = " <<
m_n20 <<
", n11 = " <<
m_n11 <<
", n02 = " <<
m_n02 << std::endl;
283 int nbrows =
static_cast<int>(I.
getHeight());
284 int nbcols =
static_cast<int>(I.
getWidth());
286 if (std::fabs(
m_me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
287 std::cout <<
"Warning: In vpMeEllipse::sample() ";
288 std::cout <<
"function called with sample step = 0. We set it rather to 10 pixels";
293 double perim = M_PI * ((3.0 * (
m_a +
m_b)) - sqrt(((3.0 *
m_a) +
m_b) * (
m_a + (3.0 *
m_b))));
295 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
m_me->
getSampleStep()));
296 double incr = (2.0 * M_PI) / nb_pt;
306 double ang =
m_alpha1 + (incr / 2.0);
314 unsigned int is_uint =
static_cast<unsigned int>(iP.
get_i());
315 unsigned int js_uint =
static_cast<unsigned int>(iP.
get_j());
318 const unsigned int crossSize = 5;
329 double contrastThreshold = fabs(convolution) * marginRatio;
347 unsigned int nb_pts_added = 0;
348 int nbrows =
static_cast<int>(I.
getHeight());
349 int nbcols =
static_cast<int>(I.
getWidth());
350 const unsigned int range_default = 2;
356 double perim = M_PI * ((3.0 * (
m_a +
m_b)) - sqrt(((3.0 *
m_a) +
m_b) * (
m_a + (3.0 *
m_b))));
358 unsigned int nb_pt =
static_cast<unsigned int>(floor(perim /
m_me->
getSampleStep()));
359 double incr = (2.0 * M_PI) / nb_pt;
363 std::list<double>::iterator angleList =
m_angleList.begin();
364 std::list<vpMeSite>::iterator meList =
m_meList.begin();
366 double ang = *angleList;
371 double nextang = *angleList;
372 if ((nextang - ang) > (2.0 * incr)) {
375 while (ang < (nextang - incr)) {
378 unsigned int is_uint =
static_cast<unsigned int>(iP.
get_i());
379 unsigned int js_uint =
static_cast<unsigned int>(iP.
get_j());
387 double contrastThreshold = fabs(convolution) * marginRatio;
394 if ((new_ang - ang) > M_PI) {
395 new_ang -= 2.0 * M_PI;
397 else if ((ang - new_ang) > M_PI) {
398 new_ang += 2.0 * M_PI;
420 double nextang = *angleList;
426 ang = (nextang + ang) / 2.0;
429 unsigned int is_uint =
static_cast<unsigned int>(iP.
get_i());
430 unsigned int js_uint =
static_cast<unsigned int>(iP.
get_j());
438 double contrastThreshold = fabs(convolution) * marginRatio;
445 if ((new_ang - ang) > M_PI) {
446 new_ang -= 2.0 * M_PI;
448 else if ((ang - new_ang) > M_PI) {
449 new_ang += 2.0 * M_PI;
465 unsigned int nbpts = 0;
468 nbpts =
static_cast<unsigned int>(floor((
m_alphamin -
m_alpha1 - (incr / 2.0)) / incr));
471 for (
unsigned int i = 0; i < nbpts; ++i) {
474 unsigned int is_uint =
static_cast<unsigned int>(iP.
get_i());
475 unsigned int js_uint =
static_cast<unsigned int>(iP.
get_j());
484 double contrastThreshold = fabs(convolution) * marginRatio;
491 if ((new_ang - ang) > M_PI) {
492 new_ang -= 2.0 * M_PI;
494 else if ((ang - new_ang) > M_PI) {
495 new_ang += 2.0 * M_PI;
508 nbpts =
static_cast<unsigned int>(floor((
m_alpha2 - (incr / 2.0) -
m_alphamax) / incr));
512 for (
unsigned int i = 0; i < nbpts; ++i) {
515 unsigned int is_uint =
static_cast<unsigned int>(iP.
get_i());
516 unsigned int js_uint =
static_cast<unsigned int>(iP.
get_j());
524 double contrastThreshold = fabs(convolution) * marginRatio;
531 if ((new_ang - ang) > M_PI) {
532 new_ang -= 2.0 * M_PI;
534 else if ((ang - new_ang) > M_PI) {
535 new_ang += 2.0 * M_PI;
562 const unsigned int nForCircle = 3;
567 std::vector<vpImagePoint> iP(n);
574 std::cout <<
"First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
576 for (
unsigned int k = 0; k < n; ++k) {
577 std::cout <<
"Click point " << (k + 1) <<
"/" << n <<
" on the circle " << std::endl;
579 const unsigned int crossSize = 10;
582 std::cout << iP[k] << std::endl;
587 std::cout <<
"First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
589 for (
unsigned int k = 0; k < n; ++k) {
590 std::cout <<
"Click point " << (k + 1) <<
"/" << n <<
" on the ellipse " << std::endl;
592 const unsigned int crossSize = 10;
595 std::cout << iP[k] << std::endl;
601 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
607 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
608 if (!opt_ips.has_value())
610 if (opt_ips ==
nullptr)
613 initTracking(I, trackCircle, trackArc);
617 if (opt_ips->size() != 0) {
618 initTracking(I, *opt_ips, trackCircle, trackArc);
623 const unsigned int nForCircle = 3;
624 m_trackCircle = trackCircle;
629 m_trackArc = trackArc;
635 std::cout <<
"First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
637 for (
unsigned int k = 0; k < n; ++k) {
638 std::cout <<
"Click point " << (k + 1) <<
"/" << n <<
" on the circle " << std::endl;
640 const unsigned int crossSize = 10;
643 std::cout << (*opt_ips)[k] << std::endl;
648 std::cout <<
"First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
650 for (
unsigned int k = 0; k < n; ++k) {
651 std::cout <<
"Click point " << (k + 1) <<
"/" << n <<
" on the ellipse " << std::endl;
653 const unsigned int crossSize = 10;
656 std::cout << (*opt_ips)[k] << std::endl;
659 initTracking(I, *opt_ips, trackCircle, trackArc);
663 bool trackCircle,
bool trackArc)
698 const unsigned int index_0 = 0;
699 const unsigned int index_1 = 1;
700 const unsigned int index_2 = 2;
701 const unsigned int index_3 = 3;
702 const unsigned int index_4 = 4;
705 if ((pt1 !=
nullptr) && (pt2 !=
nullptr)) {
709 m_uc = param[index_0];
710 m_vc = param[index_1];
711 m_n20 = param[index_2];
712 m_n11 = param[index_3];
713 m_n02 = param[index_4];
769 const unsigned int minNbGoodPoints = 5;
791 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
818 const double &B,
const double &E,
const double &smallalpha,
const double &highalpha,
819 const vpColor &color,
unsigned int thickness)
852 const double &E,
const double &smallalpha,
const double &highalpha,
853 const vpColor &color,
unsigned int thickness)
861 const double &B,
const double &E,
const double &smallalpha,
const double &highalpha,
862 const vpColor &color,
unsigned int thickness)
864 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha,
false, color, thickness,
true,
true);
868 const double &E,
const double &smallalpha,
const double &highalpha,
869 const vpColor &color,
unsigned int thickness)
871 vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha,
false, color, thickness,
true,
true);
unsigned int size() const
Return the number of elements of the 2D array.
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 bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_ij(double ii, double jj)
void set_uv(double u, double v)
unsigned int getWidth() const
unsigned int getHeight() const
static double deg(double rad)
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.
void computePointOnEllipse(const double angle, vpImagePoint &iP)
void display(const vpImage< unsigned char > &I, const vpColor &col, unsigned int thickness=1)
vpImagePoint m_iPc
The coordinates of the ellipse center.
double m_vc
Value of v coordinate of iPc.
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
unsigned int plugHoles(const vpImage< unsigned char > &I)
void printParameters() const
double m_uc
Value of u coordinate of iPc.
bool m_trackCircle
Track a circle (true) or an ellipse (false).
double m_m00
Ellipse area.
std::list< double > m_angleList
Stores the value in increasing order of the angle on the ellipse for each vpMeSite.
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)
double m_a
is the semi major axis of the ellipse.
unsigned int leastSquareRobust(const vpImage< unsigned char > &I)
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
double m_b
is the semi minor axis of the ellipse.
double computeTheta(const vpImagePoint &iP) const
double m_se
Value of sin(e).
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 .
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) VP_OVERRIDE
void track(const vpImage< unsigned char > &I)
virtual ~vpMeEllipse() VP_OVERRIDE
double m_ce
Value of cos(e).
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 computeAngleOnEllipse(const vpImagePoint &pt) const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
@ NO_SUPPRESSION
Point successfully tracked.
void setDisplay(vpMeSiteDisplayType select)
double m_ifloat
Subpixel coordinates along i of a site.
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
double m_alpha
Angle of tangent at site.
vpMeSiteState getState() const
double get_ifloat() const
double m_jfloat
Subpixel coordinates along j of a site.
double get_jfloat() const
void setContrastThreshold(const double &thresh, const vpMe &me)
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
void setState(const vpMeSiteState &flag)
Contains abstract elements for a Distance to Feature type feature.
void initTracking(const vpImage< unsigned char > &I)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
vpMeSite::vpMeSiteDisplayType m_selectDisplay
Moving-edges display type.
void track(const vpImage< unsigned char > &I)
vpMe * m_me
Moving edges initialisation parameters.
static bool inRoiMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
void display(const vpImage< unsigned char > &I)
bool outOfImage(int i, int j, int border, int nrows, int ncols)
const vpImage< bool > * m_maskCandidates
Mask used to determine candidate points for initialization in an image.
std::list< vpMeSite > m_meList
static bool inMeMaskCandidates(const vpImage< bool > *meMaskCandidates, unsigned int i, unsigned int j)
void setRange(const unsigned int &range)
double getThresholdMarginRatio() const
void setSampleStep(const double &sample_step)
double getSampleStep() const
unsigned int getRange() const
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ fatalError
Tracker fatal error.