Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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  * 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 void vpMeNurbs::sample(const vpImage<unsigned char> &I, bool doNotTrack)
295 {
296  (void)doNotTrack;
297  int rows = (int)I.getHeight();
298  int cols = (int)I.getWidth();
299  double step = 1.0 / (double)me->getPointsToTrack();
300 
301  // Delete old list
302  list.clear();
303 
304  double u = 0.0;
305  vpImagePoint *pt = NULL;
306  vpImagePoint pt_1(-rows, -cols);
307  while (u <= 1.0) {
308  if (pt != NULL)
309  delete[] pt;
310  pt = nurbs.computeCurveDersPoint(u, 1);
311  double delta = computeDelta(pt[1].get_i(), pt[1].get_j());
312 
313  // If point is in the image, add to the sample list
314  if (!outOfImage(pt[0], 0, rows, cols) &&
315  vpImagePoint::sqrDistance(pt[0], pt_1) >= vpMath::sqr(me->getSampleStep())) {
316  vpMeSite pix; //= list.value();
317  pix.init(pt[0].get_i(), pt[0].get_j(), delta);
319 
320  list.push_back(pix);
321  pt_1 = pt[0];
322  }
323  u = u + step;
324  }
325  if (pt != NULL)
326  delete[] pt;
327 }
328 
336 {
337  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
338  vpMeSite s = *it; // current reference pixel
339 
340  if (s.getState() != vpMeSite::NO_SUPPRESSION) {
341  it = list.erase(it);
342  } else
343  ++it;
344  }
345 }
346 
352 {
353  double u = 0.0;
354  double d = 1e6;
355  double d_1 = 1e6;
356  std::list<vpMeSite>::iterator it = list.begin();
357 
358  vpImagePoint Cu;
359  vpImagePoint *der = NULL;
360  double step = 0.01;
361  while (u < 1 && it != list.end()) {
362  vpMeSite s = *it;
363  vpImagePoint pt(s.i, s.j);
364  while (d <= d_1 && u < 1) {
365  Cu = nurbs.computeCurvePoint(u);
366  d_1 = d;
367  d = vpImagePoint::distance(pt, Cu);
368  u += step;
369  }
370 
371  u -= step;
372  if (der != NULL)
373  delete[] der;
374  der = nurbs.computeCurveDersPoint(u, 1);
375  // vpImagePoint toto(der[0].get_i(),der[0].get_j());
376  // vpDisplay::displayCross(I,toto,4,vpColor::red);
377 
378  s.alpha = computeDelta(der[1].get_i(), der[1].get_j());
379  *it = s;
380  ++it;
381  d = 1e6;
382  d_1 = 1.5e6;
383  }
384  if (der != NULL)
385  delete[] der;
386 }
387 
396 {
397  int rows = (int)I.getHeight();
398  int cols = (int)I.getWidth();
399 
400  vpImagePoint *begin = NULL;
401  vpImagePoint *end = NULL;
402 
403  begin = nurbs.computeCurveDersPoint(0.0, 1);
404  end = nurbs.computeCurveDersPoint(1.0, 1);
405 
406  // Check if the two extremities are not to close to eachother.
407  double d = vpImagePoint::distance(begin[0], end[0]);
408  double threshold = 3 * me->getSampleStep();
409  double sample_step = me->getSampleStep();
410  vpImagePoint pt;
411  if (d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/) {
412  vpMeSite P;
413 
414  // Init vpMeSite
415  P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign);
417 
418  // Set the range
419  unsigned int memory_range = me->getRange();
420  me->setRange(2);
421 
422  // Point at the beginning of the list
423  bool beginPtAdded = false;
424  vpImagePoint pt_max = begin[0];
425  double angle = atan2(begin[1].get_i(), begin[1].get_j());
426  double co = vpMath::abs(cos(angle));
427  co = co * vpMath::sign(begin[1].get_j());
428  double si = vpMath::abs(sin(angle));
429  si = si * vpMath::sign(begin[1].get_i());
430  for (int i = 0; i < 3; i++) {
431  P.ifloat = P.ifloat - si * sample_step;
432  P.i = (int)P.ifloat;
433  P.jfloat = P.jfloat - co * sample_step;
434  P.j = (int)P.jfloat;
435  pt.set_ij(P.ifloat, P.jfloat);
436  if (vpImagePoint::distance(end[0], pt) < threshold)
437  break;
438  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
439  P.track(I, me, false);
440 
441  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
442  list.push_front(P);
443  beginPtAdded = true;
444  pt_max = pt;
445  if (vpDEBUG_ENABLE(3)) {
447  }
448  } else {
449  if (vpDEBUG_ENABLE(3)) {
451  }
452  }
453  }
454  }
455 
456  if (!beginPtAdded)
457  beginPtFound++;
458 
459  P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
461 
462  bool endPtAdded = false;
463  angle = atan2(end[1].get_i(), end[1].get_j());
464  co = vpMath::abs(cos(angle));
465  co = co * vpMath::sign(end[1].get_j());
466  si = vpMath::abs(sin(angle));
467  si = si * vpMath::sign(end[1].get_i());
468  for (int i = 0; i < 3; i++) {
469  P.ifloat = P.ifloat + si * sample_step;
470  P.i = (int)P.ifloat;
471  P.jfloat = P.jfloat + co * sample_step;
472  P.j = (int)P.jfloat;
473  pt.set_ij(P.ifloat, P.jfloat);
474  if (vpImagePoint::distance(begin[0], pt) < threshold)
475  break;
476  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
477  P.track(I, me, false);
478 
479  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
480  list.push_back(P);
481  endPtAdded = true;
482  if (vpDEBUG_ENABLE(3)) {
484  }
485  } else {
486  if (vpDEBUG_ENABLE(3)) {
488  }
489  }
490  }
491  }
492  if (!endPtAdded)
493  endPtFound++;
494  me->setRange(memory_range);
495  } else {
496  list.pop_front();
497  }
498  /*if(begin != NULL)*/ delete[] begin;
499  /*if(end != NULL) */ delete[] end;
500 }
501 
513 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
515 #else
517 #endif
518 {
519 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
520  vpMeSite pt = list.front();
521  vpImagePoint firstPoint(pt.ifloat, pt.jfloat);
522  pt = list.back();
523  vpImagePoint lastPoint(pt.ifloat, pt.jfloat);
524  if (beginPtFound >= 3 && farFromImageEdge(I, firstPoint)) {
525  vpImagePoint *begin = NULL;
526  begin = nurbs.computeCurveDersPoint(0.0, 1);
527  vpImage<unsigned char> Isub(32, 32); // Sub image.
528  vpImagePoint topLeft(begin[0].get_i() - 15, begin[0].get_j() - 15);
529  vpRect rect(topLeft, 32, 32);
530 
532 
533  vpImageTools::crop(I, rect, Isub);
534 
535  vpImagePoint lastPtInSubIm(begin[0]);
536  double u = 0.0;
537  double step = 0.0001;
538  // Find the point of the nurbs closest from the edge of the subImage and
539  // in the subImage.
540  while (inRectangle(lastPtInSubIm, rect) && u < 1) {
541  u += step;
542  lastPtInSubIm = nurbs.computeCurvePoint(u);
543  }
544 
545  u -= step;
546  if (u > 0)
547  lastPtInSubIm = nurbs.computeCurvePoint(u);
548 
549 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
550  vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
551 #else
552  IplImage *Ip = NULL;
553  vpImageConvert::convert(Isub, Ip);
554 
555  IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
556  cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
557 
558  vpImageConvert::convert(dst, Isub);
559 #endif
560 
561  vpImagePoint firstBorder(-1, -1);
562 
563  firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
564 
565  std::list<vpImagePoint> ip_edges_list;
566  if (firstBorder != vpImagePoint(-1, -1)) {
567  unsigned int dir;
568  double fi = firstBorder.get_i();
569  double fj = firstBorder.get_j();
570  double w = Isub.getWidth() - 1;
571  double h = Isub.getHeight() - 1;
572  // if (firstBorder.get_i() == 0) dir = 4;
573  if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
574  dir = 4;
575  // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
576  else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
577  dir = 0;
578  // else if (firstBorder.get_j() == 0) dir = 2;
579  else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
580  dir = 2;
581  // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
582  else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
583  dir = 6;
584  computeFreemanChainElement(Isub, firstBorder, dir);
585  unsigned int firstDir = dir;
586  ip_edges_list.push_back(firstBorder);
587  vpImagePoint border(firstBorder);
588  vpImagePoint dBorder;
589  do {
590  computeFreemanParameters(dir, dBorder);
591  border = border + dBorder;
592  vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
593 
594  ip_edges_list.push_back(border);
595 
596  computeFreemanChainElement(Isub, border, dir);
597  } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
598  }
599 
600  if (findCenterPoint(&ip_edges_list)) {
601  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();
602  /*++it*/) {
603  vpMeSite s = *it;
604  vpImagePoint iP(s.ifloat, s.jfloat);
605  if (inRectangle(iP, rect))
606  it = list.erase(it);
607  else
608  break;
609  }
610 
611  std::list<vpMeSite>::iterator itList = list.begin();
612  double convlt;
613  double delta = 0;
614  int nbr = 0;
615  std::list<vpMeSite> addedPt;
616  for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
617  ++itEdges) {
618  vpMeSite s = *itList;
619  vpImagePoint iPtemp = *itEdges + topLeft;
620  vpMeSite pix;
621  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
622  dist = vpMeSite::sqrDistance(s, pix);
623  if (dist >= vpMath::sqr(me->getSampleStep()) /*25*/) {
624  bool exist = false;
625  for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
626  dist = vpMeSite::sqrDistance(pix, *itAdd);
627  if (dist < vpMath::sqr(me->getSampleStep()) /*25*/)
628  exist = true;
629  }
630  if (!exist) {
631  findAngle(I, iPtemp, me, delta, convlt);
632  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
634  --itList;
635  list.insert(itList, pix);
636  ++itList;
637  addedPt.push_front(pix);
638  nbr++;
639  }
640  }
641  }
642 
643  unsigned int memory_range = me->getRange();
644  me->setRange(3);
645  std::list<vpMeSite>::iterator itList2 = list.begin();
646  for (int j = 0; j < nbr; j++) {
647  vpMeSite s = *itList2;
648  s.track(I, me, false);
649  *itList2 = s;
650  ++itList2;
651  }
652  me->setRange(memory_range);
653  }
654 
655  /* if (begin != NULL) */ delete[] begin;
656  beginPtFound = 0;
657  }
658 
659  if (endPtFound >= 3 && farFromImageEdge(I, lastPoint)) {
660  vpImagePoint *end = NULL;
661  end = nurbs.computeCurveDersPoint(1.0, 1);
662 
663  vpImage<unsigned char> Isub(32, 32); // Sub image.
664  vpImagePoint topLeft(end[0].get_i() - 15, end[0].get_j() - 15);
665  vpRect rect(topLeft, 32, 32);
666 
668 
669  vpImageTools::crop(I, rect, Isub);
670 
671  vpImagePoint lastPtInSubIm(end[0]);
672  double u = 1.0;
673  double step = 0.0001;
674  // Find the point of the nurbs closest from the edge of the subImage and
675  // in the subImage.
676  while (inRectangle(lastPtInSubIm, rect) && u > 0) {
677  u -= step;
678  lastPtInSubIm = nurbs.computeCurvePoint(u);
679  }
680 
681  u += step;
682  if (u < 1.0)
683  lastPtInSubIm = nurbs.computeCurvePoint(u);
684 
685 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
686  vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
687 #else
688  IplImage *Ip = NULL;
689  vpImageConvert::convert(Isub, Ip);
690 
691  IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
692  cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
693 
694  vpImageConvert::convert(dst, Isub);
695 #endif
696 
697  vpImagePoint firstBorder(-1, -1);
698 
699  firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
700 
701  std::list<vpImagePoint> ip_edges_list;
702  if (firstBorder != vpImagePoint(-1, -1)) {
703  unsigned int dir;
704  double fi = firstBorder.get_i();
705  double fj = firstBorder.get_j();
706  double w = Isub.getWidth() - 1;
707  double h = Isub.getHeight() - 1;
708  // if (firstBorder.get_i() == 0) dir = 4;
709  if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
710  dir = 4;
711  // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
712  else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
713  dir = 0;
714  // else if (firstBorder.get_j() == 0) dir = 2;
715  else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
716  dir = 2;
717  // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
718  else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
719  dir = 6;
720 
721  computeFreemanChainElement(Isub, firstBorder, dir);
722  unsigned int firstDir = dir;
723  ip_edges_list.push_back(firstBorder);
724  vpImagePoint border(firstBorder);
725  vpImagePoint dBorder;
726  do {
727  computeFreemanParameters(dir, dBorder);
728  border = border + dBorder;
729  vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
730 
731  ip_edges_list.push_back(border);
732 
733  computeFreemanChainElement(Isub, border, dir);
734  } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
735  }
736 
737  if (findCenterPoint(&ip_edges_list)) {
738  // list.end();
739  vpMeSite s;
740  while (true) //{//!list.outside())
741  {
742  s = list.back(); // list.value() ;
743  vpImagePoint iP(s.ifloat, s.jfloat);
744  if (inRectangle(iP, rect)) {
745  list.erase(list.end());
746  // list.end();
747  } else
748  break;
749  }
750 
751  std::list<vpMeSite>::iterator itList = list.end();
752  --itList; // Move on the last element
753  double convlt;
754  double delta;
755  int nbr = 0;
756  std::list<vpMeSite> addedPt;
757  for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
758  ++itEdges) {
759  s = *itList;
760  vpImagePoint iPtemp = *itEdges + topLeft;
761  vpMeSite pix;
762  pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
763  dist = vpMeSite::sqrDistance(s, pix);
764  if (dist >= vpMath::sqr(me->getSampleStep())) {
765  bool exist = false;
766  for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
767  dist = vpMeSite::sqrDistance(pix, *itAdd);
768  if (dist < vpMath::sqr(me->getSampleStep()))
769  exist = true;
770  }
771  if (!exist) {
772  findAngle(I, iPtemp, me, delta, convlt);
773  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
775  list.push_back(pix);
776  addedPt.push_back(pix);
777  nbr++;
778  }
779  }
780  }
781 
782  unsigned int memory_range = me->getRange();
783  me->setRange(3);
784  std::list<vpMeSite>::iterator itList2 = list.end();
785  --itList2; // Move to the last element
786  for (int j = 0; j < nbr; j++) {
787  vpMeSite me_s = *itList2;
788  me_s.track(I, me, false);
789  *itList2 = me_s;
790  --itList2;
791  }
792  me->setRange(memory_range);
793  }
794 
795  /* if (end != NULL) */ delete[] end;
796  endPtFound = 0;
797  }
798 #else
799  vpTRACE("To use the canny detection, OpenCV has to be installed.");
800 #endif
801 }
802 
814 {
815  unsigned int n = numberOfSignal();
816  double nbPt = floor(dist / me->getSampleStep());
817 
818  if ((double)n < 0.7 * nbPt) {
819  sample(I);
821  }
822 }
823 
832 {
833  int rows = (int)I.getHeight();
834  int cols = (int)I.getWidth();
835  vpImagePoint *iP = NULL;
836 
837  int n = (int)numberOfSignal();
838 
839  // list.front();
840  std::list<vpMeSite>::iterator it = list.begin();
841  std::list<vpMeSite>::iterator itNext = list.begin();
842  ++itNext;
843 
844  unsigned int range_tmp = me->getRange();
845  me->setRange(2);
846 
847  while (itNext != list.end() && n <= me->getPointsToTrack()) {
848  vpMeSite s = *it; // current reference pixel
849  vpMeSite s_next = *itNext; // current reference pixel
850 
851  double d = vpMeSite::sqrDistance(s, s_next);
852  if (d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600) {
853  vpImagePoint iP0(s.ifloat, s.jfloat);
854  vpImagePoint iPend(s_next.ifloat, s_next.jfloat);
855  vpImagePoint iP_1(s.ifloat, s.jfloat);
856 
857  double u = 0.0;
858  double ubegin = 0.0;
859  double uend = 0.0;
860  double dmin1_1 = 1e6;
861  double dmin2_1 = 1e6;
862  while (u < 1) {
863  u += 0.01;
864  double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iP0);
865  double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iPend);
866 
867  if (dmin1 < dmin1_1) {
868  dmin1_1 = dmin1;
869  ubegin = u;
870  }
871 
872  if (dmin2 < dmin2_1) {
873  dmin2_1 = dmin2;
874  uend = u;
875  }
876  }
877  u = ubegin;
878 
879  // if(( u != 1.0 || uend != 1.0)
880  if ((std::fabs(u - 1.0) > std::fabs(vpMath::maximum(u, 1.0)) * std::numeric_limits<double>::epsilon()) ||
881  (std::fabs(uend - 1.0) > std::fabs(vpMath::maximum(uend, 1.0)) * std::numeric_limits<double>::epsilon())) {
882  iP = nurbs.computeCurveDersPoint(u, 1);
883 
884  while (vpImagePoint::sqrDistance(iP[0], iPend) > vpMath::sqr(me->getSampleStep()) && u < uend) {
885  u += 0.01;
886  /*if (iP!=NULL)*/ {
887  delete[] iP;
888  iP = NULL;
889  }
890  iP = nurbs.computeCurveDersPoint(u, 1);
891  if (vpImagePoint::sqrDistance(iP[0], iP_1) > vpMath::sqr(me->getSampleStep()) &&
892  !outOfImage(iP[0], 0, rows, cols)) {
893  double delta = computeDelta(iP[1].get_i(), iP[1].get_j());
894  vpMeSite pix; //= list.value();
895  pix.init(iP[0].get_i(), iP[0].get_j(), delta);
897  pix.track(I, me, false);
898  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
899  list.insert(it, pix);
900  iP_1 = iP[0];
901  }
902  }
903  }
904  /*if (iP!=NULL)*/ {
905  delete[] iP;
906  iP = NULL;
907  }
908  }
909  }
910  ++it;
911  ++itNext;
912  }
913  me->setRange(range_tmp);
914 }
915 
922 {
923 #if 0
924  // Loop through list of sites to track
925  list.front();
926  while(!list.nextOutside())
927  {
928  vpMeSite s = list.value() ;//current reference pixel
929  vpMeSite s_next = list.nextValue() ;//current reference pixel
930 
931  if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep()))
932  {
934 
935  list.next();
936  list.modify(s_next);
937  if (!list.nextOutside()) list.next();
938  }
939  else
940  list.next() ;
941  }
942 #endif
943  std::list<vpMeSite>::const_iterator it = list.begin();
944  std::list<vpMeSite>::iterator itNext = list.begin();
945  ++itNext;
946  for (; itNext != list.end();) {
947  vpMeSite s = *it; // current reference pixel
948  vpMeSite s_next = *itNext; // current reference pixel
949 
950  if (vpMeSite::sqrDistance(s, s_next) < vpMath::sqr(me->getSampleStep())) {
952 
953  *itNext = s_next;
954  ++it;
955  ++itNext;
956  if (itNext != list.end()) {
957  ++it;
958  ++itNext;
959  }
960  } else {
961  ++it;
962  ++itNext;
963  }
964  }
965 }
966 
973 {
974  // Tracking des vpMeSites
976 
977  // Suppress points which are too close to each other
979 
980  // Suppressions des points ejectes par le tracking
981  suppressPoints();
982 
983  if (list.size() == 1)
984  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
985 
986  // Recalcule les parametres
987  // nurbs.globalCurveInterp(list);
988  nurbs.globalCurveApprox(list, nbControlPoints);
989 
990  // On resample localement
991  localReSample(I);
992 
993  seekExtremities(I);
994  if (enableCannyDetection)
996 
997  // nurbs.globalCurveInterp(list);
998  nurbs.globalCurveApprox(list, nbControlPoints);
999 
1000  double u = 0.0;
1001  vpImagePoint pt;
1002  vpImagePoint pt_1;
1003  dist = 0;
1004  while (u <= 1.0) {
1005  pt = nurbs.computeCurvePoint(u);
1006  // if(u!=0)
1007  if (std::fabs(u) > std::numeric_limits<double>::epsilon())
1008  dist = dist + vpImagePoint::distance(pt, pt_1);
1009  pt_1 = pt;
1010  u = u + 0.01;
1011  }
1012 
1013  updateDelta();
1014 
1015  reSample(I);
1016 }
1017 
1029 
1046 bool vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I, vpImagePoint &iP, unsigned int &element)
1047 {
1048  vpImagePoint diP;
1049  vpImagePoint iPtemp;
1050  if (hasGoodLevel(I, iP)) {
1051  // get the point on the right of the point passed in
1052  computeFreemanParameters((element + 2) % 8, diP);
1053  iPtemp = iP + diP;
1054  if (hasGoodLevel(I, iPtemp)) {
1055  element = (element + 2) % 8; // turn right
1056  } else {
1057  computeFreemanParameters((element + 1) % 8, diP);
1058  iPtemp = iP + diP;
1059 
1060  if (hasGoodLevel(I, iPtemp)) {
1061  element = (element + 1) % 8; // turn diag right
1062  } else {
1063  computeFreemanParameters(element, diP);
1064  iPtemp = iP + diP;
1065 
1066  if (hasGoodLevel(I, iPtemp)) {
1067  // element = element; // keep same dir
1068  } else {
1069  computeFreemanParameters((element + 7) % 8, diP);
1070  iPtemp = iP + diP;
1071 
1072  if (hasGoodLevel(I, iPtemp)) {
1073  element = (element + 7) % 8; // turn diag left
1074  } else {
1075  computeFreemanParameters((element + 6) % 8, diP);
1076  iPtemp = iP + diP;
1077 
1078  if (hasGoodLevel(I, iPtemp)) {
1079  element = (element + 6) % 8; // turn left
1080  } else {
1081  computeFreemanParameters((element + 5) % 8, diP);
1082  iPtemp = iP + diP;
1083 
1084  if (hasGoodLevel(I, iPtemp)) {
1085  element = (element + 5) % 8; // turn diag down
1086  } else {
1087  computeFreemanParameters((element + 4) % 8, diP);
1088  iPtemp = iP + diP;
1089 
1090  if (hasGoodLevel(I, iPtemp)) {
1091  element = (element + 4) % 8; // turn down
1092  } else {
1093  computeFreemanParameters((element + 3) % 8, diP);
1094  iPtemp = iP + diP;
1095 
1096  if (hasGoodLevel(I, iPtemp)) {
1097  element = (element + 3) % 8; // turn diag right down
1098  } else {
1099  // No neighbor with a good level
1100  //
1101  return false;
1102  }
1103  }
1104  }
1105  }
1106  }
1107  }
1108  }
1109  }
1110  } else {
1111  return false;
1112  }
1113  return true;
1114 }
1115 
1128 bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1129 {
1130  if (!isInImage(I, iP))
1131  return false;
1132 
1133  if (I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0) {
1134  return true;
1135  } else {
1136  return false;
1137  }
1138 }
1139 
1150 bool vpMeNurbs::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
1151 {
1152  return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && iP.get_j() < I.getWidth());
1153 }
1154 
1172 void vpMeNurbs::computeFreemanParameters(unsigned int element, vpImagePoint &diP)
1173 {
1174  /*
1175  5 6 7
1176  \ | /
1177  \|/
1178  4 ------- 0
1179  /|\
1180  / | \
1181  3 2 1
1182  */
1183  switch (element) {
1184  case 0: // go right
1185  diP.set_ij(0, 1);
1186  break;
1187 
1188  case 1: // go right top
1189  diP.set_ij(1, 1);
1190  break;
1191 
1192  case 2: // go top
1193  diP.set_ij(1, 0);
1194  break;
1195 
1196  case 3:
1197  diP.set_ij(1, -1);
1198  break;
1199 
1200  case 4:
1201  diP.set_ij(0, -1);
1202  break;
1203 
1204  case 5:
1205  diP.set_ij(-1, -1);
1206  break;
1207 
1208  case 6:
1209  diP.set_ij(-1, 0);
1210  break;
1211 
1212  case 7:
1213  diP.set_ij(-1, 1);
1214  break;
1215  }
1216 }
1217 
1227 bool vpMeNurbs::farFromImageEdge(const vpImage<unsigned char> &I, const vpImagePoint &iP)
1228 {
1229  unsigned int height = I.getHeight();
1230  unsigned int width = I.getWidth();
1231  return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20);
1232 }
1233 
1245 {
1246  double u = 0.0;
1247  vpImagePoint pt;
1248  while (u <= 1) {
1249  pt = n.computeCurvePoint(u);
1250  vpDisplay::displayCross(I, pt, 4, color);
1251  u += 0.01;
1252  }
1253 }
1254 
1266 {
1267  double u = 0.0;
1268  vpImagePoint pt;
1269  while (u <= 1) {
1270  pt = n.computeCurvePoint(u);
1271  vpDisplay::displayCross(I, pt, 4, color);
1272  u += 0.01;
1273  }
1274 }
void suppressPoints()
Definition: vpMeNurbs.cpp:335
double get_i() const
Definition: vpImagePoint.h:203
virtual ~vpMeNurbs()
Definition: vpMeNurbs.cpp:240
vpMeSiteState getState() const
Definition: vpMeSite.h:190
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void init()
Definition: vpMeSite.cpp:66
double jfloat
Definition: vpMeSite.h:89
#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:234
void seekExtremitiesCanny(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:516
Provide simple list management.
Definition: vpList.h:113
Point removed because too near image borders.
Definition: vpMeSite.h:82
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 RGB colors available for display functionnalities.
Definition: vpColor.h:157
void track(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:972
double alpha
Definition: vpMeSite.h:93
int i
Definition: vpMeSite.h:87
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:220
vpMatrix * getMask() const
Definition: vpMe.h:120
static void flush(const vpImage< unsigned char > &I)
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:160
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static const vpColor orange
Definition: vpColor.h:227
double ifloat
Definition: vpMeSite.h:89
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
Error that can be emited by the vpTracker class and its derivates.
void supressNearPoints()
Definition: vpMeNurbs.cpp:921
Point used by the tracker.
Definition: vpMeSite.h:78
double getSampleStep() const
Definition: vpMe.h:285
int outOfImage(int i, int j, int half, int row, int cols)
static double sqrDistance(const vpImagePoint &iP1, const vpImagePoint &iP2)
#define vpTRACE
Definition: vpDebug.h:416
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
static double sqr(double x)
Definition: vpMath.h:116
void display(const vpImage< unsigned char > &I, vpColor col)
Definition: vpMeNurbs.cpp:1028
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:214
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
static void canny(const vpImage< unsigned char > &I, vpImage< unsigned char > &Ic, unsigned int gaussianFilterSize, double thresholdCanny, unsigned int apertureSobel)
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:813
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 setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
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:110
void localReSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:831
static int round(double x)
Definition: vpMath.h:247
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:87
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
unsigned int getHeight() const
Definition: vpImage.h:188
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:305
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
void seekExtremities(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:395
void initTracking(const vpImage< unsigned char > &I)
Defines a rectangle in the plane.
Definition: vpRect.h:79
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:87
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
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
unsigned int getWidth() const
Definition: vpImage.h:246
static int() sign(double x)
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
virtual void sample(const vpImage< unsigned char > &image, bool doNotTrack=false)
Definition: vpMeNurbs.cpp:294
unsigned int getAngleStep() const
Definition: vpMe.h:114
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:223
void updateDelta()
Definition: vpMeNurbs.cpp:351