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> 56 vpMbtMeEllipse::vpMbtMeEllipse()
57 : iPc(), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
64 vpMbtMeEllipse::vpMbtMeEllipse(
const vpMbtMeEllipse &meellipse)
65 :
vpMeTracker(meellipse), iPc(meellipse.iPc), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.),
66 thresholdWeight(0.), expecteddensity(0.)
75 mu11 = meellipse.mu11;
76 mu20 = meellipse.mu20;
77 mu02 = meellipse.mu02;
79 expecteddensity = meellipse.expecteddensity;
85 vpMbtMeEllipse::~vpMbtMeEllipse() { list.clear(); }
98 unsigned int &_nbFeatures)
191 double offset = std::floor(filterX.getRows() / 2.0f);
196 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
197 double iSite = it->ifloat;
198 double jSite = it->jfloat;
203 double theta = atan((-mu02 * jSite + mu02 * iPc.get_j() + mu11 * iSite - mu11 * iPc.get_i()) /
204 (mu20 * iSite - mu11 * jSite + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
207 double deltaNormalized = theta;
208 while (deltaNormalized < 0)
209 deltaNormalized += M_PI;
210 while (deltaNormalized > M_PI)
211 deltaNormalized -= M_PI;
214 vecSite[0] = cos(deltaNormalized);
215 vecSite[1] = sin(deltaNormalized);
218 double gradientX = 0;
219 double gradientY = 0;
221 for (
unsigned int i = 0; i < filterX.getRows(); i++) {
222 double iImg = iSite + (i - offset);
223 for (
unsigned int j = 0; j < filterX.getCols(); j++) {
224 double jImg = jSite + (j - offset);
236 gradientX += filterX[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
240 for (
unsigned int i = 0; i < filterY.getRows(); i++) {
241 double iImg = iSite + (i - offset);
242 for (
unsigned int j = 0; j < filterY.getCols(); j++) {
243 double jImg = jSite + (j - offset);
255 gradientY += filterY[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
259 double angle = atan2(gradientY, gradientX);
278 vecGrad[0] = cos(angle);
279 vecGrad[1] = sin(angle);
282 double angle1 = acos(vecSite * vecGrad);
283 double angle2 = acos(vecSite * (-vecGrad));
285 _sumErrorRad += (std::min)(angle1, angle2);
306 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
314 if (std::fabs(me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
315 std::cout <<
"In vpMbtMeEllipse::sample: ";
316 std::cout <<
"function called with sample step = 0";
323 double t = (a - b) / (a + b);
325 int nb_points_to_track = (int)(circumference / me->
getSampleStep());
326 double incr = 2 * M_PI / nb_points_to_track;
335 for (
int pt = 0; pt < nb_points_to_track; pt++) {
336 double j = a * cos(k);
337 double i = b * sin(k);
339 double iP_j = iPc.get_j() + ce * j - se * i;
340 double iP_i = iPc.get_i() + se * j + ce * i;
349 double theta = atan((-mu02 * iP_j + mu02 * iPc.get_j() + mu11 * iP_i - mu11 * iPc.get_i()) /
350 (mu20 * iP_i - mu11 * iP_j + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
354 pix.
init((
int)iP_i, (
int)iP_j, theta);
385 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
389 unsigned int n = numberOfSignal();
390 if ((
double)n < 0.9 * expecteddensity) {
401 void vpMbtMeEllipse::updateTheta()
404 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
411 double theta = atan((-mu02 * p_me.
jfloat + mu02 * iPc.get_j() + mu11 * p_me.
ifloat - mu11 * iPc.get_i()) /
412 (mu20 * p_me.
ifloat - mu11 * p_me.
jfloat + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
424 void vpMbtMeEllipse::suppressPoints()
427 for (std::list<vpMeSite>::iterator itList = list.begin(); itList != list.end();) {
430 itList = list.erase(itList);
458 if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
461 a = sqrt((mu20_p + mu02_p + val_p) / 2);
462 b = sqrt((mu20_p + mu02_p - val_p) / 2);
464 e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
502 double mu11_p,
double mu02_p)
509 if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
512 a = sqrt((mu20_p + mu02_p + val_p) / 2);
513 b = sqrt((mu20_p + mu02_p - val_p) / 2);
515 e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
534 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS Implementation of a matrix and operations on matrices.
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
vpMeSiteState getState() const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Class to define colors available for display functionnalities.
error that can be emited by ViSP classes.
static int round(const double x)
void set_i(const double ii)
Error that can be emited by the vpTracker class and its derivates.
double getSampleStep() const
static double sqr(double x)
void setDisplay(vpMeSiteDisplayType select)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
Contains abstract elements for a Distance to Feature type feature.
void setState(const vpMeSiteState &flag)
void set_j(const double jj)
unsigned int getHeight() const
Implementation of column vector and the associated operations.
void initTracking(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const