40 #include <visp3/core/vpConfig.h> 41 #ifndef DOXYGEN_SHOULD_SKIP_THIS 51 #include <visp3/core/vpRobust.h> 52 #include <visp3/core/vpTrackingException.h> 53 #include <visp3/mbt/vpMbtMeLine.h> 56 static void normalizeAngle(
double &delta)
58 while (delta > M_PI) {
61 while (delta < -M_PI) {
69 vpMbtMeLine::vpMbtMeLine()
70 : 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),
71 jmin(0), jmax(0), expecteddensity(0.)
78 vpMbtMeLine::~vpMbtMeLine() { list.clear(); }
96 double rho_,
double theta_,
99 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
104 PExt[0].ifloat = (float)ip1.
get_i();
105 PExt[0].jfloat = (float)ip1.
get_j();
106 PExt[1].ifloat = (float)ip2.
get_i();
107 PExt[1].jfloat = (float)ip2.
get_j();
110 this->theta = theta_;
117 delta = -theta + M_PI / 2.0;
118 normalizeAngle(delta);
121 sample(I, doNoTrack);
122 expecteddensity = (double)list.size();
129 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
146 if (std::fabs(me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
148 "moving-edges sample step = 0"));
152 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
153 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
160 double stepi = diffsi / (double)n_sample;
161 double stepj = diffsj / (double)n_sample;
164 double is = PExt[1].ifloat;
165 double js = PExt[1].jfloat;
179 pix.
init((
int)is, (
int)js, delta, 0, sign);
182 pix.
track(I, me,
false);
198 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
199 vpCDEBUG(1) << list.size() <<
" point inserted in the list " << std::endl;
209 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
212 if (fabs(sin(theta)) > 0.9)
214 if ((s.
i < imin) || (s.
i > imax)) {
219 else if (fabs(cos(theta)) > 0.9)
221 if ((s.
j < jmin) || (s.
j > jmax)) {
227 if ((s.
i < imin) || (s.
i > imax) || (s.
j < jmin) || (s.
j > jmax)) {
251 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
258 if (std::fabs(me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
263 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
264 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
268 double di = diffsi / sqrt(s);
269 double dj = diffsj / sqrt(s);
271 double length_p = sqrt(s);
278 P.
init((
int)PExt[0].ifloat, (
int)PExt[0].jfloat, delta_1, 0, sign);
281 unsigned int memory_range = me->
getRange();
284 for (
int i = 0; i < 3; i++) {
290 if ((P.
i < imin) || (P.
i > imax) || (P.
j < jmin) || (P.
j > jmax)) {
293 }
else if (!outOfImage(P.
i, P.
j, (
int)(me->
getRange() + me->
getMaskSize() + 1), (
int)rows, (int)cols)) {
294 P.
track(I, me,
false);
305 P.
init((
int)PExt[1].ifloat, (
int)PExt[1].jfloat, delta_1, 0, sign);
307 for (
int i = 0; i < 3; i++) {
313 if ((P.
i < imin) || (P.
i > imax) || (P.
j < jmin) || (P.
j > jmax)) {
318 else if (!outOfImage(P.
i, P.
j, (
int)(me->
getRange() + me->
getMaskSize() + 1), (
int)rows, (int)cols)) {
319 P.
track(I, me,
false);
332 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
333 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
351 unsigned int &_nbFeatures,
353 bool display,
unsigned int length,
354 unsigned int thickness)
358 double deltaNormalized = theta;
359 unsigned int iter = 0;
361 while (deltaNormalized < 0)
362 deltaNormalized += M_PI;
363 while (deltaNormalized > M_PI)
364 deltaNormalized -= M_PI;
367 vecLine[0] = cos(deltaNormalized);
368 vecLine[1] = sin(deltaNormalized);
371 double offset = std::floor(SobelX.
getRows() / 2.0f);
373 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
374 if (iter != 0 && iter + 1 != list.size()) {
375 double gradientX = 0;
376 double gradientY = 0;
378 double iSite = it->ifloat;
379 double jSite = it->jfloat;
381 for (
unsigned int i = 0; i < SobelX.
getRows(); i++) {
382 double iImg = iSite + (i - offset);
383 for (
unsigned int j = 0; j < SobelX.
getCols(); j++) {
384 double jImg = jSite + (j - offset);
396 gradientX += SobelX[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
400 for (
unsigned int i = 0; i < SobelY.
getRows(); i++) {
401 double iImg = iSite + (i - offset);
402 for (
unsigned int j = 0; j < SobelY.
getCols(); j++) {
403 double jImg = jSite + (j - offset);
415 gradientY += SobelY[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
419 double angle = atan2(gradientX, gradientY);
426 vecGrad[0] = cos(angle);
427 vecGrad[1] = sin(angle);
430 double angle1 = acos(vecLine * vecGrad);
431 double angle2 = acos(vecLine * (-vecGrad));
435 (
int)(it->get_j() + length*sin(deltaNormalized)),
vpColor::blue,
436 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
437 if (angle1 < angle2) {
440 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
443 (
int)(it->get_j() + length*sin(angle+M_PI)),
vpColor::red,
444 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
452 _sumErrorRad += (std::min)(angle1, angle2);
480 unsigned int n = numberOfSignal();
482 if ((
double)n < 0.5 * expecteddensity && n > 0) {
483 double delta_new = delta;
486 expecteddensity = (double)list.size();
509 size_t n = list.size();
511 if ((
double)n < 0.5 * expecteddensity /*&& n > 0*/)
513 double delta_new = delta;
515 PExt[0].ifloat = (float)ip1.
get_i();
516 PExt[0].jfloat = (float)ip1.
get_j();
517 PExt[1].ifloat = (float)ip2.
get_i();
518 PExt[1].jfloat = (float)ip2.
get_j();
520 expecteddensity = (double)list.size();
529 void vpMbtMeLine::updateDelta()
536 if (std::fabs(std::fabs(theta) - M_PI) <=
537 vpMath::maximum(std::fabs(theta), (
double)M_PI) * std::numeric_limits<double>::epsilon()) {
541 diff = fabs(theta - theta_1);
542 if (diff > M_PI / 2.0)
547 delta = -theta + M_PI / 2.0;
548 normalizeAngle(delta);
550 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
568 if (m_mask != NULL) {
570 expecteddensity = (double)list.size();
588 this->theta = theta_;
616 double rho_,
double theta_)
619 this->theta = theta_;
630 reSample(I, ip1, ip2);
639 void vpMbtMeLine::setExtremities()
647 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
661 PExt[0].ifloat = i_min;
662 PExt[0].jfloat = j_min;
663 PExt[1].ifloat = i_max;
664 PExt[1].jfloat = j_max;
667 if (fabs(i_min - i_max) < 25) {
668 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
682 PExt[0].ifloat = i_min;
683 PExt[0].jfloat = j_min;
684 PExt[1].ifloat = i_max;
685 PExt[1].jfloat = j_max;
696 void vpMbtMeLine::bubbleSortI()
699 unsigned int nbElmt = list.size();
700 for (
unsigned int pass = 1; pass < nbElmt; pass++)
703 for (
unsigned int i=0; i < nbElmt-pass; i++)
719 void vpMbtMeLine::bubbleSortJ()
722 unsigned int nbElmt = list.size();
723 for(
unsigned int pass=1; pass < nbElmt; pass++)
726 for (
unsigned int i=0; i < nbElmt-pass; i++)
Implementation of a matrix and operations on matrices.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpMeSiteState getState() const
#define vpDEBUG_ENABLE(level)
Point removed because too near image borders.
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
unsigned int getRows() const
static const vpColor green
unsigned int getMaskSize() const
static Type maximum(const Type &a, const Type &b)
unsigned int getCols() const
Error that can be emited by the vpTracker class and its derivates.
Point used by the tracker.
static const vpColor cyan
double getSampleStep() const
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)
static double sqr(double x)
Point removed due to a contrast problem.
void setDisplay(vpMeSiteDisplayType select)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
void setState(const vpMeSiteState &flag)
static int round(double x)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
unsigned int getHeight() const
Implementation of column vector and the associated operations.
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
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 ...
void setRange(const unsigned int &r)
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
unsigned int getWidth() const
unsigned int getRange() const
static const vpColor blue