Visual Servoing Platform version 3.5.0
vpMbtMeLine.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Make the complete tracking of an object by using its CAD model
33 *
34 * Authors:
35 * Nicolas Melchior
36 * Romain Tallonneau
37 * Eric Marchand
38 *
39 *****************************************************************************/
40#include <visp3/core/vpConfig.h>
41#ifndef DOXYGEN_SHOULD_SKIP_THIS
42
47#include <algorithm> // (std::min)
48#include <cmath> // std::fabs
49#include <limits> // numeric_limits
50
51#include <visp3/core/vpRobust.h>
52#include <visp3/core/vpTrackingException.h>
53#include <visp3/mbt/vpMbtMeLine.h>
54
56static void normalizeAngle(double &delta)
57{
58 while (delta > M_PI) {
59 delta -= M_PI;
60 }
61 while (delta < -M_PI) {
62 delta += M_PI;
63 }
64}
65
69vpMbtMeLine::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.)
72{
73}
74
78vpMbtMeLine::~vpMbtMeLine() { list.clear(); }
79
95void vpMbtMeLine::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
96 double rho_, double theta_,
97 bool doNoTrack)
98{
99 vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
100
101 try {
102 // 1. On fait ce qui concerne les droites (peut etre vide)
103 // Points extremites
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();
108
109 this->rho = rho_;
110 this->theta = theta_;
111 theta_1 = theta_;
112
113 a = cos(theta);
114 b = sin(theta);
115 c = -rho;
116
117 delta = -theta + M_PI / 2.0;
118 normalizeAngle(delta);
119 delta_1 = delta;
120
121 sample(I, doNoTrack);
122 expecteddensity = (double)list.size();
123
124 if (!doNoTrack)
126 } catch (...) {
127 throw; // throw the original exception
128 }
129 vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
130}
131
139void vpMbtMeLine::sample(const vpImage<unsigned char> &I, bool doNoTrack)
140{
141 int rows = (int)I.getHeight();
142 int cols = (int)I.getWidth();
143 double n_sample;
144
145 // if (me->getSampleStep==0)
146 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
147 throw(vpTrackingException(vpTrackingException::fatalError, "Function vpMbtMeLine::sample() called with "
148 "moving-edges sample step = 0"));
149 }
150
151 // i, j portions of the line_p
152 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
153 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
154
155 double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
156
157 // number of samples along line_p
158 n_sample = length_p / (double)me->getSampleStep();
159
160 double stepi = diffsi / (double)n_sample;
161 double stepj = diffsj / (double)n_sample;
162
163 // Choose starting point
164 double is = PExt[1].ifloat;
165 double js = PExt[1].jfloat;
166
167 // Delete old list
168 list.clear();
169
170 // sample positions at i*me->getSampleStep() interval along the
171 // line_p, starting at PSiteExt[0]
172
173 vpImagePoint ip;
174 for (int i = 0; i <= vpMath::round(n_sample); i++) {
175 // If point is in the image, add to the sample list
176 if (!outOfImage(vpMath::round(is), vpMath::round(js), (int)(me->getRange() + me->getMaskSize() + 1), (int)rows,
177 (int)cols) && vpMeTracker::inMask(m_mask, vpMath::round(is), vpMath::round(js))) {
178 vpMeSite pix; //= list.value();
179 pix.init((int)is, (int)js, delta, 0, sign);
180
181 if (!doNoTrack)
182 pix.track(I, me, false);
183
184 pix.setDisplay(selectDisplay);
185
186 if (vpDEBUG_ENABLE(3)) {
187 ip.set_i(is);
188 ip.set_j(js);
190 }
191
192 list.push_back(pix);
193 }
194 is += stepi;
195 js += stepj;
196 }
197
198 vpCDEBUG(1) << "end vpMeLine::sample() : ";
199 vpCDEBUG(1) << list.size() << " point inserted in the list " << std::endl;
200}
201
207void vpMbtMeLine::suppressPoints(const vpImage<unsigned char> &I)
208{
209 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
210 vpMeSite s = *it; // current reference pixel
211
212 if (fabs(sin(theta)) > 0.9) // Vertical line management
213 {
214 if ((s.i < imin) || (s.i > imax)) {
216 }
217 }
218
219 else if (fabs(cos(theta)) > 0.9) // Horizontal line management
220 {
221 if ((s.j < jmin) || (s.j > jmax)) {
223 }
224 }
225
226 else {
227 if ((s.i < imin) || (s.i > imax) || (s.j < jmin) || (s.j > jmax)) {
229 }
230 }
231
232 if (outOfImage(s.i, s.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)I.getHeight(), (int)I.getWidth())) {
234 }
235
237 it = list.erase(it);
238 else
239 ++it;
240 }
241}
242
249void vpMbtMeLine::seekExtremities(const vpImage<unsigned char> &I)
250{
251 vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
252
253 int rows = (int)I.getHeight();
254 int cols = (int)I.getWidth();
255 double n_sample;
256
257 // if (me->getSampleStep()==0)
258 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
259 throw(vpTrackingException(vpTrackingException::fatalError, "Function called with sample step = 0"));
260 }
261
262 // i, j portions of the line_p
263 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
264 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
265
266 double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
267
268 double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
269 double dj = diffsj / sqrt(s);
270
271 double length_p = sqrt(s); /*(vpMath::sqr(diffsi)+vpMath::sqr(diffsj))*/
272
273 // number of samples along line_p
274 n_sample = length_p / (double)me->getSampleStep();
275 double sample_step = (double)me->getSampleStep();
276
277 vpMeSite P;
278 P.init((int)PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign);
279 P.setDisplay(selectDisplay);
280
281 unsigned int memory_range = me->getRange();
282 me->setRange(1);
283
284 for (int i = 0; i < 3; i++) {
285 P.ifloat = P.ifloat + di * sample_step;
286 P.i = (int)P.ifloat;
287 P.jfloat = P.jfloat + dj * sample_step;
288 P.j = (int)P.jfloat;
289
290 if ((P.i < imin) || (P.i > imax) || (P.j < jmin) || (P.j > jmax)) {
291 if (vpDEBUG_ENABLE(3))
293 } else if (!outOfImage(P.i, P.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)rows, (int)cols)) {
294 P.track(I, me, false);
295
297 list.push_back(P);
298 if (vpDEBUG_ENABLE(3))
300 } else if (vpDEBUG_ENABLE(3))
302 }
303 }
304
305 P.init((int)PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign);
306 P.setDisplay(selectDisplay);
307 for (int i = 0; i < 3; i++) {
308 P.ifloat = P.ifloat - di * sample_step;
309 P.i = (int)P.ifloat;
310 P.jfloat = P.jfloat - dj * sample_step;
311 P.j = (int)P.jfloat;
312
313 if ((P.i < imin) || (P.i > imax) || (P.j < jmin) || (P.j > jmax)) {
314 if (vpDEBUG_ENABLE(3))
316 }
317
318 else if (!outOfImage(P.i, P.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)rows, (int)cols)) {
319 P.track(I, me, false);
320
322 list.push_back(P);
323 if (vpDEBUG_ENABLE(3))
325 } else if (vpDEBUG_ENABLE(3))
327 }
328 }
329
330 me->setRange(memory_range);
331
332 vpCDEBUG(1) << "end vpMeLine::sample() : ";
333 vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
334}
335
350void vpMbtMeLine::computeProjectionError(const vpImage<unsigned char> &_I, double &_sumErrorRad,
351 unsigned int &_nbFeatures,
352 const vpMatrix &SobelX, const vpMatrix &SobelY,
353 bool display, unsigned int length,
354 unsigned int thickness)
355{
356 _sumErrorRad = 0;
357 _nbFeatures = 0;
358 double deltaNormalized = theta;
359 unsigned int iter = 0;
360
361 while (deltaNormalized < 0)
362 deltaNormalized += M_PI;
363 while (deltaNormalized > M_PI)
364 deltaNormalized -= M_PI;
365
366 vpColVector vecLine(2);
367 vecLine[0] = cos(deltaNormalized);
368 vecLine[1] = sin(deltaNormalized);
369 vecLine.normalize();
370
371 double offset = std::floor(SobelX.getRows() / 2.0f);
372
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;
377
378 double iSite = it->ifloat;
379 double jSite = it->jfloat;
380
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);
385
386 if (iImg < 0)
387 iImg = 0.0;
388 if (jImg < 0)
389 jImg = 0.0;
390
391 if (iImg > _I.getHeight() - 1)
392 iImg = _I.getHeight() - 1;
393 if (jImg > _I.getWidth() - 1)
394 jImg = _I.getWidth() - 1;
395
396 gradientX += SobelX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
397 }
398 }
399
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);
404
405 if (iImg < 0)
406 iImg = 0.0;
407 if (jImg < 0)
408 jImg = 0.0;
409
410 if (iImg > _I.getHeight() - 1)
411 iImg = _I.getHeight() - 1;
412 if (jImg > _I.getWidth() - 1)
413 jImg = _I.getWidth() - 1;
414
415 gradientY += SobelY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
416 }
417 }
418
419 double angle = atan2(gradientX, gradientY);
420 while (angle < 0)
421 angle += M_PI;
422 while (angle > M_PI)
423 angle -= M_PI;
424
425 vpColVector vecGrad(2);
426 vecGrad[0] = cos(angle);
427 vecGrad[1] = sin(angle);
428 vecGrad.normalize();
429
430 double angle1 = acos(vecLine * vecGrad);
431 double angle2 = acos(vecLine * (-vecGrad));
432
433 if (display) {
434 vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length*cos(deltaNormalized)),
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) {
438 vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length*cos(angle)),
439 (int)(it->get_j() + length*sin(angle)), vpColor::red,
440 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
441 } else {
442 vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length*cos(angle+M_PI)),
443 (int)(it->get_j() + length*sin(angle+M_PI)), vpColor::red,
444 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
445 }
446 }
447
448 // double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
449 // double angle2 = sqrt(vpMath::sqr(deltaNormalized-
450 // (angle-M_PI)));
451
452 _sumErrorRad += (std::min)(angle1, angle2);
453
454 // if(std::fabs(deltaNormalized-angle) > M_PI / 2)
455 // {
456 // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI
457 // / 2;
458 // } else {
459 // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
460 // }
461
462 _nbFeatures++;
463 }
464 iter++;
465 }
466}
467
478void vpMbtMeLine::reSample(const vpImage<unsigned char> &I)
479{
480 unsigned int n = numberOfSignal();
481
482 if ((double)n < 0.5 * expecteddensity && n > 0) {
483 double delta_new = delta;
484 delta = delta_1;
485 sample(I);
486 expecteddensity = (double)list.size();
487 delta = delta_new;
488 // 2. On appelle ce qui n'est pas specifique
489 {
491 }
492 }
493}
494
507void vpMbtMeLine::reSample(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2)
508{
509 size_t n = list.size();
510
511 if ((double)n < 0.5 * expecteddensity /*&& n > 0*/) // n is always > 0
512 {
513 double delta_new = delta;
514 delta = delta_1;
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();
519 sample(I);
520 expecteddensity = (double)list.size();
521 delta = delta_new;
523 }
524}
525
529void vpMbtMeLine::updateDelta()
530{
531 vpMeSite p_me;
532
533 double diff = 0;
534
535 // if(fabs(theta) == M_PI )
536 if (std::fabs(std::fabs(theta) - M_PI) <=
537 vpMath::maximum(std::fabs(theta), (double)M_PI) * std::numeric_limits<double>::epsilon()) {
538 theta = 0;
539 }
540
541 diff = fabs(theta - theta_1);
542 if (diff > M_PI / 2.0)
543 sign *= -1;
544
545 theta_1 = theta;
546
547 delta = -theta + M_PI / 2.0;
548 normalizeAngle(delta);
549
550 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
551 p_me = *it;
552 p_me.alpha = delta;
553 p_me.mask_sign = sign;
554 *it = p_me;
555 }
556 delta_1 = delta;
557}
558
564void vpMbtMeLine::track(const vpImage<unsigned char> &I)
565{
566 try {
568 if (m_mask != NULL) {
569 // Expected density could be modified if some vpMeSite are no more tracked because they are outside the mask.
570 expecteddensity = (double)list.size();
571 }
572 } catch (...) {
573 throw; // throw the original exception
574 }
575}
576
585void vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, double rho_, double theta_)
586{
587 this->rho = rho_;
588 this->theta = theta_;
589 a = cos(theta);
590 b = sin(theta);
591 c = -rho;
592 // recherche de point aux extremite de la droites
593 // dans le cas d'un glissement
594 suppressPoints(I);
595 seekExtremities(I);
596 suppressPoints(I);
597 setExtremities();
598 // reechantillonage si necessaire
599 reSample(I);
600
601 // remet a jour l'angle delta pour chaque point de la liste
602 updateDelta();
603}
604
615void vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
616 double rho_, double theta_)
617{
618 this->rho = rho_;
619 this->theta = theta_;
620 a = cos(theta);
621 b = sin(theta);
622 c = -rho;
623 // recherche de point aux extremite de la droites
624 // dans le cas d'un glissement
625 suppressPoints(I);
626 seekExtremities(I);
627 suppressPoints(I);
628 setExtremities();
629 // reechantillonage si necessaire
630 reSample(I, ip1, ip2);
631
632 // remet a jour l'angle delta pour chaque point de la liste
633 updateDelta();
634}
635
639void vpMbtMeLine::setExtremities()
640{
641 double i_min = +1e6;
642 double j_min = +1e6;
643 double i_max = -1;
644 double j_max = -1;
645
646 // Loop through list of sites to track
647 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
648 vpMeSite s = *it; // current reference pixel
649 if (s.ifloat < i_min) {
650 i_min = s.ifloat;
651 j_min = s.jfloat;
652 }
653
654 if (s.ifloat > i_max) {
655 i_max = s.ifloat;
656 j_max = s.jfloat;
657 }
658 }
659
660 if (!list.empty()) {
661 PExt[0].ifloat = i_min;
662 PExt[0].jfloat = j_min;
663 PExt[1].ifloat = i_max;
664 PExt[1].jfloat = j_max;
665 }
666
667 if (fabs(i_min - i_max) < 25) {
668 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
669 vpMeSite s = *it; // current reference pixel
670 if (s.jfloat < j_min) {
671 i_min = s.ifloat;
672 j_min = s.jfloat;
673 }
674
675 if (s.jfloat > j_max) {
676 i_max = s.ifloat;
677 j_max = s.jfloat;
678 }
679 }
680
681 if (!list.empty()) {
682 PExt[0].ifloat = i_min;
683 PExt[0].jfloat = j_min;
684 PExt[1].ifloat = i_max;
685 PExt[1].jfloat = j_max;
686 }
687 bubbleSortJ();
688 }
689
690 else
691 bubbleSortI();
692}
693
694static bool sortByI(const vpMeSite &s1, const vpMeSite &s2) { return (s1.ifloat > s2.ifloat); }
695
696void vpMbtMeLine::bubbleSortI()
697{
698#if 0
699 unsigned int nbElmt = list.size();
700 for (unsigned int pass = 1; pass < nbElmt; pass++)
701 {
702 list.front();
703 for (unsigned int i=0; i < nbElmt-pass; i++)
704 {
705 vpMeSite s1 = list.value();
706 vpMeSite s2 = list.nextValue();
707 if (s1.ifloat > s2.ifloat)
708 list.swapRight();
709 else
710 list.next();
711 }
712 }
713#endif
714 list.sort(sortByI);
715}
716
717static bool sortByJ(const vpMeSite &s1, const vpMeSite &s2) { return (s1.jfloat > s2.jfloat); }
718
719void vpMbtMeLine::bubbleSortJ()
720{
721#if 0
722 unsigned int nbElmt = list.size();
723 for(unsigned int pass=1; pass < nbElmt; pass++)
724 {
725 list.front();
726 for (unsigned int i=0; i < nbElmt-pass; i++)
727 {
728 vpMeSite s1 = list.value();
729 vpMeSite s2 = list.nextValue();
730 if (s1.jfloat > s2.jfloat)
731 list.swapRight();
732 else
733 list.next();
734 }
735 }
736#endif
737 list.sort(sortByJ);
738}
739
740#endif
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
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: vpImagePoint.h:88
void set_j(double jj)
Definition: vpImagePoint.h:177
double get_j() const
Definition: vpImagePoint.h:214
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static double sqr(double x)
Definition: vpMath.h:116
static int round(double x)
Definition: vpMath.h:247
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
int j
Definition: vpMeSite.h:87
@ CONSTRAST
Point removed due to a contrast problem.
Definition: vpMeSite.h:79
@ TOO_NEAR
Point removed because too near image borders.
Definition: vpMeSite.h:82
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
double ifloat
Definition: vpMeSite.h:89
int i
Definition: vpMeSite.h:87
int mask_sign
Definition: vpMeSite.h:91
double alpha
Definition: vpMeSite.h:93
void init()
Definition: vpMeSite.cpp:66
double jfloat
Definition: vpMeSite.h:89
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
void initTracking(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
Error that can be emited by the vpTracker class and its derivates.
#define vpCDEBUG(level)
Definition: vpDebug.h:511
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538