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