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>
54 static void normalizeAngle(
double &delta)
56 while (delta > M_PI) {
59 while (delta < -M_PI) {
67 vpMbtMeLine::vpMbtMeLine()
68 : m_rho(0.), m_theta(0.), m_theta_1(M_PI / 2), m_delta(0.), m_delta_1(0), m_sign(1), m_a(0.), m_b(0.), m_c(0.),
69 imin(0), imax(0), jmin(0), jmax(0), expecteddensity(0.)
75 vpMbtMeLine::~vpMbtMeLine()
96 double rho,
double theta,
bool doNoTrack)
98 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
103 m_PExt[0].m_ifloat = (float)ip1.
get_i();
104 m_PExt[0].m_jfloat = (float)ip1.
get_j();
105 m_PExt[1].m_ifloat = (float)ip2.
get_i();
106 m_PExt[1].m_jfloat = (float)ip2.
get_j();
116 m_delta = -m_theta + M_PI / 2.0;
117 normalizeAngle(m_delta);
120 sample(I, doNoTrack);
121 expecteddensity = (double)m_meList.size();
129 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
141 int nbrows =
static_cast<int>(I.
getHeight());
142 int nbcols =
static_cast<int>(I.
getWidth());
145 if (std::fabs(m_me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
147 "moving-edges sample step = 0"));
151 double diffsi = m_PExt[0].m_ifloat - m_PExt[1].m_ifloat;
152 double diffsj = m_PExt[0].m_jfloat - m_PExt[1].m_jfloat;
157 n_sample = length_p / (double)m_me->getSampleStep();
159 double stepi = diffsi / (double)n_sample;
160 double stepj = diffsj / (double)n_sample;
163 double is = m_PExt[1].m_ifloat;
164 double js = m_PExt[1].m_jfloat;
176 unsigned int is_uint =
static_cast<unsigned int>(is);
177 unsigned int js_uint =
static_cast<unsigned int>(js);
179 if ((!outOfImage(iP, (
int)(m_me->getRange() + m_me->getMaskSize() + 1), nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)
180 && inMeMaskCandidates(m_maskCandidates, is_uint, js_uint)) {
185 const double marginRatio = m_me->getThresholdMarginRatio();
187 double contrastThreshold = fabs(convolution) * marginRatio;
191 pix.
track(I, m_me,
false);
198 m_meList.push_back(pix);
204 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
205 vpCDEBUG(1) << m_meList.size() <<
" point inserted in the list " << std::endl;
215 for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end();) {
218 if (fabs(sin(m_theta)) > 0.9)
220 if ((s.
m_i < imin) || (s.
m_i > imax)) {
225 else if (fabs(cos(m_theta)) > 0.9)
227 if ((s.
m_j < jmin) || (s.
m_j > jmax)) {
233 if ((s.
m_i < imin) || (s.
m_i > imax) || (s.
m_j < jmin) || (s.
m_j > jmax)) {
238 if (outOfImage(s.
m_i, s.
m_j, (
int)(m_me->getRange() + m_me->getMaskSize() + 1), (
int)I.
getHeight(), (
int)I.
getWidth())) {
243 it = m_meList.erase(it);
257 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
264 if (std::fabs(m_me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
269 double diffsi = m_PExt[0].m_ifloat - m_PExt[1].m_ifloat;
270 double diffsj = m_PExt[0].m_jfloat - m_PExt[1].m_jfloat;
274 double di = diffsi / sqrt(s);
275 double dj = diffsj / sqrt(s);
277 double length_p = sqrt(s);
280 n_sample = length_p / (double)m_me->getSampleStep();
281 double sample_step = (double)m_me->getSampleStep();
284 P.
init((
int)m_PExt[0].m_ifloat, (int)m_PExt[0].m_jfloat, m_delta_1, 0, m_sign);
287 unsigned int memory_range = m_me->getRange();
290 for (
int i = 0; i < 3; i++) {
296 if ((P.
m_i < imin) || (P.
m_i > imax) || (P.
m_j < jmin) || (P.
m_j > jmax)) {
300 else if (!outOfImage(P.
m_i, P.
m_j, (
int)(m_me->getRange() + m_me->getMaskSize() + 1), (
int)rows, (int)cols)) {
301 P.
track(I, m_me,
false);
304 m_meList.push_back(P);
313 P.
init((
int)m_PExt[1].m_ifloat, (int)m_PExt[1].m_jfloat, m_delta_1, 0, m_sign);
315 for (
int i = 0; i < 3; i++) {
321 if ((P.
m_i < imin) || (P.
m_i > imax) || (P.
m_j < jmin) || (P.
m_j > jmax)) {
326 else if (!outOfImage(P.
m_i, P.
m_j, (
int)(m_me->getRange() + m_me->getMaskSize() + 1), (
int)rows, (int)cols)) {
327 P.
track(I, m_me,
false);
330 m_meList.push_back(P);
339 m_me->setRange(memory_range);
341 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
342 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
361 bool display,
unsigned int length,
unsigned int thickness)
365 double deltaNormalized = m_theta;
366 unsigned int iter = 0;
368 while (deltaNormalized < 0) {
369 deltaNormalized += M_PI;
371 while (deltaNormalized > M_PI) {
372 deltaNormalized -= M_PI;
376 vecLine[0] = cos(deltaNormalized);
377 vecLine[1] = sin(deltaNormalized);
380 double offset = std::floor(SobelX.
getRows() / 2.0f);
382 for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
383 if (iter != 0 && iter + 1 != m_meList.size()) {
384 double gradientX = 0;
385 double gradientY = 0;
387 double iSite = it->m_ifloat;
388 double jSite = it->m_jfloat;
390 for (
unsigned int i = 0; i < SobelX.
getRows(); i++) {
391 double iImg = iSite + (i - offset);
392 for (
unsigned int j = 0; j < SobelX.
getCols(); j++) {
393 double jImg = jSite + (j - offset);
405 gradientX += SobelX[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
409 for (
unsigned int i = 0; i < SobelY.
getRows(); i++) {
410 double iImg = iSite + (i - offset);
411 for (
unsigned int j = 0; j < SobelY.
getCols(); j++) {
412 double jImg = jSite + (j - offset);
424 gradientY += SobelY[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
428 double angle = atan2(gradientX, gradientY);
435 vecGrad[0] = cos(angle);
436 vecGrad[1] = sin(angle);
439 double angle1 = acos(vecLine * vecGrad);
440 double angle2 = acos(vecLine * (-vecGrad));
444 (
int)(it->get_j() + length * sin(deltaNormalized)),
vpColor::blue,
445 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
446 if (angle1 < angle2) {
448 (
int)(it->get_j() + length * sin(angle)),
vpColor::red, length >= 20 ? length / 5 : 4,
449 length >= 20 ? length / 10 : 2, thickness);
453 (
int)(it->get_j() + length * sin(angle + M_PI)),
vpColor::red,
454 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
458 _sumErrorRad += std::min<double>(angle1, angle2);
478 unsigned int n = numberOfSignal();
480 if ((
double)n < 0.5 * expecteddensity && n > 0) {
481 double delta_new = m_delta;
484 expecteddensity = (double)m_meList.size();
505 size_t n = m_meList.size();
507 if ((
double)n < 0.5 * expecteddensity /*&& n > 0*/)
509 double delta_new = m_delta;
511 m_PExt[0].m_ifloat = (float)ip1.
get_i();
512 m_PExt[0].m_jfloat = (float)ip1.
get_j();
513 m_PExt[1].m_ifloat = (float)ip2.
get_i();
514 m_PExt[1].m_jfloat = (float)ip2.
get_j();
516 expecteddensity = (double)m_meList.size();
525 void vpMbtMeLine::updateDelta()
532 if (std::fabs(std::fabs(m_theta) - M_PI) <=
533 vpMath::maximum(std::fabs(m_theta), (
double)M_PI) * std::numeric_limits<double>::epsilon()) {
537 diff = fabs(m_theta - m_theta_1);
538 if (diff > M_PI / 2.0) {
544 m_delta = -m_theta + M_PI / 2.0;
545 normalizeAngle(m_delta);
547 for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
565 if (m_mask !=
nullptr) {
567 expecteddensity = (double)m_meList.size();
614 double rho,
double theta)
628 reSample(I, ip1, ip2);
637 void vpMbtMeLine::setExtremities()
645 for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
658 if (!m_meList.empty()) {
659 m_PExt[0].m_ifloat = i_min;
660 m_PExt[0].m_jfloat = j_min;
661 m_PExt[1].m_ifloat = i_max;
662 m_PExt[1].m_jfloat = j_max;
665 if (fabs(i_min - i_max) < 25) {
666 for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != m_meList.end(); ++it) {
679 if (!m_meList.empty()) {
680 m_PExt[0].m_ifloat = i_min;
681 m_PExt[0].m_jfloat = j_min;
682 m_PExt[1].m_ifloat = i_max;
683 m_PExt[1].m_jfloat = j_max;
694 void vpMbtMeLine::bubbleSortI()
697 unsigned int nbElmt = m_meList.size();
698 for (
unsigned int pass = 1; pass < nbElmt; pass++) {
700 for (
unsigned int i = 0; i < nbElmt-pass; i++) {
704 m_meList.swapRight();
710 m_meList.sort(sortByI);
715 void vpMbtMeLine::bubbleSortJ()
718 unsigned int nbElmt = m_meList.size();
719 for (
unsigned int pass = 1; pass < nbElmt; pass++) {
721 for (
unsigned int i = 0; i < nbElmt-pass; i++) {
725 m_meList.swapRight();
731 m_meList.sort(sortByJ);
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 ...
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 m_mask_sign
Mask sign.
@ TOO_NEAR
Point not tracked anymore, since too near from its neighbor.
@ CONTRAST
Point not tracked due to a contrast problem, but retained in the ME list.
@ 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
int m_j
Integer coordinates along j of a site.
int m_i
Integer coordinate along i of a site.
double m_jfloat
Subpixel coordinates along j of a site.
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)
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.
@ fatalError
Tracker fatal error.
#define vpDEBUG_ENABLE(level)