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