39#ifndef DOXYGEN_SHOULD_SKIP_THIS
41#include <visp3/mbt/vpMbtMeEllipse.h>
43#include <visp3/core/vpDebug.h>
44#include <visp3/core/vpImagePoint.h>
45#include <visp3/core/vpRobust.h>
46#include <visp3/core/vpTrackingException.h>
47#include <visp3/me/vpMe.h>
56vpMbtMeEllipse::vpMbtMeEllipse()
64vpMbtMeEllipse::vpMbtMeEllipse(
const vpMbtMeEllipse &me_ellipse)
71vpMbtMeEllipse::~vpMbtMeEllipse()
91 unsigned int &nbFeatures,
93 bool display,
unsigned int length,
94 unsigned int thickness)
99 double offset =
static_cast<double>(std::floor(SobelX.
getRows() / 2.0f));
100 int height =
static_cast<int>(I.
getHeight());
101 int width =
static_cast<int>(I.
getWidth());
103 double max_iImg = height - 1.;
104 double max_jImg = width - 1.;
109 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
110 double iSite = it->ifloat;
111 double jSite = it->jfloat;
115 double theta = computeTheta(
vpImagePoint(iSite, jSite));
117 vecSite[0] = cos(theta);
118 vecSite[1] = sin(theta);
121 double gradientX = 0;
122 double gradientY = 0;
124 for (
unsigned int i = 0; i < SobelX.
getRows(); i++) {
125 double iImg = iSite + (i - offset);
126 for (
unsigned int j = 0; j < SobelX.
getCols(); j++) {
127 double jImg = jSite + (j - offset);
139 gradientX += SobelX[i][j] * I((
unsigned int)iImg, (
unsigned int)jImg);
143 for (
unsigned int i = 0; i < SobelY.
getRows(); i++) {
144 double iImg = iSite + (i - offset);
145 for (
unsigned int j = 0; j < SobelY.
getCols(); j++) {
146 double jImg = jSite + (j - offset);
158 gradientY += SobelY[i][j] * I((
unsigned int)iImg, (
unsigned int)jImg);
162 double angle = atan2(gradientY, gradientX);
168 vecGrad[0] = cos(angle);
169 vecGrad[1] = sin(angle);
172 double angle1 = acos(vecSite * vecGrad);
173 double angle2 = acos(vecSite * (-vecGrad));
177 static_cast<int>(it->get_j() + length*sin(theta)),
vpColor::blue,
178 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
179 if (angle1 < angle2) {
181 static_cast<int>(it->get_j() + length*sin(angle)),
vpColor::red,
182 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
185 static_cast<int>(it->get_j() + length*sin(angle+M_PI)),
vpColor::red,
186 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
190 sumErrorRad += (std::min)(angle1, angle2);
198 double n20_p,
double n11_p,
double n02_p,
bool doNotTrack,
201 if (pt1 != NULL && pt2 != NULL) {
206 m_uc = center_p.
get_u();
207 m_vc = center_p.
get_v();
216 alpha1 = computeAngleOnEllipse(*pt1);
217 alpha2 = computeAngleOnEllipse(*pt2);
218 if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
219 alpha2 += 2.0 * M_PI;
230 computePointOnEllipse(alpha1, ip);
236 sample(I, doNotTrack);
255 if (m_mask != NULL) {
257 m_expectedDensity =
static_cast<unsigned int>(list.size());
268 double n20_p,
double n11_p,
double n02_p)
270 m_uc = center_p.
get_u();
271 m_vc = center_p.
get_v();
302 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
306 unsigned int n = numberOfSignal();
307 if ((
double)n < 0.9 * m_expectedDensity) {
335 int nbrows =
static_cast<int>(I.
getHeight());
336 int nbcols =
static_cast<int>(I.
getWidth());
338 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
339 std::cout <<
"In vpMeEllipse::sample: ";
340 std::cout <<
"function called with sample step = 0, set to 10 dg";
341 me->setSampleStep(10.0);
345 m_expectedDensity =
static_cast<unsigned int>(floor((alpha2 - alpha1) / incr));
346#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
347 expecteddensity =
static_cast<double>(m_expectedDensity);
351 double ang = alpha1 + ((alpha2 - alpha1) -
static_cast<double>(m_expectedDensity) * incr)/2.0;
353 for (
unsigned int i = 0; i < m_expectedDensity; i++) {
355 computePointOnEllipse(ang,iP);
359 double theta = computeTheta(iP);
366 angle.push_back(ang);
379void vpMbtMeEllipse::suppressPoints()
382 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
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 emited 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 used by the tracker.
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)
Track sampled pixels.
Error that can be emited by the vpTracker class and its derivates.