Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpMeLine.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
46 #include <visp3/me/vpMeTracker.h>
47 #include <visp3/me/vpMe.h>
48 #include <visp3/me/vpMeSite.h>
49 #include <visp3/me/vpMeLine.h>
50 #include <visp3/core/vpRobust.h>
51 #include <visp3/core/vpTrackingException.h>
52 #include <visp3/core/vpImagePoint.h>
53 #include <visp3/core/vpMath.h>
54 #include <stdlib.h>
55 #include <cmath> // std::fabs
56 #include <limits> // numeric_limits
57 #include <algorithm> // std::min
58 
59 #define INCR_MIN 1
60 
61 void computeDelta(double &delta, int i1, int j1, int i2, int j2);
62 
63 static void
64 normalizeAngle(double &delta)
65 {
66  while (delta > M_PI) { delta -= M_PI ; }
67  while (delta < -M_PI) { delta += M_PI ; }
68 }
69 
70 void
71 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 
83 static void
84 project(double a, double b, double c,
85  double i, double j, double &ip,double &jp)
86 {
87  if (fabs(a)>fabs(b))
88  {
89  jp = (vpMath::sqr(a)*j - a*b*i - c*b)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
90  ip = (-c-b*jp)/a;
91  }
92  else
93  {
94  ip = (vpMath::sqr(b)*i-a*b*j-c*a)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
95  jp = (-c-a*ip)/b;
96  }
97 }
98 
105  : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1),
106  _useIntensityForRho(true), a(0.), b(0.), c(0.)
107 {
108 }
115  : vpMeTracker(meline),
116  rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1),
117  _useIntensityForRho(true), a(0.), b(0.), c(0.)
118 
119 {
120  rho = meline.rho;
121  theta = meline.theta;
122  delta = meline.delta;
123  delta_1 = meline.delta_1;
124  angle = meline.angle;
125  angle_1 = meline.angle_1;
126  sign = meline.sign;
127 
128  a = meline.a;
129  b = meline.b;
130  c = meline.c;
132  PExt[0] = meline.PExt[0];
133  PExt[1] = meline.PExt[1];
134 }
135 
142 {
143  list.clear();
144 }
145 
156 void
158 {
159  if (!me) {
160  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
162  "Moving edges not initialized")) ;
163  }
164 
165  int rows = (int)I.getHeight() ;
166  int cols = (int)I.getWidth() ;
167  double n_sample;
168 
169  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
170  {
171  vpERROR_TRACE("function called with sample step = 0") ;
173  "sample step = 0")) ;
174  }
175 
176  // i, j portions of the line_p
177  double diffsi = PExt[0].ifloat-PExt[1].ifloat;
178  double diffsj = PExt[0].jfloat-PExt[1].jfloat;
179 
180  double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
181  if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
182  throw(vpTrackingException(vpTrackingException::fatalError,"points too close of each other to define a line")) ;
183  // number of samples along line_p
184  n_sample = length_p/(double)me->getSampleStep();
185 
186  double stepi = diffsi/(double)n_sample;
187  double stepj = diffsj/(double)n_sample;
188 
189  // Choose starting point
190  double is = PExt[1].ifloat;
191  double js = PExt[1].jfloat;
192 
193  // Delete old list
194  list.clear();
195 
196  // sample positions at i*me->getSampleStep() interval along the
197  // line_p, starting at PSiteExt[0]
198 
199  vpImagePoint ip;
200  for(int i=0; i<=vpMath::round(n_sample); i++)
201  {
202  // If point is in the image, add to the sample list
203  if(!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols))
204  {
205  vpMeSite pix ; //= list.value();
206  pix.init((int)is, (int)js, delta, 0, sign) ;
208 
209  if(vpDEBUG_ENABLE(3))
210  {
211  ip.set_i( is );
212  ip.set_j( js );
214  }
215 
216  list.push_back(pix);
217  }
218  is += stepi;
219  js += stepj;
220 
221  }
222 
223  vpCDEBUG(1) << "end vpMeLine::sample() : ";
224  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
225 }
226 
227 
240 void
242 {
243  vpMeLine::display(I,PExt[0],PExt[1],list,a,b,c,col);
244 }
245 
246 
254 void
256 {
257  vpImagePoint ip1, ip2;
258 
259  std::cout << "Click on the line first point..." <<std::endl ;
260  while (vpDisplay::getClick(I, ip1)!=true) ;
262  vpDisplay::flush(I);
263  std::cout << "Click on the line second point..." <<std::endl ;
264  while (vpDisplay::getClick(I, ip2)!=true) ;
266  vpDisplay::flush(I);
267 
268  try
269  {
270  initTracking(I, ip1, ip2) ;
271  }
272  catch(...)
273  {
274  vpERROR_TRACE("Error caught") ;
275  throw ;
276  }
277 
278 }
279 
280 
287 void
289 {
290  vpMatrix A(numberOfSignal(),2) ;
291  vpColVector x(2), x_1(2) ;
292  x_1 = 0;
293 
294  unsigned int i ;
295 
296  vpRobust r(numberOfSignal()) ;
297  r.setThreshold(2);
298  r.setIteration(0) ;
300  D.eye() ;
301  vpMatrix DA, DAmemory ;
302  vpColVector DAx ;
305  w =1 ;
306  vpMeSite p_me ;
307  unsigned int iter =0 ;
308  unsigned int nos_1 = 0 ;
309  double distance = 100;
310 
311  if (list.size() <= 2 || numberOfSignal() <= 2)
312  {
313  //vpERROR_TRACE("Not enough point") ;
314  vpCDEBUG(1) << "Not enough point";
316  "not enough point")) ;
317  }
318 
319  if ((fabs(b) >=0.9)) // Construction du systeme Ax=B
320  // a i + j + c = 0
321  // A = (i 1) B = (-j)
322  {
323  nos_1 = numberOfSignal() ;
324  unsigned int k =0 ;
325  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
326  p_me = *it;
327  if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
328  {
329  A[k][0] = p_me.ifloat ;
330  A[k][1] = 1 ;
331  B[k] = -p_me.jfloat ;
332  k++ ;
333  }
334  }
335 
336  while (iter < 4 && distance > 0.05)
337  {
338  DA = D*A ;
339  x = DA.pseudoInverse(1e-26) *D*B ;
340 
341  vpColVector residu(nos_1);
342  residu = B - A*x;
343  r.setIteration(iter) ;
344  r.MEstimator(vpRobust::TUKEY,residu,w) ;
345 
346  k = 0;
347  for (i=0 ; i < nos_1 ; i++)
348  {
349  D[k][k] =w[k] ;
350  k++;
351  }
352  iter++ ;
353  distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
354  x_1 = x;
355  }
356 
357  k =0 ;
358  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
359  p_me = *it;
360  if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
361  {
362  if (w[k] < 0.2)
363  {
365 
366  *it = p_me;
367  }
368  k++ ;
369  }
370  }
371 
372  // mise a jour de l'equation de la droite
373  a = x[0] ;
374  b = 1 ;
375  c = x[1] ;
376 
377  double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
378  a /= s ;
379  b /= s ;
380  c /= s ;
381  }
382 
383 
384  else // Construction du systeme Ax=B
385  // i + bj + c = 0
386  // A = (j 1) B = (-i)
387  {
388  nos_1 = numberOfSignal() ;
389  unsigned int k =0 ;
390  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
391  p_me = *it;
392  if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
393  {
394  A[k][0] = p_me.jfloat ;
395  A[k][1] = 1 ;
396  B[k] = -p_me.ifloat ;
397  k++ ;
398  }
399  }
400 
401  while (iter < 4 && distance > 0.05)
402  {
403  DA = D*A ;
404  x = DA.pseudoInverse(1e-26) *D*B ;
405 
406  vpColVector residu(nos_1);
407  residu = B - A*x;
408  r.setIteration(iter) ;
409  r.MEstimator(vpRobust::TUKEY,residu,w) ;
410 
411  k = 0;
412  for (i=0 ; i < nos_1 ; i++)
413  {
414  D[k][k] =w[k] ;
415  k++;
416  }
417  iter++ ;
418  distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
419  x_1 = x;
420  }
421 
422  k =0 ;
423  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
424  p_me = *it;
425  if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
426  {
427  if (w[k] < 0.2)
428  {
430 
431  *it = p_me;
432  }
433  k++ ;
434  }
435  }
436  a = 1 ;
437  b = x[0] ;
438  c = x[1] ;
439 
440  double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
441  a /= s ;
442  b /= s ;
443  c /= s ;
444  }
445 
446  // mise a jour du delta
447  delta = atan2(a,b) ;
448 
449  normalizeAngle(delta) ;
450 }
451 
452 
453 
463 void
465  const vpImagePoint &ip1,
466  const vpImagePoint &ip2)
467 {
468  vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;
469 
470  int i1s, j1s, i2s, j2s;
471 
472  i1s = vpMath::round( ip1.get_i() );
473  i2s = vpMath::round( ip2.get_i() );
474  j1s = vpMath::round( ip1.get_j() );
475  j2s = vpMath::round( ip2.get_j() );
476 
477  try{
478 
479  // 1. On fait ce qui concerne les droites (peut etre vide)
480  {
481  // Points extremites
482  PExt[0].ifloat = (float)ip1.get_i() ;
483  PExt[0].jfloat = (float)ip1.get_j() ;
484  PExt[1].ifloat = (float)ip2.get_i() ;
485  PExt[1].jfloat = (float)ip2.get_j() ;
486 
487  double angle_ = atan2((double)(i1s-i2s),(double)(j1s-j2s)) ;
488  a = cos(angle_) ;
489  b = sin(angle_) ;
490 
491  // Real values of a, b can have an other sign. So to get the good values
492  // of a and b in order to initialise then c, we call track(I) just below
493 
494  computeDelta(delta,i1s,j1s,i2s,j2s) ;
495  delta_1 = delta;
496 
497  // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a), delta);
498 
499  sample(I) ;
500 
501  }
502  // 2. On appelle ce qui n'est pas specifique
503  {
505  }
506  // Call track(I) to give the good sign to a and b and to initialise c which can be used for the display
507  track(I);
508  }
509  catch(...)
510  {
511  vpERROR_TRACE("Error caught") ;
512  throw ;
513  }
514  vpCDEBUG(1) <<" end vpMeLine::initTracking()"<<std::endl ;
515 }
516 
517 
521 void
523 {
524  // Loop through list of sites to track
525  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
526  vpMeSite s = *it;//current reference pixel
527 
529  it = list.erase(it);
530  else
531  ++it;
532  }
533 }
534 
535 
539 void
541 {
542  double imin = +1e6 ;
543  double jmin = +1e6;
544  double imax = -1 ;
545  double jmax = -1 ;
546 
547 
548  // Loop through list of sites to track
549  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
550  vpMeSite s = *it;//current reference pixel
551  if (s.ifloat < imin)
552  {
553  imin = s.ifloat ;
554  jmin = s.jfloat ;
555  }
556 
557  if (s.ifloat > imax)
558  {
559  imax = s.ifloat ;
560  jmax = s.jfloat ;
561  }
562  }
563 
564  PExt[0].ifloat = imin ;
565  PExt[0].jfloat = jmin ;
566  PExt[1].ifloat = imax ;
567  PExt[1].jfloat = jmax ;
568 
569  if (fabs(imin-imax) < 25)
570  {
571  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
572  vpMeSite s = *it;//current reference pixel
573  if (s.jfloat < jmin)
574  {
575  imin = s.ifloat ;
576  jmin = s.jfloat ;
577  }
578 
579  if (s.jfloat > jmax)
580  {
581  imax = s.ifloat ;
582  jmax = s.jfloat ;
583  }
584  }
585  PExt[0].ifloat = imin ;
586  PExt[0].jfloat = jmin ;
587  PExt[1].ifloat = imax ;
588  PExt[1].jfloat = jmax ;
589  }
590 }
591 
602 void
604 {
605  vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
606 
607  if (!me) {
608  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
610  "Moving edges not initialized")) ;
611  }
612 
613  int rows = (int)I.getHeight() ;
614  int cols = (int)I.getWidth() ;
615  double n_sample;
616 
617  //if (me->getSampleStep()==0)
618  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
619  {
620 
621  vpERROR_TRACE("function called with sample step = 0") ;
623  "sample step = 0")) ;
624  }
625 
626  // i, j portions of the line_p
627  double diffsi = PExt[0].ifloat-PExt[1].ifloat;
628  double diffsj = PExt[0].jfloat-PExt[1].jfloat;
629 
630  double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
631 
632  double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
633  double dj = diffsj/sqrt(s) ;
634 
635  double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
636 
637  // number of samples along line_p
638  n_sample = length_p/(double)me->getSampleStep();
639  double sample_step = (double)me->getSampleStep();
640 
641  vpMeSite P ;
642  P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
644 
645  unsigned int memory_range = me->getRange() ;
646  me->setRange(1);
647 
648  vpImagePoint ip;
649 
650  for (int i=0 ; i < 3 ; i++)
651  {
652  P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
653  P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
654 
655  if(!outOfImage(P.i, P.j, 5, rows, cols))
656  {
657  P.track(I,me,false) ;
658 
660  {
661  list.push_back(P);
662  if (vpDEBUG_ENABLE(3)) {
663  ip.set_i( P.i );
664  ip.set_j( P.j );
665 
667  }
668  }
669  else {
670  if (vpDEBUG_ENABLE(3)) {
671  ip.set_i( P.i );
672  ip.set_j( P.j );
674  }
675  }
676  }
677  }
678 
679  P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
681  for (int i=0 ; i < 3 ; i++)
682  {
683  P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
684  P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
685 
686  if(!outOfImage(P.i, P.j, 5, rows, cols))
687  {
688  P.track(I,me,false) ;
689 
691  {
692  list.push_back(P);
693  if (vpDEBUG_ENABLE(3)) {
694  ip.set_i( P.i );
695  ip.set_j( P.j );
697  }
698  }
699  else {
700  if (vpDEBUG_ENABLE(3)) {
701  ip.set_i( P.i );
702  ip.set_j( P.j );
704  }
705  }
706  }
707  }
708 
709  me->setRange(memory_range);
710 
711  vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
712  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
713 }
714 
715 
727 void
729 {
730  double i1,j1,i2,j2 ;
731 
732  if (!me) {
733  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
735  "Moving edges not initialized")) ;
736  }
737 
738  project(a,b,c,PExt[0].ifloat,PExt[0].jfloat,i1,j1) ;
739  project(a,b,c,PExt[1].ifloat,PExt[1].jfloat,i2,j2) ;
740 
741  // Points extremites
742  PExt[0].ifloat = i1 ;
743  PExt[0].jfloat = j1 ;
744  PExt[1].ifloat = i2 ;
745  PExt[1].jfloat = j2 ;
746 
747  double d = sqrt(vpMath::sqr(i1-i2)+vpMath::sqr(j1-j2)) ;
748 
749  unsigned int n = numberOfSignal() ;
750  double expecteddensity = d / (double)me->getSampleStep();
751 
752  if ((double)n<0.9*expecteddensity)
753  {
754  double delta_new = delta;
755  delta = delta_1;
756  sample(I) ;
757  delta = delta_new;
758  // 2. On appelle ce qui n'est pas specifique
759  {
761  }
762  }
763 }
764 
769 void
771 {
772  vpMeSite p_me ;
773 
774  double angle_ = delta + M_PI/2;
775  double diff = 0;
776 
777  while (angle_<0) angle_ += M_PI;
778  while (angle_>M_PI) angle_ -= M_PI;
779 
780  angle_ = vpMath::round(angle_ * 180 / M_PI) ;
781 
782  //if(fabs(angle_) == 180 )
783  if(std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon())
784  {
785  angle_= 0 ;
786  }
787 
788  //std::cout << "angle theta : " << theta << std::endl ;
789  diff = fabs(angle_ - angle_1);
790  if (diff > 90)
791  sign *= -1;
792 
793  angle_1 = angle_;
794 
795  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
796  p_me = *it;
797  p_me.alpha = delta ;
798  p_me.mask_sign = sign;
799  *it = p_me;
800  }
801  delta_1 = delta;
802 }
803 
804 
811 void
813 {
814  vpCDEBUG(1) <<"begin vpMeLine::track()"<<std::endl ;
815 
816  // 1. On fait ce qui concerne les droites (peut etre vide)
817  {
818  }
819  // 2. On appelle ce qui n'est pas specifique
820  {
821  vpMeTracker::track(I) ;
822  }
823 
824  // 3. On revient aux droites
825  {
826  // supression des points rejetes par les ME
827  suppressPoints() ;
828  setExtremities() ;
829 
830 
831  // Estimation des parametres de la droite aux moindres carre
832  try
833  {
834  leastSquare() ;
835  }
836  catch(...)
837  {
838  vpERROR_TRACE("Error caught") ;
839  throw ;
840  }
841 
842 
843  // recherche de point aux extremite de la droites
844  // dans le cas d'un glissement
845  seekExtremities(I) ;
846 
847  setExtremities() ;
848  try
849  {
850  leastSquare() ;
851  }
852  catch(...)
853  {
854  vpERROR_TRACE("Error caught") ;
855  throw ;
856  }
857 
858  // suppression des points rejetes par la regression robuste
859  suppressPoints() ;
860  setExtremities() ;
861 
862  //reechantillonage si necessaire
863  reSample(I) ;
864 
865  // remet a jour l'angle delta pour chaque point de la liste
866 
867  updateDelta() ;
868 
869  // Remise a jour de delta dans la liste de site me
870  if (vpDEBUG_ENABLE(2))
871  {
872  display(I,vpColor::red) ;
874  vpDisplay::flush(I) ;
875  }
876 
877 
878  }
879 
880  computeRhoTheta(I) ;
881 
882  vpCDEBUG(1) <<"end vpMeLine::track()"<<std::endl ;
883 }
884 
885 void vpMeLine::update_indices(double theta,int i,int j,int incr,int& i1,int& i2,int& j1,int& j2){
886  i1 = (int)(i + cos(theta) *incr) ;
887  j1 = (int)(j + sin(theta) *incr) ;
888 
889  i2 = (int)(i - cos(theta) *incr) ;
890  j2 = (int)(j - sin(theta) *incr) ;
891 }
892 
899 void
901 {
902  //rho = -c ;
903  //theta = atan2(a,b) ;
904  rho = fabs(c);
905  theta = atan2(b,a) ;
906 
907  while (theta >= M_PI) theta -=M_PI ;
908  while (theta < 0) theta +=M_PI ;
909 
911 
912  /* while(theta < -M_PI) theta += 2*M_PI ;
913  while(theta >= M_PI) theta -= 2*M_PI ;
914 
915  // If theta is between -90 and -180 get the equivalent
916  // between 0 and 90
917  if(theta <-M_PI/2)
918  {
919  theta += M_PI ;
920  rho *= -1 ;
921  }
922  // If theta is between 90 and 180 get the equivalent
923  // between 0 and -90
924  if(theta >M_PI/2)
925  {
926  theta -= M_PI ;
927  rho *= -1 ;
928  }
929  */
930  // convention pour choisir le signe de rho
931  int i,j ;
932  i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat )/2) ;
933  j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat )/2) ;
934 
935  int end = false ;
936  int incr = 10 ;
937 
938  int i1=0,i2=0,j1=0,j2=0 ;
939  unsigned char v1=0,v2=0 ;
940 
941  int width_ = (int)I.getWidth();
942  int height_ = (int)I.getHeight();
943  update_indices(theta,i,j,incr,i1,i2,j1,j2);
944 
945  if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
946  j1<0 || j1>=width_ || j2<0 || j2>=width_){
947  double rho_lim1 = fabs((double)i/cos(theta));
948  double rho_lim2 = fabs((double)j/sin(theta));
949 
950  double co_rho_lim1 = fabs(((double)(height_-i))/cos(theta));
951  double co_rho_lim2 = fabs(((double)(width_-j))/sin(theta));
952 
953  double rho_lim = std::min(rho_lim1,rho_lim2);
954  double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
955  incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
956  if(incr<INCR_MIN){
957  vpERROR_TRACE("increment is too small") ;
959  "increment is too small")) ;
960  }
961  update_indices(theta,i,j,incr,i1,i2,j1,j2);
962  }
963 
964  while (!end)
965  {
966  end = true;
967  unsigned int i1_ = static_cast<unsigned int>(i1);
968  unsigned int j1_ = static_cast<unsigned int>(j1);
969  unsigned int i2_ = static_cast<unsigned int>(i2);
970  unsigned int j2_ = static_cast<unsigned int>(j2);
971  v1=I[i1_][j1_];
972  v2=I[i2_][j2_];
973  if (abs(v1-v2) < 1)
974  {
975  incr-- ;
976  end = false ;
977  if (incr==1)
978  {
980  "In vpMeLine cannot determine rho sign, since there is no gray level difference between both sides of the line"));
981  }
982  }
983  update_indices(theta,i,j,incr,i1,i2,j1,j2);
984  }
985 
986  if (theta >=0 && theta <= M_PI/2)
987  {
988  if (v2 < v1)
989  {
990  theta += M_PI;
991  rho *= -1;
992  }
993  }
994 
995  else
996  {
997  double jinter;
998  jinter = -c/b;
999  if (v2 < v1)
1000  {
1001  theta += M_PI;
1002  if (jinter > 0)
1003  {
1004  rho *= -1;
1005  }
1006  }
1007 
1008  else
1009  {
1010  if (jinter < 0)
1011  {
1012  rho *= -1;
1013  }
1014  }
1015  }
1016 
1017  if (vpDEBUG_ENABLE(2))
1018  {
1019  vpImagePoint ip, ip1, ip2;
1020 
1021  ip.set_i( i );
1022  ip.set_j( j );
1023  ip1.set_i( i1 );
1024  ip1.set_j( j1 );
1025  ip2.set_i( i2 );
1026  ip2.set_j( j2 );
1027 
1029  vpDisplay::displayArrow(I, ip, ip2, vpColor::red) ;
1030  }
1031  }
1032 
1033 }
1034 
1045 double
1047 {
1048  return rho ;
1049 }
1050 
1054 double
1056 {
1057  return theta ;
1058 }
1059 
1067 void
1069 {
1070  /*Return the coordinates of the extremities of the line*/
1071  ip1.set_i( PExt[0].ifloat );
1072  ip1.set_j( PExt[0].jfloat );
1073  ip2.set_i( PExt[1].ifloat );
1074  ip2.set_j( PExt[1].jfloat );
1075 }
1076 
1077 
1090 bool
1091 vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2,
1092  vpImagePoint &ip)
1093 {
1094  double a1 = line1.a;
1095  double b1 = line1.b;
1096  double c1 = line1.c;
1097  double a2 = line2.a;
1098  double b2 = line2.b;
1099  double c2 = line2.c;
1100 
1101  try{
1102  double i=0, j=0;
1103  double denom = 0;
1104 
1105  if (a1 > 0.1)
1106  {
1107  denom = (-(a2/a1) * b1 + b2);
1108 
1109  //if (denom == 0)
1110  if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
1111  {
1112  std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1113  return (false);
1114  }
1115 
1116  //if (denom != 0 )
1117  if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
1118  {
1119  j = ( (a2/a1)*c1 - c2 ) / denom;
1120  i = (-b1*j - c1) / a1;
1121  }
1122  }
1123 
1124  else
1125  {
1126  denom = (-(b2/b1) * a1 + a2);
1127 
1128  //if (denom == 0)
1129  if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
1130  {
1131  std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
1132  return (false);
1133  }
1134 
1135  //if (denom != 0 )
1136  if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
1137  {
1138  i = ( (b2/b1)*c1 - c2 ) / denom;
1139  j = (-a1*i - c1) / b1;
1140  }
1141  }
1142  ip.set_i( i );
1143  ip.set_j( j );
1144 
1145  return (true);
1146  }
1147  catch(...)
1148  {
1149  return (false);
1150  }
1151 }
1152 
1172 void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
1173  const double &A, const double &B, const double &C,
1174  const vpColor &color, unsigned int thickness)
1175 {
1176  vpImagePoint ip1, ip2;
1177 
1178  if (fabs(A) < fabs(B)) {
1179  double i1, j1, i2, j2;
1180  i1 = 0;
1181  j1 = (-A*i1 -C) / B;
1182  i2 = I.getHeight() - 1.0;
1183  j2 = (-A*i2 -C) / B;
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  //vpDisplay::flush(I);
1191 
1192  }
1193  else {
1194  double i1, j1, i2, j2;
1195  j1 = 0;
1196  i1 = -(B * j1 + C) / A;
1197  j2 = I.getWidth() - 1.0;
1198  i2 = -(B * j2 + C) / A;
1199 
1200  ip1.set_i( i1 );
1201  ip1.set_j( j1 );
1202  ip2.set_i( i2 );
1203  ip2.set_j( j2 );
1204  vpDisplay::displayLine(I, ip1, ip2, color);
1205  //vpDisplay::flush(I);
1206  }
1207 
1208  ip1.set_i( PExt1.ifloat );
1209  ip1.set_j( PExt1.jfloat );
1210  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1211 
1212  ip1.set_i( PExt2.ifloat );
1213  ip1.set_j( PExt2.jfloat );
1214  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1215 }
1216 
1236 void vpMeLine::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
1237  const double &A, const double &B, const double &C,
1238  const vpColor &color, unsigned int thickness)
1239 {
1240  vpImagePoint ip1, ip2;
1241 
1242  if (fabs(A) < fabs(B)) {
1243  double i1, j1, i2, j2;
1244  i1 = 0;
1245  j1 = (-A*i1 -C) / B;
1246  i2 = I.getHeight() - 1.0;
1247  j2 = (-A*i2 -C) / B;
1248 
1249  ip1.set_i( i1 );
1250  ip1.set_j( j1 );
1251  ip2.set_i( i2 );
1252  ip2.set_j( j2 );
1253  vpDisplay::displayLine(I, ip1, ip2, color);
1254  //vpDisplay::flush(I);
1255 
1256  }
1257  else {
1258  double i1, j1, i2, j2;
1259  j1 = 0;
1260  i1 = -(B * j1 + C) / A;
1261  j2 = I.getWidth() - 1.0;
1262  i2 = -(B * j2 + C) / A;
1263 
1264  ip1.set_i( i1 );
1265  ip1.set_j( j1 );
1266  ip2.set_i( i2 );
1267  ip2.set_j( j2 );
1268  vpDisplay::displayLine(I, ip1, ip2, color);
1269  //vpDisplay::flush(I);
1270  }
1271 
1272  ip1.set_i( PExt1.ifloat );
1273  ip1.set_j( PExt1.jfloat );
1274  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1275 
1276  ip1.set_i( PExt2.ifloat );
1277  ip1.set_j( PExt2.jfloat );
1278  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1279 }
1280 
1302 void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
1303  const std::list<vpMeSite> &site_list,
1304  const double &A, const double &B, const double &C,
1305  const vpColor &color, unsigned int thickness)
1306 {
1307  vpImagePoint ip;
1308 
1309  for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
1310  vpMeSite pix = *it;
1311  ip.set_i( pix.ifloat );
1312  ip.set_j( pix.jfloat );
1313 
1314  if (pix.getState() == vpMeSite::M_ESTIMATOR)
1315  vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
1316  else
1317  vpDisplay::displayCross(I, ip, 5, color,thickness);
1318 
1319  //vpDisplay::flush(I);
1320  }
1321 
1322  vpImagePoint ip1, ip2;
1323 
1324  if (fabs(A) < fabs(B)) {
1325  double i1, j1, i2, j2;
1326  i1 = 0;
1327  j1 = (-A*i1 -C) / B;
1328  i2 = I.getHeight() - 1.0;
1329  j2 = (-A*i2 -C) / B;
1330 
1331  ip1.set_i( i1 );
1332  ip1.set_j( j1 );
1333  ip2.set_i( i2 );
1334  ip2.set_j( j2 );
1335  vpDisplay::displayLine(I, ip1, ip2, color);
1336  //vpDisplay::flush(I);
1337 
1338  }
1339  else {
1340  double i1, j1, i2, j2;
1341  j1 = 0;
1342  i1 = -(B * j1 + C) / A;
1343  j2 = I.getWidth() - 1.0;
1344  i2 = -(B * j2 + C) / A;
1345 
1346  ip1.set_i( i1 );
1347  ip1.set_j( j1 );
1348  ip2.set_i( i2 );
1349  ip2.set_j( j2 );
1350  vpDisplay::displayLine(I, ip1, ip2, color);
1351  //vpDisplay::flush(I);
1352  }
1353 
1354  ip1.set_i( PExt1.ifloat );
1355  ip1.set_j( PExt1.jfloat );
1356  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1357 
1358  ip1.set_i( PExt2.ifloat );
1359  ip1.set_j( PExt2.jfloat );
1360  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1361 }
1362 
1384 void vpMeLine::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
1385  const std::list<vpMeSite> &site_list,
1386  const double &A, const double &B, const double &C,
1387  const vpColor &color, unsigned int thickness)
1388 {
1389  vpImagePoint ip;
1390 
1391  for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
1392  vpMeSite pix = *it;
1393  ip.set_i( pix.ifloat );
1394  ip.set_j( pix.jfloat );
1395 
1396  if (pix.getState() == vpMeSite::M_ESTIMATOR)
1397  vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
1398  else
1399  vpDisplay::displayCross(I, ip, 5, color,thickness);
1400 
1401  //vpDisplay::flush(I);
1402  }
1403 
1404  vpImagePoint ip1, ip2;
1405 
1406  if (fabs(A) < fabs(B)) {
1407  double i1, j1, i2, j2;
1408  i1 = 0;
1409  j1 = (-A*i1 -C) / B;
1410  i2 = I.getHeight() - 1.0;
1411  j2 = (-A*i2 -C) / B;
1412 
1413  ip1.set_i( i1 );
1414  ip1.set_j( j1 );
1415  ip2.set_i( i2 );
1416  ip2.set_j( j2 );
1417  vpDisplay::displayLine(I, ip1, ip2, color);
1418  //vpDisplay::flush(I);
1419 
1420  }
1421  else {
1422  double i1, j1, i2, j2;
1423  j1 = 0;
1424  i1 = -(B * j1 + C) / A;
1425  j2 = I.getWidth() - 1.0;
1426  i2 = -(B * j2 + C) / A;
1427 
1428  ip1.set_i( i1 );
1429  ip1.set_j( j1 );
1430  ip2.set_i( i2 );
1431  ip2.set_j( j2 );
1432  vpDisplay::displayLine(I, ip1, ip2, color);
1433  //vpDisplay::flush(I);
1434  }
1435 
1436  ip1.set_i( PExt1.ifloat );
1437  ip1.set_j( PExt1.jfloat );
1438  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1439 
1440  ip1.set_i( PExt2.ifloat );
1441  ip1.set_j( PExt2.jfloat );
1442  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
1443 }
1444 
void reSample(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:728
unsigned int getRange() const
Definition: vpMe.h:167
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
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:69
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:182
double get_i() const
Definition: vpImagePoint.h:199
unsigned int getWidth() const
Definition: vpImage.h:226
double jfloat
Definition: vpMeSite.h:96
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:526
unsigned int numberOfSignal()
double getTheta() const
Definition: vpMeLine.cpp:1055
double angle
Definition: vpMeLine.h:162
void setExtremities()
Definition: vpMeLine.cpp:540
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:72
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
double delta
Definition: vpMeLine.h:161
int outOfImage(int i, int j, int half, int rows, int cols)
bool _useIntensityForRho
Flag to specify wether the intensity of the image at the middle point is used to compute the sign of ...
Definition: vpMeLine.h:166
double alpha
Definition: vpMeSite.h:100
int i
Definition: vpMeSite.h:94
error that can be emited by ViSP classes.
Definition: vpException.h:73
void track(const vpImage< unsigned char > &Im)
Definition: vpMeLine.cpp:812
int mask_sign
Definition: vpMeSite.h:98
static const vpColor green
Definition: vpColor.h:166
static void flush(const vpImage< unsigned char > &I)
static int round(const double x)
Definition: vpMath.h:249
double get_j() const
Definition: vpImagePoint.h:210
void leastSquare()
Definition: vpMeLine.cpp:288
static const vpColor red
Definition: vpColor.h:163
vpMeSiteState getState() const
Definition: vpMeSite.h:198
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeLine.cpp:241
double ifloat
Definition: vpMeSite.h:96
double rho
Definition: vpMeLine.h:160
virtual ~vpMeLine()
Definition: vpMeLine.cpp:141
void set_i(const double ii)
Definition: vpImagePoint.h:163
void computeRhoTheta(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:900
std::list< vpMeSite > list
Definition: vpMeTracker.h:73
Error that can be emited by the vpTracker class and its derivates.
void updateDelta()
Definition: vpMeLine.cpp:770
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:1068
double theta
Definition: vpMeLine.h:160
static double sqr(double x)
Definition: vpMath.h:110
void sample(const vpImage< unsigned char > &image)
Definition: vpMeLine.cpp:157
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:152
void suppressPoints()
Definition: vpMeLine.cpp:522
double delta_1
Definition: vpMeLine.h:161
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:148
#define vpCDEBUG(level)
Definition: vpDebug.h:502
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:64
void initTracking(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:255
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:185
vpMeSite PExt[2]
Definition: vpMeLine.h:158
void set_j(const double jj)
Definition: vpImagePoint.h:174
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:455
int j
Definition: vpMeSite.h:94
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:374
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:75
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:60
int sign
Definition: vpMeLine.h:163
void initTracking(const vpImage< unsigned char > &I)
double angle_1
Definition: vpMeLine.h:162
static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip)
Definition: vpMeLine.cpp:1091
unsigned int getHeight() const
Definition: vpImage.h:175
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1741
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:256
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:129
double getSampleStep() const
Definition: vpMe.h:270
double getRho() const
Definition: vpMeLine.cpp:1046
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:80
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:123
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeLine.cpp:603
void eye()
Definition: vpMatrix.cpp:194
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
static const vpColor blue
Definition: vpColor.h:169