Visual Servoing Platform  version 3.0.0
vpMeNurbs.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
38 
44 #include <visp3/me/vpMeTracker.h>
45 #include <visp3/me/vpMe.h>
46 #include <visp3/me/vpMeSite.h>
47 #include <visp3/me/vpMeNurbs.h>
48 #include <visp3/core/vpRobust.h>
49 #include <visp3/core/vpTrackingException.h>
50 #include <visp3/core/vpImagePoint.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpRect.h>
53 #include <visp3/core/vpImageTools.h>
54 #include <visp3/core/vpImageConvert.h>
55 #include <visp3/core/vpImageFilter.h>
56 #include <stdlib.h>
57 #include <cmath> // std::fabs
58 #include <limits> // numeric_limits
59 #ifdef VISP_HAVE_OPENCV
60 # if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
61 //# include <opencv2/imgproc/imgproc.hpp>
62 # include <opencv2/imgproc/imgproc_c.h>
63 # else
64 # include <cv.h>
65 # endif
66 #endif
67 
68 double computeDelta(double deltai, double deltaj);
69 void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP,
70  vpMe* me, double &angle, double &convlt);
71 vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP);
72 bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
73 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
74 vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list);
75 #endif
76 
77 //Compute the angle delta = arctan(deltai/deltaj)
78 //and normalize it between 0 and pi
79 double
80 computeDelta(double deltai, double deltaj)
81 {
82  double delta;
83  delta = atan2(deltai,deltaj) ;
84  delta -= M_PI/2.0 ;
85  while (delta > M_PI) { delta -= M_PI ; }
86  while (delta < 0) { delta += M_PI ; }
87  return(delta);
88 }
89 
90 //Check if the image point is in the image and not to close to
91 //its edge to enable the computation of a convolution whith a mask.
92 static
93 bool outOfImage( vpImagePoint iP , int half , int rows , int cols)
94 {
95  return((iP.get_i() < half + 1) || ( iP.get_i() > (rows - half - 3) )
96  ||(iP.get_j() < half + 1) || (iP.get_j() > (cols - half - 3) )) ;
97 }
98 
99 //if iP is a edge point, it computes the angle corresponding to the
100 //highest convolution result. the angle is between 0 an 179.
101 //The result gives the angle in RADIAN + pi/2 (to deal with the moving edeg alpha angle)
102 //and the corresponding convolution result.
103 void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP,
104  vpMe* me, double &angle, double &convlt)
105 {
106  int Iheight = (int)I.getHeight();
107  int Iwidth = (int)I.getWidth();
108  angle = 0.0;
109  convlt = 0.0;
110  for (int i = 0; i < 180; i++)
111  {
112  double conv = 0.0;
113  unsigned int half;
114  int index_mask;
115  half = (me->getMaskSize() - 1) >> 1 ;
116 
117  if(outOfImage( iP , (int)half + me->getStrip() , Iheight, Iwidth))
118  {
119  conv = 0.0 ;
120  }
121  else
122  {
123  if (me->getAngleStep() !=0)
124  index_mask = (int)(i/(double)me->getAngleStep());
125  else
126  throw (vpException(vpException::divideByZeroError,"angle step = 0"));
127 
128  unsigned int ihalf = (unsigned int)(iP.get_i()-half) ;
129  unsigned int jhalf = (unsigned int)(iP.get_j()-half) ;
130  unsigned int ihalfa ;
131  unsigned int a ;
132  unsigned int b ;
133  for( a = 0 ; a < me->getMaskSize() ; a++ )
134  {
135  ihalfa = ihalf+a ;
136  for( b = 0 ; b < me->getMaskSize() ; b++ )
137  {
138  conv += me->getMask()[index_mask][a][b] *
139  I(ihalfa,jhalf+b) ;
140  }
141  }
142  }
143  conv = fabs(conv);
144  if (conv > convlt)
145  {
146  convlt = conv;
147  angle = vpMath::rad(i);
148  angle += M_PI/2;
149  while (angle > M_PI) { angle -= M_PI ; }
150  while (angle < 0) { angle += M_PI ; }
151  }
152  }
153 }
154 
155 
156 //Find the point belonging to the edge of the sub image which respects the following hypotheses:
157 //- the value of the pixel is upper than zero.
158 //- the distantce between the point and iP is less than 4 pixels.
159 //The function returns the nearest point of iP which respect the hypotheses
160 //If no point is found the returned point is (-1,-1)
161 vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP)
162 {
163  double dist = 1e6;
164  double dist_1 = 1e6;
165  vpImagePoint index(-1,-1);
166  for (unsigned int i = 0; i <= Isub.getHeight(); i++)
167  {
168  for (unsigned int j = 0; j <= Isub.getWidth(); j++)
169  {
170  if(i == 0 || i == Isub.getHeight()-1 || j == 0 || j == Isub.getWidth()-1)
171  {
172  if (Isub(i,j) > 0)
173  {
175  if (dist <= 16 && dist < dist_1)
176  {
177  dist_1 = dist;
178  index.set_ij(i,j);
179  }
180  }
181  }
182  }
183  }
184  return index;
185 }
186 
187 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
188 //Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
189 //from the center of the sub image (ie the point (15,15).
190 vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
191 {
192  ip_edges_list->front();
193  double dist;
194  while (!ip_edges_list->outside())
195  {
196  vpImagePoint iP = ip_edges_list->value();
197  dist = vpImagePoint::sqrDistance(iP,vpImagePoint(15,15));
198  if (dist <= 16)
199  {
200  return true;
201  }
202  ip_edges_list->next();
203  }
204  return false;
205 }
206 #endif
207 
208 //Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
209 //from the center of the sub image (ie the point (15,15).
210 bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
211 {
212  double dist;
213  for(std::list<vpImagePoint>::const_iterator it=ip_edges_list->begin(); it!=ip_edges_list->end(); ++it){
214  vpImagePoint iP = *it;
215  dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15,15));
216  if (dist <= 16)
217  {
218  return true;
219  }
220  }
221  return false;
222 }
223 
224 /***************************************/
225 
230  : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false),
231  cannyTh1(100.), cannyTh2(200.)
232 {
233 }
234 
239  : vpMeTracker(menurbs),
240  nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false),
241  cannyTh1(100.), cannyTh2(200.)
242 {
243  nurbs = menurbs.nurbs;
244  dist = menurbs.dist;
245  nbControlPoints = menurbs.nbControlPoints;
246  beginPtFound = menurbs.beginPtFound;
247  endPtFound = menurbs.endPtFound;
248  enableCannyDetection = menurbs.enableCannyDetection;
249  cannyTh1 = menurbs.cannyTh1;
250  cannyTh2 = menurbs.cannyTh2;
251 }
252 
257 {
258 }
259 
260 
267 void
269 {
270  std::list<vpImagePoint> ptList;
271  vpImagePoint pt;
273 
274  while (vpDisplay::getClick(I, pt, b))
275  {
276  if (b == vpMouseButton::button1)
277  {
278  //std::cout<<pt<<std::endl;
279  ptList.push_back(pt);
281  vpDisplay::flush(I);
282  }
283  if (b == vpMouseButton::button3) break;
284  }
285  if (ptList.size() > 3)
286  initTracking(I, ptList);
287  else
288  throw (vpException(vpException::notInitialized,"Not enough points to initialize the Nurbs"));
289 }
290 
298 void
300  const std::list<vpImagePoint> &ptList)
301 {
302  nurbs.globalCurveInterp(ptList);
303 
304  sample(I);
305 
307  track(I);
308 }
309 
316 void
318 {
319  int rows = (int)I.getHeight() ;
320  int cols = (int)I.getWidth() ;
321  double step = 1.0 / (double)me->getPointsToTrack();
322 
323  // Delete old list
324  list.clear();
325 
326  vpImagePoint ip;
327  double u = 0.0;
328  vpImagePoint *pt = NULL;
329  vpImagePoint pt_1(-rows,-cols);
330  while (u <= 1.0)
331  {
332  if (pt!=NULL) delete[] pt;
333  pt = nurbs.computeCurveDersPoint(u, 1);
334  double delta = computeDelta(pt[1].get_i(),pt[1].get_j());
335 
336  // If point is in the image, add to the sample list
337  if(!outOfImage(pt[0], 0, rows, cols) && vpImagePoint::sqrDistance(pt[0],pt_1) >= vpMath::sqr(me->getSampleStep()))
338  {
339  vpMeSite pix ; //= list.value();
340  pix.init(pt[0].get_i(), pt[0].get_j(), delta) ;
342 
343  list.push_back(pix);
344  pt_1 = pt[0];
345  }
346  u = u+step;
347  }
348  if (pt!=NULL) delete[] pt;
349 }
350 
351 
358 void
360 {
361  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
362  vpMeSite s = *it;//current reference pixel
363 
365  {
366  it = list.erase(it);
367  }
368  else
369  ++it;
370  }
371 }
372 
373 
378 void
380 {
381  double u = 0.0;
382  double d = 1e6;
383  double d_1 = 1e6;
384  std::list<vpMeSite>::iterator it=list.begin();
385 
386  vpImagePoint Cu;
387  vpImagePoint* der = NULL;
388  double step = 0.01;
389  while (u < 1 && it!=list.end())
390  {
391  vpMeSite s = *it;
392  vpImagePoint pt(s.i,s.j);
393  while (d <= d_1 && u<1)
394  {
395  Cu = nurbs.computeCurvePoint(u);
396  d_1=d;
397  d = vpImagePoint::distance(pt,Cu);
398  u +=step;
399  }
400 
401  u-=step;
402  if (der != NULL) delete[] der;
403  der = nurbs.computeCurveDersPoint(u, 1);
404  //vpImagePoint toto(der[0].get_i(),der[0].get_j());
405  //vpDisplay::displayCross(I,toto,4,vpColor::red);
406 
407  s.alpha = computeDelta(der[1].get_i(),der[1].get_j());
408  *it = s;
409  ++it;
410  d = 1e6;
411  d_1 = 1.5e6;
412  }
413  if (der != NULL) delete[] der;
414 }
415 
416 
424 void
426 {
427  int rows = (int)I.getHeight() ;
428  int cols = (int)I.getWidth() ;
429 
430  vpImagePoint* begin = NULL;
431  vpImagePoint* end = NULL;
432 
433  begin = nurbs.computeCurveDersPoint(0.0,1);
434  end = nurbs.computeCurveDersPoint(1.0,1);
435 
436  //Check if the two extremities are not to close to eachother.
437  double d = vpImagePoint::distance(begin[0],end[0]);
438  double threshold = 3*me->getSampleStep();
439  double sample_step = me->getSampleStep();
440  vpImagePoint pt;
441  if ( d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/)
442  {
443  vpMeSite P ;
444 
445  //Init vpMeSite
446  P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign) ;
448 
449  //Set the range
450  unsigned int memory_range = me->getRange() ;
451  me->setRange(2);
452 
453  //Point at the beginning of the list
454  bool beginPtAdded = false;
455  vpImagePoint pt_max = begin[0];
456  double angle = atan2(begin[1].get_i(),begin[1].get_j());
457  double co = vpMath::abs(cos(angle));
458  co = co * vpMath::sign(begin[1].get_j());
459  double si = vpMath::abs(sin(angle));
460  si = si * vpMath::sign(begin[1].get_i());
461  for (int i=0 ; i < 3 ; i++)
462  {
463  P.ifloat = P.ifloat - si*sample_step ; P.i = (int)P.ifloat ;
464  P.jfloat = P.jfloat - co*sample_step ; P.j = (int)P.jfloat ;
465  pt.set_ij(P.ifloat,P.jfloat);
466  if (vpImagePoint::distance(end[0],pt) < threshold) break;
467  if(!outOfImage(P.i, P.j, 5, rows, cols))
468  {
469  P.track(I,me,false);
470 
472  {
473  list.push_front(P) ;
474  beginPtAdded = true;
475  pt_max = pt;
476  if (vpDEBUG_ENABLE(3)) {
478  }
479  }
480  else {
481  if (vpDEBUG_ENABLE(3)) {
483  }
484  }
485  }
486  }
487 
488  if (!beginPtAdded) beginPtFound++;
489 
490  P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
492 
493  bool endPtAdded = false;
494  angle = atan2(end[1].get_i(),end[1].get_j());
495  co = vpMath::abs(cos(angle));
496  co = co * vpMath::sign(end[1].get_j());
497  si = vpMath::abs(sin(angle));
498  si = si * vpMath::sign(end[1].get_i());
499  for (int i=0 ; i < 3 ; i++)
500  {
501  P.ifloat = P.ifloat + si*sample_step ; P.i = (int)P.ifloat ;
502  P.jfloat = P.jfloat + co*sample_step ; P.j = (int)P.jfloat ;
503  pt.set_ij(P.ifloat,P.jfloat);
504  if (vpImagePoint::distance(begin[0],pt) < threshold) break;
505  if(!outOfImage(P.i, P.j, 5, rows, cols))
506  {
507  P.track(I,me,false);
508 
510  {
511  list.push_back(P) ;
512  endPtAdded = true;
513  if (vpDEBUG_ENABLE(3)) {
515  }
516  }
517  else {
518  if (vpDEBUG_ENABLE(3)) {
520  }
521  }
522  }
523  }
524  if (!endPtAdded) endPtFound++;
525  me->setRange(memory_range);
526  }
527  else
528  {
529  list.pop_front();
530  }
531  /*if(begin != NULL)*/ delete[] begin;
532  /*if(end != NULL) */ delete[] end;
533 }
534 
535 
547 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
548 void
550 #else
551 void
553 #endif
554 {
555 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
556  vpMeSite pt = list.front();
557  vpImagePoint firstPoint(pt.ifloat,pt.jfloat);
558  pt = list.back();
559  vpImagePoint lastPoint(pt.ifloat,pt.jfloat);
560  if (beginPtFound >=3 && farFromImageEdge(I, firstPoint))
561  {
562  vpImagePoint *begin = NULL;
563  begin = nurbs.computeCurveDersPoint(0.0,1);
564  vpImage<unsigned char> Isub(32,32); //Sub image.
565  vpImagePoint topLeft(begin[0].get_i()-15,begin[0].get_j()-15);
566  vpRect rect(topLeft,32,32);
567 
569 
570  vpImageTools::createSubImage(I,rect,Isub);
571 
572  vpImagePoint lastPtInSubIm(begin[0]);
573  double u = 0.0;
574  double step =0.0001;
575  //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
576  while (inRectangle(lastPtInSubIm,rect) && u < 1)
577  {
578  u += step;
579  lastPtInSubIm = nurbs.computeCurvePoint(u);
580  }
581 
582  u -= step;
583  if( u > 0)
584  lastPtInSubIm = nurbs.computeCurvePoint(u);
585 
586 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
587  vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
588 #else
589  IplImage* Ip = NULL;
590  vpImageConvert::convert(Isub, Ip);
591 
592  IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
593  cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
594 
595  vpImageConvert::convert(dst, Isub);
596 #endif
597 
598  vpImagePoint firstBorder(-1,-1);
599 
600  firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
601 
602  std::list<vpImagePoint> ip_edges_list;
603  if (firstBorder != vpImagePoint(-1, -1))
604  {
605  unsigned int dir;
606  double fi = firstBorder.get_i();
607  double fj = firstBorder.get_j();
608  double w = Isub.getWidth()-1;
609  double h = Isub.getHeight()-1;
610  //if (firstBorder.get_i() == 0) dir = 4;
611  if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
612  //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
613  else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
614  //else if (firstBorder.get_j() == 0) dir = 2;
615  else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
616  //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
617  else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
618  computeFreemanChainElement(Isub, firstBorder , dir);
619  unsigned int firstDir = dir;
620  ip_edges_list.push_back( firstBorder );
621  vpImagePoint border(firstBorder);
622  vpImagePoint dBorder;
623  do
624  {
625  computeFreemanParameters(dir, dBorder);
626  border = border + dBorder;
627  vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
628 
629  ip_edges_list.push_back( border );
630 
631  computeFreemanChainElement(Isub, border , dir);
632  } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
633  }
634 
635  if (findCenterPoint(&ip_edges_list))
636  {
637  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); /*++it*/){
638  vpMeSite s = *it;
639  vpImagePoint iP(s.ifloat,s.jfloat);
640  if (inRectangle(iP,rect))
641  it = list.erase(it) ;
642  else
643  break;
644  }
645 
646  std::list<vpMeSite>::iterator itList=list.begin();
647  double convlt;
648  double delta = 0;
649  int nbr = 0;
650  std::list<vpMeSite> addedPt;
651  for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
652  vpMeSite s = *itList;
653  vpImagePoint iPtemp = *itEdges + topLeft;
654  vpMeSite pix;
655  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
656  dist = vpMeSite::sqrDistance(s,pix);
657  if (dist >= vpMath::sqr(me->getSampleStep())/*25*/)
658  {
659  bool exist = false;
660  for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
661  dist = vpMeSite::sqrDistance(pix, *itAdd);
662  if (dist < vpMath::sqr(me->getSampleStep())/*25*/)
663  exist = true;
664  }
665  if (!exist)
666  {
667  findAngle(I, iPtemp, me, delta, convlt);
668  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
669  pix.setDisplay(selectDisplay);
670  --itList;
671  list.insert(itList, pix);
672  ++itList;
673  addedPt.push_front(pix);
674  nbr++;
675  }
676  }
677  }
678 
679  unsigned int memory_range = me->getRange();
680  me->setRange(3);
681  std::list<vpMeSite>::iterator itList2=list.begin();
682  for (int j = 0; j < nbr; j++)
683  {
684  vpMeSite s = *itList2;
685  s.track(I,me,false);
686  *itList2 = s;
687  ++itList2;
688  }
689  me->setRange(memory_range);
690  }
691 
692  /* if (begin != NULL) */ delete[] begin;
693  beginPtFound = 0;
694  }
695 
696  if(endPtFound >= 3 && farFromImageEdge(I, lastPoint))
697  {
698  vpImagePoint *end = NULL;
699  end = nurbs.computeCurveDersPoint(1.0,1);
700 
701  vpImage<unsigned char> Isub(32,32); //Sub image.
702  vpImagePoint topLeft(end[0].get_i()-15,end[0].get_j()-15);
703  vpRect rect(topLeft,32,32);
704 
706 
707  vpImageTools::createSubImage(I,rect,Isub);
708 
709  vpImagePoint lastPtInSubIm(end[0]);
710  double u = 1.0;
711  double step =0.0001;
712  //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
713  while (inRectangle(lastPtInSubIm,rect) && u > 0)
714  {
715  u -= step;
716  lastPtInSubIm = nurbs.computeCurvePoint(u);
717  }
718 
719  u += step;
720  if( u < 1.0)
721  lastPtInSubIm = nurbs.computeCurvePoint(u);
722 
723 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
724  vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
725 #else
726  IplImage* Ip = NULL;
727  vpImageConvert::convert(Isub, Ip);
728 
729  IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
730  cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
731 
732  vpImageConvert::convert(dst, Isub);
733 #endif
734 
735  vpImagePoint firstBorder(-1,-1);
736 
737  firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
738 
739  std::list<vpImagePoint> ip_edges_list;
740  if (firstBorder != vpImagePoint(-1, -1))
741  {
742  unsigned int dir;
743  double fi = firstBorder.get_i();
744  double fj = firstBorder.get_j();
745  double w = Isub.getWidth()-1;
746  double h = Isub.getHeight()-1;
747  //if (firstBorder.get_i() == 0) dir = 4;
748  if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
749  //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
750  else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
751  //else if (firstBorder.get_j() == 0) dir = 2;
752  else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
753  //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
754  else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
755 
756 
757  computeFreemanChainElement(Isub, firstBorder , dir);
758  unsigned int firstDir = dir;
759  ip_edges_list.push_back( firstBorder );
760  vpImagePoint border(firstBorder);
761  vpImagePoint dBorder;
762  do
763  {
764  computeFreemanParameters(dir, dBorder);
765  border = border + dBorder;
766  vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
767 
768  ip_edges_list.push_back( border );
769 
770  computeFreemanChainElement(Isub, border , dir);
771  } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
772  }
773 
774  if (findCenterPoint(&ip_edges_list))
775  {
776 // list.end();
777  vpMeSite s;
778  while(true)//{//!list.outside())
779  {
780  s = list.back();//list.value() ;
781  vpImagePoint iP(s.ifloat,s.jfloat);
782  if (inRectangle(iP,rect))
783  {
784  list.erase(list.end()) ;
785 // list.end();
786  }
787  else
788  break;
789  }
790 
791  std::list<vpMeSite>::iterator itList = list.end();
792  --itList; // Move on the last element
793  double convlt;
794  double delta;
795  int nbr = 0;
796  std::list<vpMeSite> addedPt;
797  for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
798  s = *itList;
799  vpImagePoint iPtemp = *itEdges + topLeft;
800  vpMeSite pix;
801  pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
802  dist = vpMeSite::sqrDistance(s,pix);
803  if (dist >= vpMath::sqr(me->getSampleStep()))
804  {
805  bool exist = false;
806  for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
807  dist = vpMeSite::sqrDistance(pix, *itAdd);
808  if (dist < vpMath::sqr(me->getSampleStep()))
809  exist = true;
810  }
811  if (!exist)
812  {
813  findAngle(I, iPtemp, me, delta, convlt);
814  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
815  pix.setDisplay(selectDisplay);
816  list.push_back(pix);
817  addedPt.push_back(pix);
818  nbr++;
819  }
820  }
821  }
822 
823  unsigned int memory_range = me->getRange();
824  me->setRange(3);
825  std::list<vpMeSite>::iterator itList2 = list.end();
826  --itList2; // Move to the last element
827  for (int j = 0; j < nbr; j++)
828  {
829  vpMeSite me_s = *itList2;
830  me_s.track(I,me,false);
831  *itList2 = me_s;
832  --itList2;
833  }
834  me->setRange(memory_range);
835  }
836 
837  /* if (end != NULL) */ delete[] end;
838  endPtFound = 0;
839  }
840 #else
841  vpTRACE("To use the canny detection, OpenCV has to be installed.");
842 #endif
843 }
844 
845 
856 void
858 {
859  unsigned int n = numberOfSignal();
860  double nbPt = floor(dist / me->getSampleStep());
861 
862  if ((double)n < 0.7*nbPt)
863  {
864  sample(I);
866  }
867 }
868 
869 
876 void
878 {
879  int rows = (int)I.getHeight() ;
880  int cols = (int)I.getWidth() ;
881  vpImagePoint* iP = NULL;
882 
883  int n = (int)numberOfSignal();
884 
885 // list.front();
886  std::list<vpMeSite>::iterator it=list.begin();
887  std::list<vpMeSite>::iterator itNext=list.begin();
888  ++itNext;
889 
890  unsigned int range_tmp = me->getRange();
891  me->setRange(2);
892 
893  while(itNext!=list.end() && n <= me->getPointsToTrack())
894  {
895  vpMeSite s = *it;//current reference pixel
896  vpMeSite s_next = *itNext;//current reference pixel
897 
898  double d = vpMeSite::sqrDistance(s,s_next);
899  if(d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600)
900  {
901  vpImagePoint iP0(s.ifloat,s.jfloat);
902  vpImagePoint iPend(s_next.ifloat,s_next.jfloat);
903  vpImagePoint iP_1(s.ifloat,s.jfloat);
904 
905  double u = 0.0;
906  double ubegin = 0.0;
907  double uend = 0.0;
908  double dmin1_1 = 1e6;
909  double dmin2_1 = 1e6;
910  while(u < 1)
911  {
912  u+=0.01;
913  double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iP0);
914  double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iPend);
915 
916  if (dmin1 < dmin1_1)
917  {
918  dmin1_1 = dmin1;
919  ubegin = u;
920  }
921 
922  if (dmin2 < dmin2_1)
923  {
924  dmin2_1 = dmin2;
925  uend = u;
926  }
927  }
928  u = ubegin;
929 
930  //if(( u != 1.0 || uend != 1.0)
931  if( (std::fabs(u-1.0) > std::fabs(vpMath::maximum(u, 1.0))*std::numeric_limits<double>::epsilon())
932  || (std::fabs(uend-1.0) > std::fabs(vpMath::maximum(uend, 1.0))*std::numeric_limits<double>::epsilon()))
933  {
934  iP = nurbs.computeCurveDersPoint(u, 1);
935 
936  while (vpImagePoint::sqrDistance(iP[0],iPend) > vpMath::sqr(me->getSampleStep()) && u < uend)
937  {
938  u+=0.01;
939  /*if (iP!=NULL)*/ {
940  delete[] iP;
941  iP = NULL;
942  }
943  iP = nurbs.computeCurveDersPoint(u, 1);
944  if ( vpImagePoint::sqrDistance(iP[0],iP_1) > vpMath::sqr(me->getSampleStep()) && !outOfImage(iP[0], 0, rows, cols))
945  {
946  double delta = computeDelta(iP[1].get_i(),iP[1].get_j());
947  vpMeSite pix ; //= list.value();
948  pix.init(iP[0].get_i(), iP[0].get_j(), delta) ;
950  pix.track(I,me,false);
951  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
952  {
953  list.insert(it, pix);
954  iP_1 = iP[0];
955  }
956  }
957  }
958  /*if (iP!=NULL)*/ {
959  delete[] iP;
960  iP = NULL;
961  }
962  }
963  }
964  ++it;
965  ++itNext;
966  }
967  me->setRange(range_tmp);
968 }
969 
970 
976 void
978 {
979 #if 0
980  // Loop through list of sites to track
981  list.front();
982  while(!list.nextOutside())
983  {
984  vpMeSite s = list.value() ;//current reference pixel
985  vpMeSite s_next = list.nextValue() ;//current reference pixel
986 
987  if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep()))
988  {
990 
991  list.next();
992  list.modify(s_next);
993  if (!list.nextOutside()) list.next();
994  }
995  else
996  list.next() ;
997  }
998 #endif
999  std::list<vpMeSite>::const_iterator it=list.begin();
1000  std::list<vpMeSite>::iterator itNext=list.begin();
1001  ++itNext;
1002  for(;itNext!=list.end();){
1003  vpMeSite s = *it;//current reference pixel
1004  vpMeSite s_next = *itNext;//current reference pixel
1005 
1006  if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep())){
1007  s_next.setState(vpMeSite::TOO_NEAR);
1008 
1009  *itNext = s_next;
1010  ++it;
1011  ++itNext;
1012  if(itNext!=list.end()){
1013  ++it;
1014  ++itNext;
1015  }
1016  }
1017  else{
1018  ++it;
1019  ++itNext;
1020  }
1021  }
1022 }
1023 
1024 
1030 void
1032 {
1033  //Tracking des vpMeSites
1034  vpMeTracker::track(I);
1035 
1036  //Suppress points which are too close to each other
1038 
1039  //Suppressions des points ejectes par le tracking
1040  suppressPoints();
1041 
1042  if (list.size() == 1)
1043  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
1044 
1045  //Recalcule les parametres
1046 // nurbs.globalCurveInterp(list);
1047  nurbs.globalCurveApprox(list,nbControlPoints);
1048 
1049  //On resample localement
1050  localReSample(I);
1051 
1052  seekExtremities(I);
1053  if(enableCannyDetection)
1055 
1056 // nurbs.globalCurveInterp(list);
1057  nurbs.globalCurveApprox(list,nbControlPoints);
1058 
1059  double u = 0.0;
1060  vpImagePoint pt;
1061  vpImagePoint pt_1;
1062  dist = 0;
1063  while (u<=1.0)
1064  {
1065  pt = nurbs.computeCurvePoint(u);
1066  //if(u!=0)
1067  if(std::fabs(u) > std::numeric_limits<double>::epsilon())
1068  dist = dist + vpImagePoint::distance(pt,pt_1);
1069  pt_1 = pt;
1070  u=u+0.01;
1071  }
1072 
1073  updateDelta();
1074 
1075  reSample(I);
1076 }
1077 
1078 
1089 void
1091 {
1092  vpMeNurbs::display(I,nurbs,col);
1093 }
1094 
1095 
1112 bool
1113 vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I,
1114  vpImagePoint &iP,
1115  unsigned int &element)
1116 {
1117  vpImagePoint diP;
1118  vpImagePoint iPtemp;
1119  if (hasGoodLevel( I, iP )) {
1120  // get the point on the right of the point passed in
1121  computeFreemanParameters((element + 2) %8, diP);
1122  iPtemp = iP + diP;
1123  if (hasGoodLevel( I, iPtemp )) {
1124  element = (element + 2) % 8; // turn right
1125  }
1126  else {
1127  computeFreemanParameters((element + 1) %8, diP);
1128  iPtemp = iP + diP;
1129 
1130  if ( hasGoodLevel( I, iPtemp )) {
1131  element = (element + 1) % 8; // turn diag right
1132  }
1133  else {
1134  computeFreemanParameters(element, diP);
1135  iPtemp = iP + diP;
1136 
1137  if ( hasGoodLevel( I, iPtemp )) {
1138  //element = element; // keep same dir
1139  }
1140  else {
1141  computeFreemanParameters((element + 7) %8, diP);
1142  iPtemp = iP + diP;
1143 
1144  if ( hasGoodLevel( I, iPtemp )) {
1145  element = (element + 7) %8; // turn diag left
1146  }
1147  else {
1148  computeFreemanParameters((element + 6) %8, diP);
1149  iPtemp = iP + diP;
1150 
1151  if ( hasGoodLevel( I, iPtemp )) {
1152  element = (element + 6) %8 ; // turn left
1153  }
1154  else {
1155  computeFreemanParameters((element + 5) %8, diP);
1156  iPtemp = iP + diP;
1157 
1158  if ( hasGoodLevel( I, iPtemp )) {
1159  element = (element + 5) %8 ; // turn diag down
1160  }
1161  else {
1162  computeFreemanParameters((element + 4) %8, diP);
1163  iPtemp = iP + diP;
1164 
1165  if ( hasGoodLevel( I, iPtemp )) {
1166  element = (element + 4) %8 ; // turn down
1167  }
1168  else {
1169  computeFreemanParameters((element + 3) %8, diP);
1170  iPtemp = iP + diP;
1171 
1172  if ( hasGoodLevel( I, iPtemp )) {
1173  element = (element + 3) %8 ; // turn diag right down
1174  }
1175  else {
1176  // No neighbor with a good level
1177  //
1178  return false;
1179  }
1180  }
1181  }
1182  }
1183  }
1184  }
1185  }
1186  }
1187  }
1188  else {
1189  return false;
1190  }
1191  return true;
1192 }
1193 
1194 
1207 bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char>& I,
1208  const vpImagePoint& iP) const
1209 {
1210  if( !isInImage( I, iP ) )
1211  return false;
1212 
1213  if( I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0)
1214  {
1215  return true;
1216  }
1217  else
1218  {
1219  return false;
1220  }
1221 }
1222 
1223 
1234 bool vpMeNurbs::isInImage(const vpImage<unsigned char>& I, const vpImagePoint &iP) const
1235 {
1236  return (iP.get_i() >= 0
1237  && iP.get_j() >= 0
1238  && iP.get_i() < I.getHeight()
1239  && iP.get_j() < I.getWidth());
1240 }
1241 
1242 
1258 void
1259 vpMeNurbs::computeFreemanParameters( unsigned int element, vpImagePoint &diP)
1260 {
1261  /*
1262  5 6 7
1263  \ | /
1264  \|/
1265  4 ------- 0
1266  /|\
1267  / | \
1268  3 2 1
1269  */
1270  switch(element) {
1271  case 0: // go right
1272  diP.set_ij(0,1);
1273  break;
1274 
1275  case 1: // go right top
1276  diP.set_ij(1,1);
1277  break;
1278 
1279  case 2: // go top
1280  diP.set_ij(1,0);
1281  break;
1282 
1283  case 3:
1284  diP.set_ij(1,-1);
1285  break;
1286 
1287  case 4:
1288  diP.set_ij(0,-1);
1289  break;
1290 
1291  case 5:
1292  diP.set_ij(-1,-1);
1293  break;
1294 
1295  case 6:
1296  diP.set_ij(-1,0);
1297  break;
1298 
1299  case 7:
1300  diP.set_ij(-1,1);
1301  break;
1302  }
1303 }
1304 
1305 
1314 bool
1315 vpMeNurbs::farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint& iP)
1316 {
1317  unsigned int height = I.getHeight();
1318  unsigned int width = I.getWidth();
1319  return (iP.get_i() < height - 20
1320  && iP.get_j() < width - 20
1321  && iP.get_i() > 20
1322  && iP.get_j() > 20);
1323 }
1324 
1336 {
1337  double u = 0.0;
1338  vpImagePoint pt;
1339  while (u <= 1)
1340  {
1341  pt = n.computeCurvePoint(u);
1342  vpDisplay::displayCross(I,pt,4,color);
1343  u+=0.01;
1344  }
1345 }
1346 
1358 {
1359  double u = 0.0;
1360  vpImagePoint pt;
1361  while (u <= 1)
1362  {
1363  pt = n.computeCurvePoint(u);
1364  vpDisplay::displayCross(I,pt,4,color);
1365  u+=0.01;
1366  }
1367 }
1368 
unsigned int getRange() const
Definition: vpMe.h:225
unsigned int getMaskSize() const
Definition: vpMe.h:153
void suppressPoints()
Definition: vpMeNurbs.cpp:359
virtual ~vpMeNurbs()
Definition: vpMeNurbs.cpp:256
void init()
Definition: vpMeSite.cpp:69
double get_i() const
Definition: vpImagePoint.h:190
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition: vpList.h:434
unsigned int getWidth() const
Definition: vpImage.h:161
double jfloat
Definition: vpMeSite.h:96
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:526
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
unsigned int numberOfSignal()
static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:96
void track(const vpImage< unsigned char > &Im)
Definition: vpMeNurbs.cpp:1031
void seekExtremitiesCanny(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:549
Provide simple list management.
Definition: vpList.h:118
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:72
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
int outOfImage(int i, int j, int half, int rows, int cols)
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 initTracking(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:268
static void globalCurveInterp(std::vector< vpImagePoint > &l_crossingPoints, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:696
Definition: vpMe.h:59
Class that tracks in an image a edge defined by a Nurbs.
Definition: vpMeNurbs.h:125
static const vpColor green
Definition: vpColor.h:166
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
static int round(const double x)
Definition: vpMath.h:248
double get_j() const
Definition: vpImagePoint.h:201
void next(void)
position the current element on the next one
Definition: vpList.h:278
static Type abs(const Type &x)
Definition: vpMath.h:162
vpMeSiteState getState() const
Definition: vpMeSite.h:198
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:141
static const vpColor orange
Definition: vpColor.h:173
double ifloat
Definition: vpMeSite.h:96
int getStrip() const
Definition: vpMe.h:281
std::list< vpMeSite > list
Definition: vpMeTracker.h:73
unsigned int getAngleStep() const
Definition: vpMe.h:239
Error that can be emited by the vpTracker class and its derivates.
void supressNearPoints()
Definition: vpMeNurbs.cpp:977
#define vpTRACE
Definition: vpDebug.h:414
static double sqr(double x)
Definition: vpMath.h:110
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeNurbs.cpp:1090
void front(void)
Position the current element on the first element of the list.
Definition: vpList.h:389
type & value(void)
return the value of the current element
Definition: vpList.h:306
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:148
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 reSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:857
virtual void displayRectangle(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)=0
static double sqrDistance(const vpMeSite S1, const vpMeSite S2)
Definition: vpMeSite.h:240
void sample(const vpImage< unsigned char > &image)
Definition: vpMeNurbs.cpp:317
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:185
static void globalCurveApprox(std::vector< vpImagePoint > &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:872
static double rad(double deg)
Definition: vpMath.h:104
void localReSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:877
int getPointsToTrack() const
Definition: vpMe.h:211
vpMatrix * getMask() const
Definition: vpMe.h:105
int j
Definition: vpMeSite.h:94
static void createSubImage(const vpImage< Type > &I, unsigned int i_sub, unsigned int j_sub, unsigned int nrow_sub, unsigned int ncol_sub, vpImage< Type > &S)
Definition: vpImageTools.h:132
static double sqrDistance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition: vpImagePoint.h:285
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
Definition: vpMeSite.cpp:377
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:425
void initTracking(const vpImage< unsigned char > &I)
unsigned int getHeight() const
Definition: vpImage.h:152
Defines a rectangle in the plane.
Definition: vpRect.h:81
virtual bool getClick(bool blocking=true)=0
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 canny(const vpImage< unsigned char > &I, vpImage< unsigned char > &Ic, const unsigned int gaussianFilterSize, const double thresholdCanny, const unsigned int apertureSobel)
vpNurbs nurbs
The Nurbs which represents the tracked edge.
Definition: vpMeNurbs.h:133
void setRange(const unsigned int &r)
Definition: vpMe.h:218
double getSampleStep() const
Definition: vpMe.h:267
static int sign(double x)
Definition: vpMath.h:266
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:80
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve...
Definition: vpNurbs.h:85
virtual void displayPoint(const vpImagePoint &ip, const vpColor &color)=0
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition: vpImagePoint.h:270
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:176
static vpImagePoint * computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:266
static const vpColor blue
Definition: vpColor.h:169
void updateDelta()
Definition: vpMeNurbs.cpp:379