Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMeLine.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Moving edges.
32 */
33
39#include <algorithm> // (std::min)
40#include <cmath> // std::fabs
41#include <limits> // numeric_limits
42#include <stdlib.h>
43#include <visp3/core/vpImagePoint.h>
44#include <visp3/core/vpMath.h>
45#include <visp3/core/vpRobust.h>
46#include <visp3/core/vpTrackingException.h>
47#include <visp3/me/vpMe.h>
48#include <visp3/me/vpMeLine.h>
49#include <visp3/me/vpMeSite.h>
50#include <visp3/me/vpMeTracker.h>
51
52#define INCR_MIN 1
53
54void computeDelta(double &delta, int i1, int j1, int i2, int j2);
55
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
66void computeDelta(double &delta, int i1, int j1, int i2, int j2)
67{
68
69 double B = double(i1 - i2);
70 double A = double(j1 - j2);
71
72 delta = atan2(B, A);
73 delta -= M_PI / 2.0;
74 normalizeAngle(delta);
75}
76
77static void project(double a, double b, double c, double i, double j, double &ip, double &jp)
78{
79 if (fabs(a) > fabs(b)) {
80 jp = (vpMath::sqr(a) * j - a * b * i - c * b) / (vpMath::sqr(a) + vpMath::sqr(b));
81 ip = (-c - b * jp) / a;
82 }
83 else {
84 ip = (vpMath::sqr(b) * i - a * b * j - c * a) / (vpMath::sqr(a) + vpMath::sqr(b));
85 jp = (-c - a * ip) / b;
86 }
87}
88
90 : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.),
91 b(0.), c(0.)
92{ }
93
95 : vpMeTracker(meline), rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1),
96 _useIntensityForRho(true), a(0.), b(0.), c(0.)
97
98{
99 rho = meline.rho;
100 theta = meline.theta;
101 delta = meline.delta;
102 delta_1 = meline.delta_1;
103 angle = meline.angle;
104 angle_1 = meline.angle_1;
105 sign = meline.sign;
106
107 a = meline.a;
108 b = meline.b;
109 c = meline.c;
111 PExt[0] = meline.PExt[0];
112 PExt[1] = meline.PExt[1];
113}
114
116
117void vpMeLine::sample(const vpImage<unsigned char> &I, bool doNotTrack)
118{
119 (void)doNotTrack;
120 if (!me) {
121 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
122 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
123 }
124
125 int rows = (int)I.getHeight();
126 int cols = (int)I.getWidth();
127 double n_sample;
128
129 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
130 vpERROR_TRACE("function called with sample step = 0");
131 throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
132 }
133
134 // i, j portions of the line_p
135 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
136 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
137
138 double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
139 if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
140 throw(vpTrackingException(vpTrackingException::fatalError, "points too close of each other to define a line"));
141 // number of samples along line_p
142 n_sample = length_p / (double)me->getSampleStep();
143
144 double stepi = diffsi / (double)n_sample;
145 double stepj = diffsj / (double)n_sample;
146
147 // Choose starting point
148 double is = PExt[1].ifloat;
149 double js = PExt[1].jfloat;
150
151 // Delete old list
152 list.clear();
153
154 // sample positions at i*me->getSampleStep() interval along the
155 // line_p, starting at PSiteExt[0]
156
157 vpImagePoint ip;
158 for (int i = 0; i <= vpMath::round(n_sample); i++) {
159 // If point is in the image, add to the sample list
160 if (!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols)) {
161 vpMeSite pix; //= list.value();
162 pix.init((int)is, (int)js, delta, 0, sign);
164
165 if (vpDEBUG_ENABLE(3)) {
166 ip.set_i(is);
167 ip.set_j(js);
169 }
170
171 list.push_back(pix);
172 }
173 is += stepi;
174 js += stepj;
175 }
176
177 vpCDEBUG(1) << "end vpMeLine::sample() : ";
178 vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
179}
180
181void vpMeLine::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
182{
183 vpMeLine::displayLine(I, PExt[0], PExt[1], list, a, b, c, color, thickness);
184}
185
187{
188 vpImagePoint ip1, ip2;
189
191
192 std::cout << "Click on the line first point..." << std::endl;
193 while (vpDisplay::getClick(I, ip1) != true) { }
194
197 std::cout << "Click on the line second point..." << std::endl;
198 while (vpDisplay::getClick(I, ip2) != true) { }
199
202
203 try {
204 initTracking(I, ip1, ip2);
205 }
206 catch (...) {
207 vpERROR_TRACE("Error caught");
208 throw;
209 }
210}
211
213{
214 vpMatrix A(numberOfSignal(), 2);
215 vpColVector x(2), x_1(2);
216 x_1 = 0;
217
218 vpRobust r;
221 D.eye();
222 vpMatrix DA(numberOfSignal(), 2);
225 w = 1;
226 vpMeSite p_me;
227 unsigned int iter = 0;
228 unsigned int nos_1 = 0;
229 double distance = 100;
230
231 if (list.size() <= 2 || numberOfSignal() <= 2) {
232 // vpERROR_TRACE("Not enough point") ;
233 vpCDEBUG(1) << "Not enough point";
235 }
236
237 if ((fabs(b) >= 0.9)) // Construction du systeme Ax=B
238 // a i + j + c = 0
239 // A = (i 1) B = (-j)
240 {
241 nos_1 = numberOfSignal();
242 unsigned int k = 0;
243 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
244 p_me = *it;
245 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
246 A[k][0] = p_me.ifloat;
247 A[k][1] = 1;
248 B[k] = -p_me.jfloat;
249 k++;
250 }
251 }
252
253 while (iter < 4 && distance > 0.05) {
254 for (unsigned int i = 0; i < k; i++) {
255 for (unsigned int j = 0; j < 2; j++) {
256 DA[i][j] = w[i] * A[i][j];
257 }
258 }
259
260 x = DA.pseudoInverse(1e-26) * D * B;
261
262 vpColVector residu(nos_1);
263 residu = B - A * x;
264 r.MEstimator(vpRobust::TUKEY, residu, w);
265
266 k = 0;
267 for (unsigned int i = 0; i < nos_1; i++) {
268 D[k][k] = w[k];
269 k++;
270 }
271 iter++;
272 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
273 x_1 = x;
274 }
275
276 k = 0;
277 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
278 p_me = *it;
279 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
280 if (w[k] < 0.2) {
282
283 *it = p_me;
284 }
285 k++;
286 }
287 }
288
289 // mise a jour de l'equation de la droite
290 a = x[0];
291 b = 1;
292 c = x[1];
293
294 double s = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
295 a /= s;
296 b /= s;
297 c /= s;
298 }
299
300 else // Construction du systeme Ax=B
301 // i + bj + c = 0
302 // A = (j 1) B = (-i)
303 {
304 nos_1 = numberOfSignal();
305 unsigned int k = 0;
306 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
307 p_me = *it;
308 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
309 A[k][0] = p_me.jfloat;
310 A[k][1] = 1;
311 B[k] = -p_me.ifloat;
312 k++;
313 }
314 }
315
316 while (iter < 4 && distance > 0.05) {
317 DA = D * A;
318 x = DA.pseudoInverse(1e-26) * D * B;
319
320 vpColVector residu(nos_1);
321 residu = B - A * x;
322 r.MEstimator(vpRobust::TUKEY, residu, w);
323
324 k = 0;
325 for (unsigned int i = 0; i < nos_1; i++) {
326 D[k][k] = w[k];
327 k++;
328 }
329 iter++;
330 distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
331 x_1 = x;
332 }
333
334 k = 0;
335 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
336 p_me = *it;
337 if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
338 if (w[k] < 0.2) {
340
341 *it = p_me;
342 }
343 k++;
344 }
345 }
346 a = 1;
347 b = x[0];
348 c = x[1];
349
350 double s = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
351 a /= s;
352 b /= s;
353 c /= s;
354 }
355
356 // mise a jour du delta
357 delta = atan2(a, b);
358
359 normalizeAngle(delta);
360}
361
363{
364 vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
365
366 int i1s, j1s, i2s, j2s;
367
368 i1s = vpMath::round(ip1.get_i());
369 i2s = vpMath::round(ip2.get_i());
370 j1s = vpMath::round(ip1.get_j());
371 j2s = vpMath::round(ip2.get_j());
372
373 try {
374
375 // 1. On fait ce qui concerne les droites (peut etre vide)
376 {
377 // Points extremites
378 PExt[0].ifloat = (float)ip1.get_i();
379 PExt[0].jfloat = (float)ip1.get_j();
380 PExt[1].ifloat = (float)ip2.get_i();
381 PExt[1].jfloat = (float)ip2.get_j();
382
383 double angle_ = atan2((double)(i1s - i2s), (double)(j1s - j2s));
384 a = cos(angle_);
385 b = sin(angle_);
386
387 // Real values of a, b can have an other sign. So to get the good values
388 // of a and b in order to initialise then c, we call track(I) just below
389
390 computeDelta(delta, i1s, j1s, i2s, j2s);
391 delta_1 = delta;
392
393 // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a),
394 // delta);
395
396 sample(I);
397 }
398 // 2. On appelle ce qui n'est pas specifique
399 {
401 }
402 // Call track(I) to give the good sign to a and b and to initialise c
403 // which can be used for the display
404 track(I);
405 }
406 catch (...) {
407 vpERROR_TRACE("Error caught");
408 throw;
409 }
410 vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
411}
412
414{
415 // Loop through list of sites to track
416 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
417 vpMeSite s = *it; // current reference pixel
418
420 it = list.erase(it);
421 else
422 ++it;
423 }
424}
425
427{
428 double imin = +1e6;
429 double jmin = +1e6;
430 double imax = -1;
431 double jmax = -1;
432
433 // Loop through list of sites to track
434 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
435 vpMeSite s = *it; // current reference pixel
436 if (s.ifloat < imin) {
437 imin = s.ifloat;
438 jmin = s.jfloat;
439 }
440
441 if (s.ifloat > imax) {
442 imax = s.ifloat;
443 jmax = s.jfloat;
444 }
445 }
446
447 PExt[0].ifloat = imin;
448 PExt[0].jfloat = jmin;
449 PExt[1].ifloat = imax;
450 PExt[1].jfloat = jmax;
451
452 if (fabs(imin - imax) < 25) {
453 for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
454 vpMeSite s = *it; // current reference pixel
455 if (s.jfloat < jmin) {
456 imin = s.ifloat;
457 jmin = s.jfloat;
458 }
459
460 if (s.jfloat > jmax) {
461 imax = s.ifloat;
462 jmax = s.jfloat;
463 }
464 }
465 PExt[0].ifloat = imin;
466 PExt[0].jfloat = jmin;
467 PExt[1].ifloat = imax;
468 PExt[1].jfloat = jmax;
469 }
470}
471
473{
474 vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
475
476 if (!me) {
477 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
478 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
479 }
480
481 int rows = (int)I.getHeight();
482 int cols = (int)I.getWidth();
483 double n_sample;
484
485 // if (me->getSampleStep()==0)
486 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
487
488 vpERROR_TRACE("function called with sample step = 0");
489 throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
490 }
491
492 // i, j portions of the line_p
493 double diffsi = PExt[0].ifloat - PExt[1].ifloat;
494 double diffsj = PExt[0].jfloat - PExt[1].jfloat;
495
496 double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
497
498 double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
499 double dj = diffsj / sqrt(s);
500
501 double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
502
503 // number of samples along line_p
504 n_sample = length_p / (double)me->getSampleStep();
505 double sample_step = (double)me->getSampleStep();
506
507 vpMeSite P;
508 P.init((int)PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign);
510
511 unsigned int memory_range = me->getRange();
512 me->setRange(1);
513
514 vpImagePoint ip;
515
516 for (int i = 0; i < 3; i++) {
517 P.ifloat = P.ifloat + di * sample_step;
518 P.i = (int)P.ifloat;
519 P.jfloat = P.jfloat + dj * sample_step;
520 P.j = (int)P.jfloat;
521
522 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
523 P.track(I, me, false);
524
526 list.push_back(P);
527 if (vpDEBUG_ENABLE(3)) {
528 ip.set_i(P.i);
529 ip.set_j(P.j);
530
532 }
533 }
534 else {
535 if (vpDEBUG_ENABLE(3)) {
536 ip.set_i(P.i);
537 ip.set_j(P.j);
539 }
540 }
541 }
542 }
543
544 P.init((int)PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign);
546 for (int i = 0; i < 3; i++) {
547 P.ifloat = P.ifloat - di * sample_step;
548 P.i = (int)P.ifloat;
549 P.jfloat = P.jfloat - dj * sample_step;
550 P.j = (int)P.jfloat;
551
552 if (!outOfImage(P.i, P.j, 5, rows, cols)) {
553 P.track(I, me, false);
554
556 list.push_back(P);
557 if (vpDEBUG_ENABLE(3)) {
558 ip.set_i(P.i);
559 ip.set_j(P.j);
561 }
562 }
563 else {
564 if (vpDEBUG_ENABLE(3)) {
565 ip.set_i(P.i);
566 ip.set_j(P.j);
568 }
569 }
570 }
571 }
572
573 me->setRange(memory_range);
574
575 vpCDEBUG(1) << "end vpMeLine::sample() : ";
576 vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
577}
578
580{
581 double i1, j1, i2, j2;
582
583 if (!me) {
584 vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
585 throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
586 }
587
588 project(a, b, c, PExt[0].ifloat, PExt[0].jfloat, i1, j1);
589 project(a, b, c, PExt[1].ifloat, PExt[1].jfloat, i2, j2);
590
591 // Points extremites
592 PExt[0].ifloat = i1;
593 PExt[0].jfloat = j1;
594 PExt[1].ifloat = i2;
595 PExt[1].jfloat = j2;
596
597 double d = sqrt(vpMath::sqr(i1 - i2) + vpMath::sqr(j1 - j2));
598
599 unsigned int n = numberOfSignal();
600 double expecteddensity = d / (double)me->getSampleStep();
601
602 if ((double)n < 0.9 * expecteddensity) {
603 double delta_new = delta;
604 delta = delta_1;
605 sample(I);
606 delta = delta_new;
607 // 2. On appelle ce qui n'est pas specifique
608 {
610 }
611 }
612}
613
615{
616 vpMeSite p_me;
617
618 double angle_ = delta + M_PI / 2;
619 double diff = 0;
620
621 while (angle_ < 0)
622 angle_ += M_PI;
623 while (angle_ > M_PI)
624 angle_ -= M_PI;
625
626 angle_ = vpMath::round(angle_ * 180 / M_PI);
627
628 // if(fabs(angle_) == 180 )
629 if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
630 angle_ = 0;
631 }
632
633 // std::cout << "angle theta : " << theta << std::endl ;
634 diff = fabs(angle_ - angle_1);
635 if (diff > 90)
636 sign *= -1;
637
638 angle_1 = angle_;
639
640 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
641 p_me = *it;
642 p_me.alpha = delta;
643 p_me.mask_sign = sign;
644 *it = p_me;
645 }
646 delta_1 = delta;
647}
648
650{
651 vpCDEBUG(1) << "begin vpMeLine::track()" << std::endl;
652
653 // 1. On fait ce qui concerne les droites (peut etre vide)
654 {} // 2. On appelle ce qui n'est pas specifique
655 {
657 }
658
659 // 3. On revient aux droites
660 {
661 // supression des points rejetes par les ME
664
665 // Estimation des parametres de la droite aux moindres carre
666 try {
667 leastSquare();
668 }
669 catch (...) {
670 vpERROR_TRACE("Error caught");
671 throw;
672 }
673
674 // recherche de point aux extremite de la droites
675 // dans le cas d'un glissement
677
679 try {
680 leastSquare();
681 }
682 catch (...) {
683 vpERROR_TRACE("Error caught");
684 throw;
685 }
686
687 // suppression des points rejetes par la regression robuste
690
691 // reechantillonage si necessaire
692 reSample(I);
693
694 // remet a jour l'angle delta pour chaque point de la liste
695
696 updateDelta();
697
698 // Remise a jour de delta dans la liste de site me
699 if (vpDEBUG_ENABLE(2)) {
703 }
704 }
705
707
708 vpCDEBUG(1) << "end vpMeLine::track()" << std::endl;
709}
710
711void vpMeLine::update_indices(double theta, int i, int j, int incr, int &i1, int &i2, int &j1, int &j2)
712{
713 i1 = (int)(i + cos(theta) * incr);
714 j1 = (int)(j + sin(theta) * incr);
715
716 i2 = (int)(i - cos(theta) * incr);
717 j2 = (int)(j - sin(theta) * incr);
718}
719
721{
722 // rho = -c ;
723 // theta = atan2(a,b) ;
724 rho = fabs(c);
725 theta = atan2(b, a);
726
727 while (theta >= M_PI)
728 theta -= M_PI;
729 while (theta < 0)
730 theta += M_PI;
731
733 // convention pour choisir le signe de rho
734 int i, j;
735 i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat) / 2);
736 j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat) / 2);
737
738 int end = false;
739 int incr = 10;
740
741 int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
742 unsigned char v1 = 0, v2 = 0;
743
744 int width_ = (int)I.getWidth();
745 int height_ = (int)I.getHeight();
746 update_indices(theta, i, j, incr, i1, i2, j1, j2);
747
748 if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
749 double rho_lim1 = fabs((double)i / cos(theta));
750 double rho_lim2 = fabs((double)j / sin(theta));
751
752 double co_rho_lim1 = fabs(((double)(height_ - i)) / cos(theta));
753 double co_rho_lim2 = fabs(((double)(width_ - j)) / sin(theta));
754
755 double rho_lim = (std::min)(rho_lim1, rho_lim2);
756 double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2);
757 incr = (int)std::floor((std::min)(rho_lim, co_rho_lim));
758 if (incr < INCR_MIN) {
759 vpERROR_TRACE("increment is too small");
760 throw(vpTrackingException(vpTrackingException::fatalError, "increment is too small"));
761 }
762 update_indices(theta, i, j, incr, i1, i2, j1, j2);
763 }
764
765 while (!end) {
766 end = true;
767 unsigned int i1_ = static_cast<unsigned int>(i1);
768 unsigned int j1_ = static_cast<unsigned int>(j1);
769 unsigned int i2_ = static_cast<unsigned int>(i2);
770 unsigned int j2_ = static_cast<unsigned int>(j2);
771 v1 = I[i1_][j1_];
772 v2 = I[i2_][j2_];
773 if (abs(v1 - v2) < 1) {
774 incr--;
775 end = false;
776 if (incr == 1) {
777 throw(vpException(vpException::fatalError, "In vpMeLine cannot determine rho sign, since "
778 "there is no gray level difference between both "
779 "sides of the line"));
780 }
781 }
782 update_indices(theta, i, j, incr, i1, i2, j1, j2);
783 }
784
785 if (theta >= 0 && theta <= M_PI / 2) {
786 if (v2 < v1) {
787 theta += M_PI;
788 rho *= -1;
789 }
790 }
791
792 else {
793 double jinter;
794 jinter = -c / b;
795 if (v2 < v1) {
796 theta += M_PI;
797 if (jinter > 0) {
798 rho *= -1;
799 }
800 }
801
802 else {
803 if (jinter < 0) {
804 rho *= -1;
805 }
806 }
807 }
808
809 if (vpDEBUG_ENABLE(2)) {
810 vpImagePoint ip, ip1, ip2;
811
812 ip.set_i(i);
813 ip.set_j(j);
814 ip1.set_i(i1);
815 ip1.set_j(j1);
816 ip2.set_i(i2);
817 ip2.set_j(j2);
818
821 }
822 }
823}
824
825double vpMeLine::getRho() const { return rho; }
826
827double vpMeLine::getTheta() const { return theta; }
828
830{
831 /*Return the coordinates of the extremities of the line*/
832 ip1.set_i(PExt[0].ifloat);
833 ip1.set_j(PExt[0].jfloat);
834 ip2.set_i(PExt[1].ifloat);
835 ip2.set_j(PExt[1].jfloat);
836}
837
838bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
839{
840 double a1 = line1.a;
841 double b1 = line1.b;
842 double c1 = line1.c;
843 double a2 = line2.a;
844 double b2 = line2.b;
845 double c2 = line2.c;
846
847 try {
848 double i = 0, j = 0;
849 double denom = 0;
850
851 if (a1 > 0.1) {
852 denom = (-(a2 / a1) * b1 + b2);
853
854 // if (denom == 0)
855 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
856 std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
857 return (false);
858 }
859
860 // if (denom != 0 )
861 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
862 j = ((a2 / a1) * c1 - c2) / denom;
863 i = (-b1 * j - c1) / a1;
864 }
865 }
866
867 else {
868 denom = (-(b2 / b1) * a1 + a2);
869
870 // if (denom == 0)
871 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
872 std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
873 return (false);
874 }
875
876 // if (denom != 0 )
877 if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
878 i = ((b2 / b1) * c1 - c2) / denom;
879 j = (-a1 * i - c1) / b1;
880 }
881 }
882 ip.set_i(i);
883 ip.set_j(j);
884
885 return (true);
886 }
887 catch (...) {
888 return (false);
889 }
890}
891
892#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
914void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
915 const double &B, const double &C, const vpColor &color, unsigned int thickness)
916{
917 vpMeLine::displayLine(I, PExt1, PExt2, A, B, C, color, thickness);
918}
919
941void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
942 const double &B, const double &C, const vpColor &color, unsigned int thickness)
943{
944 vpMeLine::displayLine(I, PExt1, PExt2, A, B, C, color, thickness);
945}
946
970void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
971 const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
972 const vpColor &color, unsigned int thickness)
973{
974 vpMeLine::displayLine(I, PExt1, PExt2, site_list, A, B, C, color, thickness);
975}
976
1000void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1001 const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1002 const vpColor &color, unsigned int thickness)
1003{
1004
1005 vpMeLine::displayLine(I, PExt1, PExt2, site_list, A, B, C, color, thickness);
1006}
1007#endif // Deprecated
1008
1009void vpMeLine::displayLine(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1010 const double &B, const double &C, const vpColor &color, unsigned int thickness)
1011{
1012 vpImagePoint ip1, ip2;
1013
1014 if (fabs(A) < fabs(B)) {
1015 double i1, j1, i2, j2;
1016 i1 = 0;
1017 j1 = (-A * i1 - C) / B;
1018 i2 = I.getHeight() - 1.0;
1019 j2 = (-A * i2 - C) / B;
1020
1021 ip1.set_i(i1);
1022 ip1.set_j(j1);
1023 ip2.set_i(i2);
1024 ip2.set_j(j2);
1025 vpDisplay::displayLine(I, ip1, ip2, color);
1026 }
1027 else {
1028 double i1, j1, i2, j2;
1029 j1 = 0;
1030 i1 = -(B * j1 + C) / A;
1031 j2 = I.getWidth() - 1.0;
1032 i2 = -(B * j2 + C) / A;
1033
1034 ip1.set_i(i1);
1035 ip1.set_j(j1);
1036 ip2.set_i(i2);
1037 ip2.set_j(j2);
1038 vpDisplay::displayLine(I, ip1, ip2, color);
1039 }
1040
1041 ip1.set_i(PExt1.ifloat);
1042 ip1.set_j(PExt1.jfloat);
1043 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1044
1045 ip1.set_i(PExt2.ifloat);
1046 ip1.set_j(PExt2.jfloat);
1047 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1048}
1049
1050void vpMeLine::displayLine(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1051 const double &B, const double &C, const vpColor &color, unsigned int thickness)
1052{
1053 vpImagePoint ip1, ip2;
1054
1055 if (fabs(A) < fabs(B)) {
1056 double i1, j1, i2, j2;
1057 i1 = 0;
1058 j1 = (-A * i1 - C) / B;
1059 i2 = I.getHeight() - 1.0;
1060 j2 = (-A * i2 - C) / B;
1061
1062 ip1.set_i(i1);
1063 ip1.set_j(j1);
1064 ip2.set_i(i2);
1065 ip2.set_j(j2);
1066 vpDisplay::displayLine(I, ip1, ip2, color);
1067 }
1068 else {
1069 double i1, j1, i2, j2;
1070 j1 = 0;
1071 i1 = -(B * j1 + C) / A;
1072 j2 = I.getWidth() - 1.0;
1073 i2 = -(B * j2 + C) / A;
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 }
1081
1082 ip1.set_i(PExt1.ifloat);
1083 ip1.set_j(PExt1.jfloat);
1084 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1085
1086 ip1.set_i(PExt2.ifloat);
1087 ip1.set_j(PExt2.jfloat);
1088 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1089}
1090
1091void vpMeLine::displayLine(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1092 const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1093 const vpColor &color, unsigned int thickness)
1094{
1095 vpImagePoint ip;
1096
1097 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1098 vpMeSite pix = *it;
1099 ip.set_i(pix.ifloat);
1100 ip.set_j(pix.jfloat);
1101
1102 if (pix.getState() == vpMeSite::M_ESTIMATOR)
1103 vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1104 else
1105 vpDisplay::displayCross(I, ip, 5, color, thickness);
1106 }
1107
1108 vpImagePoint ip1, ip2;
1109
1110 if (fabs(A) < fabs(B)) {
1111 double i1, j1, i2, j2;
1112 i1 = 0;
1113 j1 = (-A * i1 - C) / B;
1114 i2 = I.getHeight() - 1.0;
1115 j2 = (-A * i2 - C) / B;
1116
1117 ip1.set_i(i1);
1118 ip1.set_j(j1);
1119 ip2.set_i(i2);
1120 ip2.set_j(j2);
1121 vpDisplay::displayLine(I, ip1, ip2, color);
1122 }
1123 else {
1124 double i1, j1, i2, j2;
1125 j1 = 0;
1126 i1 = -(B * j1 + C) / A;
1127 j2 = I.getWidth() - 1.0;
1128 i2 = -(B * j2 + C) / A;
1129
1130 ip1.set_i(i1);
1131 ip1.set_j(j1);
1132 ip2.set_i(i2);
1133 ip2.set_j(j2);
1134 vpDisplay::displayLine(I, ip1, ip2, color);
1135 }
1136
1137 ip1.set_i(PExt1.ifloat);
1138 ip1.set_j(PExt1.jfloat);
1139 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1140
1141 ip1.set_i(PExt2.ifloat);
1142 ip1.set_j(PExt2.jfloat);
1143 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1144}
1145
1146void vpMeLine::displayLine(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1147 const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1148 const vpColor &color, unsigned int thickness)
1149{
1150 vpImagePoint ip;
1151
1152 for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1153 vpMeSite pix = *it;
1154 ip.set_i(pix.ifloat);
1155 ip.set_j(pix.jfloat);
1156
1157 if (pix.getState() == vpMeSite::M_ESTIMATOR)
1158 vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1159 else
1160 vpDisplay::displayCross(I, ip, 5, color, thickness);
1161 }
1162
1163 vpImagePoint ip1, ip2;
1164
1165 if (fabs(A) < fabs(B)) {
1166 double i1, j1, i2, j2;
1167 i1 = 0;
1168 j1 = (-A * i1 - C) / B;
1169 i2 = I.getHeight() - 1.0;
1170 j2 = (-A * i2 - C) / B;
1171
1172 ip1.set_i(i1);
1173 ip1.set_j(j1);
1174 ip2.set_i(i2);
1175 ip2.set_j(j2);
1176 vpDisplay::displayLine(I, ip1, ip2, color);
1177 }
1178 else {
1179 double i1, j1, i2, j2;
1180 j1 = 0;
1181 i1 = -(B * j1 + C) / A;
1182 j2 = I.getWidth() - 1.0;
1183 i2 = -(B * j2 + C) / A;
1184
1185 ip1.set_i(i1);
1186 ip1.set_j(j1);
1187 ip2.set_i(i2);
1188 ip2.set_j(j2);
1189 vpDisplay::displayLine(I, ip1, ip2, color);
1190 }
1191
1192 ip1.set_i(PExt1.ifloat);
1193 ip1.set_j(PExt1.jfloat);
1194 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1195
1196 ip1.set_i(PExt2.ifloat);
1197 ip1.set_j(PExt2.jfloat);
1198 vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1199}
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
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 emitted by ViSP classes.
Definition vpException.h:59
@ fatalError
Fatal error.
Definition vpException.h:84
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 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
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that tracks in an image a line moving edges.
Definition vpMeLine.h:148
double theta
theta parameter of the line
Definition vpMeLine.h:156
void suppressPoints()
Definition vpMeLine.cpp:413
double getRho() const
Definition vpMeLine.cpp:825
double a
Parameter a of the line equation a*i + b*j + c = 0.
Definition vpMeLine.h:173
void computeRhoTheta(const vpImage< unsigned char > &I)
Definition vpMeLine.cpp:720
void updateDelta()
Definition vpMeLine.cpp:614
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1)
Definition vpMeLine.cpp:181
void reSample(const vpImage< unsigned char > &I)
Definition vpMeLine.cpp:579
double getTheta() const
Definition vpMeLine.cpp:827
double angle
Angle in deg between the extremities.
Definition vpMeLine.h:159
double c
Parameter c of the line equation a*i + b*j + c = 0.
Definition vpMeLine.h:175
double rho
rho parameter of the line
Definition vpMeLine.h:155
int sign
Sign.
Definition vpMeLine.h:161
vpMeSite PExt[2]
Line extremities.
Definition vpMeLine.h:153
void seekExtremities(const vpImage< unsigned char > &I)
Definition vpMeLine.cpp:472
void track(const vpImage< unsigned char > &I)
Definition vpMeLine.cpp:649
double angle_1
Angle in deg between the extremities.
Definition vpMeLine.h:160
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
Definition vpMeLine.cpp:838
void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
Definition vpMeLine.cpp:829
virtual ~vpMeLine()
Definition vpMeLine.cpp:115
static void displayLine(const vpImage< unsigned char > &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, const double &B, const double &C, const vpColor &color=vpColor::green, unsigned int thickness=1)
double delta_1
Angle in rad between the extremities.
Definition vpMeLine.h:158
double delta
Angle in rad between the extremities.
Definition vpMeLine.h:157
double b
Parameter b of the line equation a*i + b*j + c = 0.
Definition vpMeLine.h:174
void leastSquare()
Definition vpMeLine.cpp:212
void setExtremities()
Definition vpMeLine.cpp:426
void initTracking(const vpImage< unsigned char > &I)
Definition vpMeLine.cpp:186
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) override
Definition vpMeLine.cpp:117
bool _useIntensityForRho
Definition vpMeLine.h:165
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
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition vpMeSite.h:89
@ 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
Contains abstract elements for a Distance to Feature type feature.
Definition vpMeTracker.h:60
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition vpMeTracker.h:86
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
std::list< vpMeSite > list
Definition vpMeTracker.h:72
void display(const vpImage< unsigned char > &I)
vpMe * me
Moving edges initialisation parameters.
Definition vpMeTracker.h:74
void setRange(const unsigned int &r)
Definition vpMe.h:383
double getSampleStep() const
Definition vpMe.h:397
unsigned int getRange() const
Definition vpMe.h:273
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ initializationError
Tracker initialization error.
@ fatalError
Tracker fatal error.
#define vpCDEBUG(level)
Definition vpDebug.h:506
#define vpDERROR_TRACE
Definition vpDebug.h:459
#define vpERROR_TRACE
Definition vpDebug.h:388
#define vpDEBUG_ENABLE(level)
Definition vpDebug.h:533