38#include <visp3/core/vpConfig.h>
39#ifndef DOXYGEN_SHOULD_SKIP_THIS
49#include <visp3/core/vpRobust.h>
50#include <visp3/core/vpTrackingException.h>
51#include <visp3/mbt/vpMbtMeLine.h>
54static void normalizeAngle(
double &delta)
56 while (delta > M_PI) {
59 while (delta < -M_PI) {
67vpMbtMeLine::vpMbtMeLine()
68 : rho(0.), theta(0.), theta_1(M_PI / 2), delta(0.), delta_1(0), sign(1), a(0.), b(0.), c(0.), imin(0), imax(0),
69 jmin(0), jmax(0), expecteddensity(0.)
76vpMbtMeLine::~vpMbtMeLine() { list.clear(); }
94 double rho_,
double theta_,
bool doNoTrack)
96 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
101 PExt[0].ifloat = (float)ip1.
get_i();
102 PExt[0].jfloat = (float)ip1.
get_j();
103 PExt[1].ifloat = (float)ip2.
get_i();
104 PExt[1].jfloat = (float)ip2.
get_j();
107 this->theta = theta_;
114 delta = -theta + M_PI / 2.0;
115 normalizeAngle(delta);
118 sample(I, doNoTrack);
119 expecteddensity = (double)list.size();
126 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
143 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
145 "moving-edges sample step = 0"));
149 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
150 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
155 n_sample = length_p / (double)me->getSampleStep();
157 double stepi = diffsi / (double)n_sample;
158 double stepj = diffsj / (double)n_sample;
161 double is = PExt[1].ifloat;
162 double js = PExt[1].jfloat;
177 pix.
init((
int)is, (
int)js, delta, 0, sign);
180 pix.
track(I, me,
false);
196 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
197 vpCDEBUG(1) << list.size() <<
" point inserted in the list " << std::endl;
207 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
210 if (fabs(sin(theta)) > 0.9)
212 if ((s.
i < imin) || (s.
i > imax)) {
217 else if (fabs(cos(theta)) > 0.9)
219 if ((s.
j < jmin) || (s.
j > jmax)) {
225 if ((s.
i < imin) || (s.
i > imax) || (s.
j < jmin) || (s.
j > jmax)) {
230 if (outOfImage(s.
i, s.
j, (
int)(me->getRange() + me->getMaskSize() + 1), (
int)I.
getHeight(), (
int)I.
getWidth())) {
249 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
256 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
261 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
262 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
266 double di = diffsi / sqrt(s);
267 double dj = diffsj / sqrt(s);
269 double length_p = sqrt(s);
272 n_sample = length_p / (double)me->getSampleStep();
273 double sample_step = (double)me->getSampleStep();
276 P.
init((
int)PExt[0].ifloat, (
int)PExt[0].jfloat, delta_1, 0, sign);
279 unsigned int memory_range = me->getRange();
282 for (
int i = 0; i < 3; i++) {
288 if ((P.
i < imin) || (P.
i > imax) || (P.
j < jmin) || (P.
j > jmax)) {
291 }
else if (!outOfImage(P.
i, P.
j, (
int)(me->getRange() + me->getMaskSize() + 1), (
int)rows, (
int)cols)) {
292 P.
track(I, me,
false);
303 P.
init((
int)PExt[1].ifloat, (
int)PExt[1].jfloat, delta_1, 0, sign);
305 for (
int i = 0; i < 3; i++) {
311 if ((P.
i < imin) || (P.
i > imax) || (P.
j < jmin) || (P.
j > jmax)) {
316 else if (!outOfImage(P.
i, P.
j, (
int)(me->getRange() + me->getMaskSize() + 1), (
int)rows, (
int)cols)) {
317 P.
track(I, me,
false);
328 me->setRange(memory_range);
330 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
331 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
350 bool display,
unsigned int length,
unsigned int thickness)
354 double deltaNormalized = theta;
355 unsigned int iter = 0;
357 while (deltaNormalized < 0)
358 deltaNormalized += M_PI;
359 while (deltaNormalized > M_PI)
360 deltaNormalized -= M_PI;
363 vecLine[0] = cos(deltaNormalized);
364 vecLine[1] = sin(deltaNormalized);
367 double offset = std::floor(SobelX.
getRows() / 2.0f);
369 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
370 if (iter != 0 && iter + 1 != list.size()) {
371 double gradientX = 0;
372 double gradientY = 0;
374 double iSite = it->ifloat;
375 double jSite = it->jfloat;
377 for (
unsigned int i = 0; i < SobelX.
getRows(); i++) {
378 double iImg = iSite + (i - offset);
379 for (
unsigned int j = 0; j < SobelX.
getCols(); j++) {
380 double jImg = jSite + (j - offset);
392 gradientX += SobelX[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
396 for (
unsigned int i = 0; i < SobelY.
getRows(); i++) {
397 double iImg = iSite + (i - offset);
398 for (
unsigned int j = 0; j < SobelY.
getCols(); j++) {
399 double jImg = jSite + (j - offset);
411 gradientY += SobelY[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
415 double angle = atan2(gradientX, gradientY);
422 vecGrad[0] = cos(angle);
423 vecGrad[1] = sin(angle);
426 double angle1 = acos(vecLine * vecGrad);
427 double angle2 = acos(vecLine * (-vecGrad));
431 (
int)(it->get_j() + length * sin(deltaNormalized)),
vpColor::blue,
432 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
433 if (angle1 < angle2) {
435 (
int)(it->get_j() + length * sin(angle)),
vpColor::red, length >= 20 ? length / 5 : 4,
436 length >= 20 ? length / 10 : 2, thickness);
439 (
int)(it->get_j() + length * sin(angle + M_PI)),
vpColor::red,
440 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
448 _sumErrorRad += (std::min)(angle1, angle2);
476 unsigned int n = numberOfSignal();
478 if ((
double)n < 0.5 * expecteddensity && n > 0) {
479 double delta_new = delta;
482 expecteddensity = (double)list.size();
505 size_t n = list.size();
507 if ((
double)n < 0.5 * expecteddensity /*&& n > 0*/)
509 double delta_new = delta;
511 PExt[0].ifloat = (float)ip1.
get_i();
512 PExt[0].jfloat = (float)ip1.
get_j();
513 PExt[1].ifloat = (float)ip2.
get_i();
514 PExt[1].jfloat = (float)ip2.
get_j();
516 expecteddensity = (double)list.size();
525void vpMbtMeLine::updateDelta()
532 if (std::fabs(std::fabs(theta) - M_PI) <=
533 vpMath::maximum(std::fabs(theta), (
double)M_PI) * std::numeric_limits<double>::epsilon()) {
537 diff = fabs(theta - theta_1);
538 if (diff > M_PI / 2.0)
543 delta = -theta + M_PI / 2.0;
544 normalizeAngle(delta);
546 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
564 if (m_mask != NULL) {
566 expecteddensity = (double)list.size();
584 this->theta = theta_;
612 double rho_,
double theta_)
615 this->theta = theta_;
626 reSample(I, ip1, ip2);
635void vpMbtMeLine::setExtremities()
643 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
657 PExt[0].ifloat = i_min;
658 PExt[0].jfloat = j_min;
659 PExt[1].ifloat = i_max;
660 PExt[1].jfloat = j_max;
663 if (fabs(i_min - i_max) < 25) {
664 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
678 PExt[0].ifloat = i_min;
679 PExt[0].jfloat = j_min;
680 PExt[1].ifloat = i_max;
681 PExt[1].jfloat = j_max;
692void vpMbtMeLine::bubbleSortI()
695 unsigned int nbElmt = list.size();
696 for (
unsigned int pass = 1; pass < nbElmt; pass++)
699 for (
unsigned int i=0; i < nbElmt-pass; i++)
715void vpMbtMeLine::bubbleSortJ()
718 unsigned int nbElmt = list.size();
719 for(
unsigned int pass=1; pass < nbElmt; pass++)
722 for (
unsigned int i=0; i < nbElmt-pass; i++)
unsigned int getCols() const
unsigned int getRows() const
Implementation of column vector and the associated operations.
static const vpColor cyan
static const vpColor blue
static const vpColor green
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static int round(double x)
Implementation of a matrix and operations on matrices.
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
int j
Coordinates along j of a site.
@ TOO_NEAR
Point removed because too near image borders.
@ CONTRAST
Point removed due to a contrast problem.
@ NO_SUPPRESSION
Point used by the tracker.
void setDisplay(vpMeSiteDisplayType select)
double ifloat
Floating coordinates along i of a site.
int i
Coordinate 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)
void initTracking(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
Error that can be emitted by the vpTracker class and its derivatives.
@ fatalError
Tracker fatal error.
#define vpDEBUG_ENABLE(level)