36 #ifndef DOXYGEN_SHOULD_SKIP_THIS
38 #include <visp3/mbt/vpMbtMeEllipse.h>
40 #include <visp3/core/vpDebug.h>
41 #include <visp3/core/vpImagePoint.h>
42 #include <visp3/core/vpRobust.h>
43 #include <visp3/core/vpTrackingException.h>
44 #include <visp3/me/vpMe.h>
53 vpMbtMeEllipse::vpMbtMeEllipse() :
vpMeEllipse() { }
58 vpMbtMeEllipse::vpMbtMeEllipse(
const vpMbtMeEllipse &me_ellipse) :
vpMeEllipse(me_ellipse) { }
77 bool display,
unsigned int length,
unsigned int thickness)
82 double offset =
static_cast<double>(std::floor(SobelX.
getRows() / 2.0f));
83 int height =
static_cast<int>(I.
getHeight());
84 int width =
static_cast<int>(I.
getWidth());
86 double max_iImg = height - 1.;
87 double max_jImg = width - 1.;
92 for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
93 double iSite = it->m_ifloat;
94 double jSite = it->m_jfloat;
100 vecSite[0] = cos(theta);
101 vecSite[1] = sin(theta);
104 double gradientX = 0;
105 double gradientY = 0;
107 for (
unsigned int i = 0; i < SobelX.
getRows(); i++) {
108 double iImg = iSite + (i - offset);
109 for (
unsigned int j = 0; j < SobelX.
getCols(); j++) {
110 double jImg = jSite + (j - offset);
122 gradientX += SobelX[i][j] * I((
unsigned int)iImg, (
unsigned int)jImg);
126 for (
unsigned int i = 0; i < SobelY.
getRows(); i++) {
127 double iImg = iSite + (i - offset);
128 for (
unsigned int j = 0; j < SobelY.
getCols(); j++) {
129 double jImg = jSite + (j - offset);
141 gradientY += SobelY[i][j] * I((
unsigned int)iImg, (
unsigned int)jImg);
145 double angle = atan2(gradientY, gradientX);
151 vecGrad[0] = cos(angle);
152 vecGrad[1] = sin(angle);
155 double angle1 = acos(vecSite * vecGrad);
156 double angle2 = acos(vecSite * (-vecGrad));
160 static_cast<int>(it->get_j() + length * sin(theta)),
vpColor::blue,
161 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
162 if (angle1 < angle2) {
164 static_cast<int>(it->get_j() + length * sin(angle)),
vpColor::red,
165 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
169 static_cast<int>(it->get_i() + length * cos(angle + M_PI)),
170 static_cast<int>(it->get_j() + length * sin(angle + M_PI)),
vpColor::red,
171 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
175 sumErrorRad += std::min<double>(angle1, angle2);
183 double n11_p,
double n02_p,
bool doNotTrack,
vpImagePoint *pt1,
186 if (pt1 !=
nullptr && pt2 !=
nullptr) {
191 m_uc = center_p.
get_u();
192 m_vc = center_p.
get_v();
201 m_alpha1 = computeAngleOnEllipse(*pt1);
202 m_alpha2 = computeAngleOnEllipse(*pt2);
203 if ((m_alpha2 <= m_alpha1) || (std::fabs(m_alpha2 - m_alpha1) < m_arcEpsilon)) {
204 m_alpha2 += 2.0 * M_PI;
212 m_alpha2 = 2.0 * M_PI;
215 computePointOnEllipse(m_alpha1, ip);
222 sample(I, doNotTrack);
242 if (m_mask !=
nullptr) {
244 m_expectedDensity =
static_cast<unsigned int>(m_meList.size());
256 double n11_p,
double n02_p)
258 m_uc = center_p.
get_u();
259 m_vc = center_p.
get_v();
290 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
294 unsigned int n = numberOfSignal();
295 if ((
double)n < 0.9 * m_expectedDensity) {
323 int nbrows =
static_cast<int>(I.
getHeight());
324 int nbcols =
static_cast<int>(I.
getWidth());
326 if (std::fabs(m_me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
327 std::cout <<
"In vpMeEllipse::sample: ";
328 std::cout <<
"function called with sample step = 0, set to 10 dg";
329 m_me->setSampleStep(10.0);
333 m_expectedDensity =
static_cast<unsigned int>(floor((m_alpha2 - m_alpha1) / incr));
336 double ang = m_alpha1 + ((m_alpha2 - m_alpha1) -
static_cast<double>(m_expectedDensity) * incr) / 2.0;
338 for (
unsigned int i = 0; i < m_expectedDensity; i++) {
340 computePointOnEllipse(ang, iP);
344 double theta = computeTheta(iP);
350 m_meList.push_back(pix);
351 m_angleList.push_back(ang);
364 void vpMbtMeEllipse::suppressPoints()
367 for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end();) {
370 it = m_meList.erase(it);
unsigned int getCols() const
unsigned int getRows() const
Implementation of column vector and the associated operations.
static const vpColor blue
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
error that can be emitted by ViSP classes.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
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)
Implementation of a matrix and operations on matrices.
Class that tracks an ellipse using moving edges.
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)
vpMeSiteState getState() const
void setState(const vpMeSiteState &flag)
void initTracking(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
Error that can be emitted by the vpTracker class and its derivatives.
@ initializationError
Tracker initialization error.