Visual Servoing Platform  version 3.1.0
vpMeNurbs.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 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  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
44 #include <cmath> // std::fabs
45 #include <limits> // numeric_limits
46 #include <stdlib.h>
47 #include <visp3/core/vpImageConvert.h>
48 #include <visp3/core/vpImageFilter.h>
49 #include <visp3/core/vpImagePoint.h>
50 #include <visp3/core/vpImageTools.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpRect.h>
53 #include <visp3/core/vpRobust.h>
54 #include <visp3/core/vpTrackingException.h>
55 #include <visp3/me/vpMe.h>
56 #include <visp3/me/vpMeNurbs.h>
57 #include <visp3/me/vpMeSite.h>
58 #include <visp3/me/vpMeTracker.h>
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, vpMe *me, double &angle, double &convlt);
70 vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP);
71 bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
72 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
73 vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list);
74 #endif
75 
76 // Compute the angle delta = arctan(deltai/deltaj)
77 // and normalize it between 0 and pi
78 double computeDelta(double deltai, double deltaj)
79 {
80  double delta;
81  delta = atan2(deltai, deltaj);
82  delta -= M_PI / 2.0;
83  while (delta > M_PI) {
84  delta -= M_PI;
85  }
86  while (delta < 0) {
87  delta += M_PI;
88  }
89  return (delta);
90 }
91 
92 // Check if the image point is in the image and not to close to
93 // its edge to enable the computation of a convolution whith a mask.
94 static bool outOfImage(const vpImagePoint &iP, int half, int rows, int cols)
95 {
96  return ((iP.get_i() < half + 1) || (iP.get_i() > (rows - half - 3)) || (iP.get_j() < half + 1) ||
97  (iP.get_j() > (cols - half - 3)));
98 }
99 
100 // if iP is a edge point, it computes the angle corresponding to the
101 // highest convolution result. the angle is between 0 an 179.
102 // The result gives the angle in RADIAN + pi/2 (to deal with the moving edeg
103 // alpha angle) and the corresponding convolution result.
104 void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP, 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  double conv = 0.0;
112  unsigned int half;
113  half = (me->getMaskSize() - 1) >> 1;
114 
115  if (outOfImage(iP, (int)half + me->getStrip(), Iheight, Iwidth)) {
116  conv = 0.0;
117  } else {
118  int index_mask;
119 
120  if (me->getAngleStep() != 0)
121  index_mask = (int)(i / (double)me->getAngleStep());
122  else
123  throw(vpException(vpException::divideByZeroError, "angle step = 0"));
124 
125  unsigned int ihalf = (unsigned int)(iP.get_i() - half);
126  unsigned int jhalf = (unsigned int)(iP.get_j() - half);
127  unsigned int a;
128  unsigned int b;
129  for (a = 0; a < me->getMaskSize(); a++) {
130  unsigned int ihalfa = ihalf + a;
131  for (b = 0; b < me->getMaskSize(); b++) {
132  conv += me->getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
133  }
134  }
135  }
136  conv = fabs(conv);
137  if (conv > convlt) {
138  convlt = conv;
139  angle = vpMath::rad(i);
140  angle += M_PI / 2;
141  while (angle > M_PI) {
142  angle -= M_PI;
143  }
144  while (angle < 0) {
145  angle += M_PI;
146  }
147  }
148  }
149 }
150 
151 // Find the point belonging to the edge of the sub image which respects the
152 // following hypotheses:
153 //- the value of the pixel is upper than zero.
154 //- the distantce between the point and iP is less than 4 pixels.
155 // The function returns the nearest point of iP which respect the hypotheses
156 // If no point is found the returned point is (-1,-1)
157 vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP)
158 {
159  double dist = 1e6;
160  double dist_1 = 1e6;
161  vpImagePoint index(-1, -1);
162  for (unsigned int i = 0; i <= Isub.getHeight(); i++) {
163  for (unsigned int j = 0; j <= Isub.getWidth(); j++) {
164  if (i == 0 || i == Isub.getHeight() - 1 || j == 0 || j == Isub.getWidth() - 1) {
165  if (Isub(i, j) > 0) {
167  if (dist <= 16 && dist < dist_1) {
168  dist_1 = dist;
169  index.set_ij(i, j);
170  }
171  }
172  }
173  }
174  }
175  return index;
176 }
177 
178 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
179 // Check if the list of vpImagePoint contains a distant point of less tha 4
180 // pixels from the center of the sub image (ie the point (15,15).
181 vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
182 {
183  ip_edges_list->front();
184  while (!ip_edges_list->outside()) {
185  vpImagePoint iP = ip_edges_list->value();
186  double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
187  if (dist <= 16) {
188  return true;
189  }
190  ip_edges_list->next();
191  }
192  return false;
193 }
194 #endif
195 
196 // Check if the list of vpImagePoint contains a distant point of less tha 4
197 // pixels from the center of the sub image (ie the point (15,15).
198 bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
199 {
200  for (std::list<vpImagePoint>::const_iterator it = ip_edges_list->begin(); it != ip_edges_list->end(); ++it) {
201  vpImagePoint iP = *it;
202  double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
203  if (dist <= 16) {
204  return true;
205  }
206  }
207  return false;
208 }
209 
210 /***************************************/
211 
216  : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false), cannyTh1(100.),
217  cannyTh2(200.)
218 {
219 }
220 
225  : vpMeTracker(menurbs), nurbs(menurbs.nurbs), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0),
226  enableCannyDetection(false), cannyTh1(100.), cannyTh2(200.)
227 {
228  dist = menurbs.dist;
229  nbControlPoints = menurbs.nbControlPoints;
230  beginPtFound = menurbs.beginPtFound;
231  endPtFound = menurbs.endPtFound;
232  enableCannyDetection = menurbs.enableCannyDetection;
233  cannyTh1 = menurbs.cannyTh1;
234  cannyTh2 = menurbs.cannyTh2;
235 }
236 
241 
249 {
250  std::list<vpImagePoint> ptList;
251  vpImagePoint pt;
253 
254  while (vpDisplay::getClick(I, pt, b)) {
255  if (b == vpMouseButton::button1) {
256  // std::cout<<pt<<std::endl;
257  ptList.push_back(pt);
259  vpDisplay::flush(I);
260  }
261  if (b == vpMouseButton::button3)
262  break;
263  }
264  if (ptList.size() > 3)
265  initTracking(I, ptList);
266  else
267  throw(vpException(vpException::notInitialized, "Not enough points to initialize the Nurbs"));
268 }
269 
277 void vpMeNurbs::initTracking(const vpImage<unsigned char> &I, const std::list<vpImagePoint> &ptList)
278 {
279  nurbs.globalCurveInterp(ptList);
280 
281  sample(I);
282 
284  track(I);
285 }
286 
294 {
295  int rows = (int)I.getHeight();
296  int cols = (int)I.getWidth();
297  double step = 1.0 / (double)me->getPointsToTrack();
298 
299  // Delete old list
300  list.clear();
301 
302  vpImagePoint ip;
303  double u = 0.0;
304  vpImagePoint *pt = NULL;
305  vpImagePoint pt_1(-rows, -cols);
306  while (u <= 1.0) {
307  if (pt != NULL)
308  delete[] pt;
309  pt = nurbs.computeCurveDersPoint(u, 1);
310  double delta = computeDelta(pt[1].get_i(), pt[1].get_j());
311 
312  // If point is in the image, add to the sample list
313  if (!outOfImage(pt[0], 0, rows, cols) &&
314  vpImagePoint::sqrDistance(pt[0], pt_1) >= vpMath::sqr(me->getSampleStep())) {
315  vpMeSite pix; //= list.value();
316  pix.init(pt[0].get_i(), pt[0].get_j(), delta);
318 
319  list.push_back(pix);
320  pt_1 = pt[0];
321  }
322  u = u + step;
323  }
324  if (pt != NULL)
325  delete[] pt;
326 }
327 
335 {
336  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
337  vpMeSite s = *it; // current reference pixel
338 
339  if (s.getState() != vpMeSite::NO_SUPPRESSION) {
340  it = list.erase(it);
341  } else
342  ++it;
343  }
344 }
345 
351 {
352  double u = 0.0;
353  double d = 1e6;
354  double d_1 = 1e6;
355  std::list<vpMeSite>::iterator it = list.begin();
356 
357  vpImagePoint Cu;
358  vpImagePoint *der = NULL;
359  double step = 0.01;
360  while (u < 1 && it != list.end()) {
361  vpMeSite s = *it;
362  vpImagePoint pt(s.i, s.j);
363  while (d <= d_1 && u < 1) {
364  Cu = nurbs.computeCurvePoint(u);
365  d_1 = d;
366  d = vpImagePoint::distance(pt, Cu);
367  u += step;
368  }
369 
370  u -= step;
371  if (der != NULL)
372  delete[] der;
373  der = nurbs.computeCurveDersPoint(u, 1);
374  // vpImagePoint toto(der[0].get_i(),der[0].get_j());
375  // vpDisplay::displayCross(I,toto,4,vpColor::red);
376 
377  s.alpha = computeDelta(der[1].get_i(), der[1].get_j());
378  *it = s;
379  ++it;
380  d = 1e6;
381  d_1 = 1.5e6;
382  }
383  if (der != NULL)
384  delete[] der;
385 }
386 
395 {
396  int rows = (int)I.getHeight();
397  int cols = (int)I.getWidth();
398 
399  vpImagePoint *begin = NULL;
400  vpImagePoint *end = NULL;
401 
402  begin = nurbs.computeCurveDersPoint(0.0, 1);
403  end = nurbs.computeCurveDersPoint(1.0, 1);
404 
405  // Check if the two extremities are not to close to eachother.
406  double d = vpImagePoint::distance(begin[0], end[0]);
407  double threshold = 3 * me->getSampleStep();
408  double sample_step = me->getSampleStep();
409  vpImagePoint pt;
410  if (d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/) {
411  vpMeSite P;
412 
413  // Init vpMeSite
414  P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign);
416 
417  // Set the range
418  unsigned int memory_range = me->getRange();
419  me->setRange(2);
420 
421  // Point at the beginning of the list
422  bool beginPtAdded = false;
423  vpImagePoint pt_max = begin[0];
424  double angle = atan2(begin[1].get_i(), begin[1].get_j());
425  double co = vpMath::abs(cos(angle));
426  co = co * vpMath::sign(begin[1].get_j());
427  double si = vpMath::abs(sin(angle));
428  si = si * vpMath::sign(begin[1].get_i());
429  for (int i = 0; i < 3; i++) {
430  P.ifloat = P.ifloat - si * sample_step;
431  P.i = (int)P.ifloat;
432  P.jfloat = P.jfloat - co * sample_step;
433  P.j = (int)P.jfloat;
434  pt.set_ij(P.ifloat, P.jfloat);
435  if (vpImagePoint::distance(end[0], pt) < threshold)
436  break;
437  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
438  P.track(I, me, false);
439 
440  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
441  list.push_front(P);
442  beginPtAdded = true;
443  pt_max = pt;
444  if (vpDEBUG_ENABLE(3)) {
446  }
447  } else {
448  if (vpDEBUG_ENABLE(3)) {
450  }
451  }
452  }
453  }
454 
455  if (!beginPtAdded)
456  beginPtFound++;
457 
458  P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
460 
461  bool endPtAdded = false;
462  angle = atan2(end[1].get_i(), end[1].get_j());
463  co = vpMath::abs(cos(angle));
464  co = co * vpMath::sign(end[1].get_j());
465  si = vpMath::abs(sin(angle));
466  si = si * vpMath::sign(end[1].get_i());
467  for (int i = 0; i < 3; i++) {
468  P.ifloat = P.ifloat + si * sample_step;
469  P.i = (int)P.ifloat;
470  P.jfloat = P.jfloat + co * sample_step;
471  P.j = (int)P.jfloat;
472  pt.set_ij(P.ifloat, P.jfloat);
473  if (vpImagePoint::distance(begin[0], pt) < threshold)
474  break;
475  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
476  P.track(I, me, false);
477 
478  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
479  list.push_back(P);
480  endPtAdded = true;
481  if (vpDEBUG_ENABLE(3)) {
483  }
484  } else {
485  if (vpDEBUG_ENABLE(3)) {
487  }
488  }
489  }
490  }
491  if (!endPtAdded)
492  endPtFound++;
493  me->setRange(memory_range);
494  } else {
495  list.pop_front();
496  }
497  /*if(begin != NULL)*/ delete[] begin;
498  /*if(end != NULL) */ delete[] end;
499 }
500 
512 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
514 #else
516 #endif
517 {
518 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
519  vpMeSite pt = list.front();
520  vpImagePoint firstPoint(pt.ifloat, pt.jfloat);
521  pt = list.back();
522  vpImagePoint lastPoint(pt.ifloat, pt.jfloat);
523  if (beginPtFound >= 3 && farFromImageEdge(I, firstPoint)) {
524  vpImagePoint *begin = NULL;
525  begin = nurbs.computeCurveDersPoint(0.0, 1);
526  vpImage<unsigned char> Isub(32, 32); // Sub image.
527  vpImagePoint topLeft(begin[0].get_i() - 15, begin[0].get_j() - 15);
528  vpRect rect(topLeft, 32, 32);
529 
531 
532  vpImageTools::crop(I, rect, Isub);
533 
534  vpImagePoint lastPtInSubIm(begin[0]);
535  double u = 0.0;
536  double step = 0.0001;
537  // Find the point of the nurbs closest from the edge of the subImage and
538  // in the subImage.
539  while (inRectangle(lastPtInSubIm, rect) && u < 1) {
540  u += step;
541  lastPtInSubIm = nurbs.computeCurvePoint(u);
542  }
543 
544  u -= step;
545  if (u > 0)
546  lastPtInSubIm = nurbs.computeCurvePoint(u);
547 
548 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
549  vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
550 #else
551  IplImage *Ip = NULL;
552  vpImageConvert::convert(Isub, Ip);
553 
554  IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
555  cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
556 
557  vpImageConvert::convert(dst, Isub);
558 #endif
559 
560  vpImagePoint firstBorder(-1, -1);
561 
562  firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
563 
564  std::list<vpImagePoint> ip_edges_list;
565  if (firstBorder != vpImagePoint(-1, -1)) {
566  unsigned int dir;
567  double fi = firstBorder.get_i();
568  double fj = firstBorder.get_j();
569  double w = Isub.getWidth() - 1;
570  double h = Isub.getHeight() - 1;
571  // if (firstBorder.get_i() == 0) dir = 4;
572  if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
573  dir = 4;
574  // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
575  else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
576  dir = 0;
577  // else if (firstBorder.get_j() == 0) dir = 2;
578  else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
579  dir = 2;
580  // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
581  else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
582  dir = 6;
583  computeFreemanChainElement(Isub, firstBorder, dir);
584  unsigned int firstDir = dir;
585  ip_edges_list.push_back(firstBorder);
586  vpImagePoint border(firstBorder);
587  vpImagePoint dBorder;
588  do {
589  computeFreemanParameters(dir, dBorder);
590  border = border + dBorder;
591  vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
592 
593  ip_edges_list.push_back(border);
594 
595  computeFreemanChainElement(Isub, border, dir);
596  } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
597  }
598 
599  if (findCenterPoint(&ip_edges_list)) {
600  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();
601  /*++it*/) {
602  vpMeSite s = *it;
603  vpImagePoint iP(s.ifloat, s.jfloat);
604  if (inRectangle(iP, rect))
605  it = list.erase(it);
606  else
607  break;
608  }
609 
610  std::list<vpMeSite>::iterator itList = list.begin();
611  double convlt;
612  double delta = 0;
613  int nbr = 0;
614  std::list<vpMeSite> addedPt;
615  for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
616  ++itEdges) {
617  vpMeSite s = *itList;
618  vpImagePoint iPtemp = *itEdges + topLeft;
619  vpMeSite pix;
620  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
621  dist = vpMeSite::sqrDistance(s, pix);
622  if (dist >= vpMath::sqr(me->getSampleStep()) /*25*/) {
623  bool exist = false;
624  for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
625  dist = vpMeSite::sqrDistance(pix, *itAdd);
626  if (dist < vpMath::sqr(me->getSampleStep()) /*25*/)
627  exist = true;
628  }
629  if (!exist) {
630  findAngle(I, iPtemp, me, delta, convlt);
631  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
633  --itList;
634  list.insert(itList, pix);
635  ++itList;
636  addedPt.push_front(pix);
637  nbr++;
638  }
639  }
640  }
641 
642  unsigned int memory_range = me->getRange();
643  me->setRange(3);
644  std::list<vpMeSite>::iterator itList2 = list.begin();
645  for (int j = 0; j < nbr; j++) {
646  vpMeSite s = *itList2;
647  s.track(I, me, false);
648  *itList2 = s;
649  ++itList2;
650  }
651  me->setRange(memory_range);
652  }
653 
654  /* if (begin != NULL) */ delete[] begin;
655  beginPtFound = 0;
656  }
657 
658  if (endPtFound >= 3 && farFromImageEdge(I, lastPoint)) {
659  vpImagePoint *end = NULL;
660  end = nurbs.computeCurveDersPoint(1.0, 1);
661 
662  vpImage<unsigned char> Isub(32, 32); // Sub image.
663  vpImagePoint topLeft(end[0].get_i() - 15, end[0].get_j() - 15);
664  vpRect rect(topLeft, 32, 32);
665 
667 
668  vpImageTools::crop(I, rect, Isub);
669 
670  vpImagePoint lastPtInSubIm(end[0]);
671  double u = 1.0;
672  double step = 0.0001;
673  // Find the point of the nurbs closest from the edge of the subImage and
674  // in the subImage.
675  while (inRectangle(lastPtInSubIm, rect) && u > 0) {
676  u -= step;
677  lastPtInSubIm = nurbs.computeCurvePoint(u);
678  }
679 
680  u += step;
681  if (u < 1.0)
682  lastPtInSubIm = nurbs.computeCurvePoint(u);
683 
684 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
685  vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
686 #else
687  IplImage *Ip = NULL;
688  vpImageConvert::convert(Isub, Ip);
689 
690  IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
691  cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
692 
693  vpImageConvert::convert(dst, Isub);
694 #endif
695 
696  vpImagePoint firstBorder(-1, -1);
697 
698  firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
699 
700  std::list<vpImagePoint> ip_edges_list;
701  if (firstBorder != vpImagePoint(-1, -1)) {
702  unsigned int dir;
703  double fi = firstBorder.get_i();
704  double fj = firstBorder.get_j();
705  double w = Isub.getWidth() - 1;
706  double h = Isub.getHeight() - 1;
707  // if (firstBorder.get_i() == 0) dir = 4;
708  if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
709  dir = 4;
710  // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
711  else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
712  dir = 0;
713  // else if (firstBorder.get_j() == 0) dir = 2;
714  else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
715  dir = 2;
716  // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
717  else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
718  dir = 6;
719 
720  computeFreemanChainElement(Isub, firstBorder, dir);
721  unsigned int firstDir = dir;
722  ip_edges_list.push_back(firstBorder);
723  vpImagePoint border(firstBorder);
724  vpImagePoint dBorder;
725  do {
726  computeFreemanParameters(dir, dBorder);
727  border = border + dBorder;
728  vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
729 
730  ip_edges_list.push_back(border);
731 
732  computeFreemanChainElement(Isub, border, dir);
733  } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
734  }
735 
736  if (findCenterPoint(&ip_edges_list)) {
737  // list.end();
738  vpMeSite s;
739  while (true) //{//!list.outside())
740  {
741  s = list.back(); // list.value() ;
742  vpImagePoint iP(s.ifloat, s.jfloat);
743  if (inRectangle(iP, rect)) {
744  list.erase(list.end());
745  // list.end();
746  } else
747  break;
748  }
749 
750  std::list<vpMeSite>::iterator itList = list.end();
751  --itList; // Move on the last element
752  double convlt;
753  double delta;
754  int nbr = 0;
755  std::list<vpMeSite> addedPt;
756  for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
757  ++itEdges) {
758  s = *itList;
759  vpImagePoint iPtemp = *itEdges + topLeft;
760  vpMeSite pix;
761  pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
762  dist = vpMeSite::sqrDistance(s, pix);
763  if (dist >= vpMath::sqr(me->getSampleStep())) {
764  bool exist = false;
765  for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
766  dist = vpMeSite::sqrDistance(pix, *itAdd);
767  if (dist < vpMath::sqr(me->getSampleStep()))
768  exist = true;
769  }
770  if (!exist) {
771  findAngle(I, iPtemp, me, delta, convlt);
772  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
774  list.push_back(pix);
775  addedPt.push_back(pix);
776  nbr++;
777  }
778  }
779  }
780 
781  unsigned int memory_range = me->getRange();
782  me->setRange(3);
783  std::list<vpMeSite>::iterator itList2 = list.end();
784  --itList2; // Move to the last element
785  for (int j = 0; j < nbr; j++) {
786  vpMeSite me_s = *itList2;
787  me_s.track(I, me, false);
788  *itList2 = me_s;
789  --itList2;
790  }
791  me->setRange(memory_range);
792  }
793 
794  /* if (end != NULL) */ delete[] end;
795  endPtFound = 0;
796  }
797 #else
798  vpTRACE("To use the canny detection, OpenCV has to be installed.");
799 #endif
800 }
801 
813 {
814  unsigned int n = numberOfSignal();
815  double nbPt = floor(dist / me->getSampleStep());
816 
817  if ((double)n < 0.7 * nbPt) {
818  sample(I);
820  }
821 }
822 
831 {
832  int rows = (int)I.getHeight();
833  int cols = (int)I.getWidth();
834  vpImagePoint *iP = NULL;
835 
836  int n = (int)numberOfSignal();
837 
838  // list.front();
839  std::list<vpMeSite>::iterator it = list.begin();
840  std::list<vpMeSite>::iterator itNext = list.begin();
841  ++itNext;
842 
843  unsigned int range_tmp = me->getRange();
844  me->setRange(2);
845 
846  while (itNext != list.end() && n <= me->getPointsToTrack()) {
847  vpMeSite s = *it; // current reference pixel
848  vpMeSite s_next = *itNext; // current reference pixel
849 
850  double d = vpMeSite::sqrDistance(s, s_next);
851  if (d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600) {
852  vpImagePoint iP0(s.ifloat, s.jfloat);
853  vpImagePoint iPend(s_next.ifloat, s_next.jfloat);
854  vpImagePoint iP_1(s.ifloat, s.jfloat);
855 
856  double u = 0.0;
857  double ubegin = 0.0;
858  double uend = 0.0;
859  double dmin1_1 = 1e6;
860  double dmin2_1 = 1e6;
861  while (u < 1) {
862  u += 0.01;
863  double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iP0);
864  double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iPend);
865 
866  if (dmin1 < dmin1_1) {
867  dmin1_1 = dmin1;
868  ubegin = u;
869  }
870 
871  if (dmin2 < dmin2_1) {
872  dmin2_1 = dmin2;
873  uend = u;
874  }
875  }
876  u = ubegin;
877 
878  // if(( u != 1.0 || uend != 1.0)
879  if ((std::fabs(u - 1.0) > std::fabs(vpMath::maximum(u, 1.0)) * std::numeric_limits<double>::epsilon()) ||
880  (std::fabs(uend - 1.0) > std::fabs(vpMath::maximum(uend, 1.0)) * std::numeric_limits<double>::epsilon())) {
881  iP = nurbs.computeCurveDersPoint(u, 1);
882 
883  while (vpImagePoint::sqrDistance(iP[0], iPend) > vpMath::sqr(me->getSampleStep()) && u < uend) {
884  u += 0.01;
885  /*if (iP!=NULL)*/ {
886  delete[] iP;
887  iP = NULL;
888  }
889  iP = nurbs.computeCurveDersPoint(u, 1);
890  if (vpImagePoint::sqrDistance(iP[0], iP_1) > vpMath::sqr(me->getSampleStep()) &&
891  !outOfImage(iP[0], 0, rows, cols)) {
892  double delta = computeDelta(iP[1].get_i(), iP[1].get_j());
893  vpMeSite pix; //= list.value();
894  pix.init(iP[0].get_i(), iP[0].get_j(), delta);
896  pix.track(I, me, false);
897  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
898  list.insert(it, pix);
899  iP_1 = iP[0];
900  }
901  }
902  }
903  /*if (iP!=NULL)*/ {
904  delete[] iP;
905  iP = NULL;
906  }
907  }
908  }
909  ++it;
910  ++itNext;
911  }
912  me->setRange(range_tmp);
913 }
914 
921 {
922 #if 0
923  // Loop through list of sites to track
924  list.front();
925  while(!list.nextOutside())
926  {
927  vpMeSite s = list.value() ;//current reference pixel
928  vpMeSite s_next = list.nextValue() ;//current reference pixel
929 
930  if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep()))
931  {
933 
934  list.next();
935  list.modify(s_next);
936  if (!list.nextOutside()) list.next();
937  }
938  else
939  list.next() ;
940  }
941 #endif
942  std::list<vpMeSite>::const_iterator it = list.begin();
943  std::list<vpMeSite>::iterator itNext = list.begin();
944  ++itNext;
945  for (; itNext != list.end();) {
946  vpMeSite s = *it; // current reference pixel
947  vpMeSite s_next = *itNext; // current reference pixel
948 
949  if (vpMeSite::sqrDistance(s, s_next) < vpMath::sqr(me->getSampleStep())) {
951 
952  *itNext = s_next;
953  ++it;
954  ++itNext;
955  if (itNext != list.end()) {
956  ++it;
957  ++itNext;
958  }
959  } else {
960  ++it;
961  ++itNext;
962  }
963  }
964 }
965 
972 {
973  // Tracking des vpMeSites
975 
976  // Suppress points which are too close to each other
978 
979  // Suppressions des points ejectes par le tracking
980  suppressPoints();
981 
982  if (list.size() == 1)
983  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
984 
985  // Recalcule les parametres
986  // nurbs.globalCurveInterp(list);
987  nurbs.globalCurveApprox(list, nbControlPoints);
988 
989  // On resample localement
990  localReSample(I);
991 
992  seekExtremities(I);
993  if (enableCannyDetection)
995 
996  // nurbs.globalCurveInterp(list);
997  nurbs.globalCurveApprox(list, nbControlPoints);
998 
999  double u = 0.0;
1000  vpImagePoint pt;
1001  vpImagePoint pt_1;
1002  dist = 0;
1003  while (u <= 1.0) {
1004  pt = nurbs.computeCurvePoint(u);
1005  // if(u!=0)
1006  if (std::fabs(u) > std::numeric_limits<double>::epsilon())
1007  dist = dist + vpImagePoint::distance(pt, pt_1);
1008  pt_1 = pt;
1009  u = u + 0.01;
1010  }
1011 
1012  updateDelta();
1013 
1014  reSample(I);
1015 }
1016 
1028 
1045 bool vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I, vpImagePoint &iP, unsigned int &element)
1046 {
1047  vpImagePoint diP;
1048  vpImagePoint iPtemp;
1049  if (hasGoodLevel(I, iP)) {
1050  // get the point on the right of the point passed in
1051  computeFreemanParameters((element + 2) % 8, diP);
1052  iPtemp = iP + diP;
1053  if (hasGoodLevel(I, iPtemp)) {
1054  element = (element + 2) % 8; // turn right
1055  } else {
1056  computeFreemanParameters((element + 1) % 8, diP);
1057  iPtemp = iP + diP;
1058 
1059  if (hasGoodLevel(I, iPtemp)) {
1060  element = (element + 1) % 8; // turn diag right
1061  } else {
1062  computeFreemanParameters(element, diP);
1063  iPtemp = iP + diP;
1064 
1065  if (hasGoodLevel(I, iPtemp)) {
1066  // element = element; // keep same dir
1067  } else {
1068  computeFreemanParameters((element + 7) % 8, diP);
1069  iPtemp = iP + diP;
1070 
1071  if (hasGoodLevel(I, iPtemp)) {
1072  element = (element + 7) % 8; // turn diag left
1073  } else {
1074  computeFreemanParameters((element + 6) % 8, diP);
1075  iPtemp = iP + diP;
1076 
1077  if (hasGoodLevel(I, iPtemp)) {
1078  element = (element + 6) % 8; // turn left
1079  } else {
1080  computeFreemanParameters((element + 5) % 8, diP);
1081  iPtemp = iP + diP;
1082 
1083  if (hasGoodLevel(I, iPtemp)) {
1084  element = (element + 5) % 8; // turn diag down
1085  } else {
1086  computeFreemanParameters((element + 4) % 8, diP);
1087  iPtemp = iP + diP;
1088 
1089  if (hasGoodLevel(I, iPtemp)) {
1090  element = (element + 4) % 8; // turn down
1091  } else {
1092  computeFreemanParameters((element + 3) % 8, diP);
1093  iPtemp = iP + diP;
1094 
1095  if (hasGoodLevel(I, iPtemp)) {
1096  element = (element + 3) % 8; // turn diag right down
1097  } else {
1098  // No neighbor with a good level
1099  //
1100  return false;
1101  }
1102  }
1103  }
1104  }
1105  }
1106  }
1107  }
1108  }
1109  } else {
1110  return false;
1111  }
1112  return true;
1113 }
1114 
1127 bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1128 {
1129  if (!isInImage(I, iP))
1130  return false;
1131 
1132  if (I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0) {
1133  return true;
1134  } else {
1135  return false;
1136  }
1137 }
1138 
1149 bool vpMeNurbs::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1150 {
1151  return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && iP.get_j() < I.getWidth());
1152 }
1153 
1171 void vpMeNurbs::computeFreemanParameters(unsigned int element, vpImagePoint &diP)
1172 {
1173  /*
1174  5 6 7
1175  \ | /
1176  \|/
1177  4 ------- 0
1178  /|\
1179  / | \
1180  3 2 1
1181  */
1182  switch (element) {
1183  case 0: // go right
1184  diP.set_ij(0, 1);
1185  break;
1186 
1187  case 1: // go right top
1188  diP.set_ij(1, 1);
1189  break;
1190 
1191  case 2: // go top
1192  diP.set_ij(1, 0);
1193  break;
1194 
1195  case 3:
1196  diP.set_ij(1, -1);
1197  break;
1198 
1199  case 4:
1200  diP.set_ij(0, -1);
1201  break;
1202 
1203  case 5:
1204  diP.set_ij(-1, -1);
1205  break;
1206 
1207  case 6:
1208  diP.set_ij(-1, 0);
1209  break;
1210 
1211  case 7:
1212  diP.set_ij(-1, 1);
1213  break;
1214  }
1215 }
1216 
1226 bool vpMeNurbs::farFromImageEdge(const vpImage<unsigned char> &I, const vpImagePoint &iP)
1227 {
1228  unsigned int height = I.getHeight();
1229  unsigned int width = I.getWidth();
1230  return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20);
1231 }
1232 
1244 {
1245  double u = 0.0;
1246  vpImagePoint pt;
1247  while (u <= 1) {
1248  pt = n.computeCurvePoint(u);
1249  vpDisplay::displayCross(I, pt, 4, color);
1250  u += 0.01;
1251  }
1252 }
1253 
1265 {
1266  double u = 0.0;
1267  vpImagePoint pt;
1268  while (u <= 1) {
1269  pt = n.computeCurvePoint(u);
1270  vpDisplay::displayCross(I, pt, 4, color);
1271  u += 0.01;
1272  }
1273 }
void suppressPoints()
Definition: vpMeNurbs.cpp:334
double get_i() const
Definition: vpImagePoint.h:204
virtual ~vpMeNurbs()
Definition: vpMeNurbs.cpp:240
vpMeSiteState getState() const
Definition: vpMeSite.h:188
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void init()
Definition: vpMeSite.cpp:66
double jfloat
Definition: vpMeSite.h:88
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538
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:85
static double sqrDistance(const vpMeSite &S1, const vpMeSite &S2)
Definition: vpMeSite.h:232
void track(const vpImage< unsigned char > &Im)
Definition: vpMeNurbs.cpp:971
void seekExtremitiesCanny(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:513
Provide simple list management.
Definition: vpList.h:113
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
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 initTracking(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:248
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:685
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Definition: vpMe.h:60
Class that tracks in an image a edge defined by a Nurbs.
Definition: vpMeNurbs.h:130
static const vpColor green
Definition: vpColor.h:183
vpMatrix * getMask() const
Definition: vpMe.h:120
static void flush(const vpImage< unsigned char > &I)
static int round(const double x)
Definition: vpMath.h:235
unsigned int getMaskSize() const
Definition: vpMe.h:142
void next(void)
position the current element on the next one
Definition: vpList.h:250
static Type abs(const Type &x)
Definition: vpMath.h:152
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
static const vpColor orange
Definition: vpColor.h:190
double ifloat
Definition: vpMeSite.h:88
std::list< vpMeSite > list
Definition: vpMeTracker.h:74
Error that can be emited by the vpTracker class and its derivates.
void supressNearPoints()
Definition: vpMeNurbs.cpp:920
double getSampleStep() const
Definition: vpMe.h:285
int outOfImage(int i, int j, int half, int row, int cols)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:108
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeNurbs.cpp:1027
void front(void)
Position the current element on the first element of the list.
Definition: vpList.h:323
type & value(void)
return the value of the current element
Definition: vpList.h:269
double get_j() const
Definition: vpImagePoint.h:215
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:137
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 reSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:812
static void displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
int getStrip() const
Definition: vpMe.h:185
void sample(const vpImage< unsigned char > &image)
Definition: vpMeNurbs.cpp:293
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:174
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:866
static double rad(double deg)
Definition: vpMath.h:102
void localReSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:830
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
int j
Definition: vpMeSite.h:86
unsigned int getHeight() const
Definition: vpImage.h:178
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:237
static double sqrDistance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition: vpImagePoint.h:301
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
Definition: vpMeSite.cpp:362
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:394
void initTracking(const vpImage< unsigned char > &I)
Defines a rectangle in the plane.
Definition: vpRect.h:78
int getPointsToTrack() const
Definition: vpMe.h:173
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:138
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition: vpList.h:356
void setRange(const unsigned int &r)
Definition: vpMe.h:271
static int() sign(double x)
Definition: vpMath.h:262
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:81
unsigned int getWidth() const
Definition: vpImage.h:229
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve...
Definition: vpNurbs.h:97
unsigned int getRange() const
Definition: vpMe.h:179
unsigned int getAngleStep() const
Definition: vpMe.h:114
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition: vpImagePoint.h:285
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:189
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:260
static const vpColor blue
Definition: vpColor.h:186
void updateDelta()
Definition: vpMeNurbs.cpp:350