Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbtMeLine.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 * Romain Tallonneau
36 *
37*****************************************************************************/
38#include <visp3/core/vpConfig.h>
39#ifndef DOXYGEN_SHOULD_SKIP_THIS
40
45#include <algorithm> // (std::min)
46#include <cmath> // std::fabs
47#include <limits> // numeric_limits
48
49#include <visp3/core/vpRobust.h>
50#include <visp3/core/vpTrackingException.h>
51#include <visp3/mbt/vpMbtMeLine.h>
52
54static void normalizeAngle(double &delta)
55{
56 while (delta > M_PI) {
57 delta -= M_PI;
58 }
59 while (delta < -M_PI) {
60 delta += M_PI;
61 }
62}
63
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.)
70{
71}
72
76vpMbtMeLine::~vpMbtMeLine() { list.clear(); }
77
93void vpMbtMeLine::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
94 double rho_, double theta_, bool doNoTrack)
95{
96 vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
97
98 try {
99 // 1. On fait ce qui concerne les droites (peut etre vide)
100 // Points extremites
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();
105
106 this->rho = rho_;
107 this->theta = theta_;
108 theta_1 = theta_;
109
110 a = cos(theta);
111 b = sin(theta);
112 c = -rho;
113
114 delta = -theta + M_PI / 2.0;
115 normalizeAngle(delta);
116 delta_1 = delta;
117
118 sample(I, doNoTrack);
119 expecteddensity = (double)list.size();
120
121 if (!doNoTrack)
123 } catch (...) {
124 throw; // throw the original exception
125 }
126 vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
127}
128
136void vpMbtMeLine::sample(const vpImage<unsigned char> &I, bool doNoTrack)
137{
138 int rows = (int)I.getHeight();
139 int cols = (int)I.getWidth();
140 double n_sample;
141
142 // if (me->getSampleStep==0)
143 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
144 throw(vpTrackingException(vpTrackingException::fatalError, "Function vpMbtMeLine::sample() called with "
145 "moving-edges sample step = 0"));
146 }
147
148 // i, j portions of the line_p
149 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
150 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
151
152 double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
153
154 // number of samples along line_p
155 n_sample = length_p / (double)me->getSampleStep();
156
157 double stepi = diffsi / (double)n_sample;
158 double stepj = diffsj / (double)n_sample;
159
160 // Choose starting point
161 double is = PExt[1].ifloat;
162 double js = PExt[1].jfloat;
163
164 // Delete old list
165 list.clear();
166
167 // sample positions at i*me->getSampleStep() interval along the
168 // line_p, starting at PSiteExt[0]
169
170 vpImagePoint ip;
171 for (int i = 0; i <= vpMath::round(n_sample); i++) {
172 // If point is in the image, add to the sample list
173 if (!outOfImage(vpMath::round(is), vpMath::round(js), (int)(me->getRange() + me->getMaskSize() + 1), (int)rows,
174 (int)cols) &&
176 vpMeSite pix; //= list.value();
177 pix.init((int)is, (int)js, delta, 0, sign);
178
179 if (!doNoTrack)
180 pix.track(I, me, false);
181
182 pix.setDisplay(selectDisplay);
183
184 if (vpDEBUG_ENABLE(3)) {
185 ip.set_i(is);
186 ip.set_j(js);
188 }
189
190 list.push_back(pix);
191 }
192 is += stepi;
193 js += stepj;
194 }
195
196 vpCDEBUG(1) << "end vpMeLine::sample() : ";
197 vpCDEBUG(1) << list.size() << " point inserted in the list " << std::endl;
198}
199
205void vpMbtMeLine::suppressPoints(const vpImage<unsigned char> &I)
206{
207 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
208 vpMeSite s = *it; // current reference pixel
209
210 if (fabs(sin(theta)) > 0.9) // Vertical line management
211 {
212 if ((s.i < imin) || (s.i > imax)) {
214 }
215 }
216
217 else if (fabs(cos(theta)) > 0.9) // Horizontal line management
218 {
219 if ((s.j < jmin) || (s.j > jmax)) {
221 }
222 }
223
224 else {
225 if ((s.i < imin) || (s.i > imax) || (s.j < jmin) || (s.j > jmax)) {
227 }
228 }
229
230 if (outOfImage(s.i, s.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)I.getHeight(), (int)I.getWidth())) {
232 }
233
235 it = list.erase(it);
236 else
237 ++it;
238 }
239}
240
247void vpMbtMeLine::seekExtremities(const vpImage<unsigned char> &I)
248{
249 vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
250
251 int rows = (int)I.getHeight();
252 int cols = (int)I.getWidth();
253 double n_sample;
254
255 // if (me->getSampleStep()==0)
256 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
257 throw(vpTrackingException(vpTrackingException::fatalError, "Function called with sample step = 0"));
258 }
259
260 // i, j portions of the line_p
261 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
262 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
263
264 double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
265
266 double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
267 double dj = diffsj / sqrt(s);
268
269 double length_p = sqrt(s); /*(vpMath::sqr(diffsi)+vpMath::sqr(diffsj))*/
270
271 // number of samples along line_p
272 n_sample = length_p / (double)me->getSampleStep();
273 double sample_step = (double)me->getSampleStep();
274
275 vpMeSite P;
276 P.init((int)PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign);
277 P.setDisplay(selectDisplay);
278
279 unsigned int memory_range = me->getRange();
280 me->setRange(1);
281
282 for (int i = 0; i < 3; i++) {
283 P.ifloat = P.ifloat + di * sample_step;
284 P.i = (int)P.ifloat;
285 P.jfloat = P.jfloat + dj * sample_step;
286 P.j = (int)P.jfloat;
287
288 if ((P.i < imin) || (P.i > imax) || (P.j < jmin) || (P.j > jmax)) {
289 if (vpDEBUG_ENABLE(3))
291 } else if (!outOfImage(P.i, P.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)rows, (int)cols)) {
292 P.track(I, me, false);
293
295 list.push_back(P);
296 if (vpDEBUG_ENABLE(3))
298 } else if (vpDEBUG_ENABLE(3))
300 }
301 }
302
303 P.init((int)PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign);
304 P.setDisplay(selectDisplay);
305 for (int i = 0; i < 3; i++) {
306 P.ifloat = P.ifloat - di * sample_step;
307 P.i = (int)P.ifloat;
308 P.jfloat = P.jfloat - dj * sample_step;
309 P.j = (int)P.jfloat;
310
311 if ((P.i < imin) || (P.i > imax) || (P.j < jmin) || (P.j > jmax)) {
312 if (vpDEBUG_ENABLE(3))
314 }
315
316 else if (!outOfImage(P.i, P.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)rows, (int)cols)) {
317 P.track(I, me, false);
318
320 list.push_back(P);
321 if (vpDEBUG_ENABLE(3))
323 } else if (vpDEBUG_ENABLE(3))
325 }
326 }
327
328 me->setRange(memory_range);
329
330 vpCDEBUG(1) << "end vpMeLine::sample() : ";
331 vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
332}
333
348void vpMbtMeLine::computeProjectionError(const vpImage<unsigned char> &_I, double &_sumErrorRad,
349 unsigned int &_nbFeatures, const vpMatrix &SobelX, const vpMatrix &SobelY,
350 bool display, unsigned int length, unsigned int thickness)
351{
352 _sumErrorRad = 0;
353 _nbFeatures = 0;
354 double deltaNormalized = theta;
355 unsigned int iter = 0;
356
357 while (deltaNormalized < 0)
358 deltaNormalized += M_PI;
359 while (deltaNormalized > M_PI)
360 deltaNormalized -= M_PI;
361
362 vpColVector vecLine(2);
363 vecLine[0] = cos(deltaNormalized);
364 vecLine[1] = sin(deltaNormalized);
365 vecLine.normalize();
366
367 double offset = std::floor(SobelX.getRows() / 2.0f);
368
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;
373
374 double iSite = it->ifloat;
375 double jSite = it->jfloat;
376
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);
381
382 if (iImg < 0)
383 iImg = 0.0;
384 if (jImg < 0)
385 jImg = 0.0;
386
387 if (iImg > _I.getHeight() - 1)
388 iImg = _I.getHeight() - 1;
389 if (jImg > _I.getWidth() - 1)
390 jImg = _I.getWidth() - 1;
391
392 gradientX += SobelX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
393 }
394 }
395
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);
400
401 if (iImg < 0)
402 iImg = 0.0;
403 if (jImg < 0)
404 jImg = 0.0;
405
406 if (iImg > _I.getHeight() - 1)
407 iImg = _I.getHeight() - 1;
408 if (jImg > _I.getWidth() - 1)
409 jImg = _I.getWidth() - 1;
410
411 gradientY += SobelY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
412 }
413 }
414
415 double angle = atan2(gradientX, gradientY);
416 while (angle < 0)
417 angle += M_PI;
418 while (angle > M_PI)
419 angle -= M_PI;
420
421 vpColVector vecGrad(2);
422 vecGrad[0] = cos(angle);
423 vecGrad[1] = sin(angle);
424 vecGrad.normalize();
425
426 double angle1 = acos(vecLine * vecGrad);
427 double angle2 = acos(vecLine * (-vecGrad));
428
429 if (display) {
430 vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length * cos(deltaNormalized)),
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) {
434 vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length * cos(angle)),
435 (int)(it->get_j() + length * sin(angle)), vpColor::red, length >= 20 ? length / 5 : 4,
436 length >= 20 ? length / 10 : 2, thickness);
437 } else {
438 vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length * cos(angle + M_PI)),
439 (int)(it->get_j() + length * sin(angle + M_PI)), vpColor::red,
440 length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness);
441 }
442 }
443
444 // double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
445 // double angle2 = sqrt(vpMath::sqr(deltaNormalized-
446 // (angle-M_PI)));
447
448 _sumErrorRad += (std::min)(angle1, angle2);
449
450 // if(std::fabs(deltaNormalized-angle) > M_PI / 2)
451 // {
452 // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI
453 // / 2;
454 // } else {
455 // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
456 // }
457
458 _nbFeatures++;
459 }
460 iter++;
461 }
462}
463
474void vpMbtMeLine::reSample(const vpImage<unsigned char> &I)
475{
476 unsigned int n = numberOfSignal();
477
478 if ((double)n < 0.5 * expecteddensity && n > 0) {
479 double delta_new = delta;
480 delta = delta_1;
481 sample(I);
482 expecteddensity = (double)list.size();
483 delta = delta_new;
484 // 2. On appelle ce qui n'est pas specifique
485 {
487 }
488 }
489}
490
503void vpMbtMeLine::reSample(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2)
504{
505 size_t n = list.size();
506
507 if ((double)n < 0.5 * expecteddensity /*&& n > 0*/) // n is always > 0
508 {
509 double delta_new = delta;
510 delta = delta_1;
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();
515 sample(I);
516 expecteddensity = (double)list.size();
517 delta = delta_new;
519 }
520}
521
525void vpMbtMeLine::updateDelta()
526{
527 vpMeSite p_me;
528
529 double diff = 0;
530
531 // if(fabs(theta) == M_PI )
532 if (std::fabs(std::fabs(theta) - M_PI) <=
533 vpMath::maximum(std::fabs(theta), (double)M_PI) * std::numeric_limits<double>::epsilon()) {
534 theta = 0;
535 }
536
537 diff = fabs(theta - theta_1);
538 if (diff > M_PI / 2.0)
539 sign *= -1;
540
541 theta_1 = theta;
542
543 delta = -theta + M_PI / 2.0;
544 normalizeAngle(delta);
545
546 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
547 p_me = *it;
548 p_me.alpha = delta;
549 p_me.mask_sign = sign;
550 *it = p_me;
551 }
552 delta_1 = delta;
553}
554
560void vpMbtMeLine::track(const vpImage<unsigned char> &I)
561{
562 try {
564 if (m_mask != NULL) {
565 // Expected density could be modified if some vpMeSite are no more tracked because they are outside the mask.
566 expecteddensity = (double)list.size();
567 }
568 } catch (...) {
569 throw; // throw the original exception
570 }
571}
572
581void vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, double rho_, double theta_)
582{
583 this->rho = rho_;
584 this->theta = theta_;
585 a = cos(theta);
586 b = sin(theta);
587 c = -rho;
588 // recherche de point aux extremite de la droites
589 // dans le cas d'un glissement
590 suppressPoints(I);
591 seekExtremities(I);
592 suppressPoints(I);
593 setExtremities();
594 // reechantillonage si necessaire
595 reSample(I);
596
597 // remet a jour l'angle delta pour chaque point de la liste
598 updateDelta();
599}
600
611void vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
612 double rho_, double theta_)
613{
614 this->rho = rho_;
615 this->theta = theta_;
616 a = cos(theta);
617 b = sin(theta);
618 c = -rho;
619 // recherche de point aux extremite de la droites
620 // dans le cas d'un glissement
621 suppressPoints(I);
622 seekExtremities(I);
623 suppressPoints(I);
624 setExtremities();
625 // reechantillonage si necessaire
626 reSample(I, ip1, ip2);
627
628 // remet a jour l'angle delta pour chaque point de la liste
629 updateDelta();
630}
631
635void vpMbtMeLine::setExtremities()
636{
637 double i_min = +1e6;
638 double j_min = +1e6;
639 double i_max = -1;
640 double j_max = -1;
641
642 // Loop through list of sites to track
643 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
644 vpMeSite s = *it; // current reference pixel
645 if (s.ifloat < i_min) {
646 i_min = s.ifloat;
647 j_min = s.jfloat;
648 }
649
650 if (s.ifloat > i_max) {
651 i_max = s.ifloat;
652 j_max = s.jfloat;
653 }
654 }
655
656 if (!list.empty()) {
657 PExt[0].ifloat = i_min;
658 PExt[0].jfloat = j_min;
659 PExt[1].ifloat = i_max;
660 PExt[1].jfloat = j_max;
661 }
662
663 if (fabs(i_min - i_max) < 25) {
664 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
665 vpMeSite s = *it; // current reference pixel
666 if (s.jfloat < j_min) {
667 i_min = s.ifloat;
668 j_min = s.jfloat;
669 }
670
671 if (s.jfloat > j_max) {
672 i_max = s.ifloat;
673 j_max = s.jfloat;
674 }
675 }
676
677 if (!list.empty()) {
678 PExt[0].ifloat = i_min;
679 PExt[0].jfloat = j_min;
680 PExt[1].ifloat = i_max;
681 PExt[1].jfloat = j_max;
682 }
683 bubbleSortJ();
684 }
685
686 else
687 bubbleSortI();
688}
689
690static bool sortByI(const vpMeSite &s1, const vpMeSite &s2) { return (s1.ifloat > s2.ifloat); }
691
692void vpMbtMeLine::bubbleSortI()
693{
694#if 0
695 unsigned int nbElmt = list.size();
696 for (unsigned int pass = 1; pass < nbElmt; pass++)
697 {
698 list.front();
699 for (unsigned int i=0; i < nbElmt-pass; i++)
700 {
701 vpMeSite s1 = list.value();
702 vpMeSite s2 = list.nextValue();
703 if (s1.ifloat > s2.ifloat)
704 list.swapRight();
705 else
706 list.next();
707 }
708 }
709#endif
710 list.sort(sortByI);
711}
712
713static bool sortByJ(const vpMeSite &s1, const vpMeSite &s2) { return (s1.jfloat > s2.jfloat); }
714
715void vpMbtMeLine::bubbleSortJ()
716{
717#if 0
718 unsigned int nbElmt = list.size();
719 for(unsigned int pass=1; pass < nbElmt; pass++)
720 {
721 list.front();
722 for (unsigned int i=0; i < nbElmt-pass; i++)
723 {
724 vpMeSite s1 = list.value();
725 vpMeSite s2 = list.nextValue();
726 if (s1.jfloat > s2.jfloat)
727 list.swapRight();
728 else
729 list.next();
730 }
731 }
732#endif
733 list.sort(sortByJ);
734}
735
736#endif
unsigned int getCols() const
Definition vpArray2D.h:280
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor cyan
Definition vpColor.h:220
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
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 ...
void set_j(double jj)
double get_j() const
void set_i(double ii)
double get_i() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static double sqr(double x)
Definition vpMath.h:124
static int round(double x)
Definition vpMath.h:323
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition vpMeSite.h:65
int j
Coordinates along j of a site.
Definition vpMeSite.h:98
@ TOO_NEAR
Point removed because too near image borders.
Definition vpMeSite.h:90
@ CONTRAST
Point removed due to a contrast problem.
Definition vpMeSite.h:84
@ NO_SUPPRESSION
Point used by the tracker.
Definition vpMeSite.h:83
void setDisplay(vpMeSiteDisplayType select)
Definition vpMeSite.h:210
double ifloat
Floating coordinates along i of a site.
Definition vpMeSite.h:100
int i
Coordinate along i of a site.
Definition vpMeSite.h:96
int mask_sign
Mask sign.
Definition vpMeSite.h:104
double alpha
Angle of tangent at site.
Definition vpMeSite.h:106
void init()
Definition vpMeSite.cpp:59
double jfloat
Floating coordinates along j of a site.
Definition vpMeSite.h:102
vpMeSiteState getState() const
Definition vpMeSite.h:261
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
Definition vpMeSite.cpp:305
void setState(const vpMeSiteState &flag)
Definition vpMeSite.h:247
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 vpCDEBUG(level)
Definition vpDebug.h:506
#define vpDEBUG_ENABLE(level)
Definition vpDebug.h:533