Visual Servoing Platform  version 3.6.1 under development (2023-12-07)
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 
54 void computeDelta(double &delta, int i1, int j1, int i2, int j2);
55 
56 static 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 
66 void 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 
77 static 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 
115 vpMeLine::~vpMeLine() { list.clear(); }
116 
117 void 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 
181 void 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 
190  vpDisplay::flush(I);
191 
192  std::cout << "Click on the line first point..." << std::endl;
193  while (vpDisplay::getClick(I, ip1) != true) { }
194 
196  vpDisplay::flush(I);
197  std::cout << "Click on the line second point..." << std::endl;
198  while (vpDisplay::getClick(I, ip2) != true) { }
199 
201  vpDisplay::flush(I);
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 
525  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
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 
555  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
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
662  suppressPoints();
663  setExtremities();
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
676  seekExtremities(I);
677 
678  setExtremities();
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
688  suppressPoints();
689  setExtremities();
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)) {
700  display(I, vpColor::red);
702  vpDisplay::flush(I);
703  }
704  }
705 
706  computeRhoTheta(I);
707 
708  vpCDEBUG(1) << "end vpMeLine::track()" << std::endl;
709 }
710 
711 void 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 
732  if (_useIntensityForRho) {
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 
825 double vpMeLine::getRho() const { return rho; }
826 
827 double 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 
838 bool 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
914 void 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 
941 void 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 
970 void 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 
1000 void 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 
1009 void 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 
1050 void 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 
1091 void 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 
1146 void 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.
Definition: vpColVector.h:163
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 ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:294
double get_j() const
Definition: vpImagePoint.h:125
void set_i(double ii)
Definition: vpImagePoint.h:283
double get_i() const
Definition: vpImagePoint.h:114
unsigned int getWidth() const
Definition: vpImage.h:240
unsigned int getHeight() const
Definition: vpImage.h:182
static double sqr(double x)
Definition: vpMath.h:201
static int round(double x)
Definition: vpMath.h:403
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2227
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:147
double theta
theta parameter of the line
Definition: vpMeLine.h:155
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:166
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:158
double c
Parameter c of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:168
double rho
rho parameter of the line
Definition: vpMeLine.h:154
int sign
Sign.
Definition: vpMeLine.h:160
vpMeSite PExt[2]
Line extremities.
Definition: vpMeLine.h:152
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:472
vpMeLine()
Definition: vpMeLine.cpp:89
void track(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:649
double angle_1
Angle in deg between the extremities.
Definition: vpMeLine.h:159
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
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)
Definition: vpMeLine.cpp:1009
double delta_1
Angle in rad between the extremities.
Definition: vpMeLine.h:157
double delta
Angle in rad between the extremities.
Definition: vpMeLine.h:156
double b
Parameter b of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:167
void leastSquare()
Definition: vpMeLine.cpp:212
void setExtremities()
Definition: vpMeLine.cpp:426
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:186
virtual ~vpMeLine() override
Definition: vpMeLine.cpp:115
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) override
Definition: vpMeLine.cpp:117
bool _useIntensityForRho
Definition: vpMeLine.h:164
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:97
@ 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:204
double ifloat
Floating coordinates along i of a site.
Definition: vpMeSite.h:99
int i
Coordinate along i of a site.
Definition: vpMeSite.h:95
int mask_sign
Mask sign.
Definition: vpMeSite.h:103
double alpha
Angle of tangent at site.
Definition: vpMeSite.h:105
void init()
Definition: vpMeSite.cpp:59
double jfloat
Floating coordinates along j of a site.
Definition: vpMeSite.h:101
vpMeSiteState getState() const
Definition: vpMeSite.h:251
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_likelihood=true)
Definition: vpMeSite.cpp:252
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:241
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 &range)
Definition: vpMe.h:393
double getSampleStep() const
Definition: vpMe.h:280
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:88
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:134
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:154
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