Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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, const 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  unsigned int i;
272 
274  r.setThreshold(2);
275  r.setIteration(0);
277  D.eye();
278  vpMatrix DA, DAmemory;
279  vpColVector DAx;
282  w = 1;
283  vpMeSite p_me;
284  unsigned int iter = 0;
285  unsigned int nos_1 = 0;
286  double distance = 100;
287 
288  if (list.size() <= 2 || numberOfSignal() <= 2) {
289  // vpERROR_TRACE("Not enough point") ;
290  vpCDEBUG(1) << "Not enough point";
292  }
293 
294  if ((fabs(b) >= 0.9)) // Construction du systeme Ax=B
295  // a i + j + c = 0
296  // A = (i 1) B = (-j)
297  {
298  nos_1 = numberOfSignal();
299  unsigned int k = 0;
300  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
301  p_me = *it;
302  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
303  A[k][0] = p_me.ifloat;
304  A[k][1] = 1;
305  B[k] = -p_me.jfloat;
306  k++;
307  }
308  }
309 
310  while (iter < 4 && distance > 0.05) {
311  DA = D * A;
312  x = DA.pseudoInverse(1e-26) * D * B;
313 
314  vpColVector residu(nos_1);
315  residu = B - A * x;
316  r.setIteration(iter);
317  r.MEstimator(vpRobust::TUKEY, residu, w);
318 
319  k = 0;
320  for (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.setIteration(iter);
376  r.MEstimator(vpRobust::TUKEY, residu, w);
377 
378  k = 0;
379  for (i = 0; i < nos_1; i++) {
380  D[k][k] = w[k];
381  k++;
382  }
383  iter++;
384  distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
385  x_1 = x;
386  }
387 
388  k = 0;
389  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
390  p_me = *it;
391  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
392  if (w[k] < 0.2) {
394 
395  *it = p_me;
396  }
397  k++;
398  }
399  }
400  a = 1;
401  b = x[0];
402  c = x[1];
403 
404  double s = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
405  a /= s;
406  b /= s;
407  c /= s;
408  }
409 
410  // mise a jour du delta
411  delta = atan2(a, b);
412 
413  normalizeAngle(delta);
414 }
415 
426 {
427  vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
428 
429  int i1s, j1s, i2s, j2s;
430 
431  i1s = vpMath::round(ip1.get_i());
432  i2s = vpMath::round(ip2.get_i());
433  j1s = vpMath::round(ip1.get_j());
434  j2s = vpMath::round(ip2.get_j());
435 
436  try {
437 
438  // 1. On fait ce qui concerne les droites (peut etre vide)
439  {
440  // Points extremites
441  PExt[0].ifloat = (float)ip1.get_i();
442  PExt[0].jfloat = (float)ip1.get_j();
443  PExt[1].ifloat = (float)ip2.get_i();
444  PExt[1].jfloat = (float)ip2.get_j();
445 
446  double angle_ = atan2((double)(i1s - i2s), (double)(j1s - j2s));
447  a = cos(angle_);
448  b = sin(angle_);
449 
450  // Real values of a, b can have an other sign. So to get the good values
451  // of a and b in order to initialise then c, we call track(I) just below
452 
453  computeDelta(delta, i1s, j1s, i2s, j2s);
454  delta_1 = delta;
455 
456  // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a),
457  // delta);
458 
459  sample(I);
460  }
461  // 2. On appelle ce qui n'est pas specifique
462  {
464  }
465  // Call track(I) to give the good sign to a and b and to initialise c
466  // which can be used for the display
467  track(I);
468  } catch (...) {
469  vpERROR_TRACE("Error caught");
470  throw;
471  }
472  vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
473 }
474 
479 {
480  // Loop through list of sites to track
481  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
482  vpMeSite s = *it; // current reference pixel
483 
485  it = list.erase(it);
486  else
487  ++it;
488  }
489 }
490 
495 {
496  double imin = +1e6;
497  double jmin = +1e6;
498  double imax = -1;
499  double jmax = -1;
500 
501  // Loop through list of sites to track
502  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
503  vpMeSite s = *it; // current reference pixel
504  if (s.ifloat < imin) {
505  imin = s.ifloat;
506  jmin = s.jfloat;
507  }
508 
509  if (s.ifloat > imax) {
510  imax = s.ifloat;
511  jmax = s.jfloat;
512  }
513  }
514 
515  PExt[0].ifloat = imin;
516  PExt[0].jfloat = jmin;
517  PExt[1].ifloat = imax;
518  PExt[1].jfloat = jmax;
519 
520  if (fabs(imin - imax) < 25) {
521  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
522  vpMeSite s = *it; // current reference pixel
523  if (s.jfloat < jmin) {
524  imin = s.ifloat;
525  jmin = s.jfloat;
526  }
527 
528  if (s.jfloat > jmax) {
529  imax = s.ifloat;
530  jmax = s.jfloat;
531  }
532  }
533  PExt[0].ifloat = imin;
534  PExt[0].jfloat = jmin;
535  PExt[1].ifloat = imax;
536  PExt[1].jfloat = jmax;
537  }
538 }
539 
552 {
553  vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
554 
555  if (!me) {
556  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
557  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
558  }
559 
560  int rows = (int)I.getHeight();
561  int cols = (int)I.getWidth();
562  double n_sample;
563 
564  // if (me->getSampleStep()==0)
565  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
566 
567  vpERROR_TRACE("function called with sample step = 0");
568  throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
569  }
570 
571  // i, j portions of the line_p
572  double diffsi = PExt[0].ifloat - PExt[1].ifloat;
573  double diffsj = PExt[0].jfloat - PExt[1].jfloat;
574 
575  double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
576 
577  double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
578  double dj = diffsj / sqrt(s);
579 
580  double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
581 
582  // number of samples along line_p
583  n_sample = length_p / (double)me->getSampleStep();
584  double sample_step = (double)me->getSampleStep();
585 
586  vpMeSite P;
587  P.init((int)PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign);
589 
590  unsigned int memory_range = me->getRange();
591  me->setRange(1);
592 
593  vpImagePoint ip;
594 
595  for (int i = 0; i < 3; i++) {
596  P.ifloat = P.ifloat + di * sample_step;
597  P.i = (int)P.ifloat;
598  P.jfloat = P.jfloat + dj * sample_step;
599  P.j = (int)P.jfloat;
600 
601  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
602  P.track(I, me, false);
603 
604  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
605  list.push_back(P);
606  if (vpDEBUG_ENABLE(3)) {
607  ip.set_i(P.i);
608  ip.set_j(P.j);
609 
611  }
612  } else {
613  if (vpDEBUG_ENABLE(3)) {
614  ip.set_i(P.i);
615  ip.set_j(P.j);
617  }
618  }
619  }
620  }
621 
622  P.init((int)PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign);
624  for (int i = 0; i < 3; i++) {
625  P.ifloat = P.ifloat - di * sample_step;
626  P.i = (int)P.ifloat;
627  P.jfloat = P.jfloat - dj * sample_step;
628  P.j = (int)P.jfloat;
629 
630  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
631  P.track(I, me, false);
632 
633  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
634  list.push_back(P);
635  if (vpDEBUG_ENABLE(3)) {
636  ip.set_i(P.i);
637  ip.set_j(P.j);
639  }
640  } else {
641  if (vpDEBUG_ENABLE(3)) {
642  ip.set_i(P.i);
643  ip.set_j(P.j);
645  }
646  }
647  }
648  }
649 
650  me->setRange(memory_range);
651 
652  vpCDEBUG(1) << "end vpMeLine::sample() : ";
653  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
654 }
655 
668 {
669  double i1, j1, i2, j2;
670 
671  if (!me) {
672  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
673  throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
674  }
675 
676  project(a, b, c, PExt[0].ifloat, PExt[0].jfloat, i1, j1);
677  project(a, b, c, PExt[1].ifloat, PExt[1].jfloat, i2, j2);
678 
679  // Points extremites
680  PExt[0].ifloat = i1;
681  PExt[0].jfloat = j1;
682  PExt[1].ifloat = i2;
683  PExt[1].jfloat = j2;
684 
685  double d = sqrt(vpMath::sqr(i1 - i2) + vpMath::sqr(j1 - j2));
686 
687  unsigned int n = numberOfSignal();
688  double expecteddensity = d / (double)me->getSampleStep();
689 
690  if ((double)n < 0.9 * expecteddensity) {
691  double delta_new = delta;
692  delta = delta_1;
693  sample(I);
694  delta = delta_new;
695  // 2. On appelle ce qui n'est pas specifique
696  {
698  }
699  }
700 }
701 
707 {
708  vpMeSite p_me;
709 
710  double angle_ = delta + M_PI / 2;
711  double diff = 0;
712 
713  while (angle_ < 0)
714  angle_ += M_PI;
715  while (angle_ > M_PI)
716  angle_ -= M_PI;
717 
718  angle_ = vpMath::round(angle_ * 180 / M_PI);
719 
720  // if(fabs(angle_) == 180 )
721  if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
722  angle_ = 0;
723  }
724 
725  // std::cout << "angle theta : " << theta << std::endl ;
726  diff = fabs(angle_ - angle_1);
727  if (diff > 90)
728  sign *= -1;
729 
730  angle_1 = angle_;
731 
732  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
733  p_me = *it;
734  p_me.alpha = delta;
735  p_me.mask_sign = sign;
736  *it = p_me;
737  }
738  delta_1 = delta;
739 }
740 
748 {
749  vpCDEBUG(1) << "begin vpMeLine::track()" << std::endl;
750 
751  // 1. On fait ce qui concerne les droites (peut etre vide)
752  {} // 2. On appelle ce qui n'est pas specifique
753  {
755  }
756 
757  // 3. On revient aux droites
758  {
759  // supression des points rejetes par les ME
760  suppressPoints();
761  setExtremities();
762 
763  // Estimation des parametres de la droite aux moindres carre
764  try {
765  leastSquare();
766  } catch (...) {
767  vpERROR_TRACE("Error caught");
768  throw;
769  }
770 
771  // recherche de point aux extremite de la droites
772  // dans le cas d'un glissement
773  seekExtremities(I);
774 
775  setExtremities();
776  try {
777  leastSquare();
778  } catch (...) {
779  vpERROR_TRACE("Error caught");
780  throw;
781  }
782 
783  // suppression des points rejetes par la regression robuste
784  suppressPoints();
785  setExtremities();
786 
787  // reechantillonage si necessaire
788  reSample(I);
789 
790  // remet a jour l'angle delta pour chaque point de la liste
791 
792  updateDelta();
793 
794  // Remise a jour de delta dans la liste de site me
795  if (vpDEBUG_ENABLE(2)) {
796  display(I, vpColor::red);
798  vpDisplay::flush(I);
799  }
800  }
801 
802  computeRhoTheta(I);
803 
804  vpCDEBUG(1) << "end vpMeLine::track()" << std::endl;
805 }
806 
807 void vpMeLine::update_indices(double theta, int i, int j, int incr, int &i1, int &i2, int &j1, int &j2)
808 {
809  i1 = (int)(i + cos(theta) * incr);
810  j1 = (int)(j + sin(theta) * incr);
811 
812  i2 = (int)(i - cos(theta) * incr);
813  j2 = (int)(j - sin(theta) * incr);
814 }
815 
823 {
824  // rho = -c ;
825  // theta = atan2(a,b) ;
826  rho = fabs(c);
827  theta = atan2(b, a);
828 
829  while (theta >= M_PI)
830  theta -= M_PI;
831  while (theta < 0)
832  theta += M_PI;
833 
834  if (_useIntensityForRho) {
835 
836  /* while(theta < -M_PI) theta += 2*M_PI ;
837  while(theta >= M_PI) theta -= 2*M_PI ;
838 
839  // If theta is between -90 and -180 get the equivalent
840  // between 0 and 90
841  if(theta <-M_PI/2)
842  {
843  theta += M_PI ;
844  rho *= -1 ;
845  }
846  // If theta is between 90 and 180 get the equivalent
847  // between 0 and -90
848  if(theta >M_PI/2)
849  {
850  theta -= M_PI ;
851  rho *= -1 ;
852  }
853  */
854  // convention pour choisir le signe de rho
855  int i, j;
856  i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat) / 2);
857  j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat) / 2);
858 
859  int end = false;
860  int incr = 10;
861 
862  int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
863  unsigned char v1 = 0, v2 = 0;
864 
865  int width_ = (int)I.getWidth();
866  int height_ = (int)I.getHeight();
867  update_indices(theta, i, j, incr, i1, i2, j1, j2);
868 
869  if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
870  double rho_lim1 = fabs((double)i / cos(theta));
871  double rho_lim2 = fabs((double)j / sin(theta));
872 
873  double co_rho_lim1 = fabs(((double)(height_ - i)) / cos(theta));
874  double co_rho_lim2 = fabs(((double)(width_ - j)) / sin(theta));
875 
876  double rho_lim = (std::min)(rho_lim1, rho_lim2);
877  double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2);
878  incr = (int)std::floor((std::min)(rho_lim, co_rho_lim));
879  if (incr < INCR_MIN) {
880  vpERROR_TRACE("increment is too small");
881  throw(vpTrackingException(vpTrackingException::fatalError, "increment is too small"));
882  }
883  update_indices(theta, i, j, incr, i1, i2, j1, j2);
884  }
885 
886  while (!end) {
887  end = true;
888  unsigned int i1_ = static_cast<unsigned int>(i1);
889  unsigned int j1_ = static_cast<unsigned int>(j1);
890  unsigned int i2_ = static_cast<unsigned int>(i2);
891  unsigned int j2_ = static_cast<unsigned int>(j2);
892  v1 = I[i1_][j1_];
893  v2 = I[i2_][j2_];
894  if (abs(v1 - v2) < 1) {
895  incr--;
896  end = false;
897  if (incr == 1) {
898  throw(vpException(vpException::fatalError, "In vpMeLine cannot determine rho sign, since "
899  "there is no gray level difference between both "
900  "sides of the line"));
901  }
902  }
903  update_indices(theta, i, j, incr, i1, i2, j1, j2);
904  }
905 
906  if (theta >= 0 && theta <= M_PI / 2) {
907  if (v2 < v1) {
908  theta += M_PI;
909  rho *= -1;
910  }
911  }
912 
913  else {
914  double jinter;
915  jinter = -c / b;
916  if (v2 < v1) {
917  theta += M_PI;
918  if (jinter > 0) {
919  rho *= -1;
920  }
921  }
922 
923  else {
924  if (jinter < 0) {
925  rho *= -1;
926  }
927  }
928  }
929 
930  if (vpDEBUG_ENABLE(2)) {
931  vpImagePoint ip, ip1, ip2;
932 
933  ip.set_i(i);
934  ip.set_j(j);
935  ip1.set_i(i1);
936  ip1.set_j(j1);
937  ip2.set_i(i2);
938  ip2.set_j(j2);
939 
942  }
943  }
944 }
945 
956 double vpMeLine::getRho() const { return rho; }
957 
961 double vpMeLine::getTheta() const { return theta; }
962 
971 {
972  /*Return the coordinates of the extremities of the line*/
973  ip1.set_i(PExt[0].ifloat);
974  ip1.set_j(PExt[0].jfloat);
975  ip2.set_i(PExt[1].ifloat);
976  ip2.set_j(PExt[1].jfloat);
977 }
978 
991 bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
992 {
993  double a1 = line1.a;
994  double b1 = line1.b;
995  double c1 = line1.c;
996  double a2 = line2.a;
997  double b2 = line2.b;
998  double c2 = line2.c;
999 
1000  try {
1001  double i = 0, j = 0;
1002  double denom = 0;
1003 
1004  if (a1 > 0.1) {
1005  denom = (-(a2 / a1) * b1 + b2);
1006 
1007  // if (denom == 0)
1008  if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1009  std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1010  return (false);
1011  }
1012 
1013  // if (denom != 0 )
1014  if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1015  j = ((a2 / a1) * c1 - c2) / denom;
1016  i = (-b1 * j - c1) / a1;
1017  }
1018  }
1019 
1020  else {
1021  denom = (-(b2 / b1) * a1 + a2);
1022 
1023  // if (denom == 0)
1024  if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
1025  std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1026  return (false);
1027  }
1028 
1029  // if (denom != 0 )
1030  if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
1031  i = ((b2 / b1) * c1 - c2) / denom;
1032  j = (-a1 * i - c1) / b1;
1033  }
1034  }
1035  ip.set_i(i);
1036  ip.set_j(j);
1037 
1038  return (true);
1039  } catch (...) {
1040  return (false);
1041  }
1042 }
1043 
1064 void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1065  const double &B, const double &C, const vpColor &color, unsigned int thickness)
1066 {
1067  vpImagePoint ip1, ip2;
1068 
1069  if (fabs(A) < fabs(B)) {
1070  double i1, j1, i2, j2;
1071  i1 = 0;
1072  j1 = (-A * i1 - C) / B;
1073  i2 = I.getHeight() - 1.0;
1074  j2 = (-A * i2 - C) / B;
1075 
1076  ip1.set_i(i1);
1077  ip1.set_j(j1);
1078  ip2.set_i(i2);
1079  ip2.set_j(j2);
1080  vpDisplay::displayLine(I, ip1, ip2, color);
1081  // vpDisplay::flush(I);
1082 
1083  } else {
1084  double i1, j1, i2, j2;
1085  j1 = 0;
1086  i1 = -(B * j1 + C) / A;
1087  j2 = I.getWidth() - 1.0;
1088  i2 = -(B * j2 + C) / A;
1089 
1090  ip1.set_i(i1);
1091  ip1.set_j(j1);
1092  ip2.set_i(i2);
1093  ip2.set_j(j2);
1094  vpDisplay::displayLine(I, ip1, ip2, color);
1095  // vpDisplay::flush(I);
1096  }
1097 
1098  ip1.set_i(PExt1.ifloat);
1099  ip1.set_j(PExt1.jfloat);
1100  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1101 
1102  ip1.set_i(PExt2.ifloat);
1103  ip1.set_j(PExt2.jfloat);
1104  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1105 }
1106 
1127 void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
1128  const double &B, const double &C, const vpColor &color, unsigned int thickness)
1129 {
1130  vpImagePoint ip1, ip2;
1131 
1132  if (fabs(A) < fabs(B)) {
1133  double i1, j1, i2, j2;
1134  i1 = 0;
1135  j1 = (-A * i1 - C) / B;
1136  i2 = I.getHeight() - 1.0;
1137  j2 = (-A * i2 - C) / B;
1138 
1139  ip1.set_i(i1);
1140  ip1.set_j(j1);
1141  ip2.set_i(i2);
1142  ip2.set_j(j2);
1143  vpDisplay::displayLine(I, ip1, ip2, color);
1144  // vpDisplay::flush(I);
1145 
1146  } else {
1147  double i1, j1, i2, j2;
1148  j1 = 0;
1149  i1 = -(B * j1 + C) / A;
1150  j2 = I.getWidth() - 1.0;
1151  i2 = -(B * j2 + C) / A;
1152 
1153  ip1.set_i(i1);
1154  ip1.set_j(j1);
1155  ip2.set_i(i2);
1156  ip2.set_j(j2);
1157  vpDisplay::displayLine(I, ip1, ip2, color);
1158  // vpDisplay::flush(I);
1159  }
1160 
1161  ip1.set_i(PExt1.ifloat);
1162  ip1.set_j(PExt1.jfloat);
1163  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1164 
1165  ip1.set_i(PExt2.ifloat);
1166  ip1.set_j(PExt2.jfloat);
1167  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1168 }
1169 
1192 void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1193  const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1194  const vpColor &color, unsigned int thickness)
1195 {
1196  vpImagePoint ip;
1197 
1198  for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1199  vpMeSite pix = *it;
1200  ip.set_i(pix.ifloat);
1201  ip.set_j(pix.jfloat);
1202 
1203  if (pix.getState() == vpMeSite::M_ESTIMATOR)
1204  vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1205  else
1206  vpDisplay::displayCross(I, ip, 5, color, thickness);
1207 
1208  // vpDisplay::flush(I);
1209  }
1210 
1211  vpImagePoint ip1, ip2;
1212 
1213  if (fabs(A) < fabs(B)) {
1214  double i1, j1, i2, j2;
1215  i1 = 0;
1216  j1 = (-A * i1 - C) / B;
1217  i2 = I.getHeight() - 1.0;
1218  j2 = (-A * i2 - C) / B;
1219 
1220  ip1.set_i(i1);
1221  ip1.set_j(j1);
1222  ip2.set_i(i2);
1223  ip2.set_j(j2);
1224  vpDisplay::displayLine(I, ip1, ip2, color);
1225  // vpDisplay::flush(I);
1226 
1227  } else {
1228  double i1, j1, i2, j2;
1229  j1 = 0;
1230  i1 = -(B * j1 + C) / A;
1231  j2 = I.getWidth() - 1.0;
1232  i2 = -(B * j2 + C) / A;
1233 
1234  ip1.set_i(i1);
1235  ip1.set_j(j1);
1236  ip2.set_i(i2);
1237  ip2.set_j(j2);
1238  vpDisplay::displayLine(I, ip1, ip2, color);
1239  // vpDisplay::flush(I);
1240  }
1241 
1242  ip1.set_i(PExt1.ifloat);
1243  ip1.set_j(PExt1.jfloat);
1244  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1245 
1246  ip1.set_i(PExt2.ifloat);
1247  ip1.set_j(PExt2.jfloat);
1248  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1249 }
1250 
1273 void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
1274  const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
1275  const vpColor &color, unsigned int thickness)
1276 {
1277  vpImagePoint ip;
1278 
1279  for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
1280  vpMeSite pix = *it;
1281  ip.set_i(pix.ifloat);
1282  ip.set_j(pix.jfloat);
1283 
1284  if (pix.getState() == vpMeSite::M_ESTIMATOR)
1285  vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
1286  else
1287  vpDisplay::displayCross(I, ip, 5, color, thickness);
1288 
1289  // vpDisplay::flush(I);
1290  }
1291 
1292  vpImagePoint ip1, ip2;
1293 
1294  if (fabs(A) < fabs(B)) {
1295  double i1, j1, i2, j2;
1296  i1 = 0;
1297  j1 = (-A * i1 - C) / B;
1298  i2 = I.getHeight() - 1.0;
1299  j2 = (-A * i2 - C) / B;
1300 
1301  ip1.set_i(i1);
1302  ip1.set_j(j1);
1303  ip2.set_i(i2);
1304  ip2.set_j(j2);
1305  vpDisplay::displayLine(I, ip1, ip2, color);
1306  // vpDisplay::flush(I);
1307 
1308  } else {
1309  double i1, j1, i2, j2;
1310  j1 = 0;
1311  i1 = -(B * j1 + C) / A;
1312  j2 = I.getWidth() - 1.0;
1313  i2 = -(B * j2 + C) / A;
1314 
1315  ip1.set_i(i1);
1316  ip1.set_j(j1);
1317  ip2.set_i(i2);
1318  ip2.set_j(j2);
1319  vpDisplay::displayLine(I, ip1, ip2, color);
1320  // vpDisplay::flush(I);
1321  }
1322 
1323  ip1.set_i(PExt1.ifloat);
1324  ip1.set_j(PExt1.jfloat);
1325  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1326 
1327  ip1.set_i(PExt2.ifloat);
1328  ip1.set_j(PExt2.jfloat);
1329  vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
1330 }
vpMeLine()
Definition: vpMeLine.cpp:98
void reSample(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:667
unsigned int getRange() const
Definition: vpMe.h:179
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
double c
Parameter c of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:176
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)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
double get_i() const
Definition: vpImagePoint.h:204
unsigned int getWidth() const
Definition: vpImage.h:239
double jfloat
Definition: vpMeSite.h:88
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538
unsigned int numberOfSignal()
double getTheta() const
Definition: vpMeLine.cpp:961
double angle
Definition: vpMeLine.h:161
void setExtremities()
Definition: vpMeLine.cpp:494
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 colors available for display functionnalities.
Definition: vpColor.h:120
double delta
Definition: vpMeLine.h:160
bool _useIntensityForRho
Definition: vpMeLine.h:166
double alpha
Definition: vpMeSite.h:92
int i
Definition: vpMeSite.h:86
error that can be emited by ViSP classes.
Definition: vpException.h:71
void track(const vpImage< unsigned char > &Im)
Definition: vpMeLine.cpp:747
int mask_sign
Definition: vpMeSite.h:90
static const vpColor green
Definition: vpColor.h:183
static void flush(const vpImage< unsigned char > &I)
static int round(const double x)
Definition: vpMath.h:235
double get_j() const
Definition: vpImagePoint.h:215
void leastSquare()
Definition: vpMeLine.cpp:265
static const vpColor red
Definition: vpColor.h:180
vpMeSiteState getState() const
Definition: vpMeSite.h:188
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeLine.cpp:224
double ifloat
Definition: vpMeSite.h:88
double rho
Definition: vpMeLine.h:159
virtual ~vpMeLine()
Definition: vpMeLine.cpp:134
void set_i(const double ii)
Definition: vpImagePoint.h:167
void computeRhoTheta(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:822
std::list< vpMeSite > list
Definition: vpMeTracker.h:74
Error that can be emited by the vpTracker class and its derivates.
void updateDelta()
Definition: vpMeLine.cpp:706
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:970
double theta
Definition: vpMeLine.h:159
static double sqr(double x)
Definition: vpMath.h:108
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:151
void suppressPoints()
Definition: vpMeLine.cpp:478
double delta_1
Definition: vpMeLine.h:160
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:137
#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:174
vpMeSite PExt[2]
Definition: vpMeLine.h:157
void set_j(const double jj)
Definition: vpImagePoint.h:178
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:86
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
double b
Parameter b of the line equation a*i + b*j + c = 0.
Definition: vpMeLine.h:175
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
Definition: vpMeSite.cpp:355
virtual void sample(const vpImage< unsigned char > &image, const bool doNotTrack=false)
Definition: vpMeLine.cpp:148
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:76
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
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:991
unsigned int getHeight() const
Definition: vpImage.h:178
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1932
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void setRange(const unsigned int &r)
Definition: vpMe.h:271
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
double getSampleStep() const
Definition: vpMe.h:285
double getRho() const
Definition: vpMeLine.cpp:956
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:83
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:109
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:551
void eye()
Definition: vpMatrix.cpp:360
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
static const vpColor blue
Definition: vpColor.h:186