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(); }
95 double rho_,
double theta_)
97 vpCDEBUG(1) <<
" begin vpMeLine::initTracking()" << std::endl;
102 PExt[0].ifloat = (float)ip1.
get_i();
103 PExt[0].jfloat = (float)ip1.
get_j();
104 PExt[1].ifloat = (float)ip2.
get_i();
105 PExt[1].jfloat = (float)ip2.
get_j();
108 this->theta = theta_;
115 delta = -theta + M_PI / 2.0;
116 normalizeAngle(delta);
120 expecteddensity = (double)list.size();
126 vpCDEBUG(1) <<
" end vpMeLine::initTracking()" << std::endl;
142 if (std::fabs(me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
144 "moving-edges sample step = 0"));
148 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
149 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
156 double stepi = diffsi / (double)n_sample;
157 double stepj = diffsj / (double)n_sample;
160 double is = PExt[1].ifloat;
161 double js = PExt[1].jfloat;
175 pix.
init((
int)is, (
int)js, delta, 0, sign);
177 pix.
track(I, me,
false);
193 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
194 vpCDEBUG(1) << list.size() <<
" point inserted in the list " << std::endl;
204 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
207 if (fabs(sin(theta)) > 0.9)
209 if ((s.
i < imin) || (s.
i > imax)) {
214 else if (fabs(cos(theta)) > 0.9)
216 if ((s.
j < jmin) || (s.
j > jmax)) {
222 if ((s.
i < imin) || (s.
i > imax) || (s.
j < jmin) || (s.
j > jmax)) {
246 vpCDEBUG(1) <<
"begin vpMeLine::sample() : " << std::endl;
253 if (std::fabs(me->
getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
258 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
259 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
263 double di = diffsi / sqrt(s);
264 double dj = diffsj / sqrt(s);
266 double length_p = sqrt(s);
273 P.
init((
int)PExt[0].ifloat, (
int)PExt[0].jfloat, delta_1, 0, sign);
276 unsigned int memory_range = me->
getRange();
279 for (
int i = 0; i < 3; i++) {
285 if ((P.
i < imin) || (P.
i > imax) || (P.
j < jmin) || (P.
j > jmax)) {
288 }
else if (!outOfImage(P.
i, P.
j, (
int)(me->
getRange() + me->
getMaskSize() + 1), (
int)rows, (int)cols)) {
289 P.
track(I, me,
false);
300 P.
init((
int)PExt[1].ifloat, (
int)PExt[1].jfloat, delta_1, 0, sign);
302 for (
int i = 0; i < 3; i++) {
308 if ((P.
i < imin) || (P.
i > imax) || (P.
j < jmin) || (P.
j > jmax)) {
313 else if (!outOfImage(P.
i, P.
j, (
int)(me->
getRange() + me->
getMaskSize() + 1), (
int)rows, (int)cols)) {
314 P.
track(I, me,
false);
327 vpCDEBUG(1) <<
"end vpMeLine::sample() : ";
328 vpCDEBUG(1) << n_sample <<
" point inserted in the list " << std::endl;
341 unsigned int &_nbFeatures)
345 double deltaNormalized = theta;
346 unsigned int iter = 0;
348 while (deltaNormalized < 0)
349 deltaNormalized += M_PI;
350 while (deltaNormalized > M_PI)
351 deltaNormalized -= M_PI;
354 vecLine[0] = cos(deltaNormalized);
355 vecLine[1] = sin(deltaNormalized);
446 double offset = std::floor(filterX.getRows() / 2.0f);
448 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
449 if (iter != 0 && iter + 1 != list.size()) {
450 double gradientX = 0;
451 double gradientY = 0;
453 double iSite = it->ifloat;
454 double jSite = it->jfloat;
456 for (
unsigned int i = 0; i < filterX.getRows(); i++) {
457 double iImg = iSite + (i - offset);
458 for (
unsigned int j = 0; j < filterX.getCols(); j++) {
459 double jImg = jSite + (j - offset);
471 gradientX += filterX[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
475 for (
unsigned int i = 0; i < filterY.getRows(); i++) {
476 double iImg = iSite + (i - offset);
477 for (
unsigned int j = 0; j < filterY.getCols(); j++) {
478 double jImg = jSite + (j - offset);
490 gradientY += filterY[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
494 double angle = atan2(gradientX, gradientY);
501 vecGrad[0] = cos(angle);
502 vecGrad[1] = sin(angle);
505 double angle1 = acos(vecLine * vecGrad);
506 double angle2 = acos(vecLine * (-vecGrad));
512 _sumErrorRad += (std::min)(angle1, angle2);
540 unsigned int n = numberOfSignal();
542 if ((
double)n < 0.5 * expecteddensity && n > 0) {
543 double delta_new = delta;
546 expecteddensity = (double)list.size();
569 size_t n = list.size();
571 if ((
double)n < 0.5 * expecteddensity /*&& n > 0*/)
573 double delta_new = delta;
575 PExt[0].ifloat = (float)ip1.
get_i();
576 PExt[0].jfloat = (float)ip1.
get_j();
577 PExt[1].ifloat = (float)ip2.
get_i();
578 PExt[1].jfloat = (float)ip2.
get_j();
580 expecteddensity = (double)list.size();
589 void vpMbtMeLine::updateDelta()
596 if (std::fabs(std::fabs(theta) - M_PI) <=
597 vpMath::maximum(std::fabs(theta), (
double)M_PI) * std::numeric_limits<double>::epsilon()) {
601 diff = fabs(theta - theta_1);
602 if (diff > M_PI / 2.0)
607 delta = -theta + M_PI / 2.0;
608 normalizeAngle(delta);
610 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
649 this->theta = theta_;
677 double rho_,
double theta_)
680 this->theta = theta_;
691 reSample(I, ip1, ip2);
700 void vpMbtMeLine::setExtremities()
708 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
722 PExt[0].ifloat = i_min;
723 PExt[0].jfloat = j_min;
724 PExt[1].ifloat = i_max;
725 PExt[1].jfloat = j_max;
728 if (fabs(i_min - i_max) < 25) {
729 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
743 PExt[0].ifloat = i_min;
744 PExt[0].jfloat = j_min;
745 PExt[1].ifloat = i_max;
746 PExt[1].jfloat = j_max;
757 void vpMbtMeLine::bubbleSortI()
760 unsigned int nbElmt = list.size();
761 for (
unsigned int pass = 1; pass < nbElmt; pass++)
764 for (
unsigned int i=0; i < nbElmt-pass; i++)
780 void vpMbtMeLine::bubbleSortJ()
783 unsigned int nbElmt = list.size();
784 for(
unsigned int pass=1; pass < nbElmt; pass++)
787 for (
unsigned int i=0; i < nbElmt-pass; i++)
Implementation of a matrix and operations on matrices.
vpMeSiteState getState() const
#define vpDEBUG_ENABLE(level)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
static const vpColor green
static int round(const double x)
unsigned int getMaskSize() const
static Type maximum(const Type &a, const Type &b)
void set_i(const double ii)
Error that can be emited by the vpTracker class and its derivates.
static const vpColor cyan
double getSampleStep() const
static double sqr(double x)
void setDisplay(vpMeSiteDisplayType select)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
void setState(const vpMeSiteState &flag)
void set_j(const double jj)
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.
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
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)
unsigned int getWidth() const
unsigned int getRange() const
static const vpColor blue