Visual Servoing Platform  version 3.4.1 under development (2021-04-20)
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 
59 void computeDelta(double &delta, int i1, int j1, int i2, int j2);
60 
61 static 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 
71 void 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 
82 static 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 
134 vpMeLine::~vpMeLine() { list.clear(); }
135 
148 void 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  ;
244  vpDisplay::flush(I);
245  std::cout << "Click on the line second point..." << std::endl;
246  while (vpDisplay::getClick(I, ip2) != true)
247  ;
249  vpDisplay::flush(I);
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 
603  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
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 
632  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
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
759  suppressPoints();
760  setExtremities();
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
772  seekExtremities(I);
773 
774  setExtremities();
775  try {
776  leastSquare();
777  } catch (...) {
778  vpERROR_TRACE("Error caught");
779  throw;
780  }
781 
782  // suppression des points rejetes par la regression robuste
783  suppressPoints();
784  setExtremities();
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)) {
795  display(I, vpColor::red);
797  vpDisplay::flush(I);
798  }
799  }
800 
801  computeRhoTheta(I);
802 
803  vpCDEBUG(1) << "end vpMeLine::track()" << std::endl;
804 }
805 
806 void 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 
833  if (_useIntensityForRho) {
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 
955 double vpMeLine::getRho() const { return rho; }
956 
960 double 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 
990 bool 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 
1063 void 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 
1126 void 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 
1191 void 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 
1272 void 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 }
vpMeLine()
Definition: vpMeLine.cpp:98
void reSample(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:666
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
double c
Parameter c of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:176
double get_i() const
Definition: vpImagePoint.h:203
vpMeSiteState getState() const
Definition: vpMeSite.h:190
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
double getTheta() const
Definition: vpMeLine.cpp:960
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void init()
Definition: vpMeSite.cpp:66
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
double jfloat
Definition: vpMeSite.h:89
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538
unsigned int numberOfSignal()
double angle
Definition: vpMeLine.h:161
void setExtremities()
Definition: vpMeLine.cpp:493
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
double getRho() const
Definition: vpMeLine.cpp:955
double delta
Definition: vpMeLine.h:160
bool _useIntensityForRho
Definition: vpMeLine.h:166
double alpha
Definition: vpMeSite.h:93
int i
Definition: vpMeSite.h:87
error that can be emited by ViSP classes.
Definition: vpException.h:71
void track(const vpImage< unsigned char > &Im)
Definition: vpMeLine.cpp:746
int mask_sign
Definition: vpMeSite.h:91
static const vpColor green
Definition: vpColor.h:220
static void flush(const vpImage< unsigned char > &I)
void leastSquare()
Definition: vpMeLine.cpp:265
static const vpColor red
Definition: vpColor.h:217
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeLine.cpp:224
double ifloat
Definition: vpMeSite.h:89
double rho
Definition: vpMeLine.h:159
virtual ~vpMeLine()
Definition: vpMeLine.cpp:134
void computeRhoTheta(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:821
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
Error that can be emited by the vpTracker class and its derivates.
Point used by the tracker.
Definition: vpMeSite.h:78
void set_i(double ii)
Definition: vpImagePoint.h:166
double getSampleStep() const
Definition: vpMe.h:285
void updateDelta()
Definition: vpMeLine.cpp:705
int outOfImage(int i, int j, int half, int row, int cols)
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)
void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
Definition: vpMeLine.cpp:969
double theta
Definition: vpMeLine.h:159
static double sqr(double x)
Definition: vpMath.h:116
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:151
void suppressPoints()
Definition: vpMeLine.cpp:477
double get_j() const
Definition: vpImagePoint.h:214
double delta_1
Definition: vpMeLine.h:160
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
#define vpCDEBUG(level)
Definition: vpDebug.h:511
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:65
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:236
double a
Parameter a of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:174
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
vpMeSite PExt[2]
Definition: vpMeLine.h:157
static int round(double x)
Definition: vpMath.h:245
void set_j(double jj)
Definition: vpImagePoint.h:177
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
#define vpDERROR_TRACE
Definition: vpDebug.h:464
int j
Definition: vpMeSite.h:87
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double b
Parameter b of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:175
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:80
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
Tukey influence function.
Definition: vpRobust.h:93
int sign
Definition: vpMeLine.h:162
void initTracking(const vpImage< unsigned char > &I)
double angle_1
Definition: vpMeLine.h:161
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
Definition: vpMeLine.cpp:990
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
void setRange(const unsigned int &r)
Definition: vpMe.h:271
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
unsigned int getWidth() const
Definition: vpImage.h:246
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
unsigned int getRange() const
Definition: vpMe.h:179
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:550
void eye()
Definition: vpMatrix.cpp:449
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
virtual void sample(const vpImage< unsigned char > &image, bool doNotTrack=false)
Definition: vpMeLine.cpp:148
static const vpColor blue
Definition: vpColor.h:223