Visual Servoing Platform version 3.5.0
vpMeLine.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 * Moving edges.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
44#include <algorithm> // (std::min)
45#include <cmath> // std::fabs
46#include <limits> // numeric_limits
47#include <stdlib.h>
48#include <visp3/core/vpImagePoint.h>
49#include <visp3/core/vpMath.h>
50#include <visp3/core/vpRobust.h>
51#include <visp3/core/vpTrackingException.h>
52#include <visp3/me/vpMe.h>
53#include <visp3/me/vpMeLine.h>
54#include <visp3/me/vpMeSite.h>
55#include <visp3/me/vpMeTracker.h>
56
57#define INCR_MIN 1
58
59void computeDelta(double &delta, int i1, int j1, int i2, int j2);
60
61static void normalizeAngle(double &delta)
62{
63 while (delta > M_PI) {
64 delta -= M_PI;
65 }
66 while (delta < -M_PI) {
67 delta += M_PI;
68 }
69}
70
71void computeDelta(double &delta, int i1, int j1, int i2, int j2)
72{
73
74 double B = double(i1 - i2);
75 double A = double(j1 - j2);
76
77 delta = atan2(B, A);
78 delta -= M_PI / 2.0;
79 normalizeAngle(delta);
80}
81
82static void project(double a, double b, double c, double i, double j, double &ip, double &jp)
83{
84 if (fabs(a) > fabs(b)) {
85 jp = (vpMath::sqr(a) * j - a * b * i - c * b) / (vpMath::sqr(a) + vpMath::sqr(b));
86 ip = (-c - b * jp) / a;
87 } else {
88 ip = (vpMath::sqr(b) * i - a * b * j - c * a) / (vpMath::sqr(a) + vpMath::sqr(b));
89 jp = (-c - a * ip) / b;
90 }
91}
92
99 : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.),
100 b(0.), c(0.)
101{
102}
109 : vpMeTracker(meline), rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1),
110 _useIntensityForRho(true), a(0.), b(0.), c(0.)
111
112{
113 rho = meline.rho;
114 theta = meline.theta;
115 delta = meline.delta;
116 delta_1 = meline.delta_1;
117 angle = meline.angle;
118 angle_1 = meline.angle_1;
119 sign = meline.sign;
120
121 a = meline.a;
122 b = meline.b;
123 c = meline.c;
125 PExt[0] = meline.PExt[0];
126 PExt[1] = meline.PExt[1];
127}
128
135
148void vpMeLine::sample(const vpImage<unsigned char> &I, bool doNotTrack)
149{
150 (void)doNotTrack;
151 if (!me) {
152 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
153 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
154 }
155
156 int rows = (int)I.getHeight();
157 int cols = (int)I.getWidth();
158 double n_sample;
159
160 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
161 vpERROR_TRACE("function called with sample step = 0");
162 throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
163 }
164
165 // i, j portions of the line_p
166 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
167 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
168
169 double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
170 if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
171 throw(vpTrackingException(vpTrackingException::fatalError, "points too close of each other to define a line"));
172 // number of samples along line_p
173 n_sample = length_p / (double)me->getSampleStep();
174
175 double stepi = diffsi / (double)n_sample;
176 double stepj = diffsj / (double)n_sample;
177
178 // Choose starting point
179 double is = PExt[1].ifloat;
180 double js = PExt[1].jfloat;
181
182 // Delete old list
183 list.clear();
184
185 // sample positions at i*me->getSampleStep() interval along the
186 // line_p, starting at PSiteExt[0]
187
188 vpImagePoint ip;
189 for (int i = 0; i <= vpMath::round(n_sample); i++) {
190 // If point is in the image, add to the sample list
191 if (!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols)) {
192 vpMeSite pix; //= list.value();
193 pix.init((int)is, (int)js, delta, 0, sign);
195
196 if (vpDEBUG_ENABLE(3)) {
197 ip.set_i(is);
198 ip.set_j(js);
200 }
201
202 list.push_back(pix);
203 }
204 is += stepi;
205 js += stepj;
206 }
207
208 vpCDEBUG(1) << "end vpMeLine::sample() : ";
209 vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
210}
211
225{
226 vpMeLine::display(I, PExt[0], PExt[1], list, a, b, c, col);
227}
228
237{
238 vpImagePoint ip1, ip2;
239
240 std::cout << "Click on the line first point..." << std::endl;
241 while (vpDisplay::getClick(I, ip1) != true)
242 ;
245 std::cout << "Click on the line second point..." << std::endl;
246 while (vpDisplay::getClick(I, ip2) != true)
247 ;
250
251 try {
252 initTracking(I, ip1, ip2);
253 } catch (...) {
254 vpERROR_TRACE("Error caught");
255 throw;
256 }
257}
258
266{
267 vpMatrix A(numberOfSignal(), 2);
268 vpColVector x(2), x_1(2);
269 x_1 = 0;
270
271 vpRobust r;
274 D.eye();
275 vpMatrix DA(numberOfSignal(), 2);
278 w = 1;
279 vpMeSite p_me;
280 unsigned int iter = 0;
281 unsigned int nos_1 = 0;
282 double distance = 100;
283
284 if (list.size() <= 2 || numberOfSignal() <= 2) {
285 // vpERROR_TRACE("Not enough point") ;
286 vpCDEBUG(1) << "Not enough point";
288 }
289
290 if ((fabs(b) >= 0.9)) // Construction du systeme Ax=B
291 // a i + j + c = 0
292 // A = (i 1) B = (-j)
293 {
294 nos_1 = numberOfSignal();
295 unsigned int k = 0;
296 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
297 p_me = *it;
298 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
299 A[k][0] = p_me.ifloat;
300 A[k][1] = 1;
301 B[k] = -p_me.jfloat;
302 k++;
303 }
304 }
305
306 while (iter < 4 && distance > 0.05) {
307 for (unsigned int i = 0; i < k ; i++) {
308 for (unsigned int j = 0; j < 2 ; j++) {
309 DA[i][j] = w[i] * A[i][j];
310 }
311 }
312
313 x = DA.pseudoInverse(1e-26) * D * B;
314
315 vpColVector residu(nos_1);
316 residu = B - A * x;
317 r.MEstimator(vpRobust::TUKEY, residu, w);
318
319 k = 0;
320 for (unsigned int i = 0; i < nos_1; i++) {
321 D[k][k] = w[k];
322 k++;
323 }
324 iter++;
325 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
326 x_1 = x;
327 }
328
329 k = 0;
330 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
331 p_me = *it;
332 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
333 if (w[k] < 0.2) {
335
336 *it = p_me;
337 }
338 k++;
339 }
340 }
341
342 // mise a jour de l'equation de la droite
343 a = x[0];
344 b = 1;
345 c = x[1];
346
347 double s = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
348 a /= s;
349 b /= s;
350 c /= s;
351 }
352
353 else // Construction du systeme Ax=B
354 // i + bj + c = 0
355 // A = (j 1) B = (-i)
356 {
357 nos_1 = numberOfSignal();
358 unsigned int k = 0;
359 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
360 p_me = *it;
361 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
362 A[k][0] = p_me.jfloat;
363 A[k][1] = 1;
364 B[k] = -p_me.ifloat;
365 k++;
366 }
367 }
368
369 while (iter < 4 && distance > 0.05) {
370 DA = D * A;
371 x = DA.pseudoInverse(1e-26) * D * B;
372
373 vpColVector residu(nos_1);
374 residu = B - A * x;
375 r.MEstimator(vpRobust::TUKEY, residu, w);
376
377 k = 0;
378 for (unsigned int i = 0; i < nos_1; i++) {
379 D[k][k] = w[k];
380 k++;
381 }
382 iter++;
383 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
384 x_1 = x;
385 }
386
387 k = 0;
388 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
389 p_me = *it;
390 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
391 if (w[k] < 0.2) {
393
394 *it = p_me;
395 }
396 k++;
397 }
398 }
399 a = 1;
400 b = x[0];
401 c = x[1];
402
403 double s = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
404 a /= s;
405 b /= s;
406 c /= s;
407 }
408
409 // mise a jour du delta
410 delta = atan2(a, b);
411
412 normalizeAngle(delta);
413}
414
425{
426 vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
427
428 int i1s, j1s, i2s, j2s;
429
430 i1s = vpMath::round(ip1.get_i());
431 i2s = vpMath::round(ip2.get_i());
432 j1s = vpMath::round(ip1.get_j());
433 j2s = vpMath::round(ip2.get_j());
434
435 try {
436
437 // 1. On fait ce qui concerne les droites (peut etre vide)
438 {
439 // Points extremites
440 PExt[0].ifloat = (float)ip1.get_i();
441 PExt[0].jfloat = (float)ip1.get_j();
442 PExt[1].ifloat = (float)ip2.get_i();
443 PExt[1].jfloat = (float)ip2.get_j();
444
445 double angle_ = atan2((double)(i1s - i2s), (double)(j1s - j2s));
446 a = cos(angle_);
447 b = sin(angle_);
448
449 // Real values of a, b can have an other sign. So to get the good values
450 // of a and b in order to initialise then c, we call track(I) just below
451
452 computeDelta(delta, i1s, j1s, i2s, j2s);
453 delta_1 = delta;
454
455 // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a),
456 // delta);
457
458 sample(I);
459 }
460 // 2. On appelle ce qui n'est pas specifique
461 {
463 }
464 // Call track(I) to give the good sign to a and b and to initialise c
465 // which can be used for the display
466 track(I);
467 } catch (...) {
468 vpERROR_TRACE("Error caught");
469 throw;
470 }
471 vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
472}
473
478{
479 // Loop through list of sites to track
480 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
481 vpMeSite s = *it; // current reference pixel
482
484 it = list.erase(it);
485 else
486 ++it;
487 }
488}
489
494{
495 double imin = +1e6;
496 double jmin = +1e6;
497 double imax = -1;
498 double jmax = -1;
499
500 // Loop through list of sites to track
501 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
502 vpMeSite s = *it; // current reference pixel
503 if (s.ifloat < imin) {
504 imin = s.ifloat;
505 jmin = s.jfloat;
506 }
507
508 if (s.ifloat > imax) {
509 imax = s.ifloat;
510 jmax = s.jfloat;
511 }
512 }
513
514 PExt[0].ifloat = imin;
515 PExt[0].jfloat = jmin;
516 PExt[1].ifloat = imax;
517 PExt[1].jfloat = jmax;
518
519 if (fabs(imin - imax) < 25) {
520 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
521 vpMeSite s = *it; // current reference pixel
522 if (s.jfloat < jmin) {
523 imin = s.ifloat;
524 jmin = s.jfloat;
525 }
526
527 if (s.jfloat > jmax) {
528 imax = s.ifloat;
529 jmax = s.jfloat;
530 }
531 }
532 PExt[0].ifloat = imin;
533 PExt[0].jfloat = jmin;
534 PExt[1].ifloat = imax;
535 PExt[1].jfloat = jmax;
536 }
537}
538
551{
552 vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
553
554 if (!me) {
555 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
556 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
557 }
558
559 int rows = (int)I.getHeight();
560 int cols = (int)I.getWidth();
561 double n_sample;
562
563 // if (me->getSampleStep()==0)
564 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
565
566 vpERROR_TRACE("function called with sample step = 0");
567 throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
568 }
569
570 // i, j portions of the line_p
571 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
572 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
573
574 double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
575
576 double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
577 double dj = diffsj / sqrt(s);
578
579 double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
580
581 // number of samples along line_p
582 n_sample = length_p / (double)me->getSampleStep();
583 double sample_step = (double)me->getSampleStep();
584
585 vpMeSite P;
586 P.init((int)PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign);
588
589 unsigned int memory_range = me->getRange();
590 me->setRange(1);
591
592 vpImagePoint ip;
593
594 for (int i = 0; i < 3; i++) {
595 P.ifloat = P.ifloat + di * sample_step;
596 P.i = (int)P.ifloat;
597 P.jfloat = P.jfloat + dj * sample_step;
598 P.j = (int)P.jfloat;
599
600 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
601 P.track(I, me, false);
602
604 list.push_back(P);
605 if (vpDEBUG_ENABLE(3)) {
606 ip.set_i(P.i);
607 ip.set_j(P.j);
608
610 }
611 } else {
612 if (vpDEBUG_ENABLE(3)) {
613 ip.set_i(P.i);
614 ip.set_j(P.j);
616 }
617 }
618 }
619 }
620
621 P.init((int)PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign);
623 for (int i = 0; i < 3; i++) {
624 P.ifloat = P.ifloat - di * sample_step;
625 P.i = (int)P.ifloat;
626 P.jfloat = P.jfloat - dj * sample_step;
627 P.j = (int)P.jfloat;
628
629 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
630 P.track(I, me, false);
631
633 list.push_back(P);
634 if (vpDEBUG_ENABLE(3)) {
635 ip.set_i(P.i);
636 ip.set_j(P.j);
638 }
639 } else {
640 if (vpDEBUG_ENABLE(3)) {
641 ip.set_i(P.i);
642 ip.set_j(P.j);
644 }
645 }
646 }
647 }
648
649 me->setRange(memory_range);
650
651 vpCDEBUG(1) << "end vpMeLine::sample() : ";
652 vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
653}
654
667{
668 double i1, j1, i2, j2;
669
670 if (!me) {
671 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
672 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
673 }
674
675 project(a, b, c, PExt[0].ifloat, PExt[0].jfloat, i1, j1);
676 project(a, b, c, PExt[1].ifloat, PExt[1].jfloat, i2, j2);
677
678 // Points extremites
679 PExt[0].ifloat = i1;
680 PExt[0].jfloat = j1;
681 PExt[1].ifloat = i2;
682 PExt[1].jfloat = j2;
683
684 double d = sqrt(vpMath::sqr(i1 - i2) + vpMath::sqr(j1 - j2));
685
686 unsigned int n = numberOfSignal();
687 double expecteddensity = d / (double)me->getSampleStep();
688
689 if ((double)n < 0.9 * expecteddensity) {
690 double delta_new = delta;
691 delta = delta_1;
692 sample(I);
693 delta = delta_new;
694 // 2. On appelle ce qui n'est pas specifique
695 {
697 }
698 }
699}
700
706{
707 vpMeSite p_me;
708
709 double angle_ = delta + M_PI / 2;
710 double diff = 0;
711
712 while (angle_ < 0)
713 angle_ += M_PI;
714 while (angle_ > M_PI)
715 angle_ -= M_PI;
716
717 angle_ = vpMath::round(angle_ * 180 / M_PI);
718
719 // if(fabs(angle_) == 180 )
720 if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
721 angle_ = 0;
722 }
723
724 // std::cout << "angle theta : " << theta << std::endl ;
725 diff = fabs(angle_ - angle_1);
726 if (diff > 90)
727 sign *= -1;
728
729 angle_1 = angle_;
730
731 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
732 p_me = *it;
733 p_me.alpha = delta;
734 p_me.mask_sign = sign;
735 *it = p_me;
736 }
737 delta_1 = delta;
738}
739
747{
748 vpCDEBUG(1) << "begin vpMeLine::track()" << std::endl;
749
750 // 1. On fait ce qui concerne les droites (peut etre vide)
751 {} // 2. On appelle ce qui n'est pas specifique
752 {
754 }
755
756 // 3. On revient aux droites
757 {
758 // supression des points rejetes par les ME
761
762 // Estimation des parametres de la droite aux moindres carre
763 try {
764 leastSquare();
765 } catch (...) {
766 vpERROR_TRACE("Error caught");
767 throw;
768 }
769
770 // recherche de point aux extremite de la droites
771 // dans le cas d'un glissement
773
775 try {
776 leastSquare();
777 } catch (...) {
778 vpERROR_TRACE("Error caught");
779 throw;
780 }
781
782 // suppression des points rejetes par la regression robuste
785
786 // reechantillonage si necessaire
787 reSample(I);
788
789 // remet a jour l'angle delta pour chaque point de la liste
790
791 updateDelta();
792
793 // Remise a jour de delta dans la liste de site me
794 if (vpDEBUG_ENABLE(2)) {
798 }
799 }
800
802
803 vpCDEBUG(1) << "end vpMeLine::track()" << std::endl;
804}
805
806void vpMeLine::update_indices(double theta, int i, int j, int incr, int &i1, int &i2, int &j1, int &j2)
807{
808 i1 = (int)(i + cos(theta) * incr);
809 j1 = (int)(j + sin(theta) * incr);
810
811 i2 = (int)(i - cos(theta) * incr);
812 j2 = (int)(j - sin(theta) * incr);
813}
814
822{
823 // rho = -c ;
824 // theta = atan2(a,b) ;
825 rho = fabs(c);
826 theta = atan2(b, a);
827
828 while (theta >= M_PI)
829 theta -= M_PI;
830 while (theta < 0)
831 theta += M_PI;
832
834
835 /* while(theta < -M_PI) theta += 2*M_PI ;
836 while(theta >= M_PI) theta -= 2*M_PI ;
837
838 // If theta is between -90 and -180 get the equivalent
839 // between 0 and 90
840 if(theta <-M_PI/2)
841 {
842 theta += M_PI ;
843 rho *= -1 ;
844 }
845 // If theta is between 90 and 180 get the equivalent
846 // between 0 and -90
847 if(theta >M_PI/2)
848 {
849 theta -= M_PI ;
850 rho *= -1 ;
851 }
852 */
853 // convention pour choisir le signe de rho
854 int i, j;
855 i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat) / 2);
856 j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat) / 2);
857
858 int end = false;
859 int incr = 10;
860
861 int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
862 unsigned char v1 = 0, v2 = 0;
863
864 int width_ = (int)I.getWidth();
865 int height_ = (int)I.getHeight();
866 update_indices(theta, i, j, incr, i1, i2, j1, j2);
867
868 if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
869 double rho_lim1 = fabs((double)i / cos(theta));
870 double rho_lim2 = fabs((double)j / sin(theta));
871
872 double co_rho_lim1 = fabs(((double)(height_ - i)) / cos(theta));
873 double co_rho_lim2 = fabs(((double)(width_ - j)) / sin(theta));
874
875 double rho_lim = (std::min)(rho_lim1, rho_lim2);
876 double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2);
877 incr = (int)std::floor((std::min)(rho_lim, co_rho_lim));
878 if (incr < INCR_MIN) {
879 vpERROR_TRACE("increment is too small");
880 throw(vpTrackingException(vpTrackingException::fatalError, "increment is too small"));
881 }
882 update_indices(theta, i, j, incr, i1, i2, j1, j2);
883 }
884
885 while (!end) {
886 end = true;
887 unsigned int i1_ = static_cast<unsigned int>(i1);
888 unsigned int j1_ = static_cast<unsigned int>(j1);
889 unsigned int i2_ = static_cast<unsigned int>(i2);
890 unsigned int j2_ = static_cast<unsigned int>(j2);
891 v1 = I[i1_][j1_];
892 v2 = I[i2_][j2_];
893 if (abs(v1 - v2) < 1) {
894 incr--;
895 end = false;
896 if (incr == 1) {
897 throw(vpException(vpException::fatalError, "In vpMeLine cannot determine rho sign, since "
898 "there is no gray level difference between both "
899 "sides of the line"));
900 }
901 }
902 update_indices(theta, i, j, incr, i1, i2, j1, j2);
903 }
904
905 if (theta >= 0 && theta <= M_PI / 2) {
906 if (v2 < v1) {
907 theta += M_PI;
908 rho *= -1;
909 }
910 }
911
912 else {
913 double jinter;
914 jinter = -c / b;
915 if (v2 < v1) {
916 theta += M_PI;
917 if (jinter > 0) {
918 rho *= -1;
919 }
920 }
921
922 else {
923 if (jinter < 0) {
924 rho *= -1;
925 }
926 }
927 }
928
929 if (vpDEBUG_ENABLE(2)) {
930 vpImagePoint ip, ip1, ip2;
931
932 ip.set_i(i);
933 ip.set_j(j);
934 ip1.set_i(i1);
935 ip1.set_j(j1);
936 ip2.set_i(i2);
937 ip2.set_j(j2);
938
941 }
942 }
943}
944
955double vpMeLine::getRho() const { return rho; }
956
960double vpMeLine::getTheta() const { return theta; }
961
970{
971 /*Return the coordinates of the extremities of the line*/
972 ip1.set_i(PExt[0].ifloat);
973 ip1.set_j(PExt[0].jfloat);
974 ip2.set_i(PExt[1].ifloat);
975 ip2.set_j(PExt[1].jfloat);
976}
977
990bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
991{
992 double a1 = line1.a;
993 double b1 = line1.b;
994 double c1 = line1.c;
995 double a2 = line2.a;
996 double b2 = line2.b;
997 double c2 = line2.c;
998
999 try {
1000 double i = 0, j = 0;
1001 double denom = 0;
1002
1003 if (a1 > 0.1) {
1004 denom = (-(a2 / a1) * b1 + b2);
1005
1006 // if (denom == 0)
1007 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1008 std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1009 return (false);
1010 }
1011
1012 // if (denom != 0 )
1013 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1014 j = ((a2 / a1) * c1 - c2) / denom;
1015 i = (-b1 * j - c1) / a1;
1016 }
1017 }
1018
1019 else {
1020 denom = (-(b2 / b1) * a1 + a2);
1021
1022 // if (denom == 0)
1023 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1024 std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1025 return (false);
1026 }
1027
1028 // if (denom != 0 )
1029 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1030 i = ((b2 / b1) * c1 - c2) / denom;
1031 j = (-a1 * i - c1) / b1;
1032 }
1033 }
1034 ip.set_i(i);
1035 ip.set_j(j);
1036
1037 return (true);
1038 } catch (...) {
1039 return (false);
1040 }
1041}
1042
1063void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1064 const double &B, const double &C, const vpColor &color, unsigned int thickness)
1065{
1066 vpImagePoint ip1, ip2;
1067
1068 if (fabs(A) < fabs(B)) {
1069 double i1, j1, i2, j2;
1070 i1 = 0;
1071 j1 = (-A * i1 - C) / B;
1072 i2 = I.getHeight() - 1.0;
1073 j2 = (-A * i2 - C) / B;
1074
1075 ip1.set_i(i1);
1076 ip1.set_j(j1);
1077 ip2.set_i(i2);
1078 ip2.set_j(j2);
1079 vpDisplay::displayLine(I, ip1, ip2, color);
1080 // vpDisplay::flush(I);
1081
1082 } else {
1083 double i1, j1, i2, j2;
1084 j1 = 0;
1085 i1 = -(B * j1 + C) / A;
1086 j2 = I.getWidth() - 1.0;
1087 i2 = -(B * j2 + C) / A;
1088
1089 ip1.set_i(i1);
1090 ip1.set_j(j1);
1091 ip2.set_i(i2);
1092 ip2.set_j(j2);
1093 vpDisplay::displayLine(I, ip1, ip2, color);
1094 // vpDisplay::flush(I);
1095 }
1096
1097 ip1.set_i(PExt1.ifloat);
1098 ip1.set_j(PExt1.jfloat);
1099 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1100
1101 ip1.set_i(PExt2.ifloat);
1102 ip1.set_j(PExt2.jfloat);
1103 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1104}
1105
1126void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1127 const double &B, const double &C, const vpColor &color, unsigned int thickness)
1128{
1129 vpImagePoint ip1, ip2;
1130
1131 if (fabs(A) < fabs(B)) {
1132 double i1, j1, i2, j2;
1133 i1 = 0;
1134 j1 = (-A * i1 - C) / B;
1135 i2 = I.getHeight() - 1.0;
1136 j2 = (-A * i2 - C) / B;
1137
1138 ip1.set_i(i1);
1139 ip1.set_j(j1);
1140 ip2.set_i(i2);
1141 ip2.set_j(j2);
1142 vpDisplay::displayLine(I, ip1, ip2, color);
1143 // vpDisplay::flush(I);
1144
1145 } else {
1146 double i1, j1, i2, j2;
1147 j1 = 0;
1148 i1 = -(B * j1 + C) / A;
1149 j2 = I.getWidth() - 1.0;
1150 i2 = -(B * j2 + C) / A;
1151
1152 ip1.set_i(i1);
1153 ip1.set_j(j1);
1154 ip2.set_i(i2);
1155 ip2.set_j(j2);
1156 vpDisplay::displayLine(I, ip1, ip2, color);
1157 // vpDisplay::flush(I);
1158 }
1159
1160 ip1.set_i(PExt1.ifloat);
1161 ip1.set_j(PExt1.jfloat);
1162 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1163
1164 ip1.set_i(PExt2.ifloat);
1165 ip1.set_j(PExt2.jfloat);
1166 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1167}
1168
1191void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1192 const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1193 const vpColor &color, unsigned int thickness)
1194{
1195 vpImagePoint ip;
1196
1197 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1198 vpMeSite pix = *it;
1199 ip.set_i(pix.ifloat);
1200 ip.set_j(pix.jfloat);
1201
1202 if (pix.getState() == vpMeSite::M_ESTIMATOR)
1203 vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1204 else
1205 vpDisplay::displayCross(I, ip, 5, color, thickness);
1206
1207 // vpDisplay::flush(I);
1208 }
1209
1210 vpImagePoint ip1, ip2;
1211
1212 if (fabs(A) < fabs(B)) {
1213 double i1, j1, i2, j2;
1214 i1 = 0;
1215 j1 = (-A * i1 - C) / B;
1216 i2 = I.getHeight() - 1.0;
1217 j2 = (-A * i2 - C) / B;
1218
1219 ip1.set_i(i1);
1220 ip1.set_j(j1);
1221 ip2.set_i(i2);
1222 ip2.set_j(j2);
1223 vpDisplay::displayLine(I, ip1, ip2, color);
1224 // vpDisplay::flush(I);
1225
1226 } else {
1227 double i1, j1, i2, j2;
1228 j1 = 0;
1229 i1 = -(B * j1 + C) / A;
1230 j2 = I.getWidth() - 1.0;
1231 i2 = -(B * j2 + C) / A;
1232
1233 ip1.set_i(i1);
1234 ip1.set_j(j1);
1235 ip2.set_i(i2);
1236 ip2.set_j(j2);
1237 vpDisplay::displayLine(I, ip1, ip2, color);
1238 // vpDisplay::flush(I);
1239 }
1240
1241 ip1.set_i(PExt1.ifloat);
1242 ip1.set_j(PExt1.jfloat);
1243 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1244
1245 ip1.set_i(PExt2.ifloat);
1246 ip1.set_j(PExt2.jfloat);
1247 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1248}
1249
1272void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1273 const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1274 const vpColor &color, unsigned int thickness)
1275{
1276 vpImagePoint ip;
1277
1278 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1279 vpMeSite pix = *it;
1280 ip.set_i(pix.ifloat);
1281 ip.set_j(pix.jfloat);
1282
1283 if (pix.getState() == vpMeSite::M_ESTIMATOR)
1284 vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1285 else
1286 vpDisplay::displayCross(I, ip, 5, color, thickness);
1287
1288 // vpDisplay::flush(I);
1289 }
1290
1291 vpImagePoint ip1, ip2;
1292
1293 if (fabs(A) < fabs(B)) {
1294 double i1, j1, i2, j2;
1295 i1 = 0;
1296 j1 = (-A * i1 - C) / B;
1297 i2 = I.getHeight() - 1.0;
1298 j2 = (-A * i2 - C) / B;
1299
1300 ip1.set_i(i1);
1301 ip1.set_j(j1);
1302 ip2.set_i(i2);
1303 ip2.set_j(j2);
1304 vpDisplay::displayLine(I, ip1, ip2, color);
1305 // vpDisplay::flush(I);
1306
1307 } else {
1308 double i1, j1, i2, j2;
1309 j1 = 0;
1310 i1 = -(B * j1 + C) / A;
1311 j2 = I.getWidth() - 1.0;
1312 i2 = -(B * j2 + C) / A;
1313
1314 ip1.set_i(i1);
1315 ip1.set_j(j1);
1316 ip2.set_i(i2);
1317 ip2.set_j(j2);
1318 vpDisplay::displayLine(I, ip1, ip2, color);
1319 // vpDisplay::flush(I);
1320 }
1321
1322 ip1.set_i(PExt1.ifloat);
1323 ip1.set_j(PExt1.jfloat);
1324 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1325
1326 ip1.set_i(PExt2.ifloat);
1327 ip1.set_j(PExt2.jfloat);
1328 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1329}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
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)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
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 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
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:152
double theta
Definition: vpMeLine.h:159
virtual void sample(const vpImage< unsigned char > &image, bool doNotTrack=false)
Definition: vpMeLine.cpp:148
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeLine.cpp:224
void track(const vpImage< unsigned char > &Im)
Definition: vpMeLine.cpp:746
void suppressPoints()
Definition: vpMeLine.cpp:477
double getRho() const
Definition: vpMeLine.cpp:955
double a
Parameter a of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:174
void computeRhoTheta(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:821
void updateDelta()
Definition: vpMeLine.cpp:705
void reSample(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:666
double getTheta() const
Definition: vpMeLine.cpp:960
double angle
Definition: vpMeLine.h:161
double c
Parameter c of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:176
double rho
Definition: vpMeLine.h:159
int sign
Definition: vpMeLine.h:162
vpMeSite PExt[2]
Definition: vpMeLine.h:157
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:550
vpMeLine()
Definition: vpMeLine.cpp:98
double angle_1
Definition: vpMeLine.h:161
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
Definition: vpMeLine.cpp:990
void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
Definition: vpMeLine.cpp:969
virtual ~vpMeLine()
Definition: vpMeLine.cpp:134
double delta_1
Definition: vpMeLine.h:160
double delta
Definition: vpMeLine.h:160
double b
Parameter b of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:175
void leastSquare()
Definition: vpMeLine.cpp:265
void setExtremities()
Definition: vpMeLine.cpp:493
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:236
bool _useIntensityForRho
Definition: vpMeLine.h:166
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
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
@ 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
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:66
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:80
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
void setRange(const unsigned int &r)
Definition: vpMe.h:271
double getSampleStep() const
Definition: vpMe.h:285
unsigned int getRange() const
Definition: vpMe.h:179
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Error that can be emited by the vpTracker class and its derivates.
#define vpCDEBUG(level)
Definition: vpDebug.h:511
#define vpDERROR_TRACE
Definition: vpDebug.h:464
#define vpERROR_TRACE
Definition: vpDebug.h:393
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538