Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
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)m_me->getPointsToTrack();
239 
240  // Delete old list
241  m_meList.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;
256  pix.init(pt[0].get_i(), pt[0].get_j(), delta);
258 
259  m_meList.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 = m_meList.begin(); it != m_meList.end();) {
271  vpMeSite s = *it; // current reference pixel
272 
273  if (s.getState() != vpMeSite::NO_SUPPRESSION) {
274  it = m_meList.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 = m_meList.begin();
287 
288  vpImagePoint Cu;
289  vpImagePoint *der = nullptr;
290  double step = 0.01;
291  while (u < 1 && it != m_meList.end()) {
292  vpMeSite s = *it;
293  vpImagePoint pt(s.get_i(), s.get_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.setAlpha(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 * m_me->getSampleStep();
332  double sample_step = m_me->getSampleStep();
333  vpImagePoint pt;
334  if (d > threshold /*|| (m_meList.firstValue()).m_mask_sign != (m_meList.lastValue()).m_mask_sign*/) {
335  vpMeSite P;
336 
337  // Init vpMeSite
338  P.init(begin[0].get_i(), begin[0].get_j(), (m_meList.front()).getAlpha(), 0, (m_meList.front()).m_mask_sign);
340 
341  // Set the range
342  unsigned int memory_range = m_me->getRange();
343  m_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.m_ifloat = P.m_ifloat - si * sample_step;
355  P.m_i = (int)P.m_ifloat;
356  P.m_jfloat = P.m_jfloat - co * sample_step;
357  P.m_j = (int)P.m_jfloat;
358  pt.set_ij(P.m_ifloat, P.m_jfloat);
359  if (vpImagePoint::distance(end[0], pt) < threshold)
360  break;
361  if (!outOfImage(P.get_i(), P.get_j(), 5, rows, cols)) {
362  P.track(I, m_me, false);
363 
364  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
365  m_meList.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(), (m_meList.back()).getAlpha(), 0, (m_meList.back()).m_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.m_ifloat = P.m_ifloat + si * sample_step;
394  P.m_i = (int)P.m_ifloat;
395  P.m_jfloat = P.m_jfloat + co * sample_step;
396  P.m_j = (int)P.m_jfloat;
397  pt.set_ij(P.m_ifloat, P.m_jfloat);
398  if (vpImagePoint::distance(begin[0], pt) < threshold)
399  break;
400  if (!outOfImage(P.get_i(), P.get_j(), 5, rows, cols)) {
401  P.track(I, m_me, false);
402 
403  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
404  m_meList.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  m_me->setRange(memory_range);
420  }
421  else {
422  m_meList.pop_front();
423  }
424  /*if(begin != nullptr)*/ delete[] begin;
425  /*if(end != nullptr) */ delete[] end;
426 }
427 
429 {
430  vpMeSite pt = m_meList.front();
431  vpImagePoint firstPoint(pt.m_ifloat, pt.m_jfloat);
432  pt = m_meList.back();
433  vpImagePoint lastPoint(pt.m_ifloat, pt.m_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 = m_meList.begin(); it != m_meList.end();
502  /*++it*/) {
503  vpMeSite s = *it;
504  vpImagePoint iP(s.m_ifloat, s.m_jfloat);
505  if (inRectangle(iP, rect))
506  it = m_meList.erase(it);
507  else
508  break;
509  }
510 
511  std::list<vpMeSite>::iterator itList = m_meList.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(m_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(m_me->getSampleStep()) /*25*/)
528  exist = true;
529  }
530  if (!exist) {
531  findAngle(I, iPtemp, m_me, delta, convlt);
532  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
534  --itList;
535  m_meList.insert(itList, pix);
536  ++itList;
537  addedPt.push_front(pix);
538  nbr++;
539  }
540  }
541  }
542 
543  unsigned int memory_range = m_me->getRange();
544  m_me->setRange(3);
545  std::list<vpMeSite>::iterator itList2 = m_meList.begin();
546  for (unsigned int j = 0; j < nbr; j++) {
547  vpMeSite s = *itList2;
548  s.track(I, m_me, false);
549  *itList2 = s;
550  ++itList2;
551  }
552  m_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  vpMeSite s;
629 
630  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it!=m_meList.end(); ++it) {
631  s = *it;
632  vpImagePoint iP(s.m_ifloat, s.m_jfloat);
633  if (inRectangle(iP, rect)) {
634  m_meList.erase(it);
635  }
636  else
637  break;
638  }
639 
640  std::list<vpMeSite>::iterator itList = m_meList.end();
641  --itList; // Move on the last element
642  double convlt;
643  double delta;
644  unsigned int nbr = 0;
645  std::list<vpMeSite> addedPt;
646  for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
647  ++itEdges) {
648  s = *itList;
649  vpImagePoint iPtemp = *itEdges + topLeft;
650  vpMeSite pix;
651  pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
652  dist = vpMeSite::sqrDistance(s, pix);
653  if (dist >= vpMath::sqr(m_me->getSampleStep())) {
654  bool exist = false;
655  for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
656  dist = vpMeSite::sqrDistance(pix, *itAdd);
657  if (dist < vpMath::sqr(m_me->getSampleStep()))
658  exist = true;
659  }
660  if (!exist) {
661  findAngle(I, iPtemp, m_me, delta, convlt);
662  pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
664  m_meList.push_back(pix);
665  addedPt.push_back(pix);
666  nbr++;
667  }
668  }
669  }
670 
671  unsigned int memory_range = m_me->getRange();
672  m_me->setRange(3);
673  std::list<vpMeSite>::iterator itList2 = m_meList.end();
674  --itList2; // Move to the last element
675  for (unsigned int j = 0; j < nbr; j++) {
676  vpMeSite me_s = *itList2;
677  me_s.track(I, m_me, false);
678  *itList2 = me_s;
679  --itList2;
680  }
681  m_me->setRange(memory_range);
682  }
683 
684  /* if (end != nullptr) */ delete[] end;
685  endPtFound = 0;
686  }
687 }
688 
690 {
691  unsigned int n = numberOfSignal();
692  double nbPt = floor(dist / m_me->getSampleStep());
693 
694  if ((double)n < 0.7 * nbPt) {
695  sample(I);
697  }
698 }
699 
701 {
702  int rows = (int)I.getHeight();
703  int cols = (int)I.getWidth();
704  vpImagePoint *iP = nullptr;
705 
706  int n = (int)numberOfSignal();
707 
708  std::list<vpMeSite>::iterator it = m_meList.begin();
709  std::list<vpMeSite>::iterator itNext = m_meList.begin();
710  ++itNext;
711 
712  unsigned int range_tmp = m_me->getRange();
713  m_me->setRange(2);
714 
715  while (itNext != m_meList.end() && n <= m_me->getPointsToTrack()) {
716  vpMeSite s = *it; // current reference pixel
717  vpMeSite s_next = *itNext; // current reference pixel
718 
719  double d = vpMeSite::sqrDistance(s, s_next);
720  if (d > 4 * vpMath::sqr(m_me->getSampleStep()) && d < 1600) {
721  vpImagePoint iP0(s.m_ifloat, s.m_jfloat);
722  vpImagePoint iPend(s_next.m_ifloat, s_next.m_jfloat);
723  vpImagePoint iP_1(s.m_ifloat, s.m_jfloat);
724 
725  double u = 0.0;
726  double ubegin = 0.0;
727  double uend = 0.0;
728  double dmin1_1 = 1e6;
729  double dmin2_1 = 1e6;
730  while (u < 1) {
731  u += 0.01;
732  double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iP0);
733  double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iPend);
734 
735  if (dmin1 < dmin1_1) {
736  dmin1_1 = dmin1;
737  ubegin = u;
738  }
739 
740  if (dmin2 < dmin2_1) {
741  dmin2_1 = dmin2;
742  uend = u;
743  }
744  }
745  u = ubegin;
746 
747  // if(( u != 1.0 || uend != 1.0)
748  if ((std::fabs(u - 1.0) > std::fabs(vpMath::maximum(u, 1.0)) * std::numeric_limits<double>::epsilon()) ||
749  (std::fabs(uend - 1.0) > std::fabs(vpMath::maximum(uend, 1.0)) * std::numeric_limits<double>::epsilon())) {
750  iP = nurbs.computeCurveDersPoint(u, 1);
751 
752  while (vpImagePoint::sqrDistance(iP[0], iPend) > vpMath::sqr(m_me->getSampleStep()) && u < uend) {
753  u += 0.01;
754  /*if (iP!=nullptr)*/ {
755  delete[] iP;
756  iP = nullptr;
757  }
758  iP = nurbs.computeCurveDersPoint(u, 1);
759  if (vpImagePoint::sqrDistance(iP[0], iP_1) > vpMath::sqr(m_me->getSampleStep()) &&
760  !outOfImage(iP[0], 0, rows, cols)) {
761  double delta = computeDelta(iP[1].get_i(), iP[1].get_j());
762  vpMeSite pix;
763  pix.init(iP[0].get_i(), iP[0].get_j(), delta);
765  pix.track(I, m_me, false);
766  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
767  m_meList.insert(it, pix);
768  iP_1 = iP[0];
769  }
770  }
771  }
772  /*if (iP!=nullptr)*/ {
773  delete[] iP;
774  iP = nullptr;
775  }
776  }
777  }
778  ++it;
779  ++itNext;
780  }
781  m_me->setRange(range_tmp);
782 }
783 
785 {
786  std::list<vpMeSite>::const_iterator it = m_meList.begin();
787  std::list<vpMeSite>::iterator itNext = m_meList.begin();
788  ++itNext;
789  for (; itNext != m_meList.end();) {
790  vpMeSite s = *it; // current reference pixel
791  vpMeSite s_next = *itNext; // current reference pixel
792 
793  if (vpMeSite::sqrDistance(s, s_next) < vpMath::sqr(m_me->getSampleStep())) {
795 
796  *itNext = s_next;
797  ++it;
798  ++itNext;
799  if (itNext != m_meList.end()) {
800  ++it;
801  ++itNext;
802  }
803  }
804  else {
805  ++it;
806  ++itNext;
807  }
808  }
809 }
810 
812 {
813  // Tracking des vpMeSites
815 
816  // Suppress points which are too close to each other
818 
819  // Suppressions des points ejectes par le tracking
820  suppressPoints();
821 
822  if (m_meList.size() == 1)
823  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
824 
825  // Recalcule les parametres
826  // nurbs.globalCurveInterp(m_meList);
827  nurbs.globalCurveApprox(m_meList, nbControlPoints);
828 
829  // On resample localement
830  localReSample(I);
831 
832  seekExtremities(I);
833  if (enableCannyDetection)
835 
836  // nurbs.globalCurveInterp(m_meList);
837  nurbs.globalCurveApprox(m_meList, nbControlPoints);
838 
839  double u = 0.0;
840  vpImagePoint pt;
841  vpImagePoint pt_1;
842  dist = 0;
843  while (u <= 1.0) {
844  pt = nurbs.computeCurvePoint(u);
845  // if(u!=0)
846  if (std::fabs(u) > std::numeric_limits<double>::epsilon())
847  dist = dist + vpImagePoint::distance(pt, pt_1);
848  pt_1 = pt;
849  u = u + 0.01;
850  }
851 
852  updateDelta();
853 
854  reSample(I);
855 }
856 
857 void vpMeNurbs::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
858 {
859  vpMeNurbs::display(I, nurbs, color, thickness);
860 }
861 
878 bool vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I, vpImagePoint &iP, unsigned int &element)
879 {
880  vpImagePoint diP;
881  vpImagePoint iPtemp;
882  if (hasGoodLevel(I, iP)) {
883  // get the point on the right of the point passed in
884  computeFreemanParameters((element + 2) % 8, diP);
885  iPtemp = iP + diP;
886  if (hasGoodLevel(I, iPtemp)) {
887  element = (element + 2) % 8; // turn right
888  }
889  else {
890  computeFreemanParameters((element + 1) % 8, diP);
891  iPtemp = iP + diP;
892 
893  if (hasGoodLevel(I, iPtemp)) {
894  element = (element + 1) % 8; // turn diag right
895  }
896  else {
897  computeFreemanParameters(element, diP);
898  iPtemp = iP + diP;
899 
900  if (hasGoodLevel(I, iPtemp)) {
901  // element = element; // keep same dir
902  }
903  else {
904  computeFreemanParameters((element + 7) % 8, diP);
905  iPtemp = iP + diP;
906 
907  if (hasGoodLevel(I, iPtemp)) {
908  element = (element + 7) % 8; // turn diag left
909  }
910  else {
911  computeFreemanParameters((element + 6) % 8, diP);
912  iPtemp = iP + diP;
913 
914  if (hasGoodLevel(I, iPtemp)) {
915  element = (element + 6) % 8; // turn left
916  }
917  else {
918  computeFreemanParameters((element + 5) % 8, diP);
919  iPtemp = iP + diP;
920 
921  if (hasGoodLevel(I, iPtemp)) {
922  element = (element + 5) % 8; // turn diag down
923  }
924  else {
925  computeFreemanParameters((element + 4) % 8, diP);
926  iPtemp = iP + diP;
927 
928  if (hasGoodLevel(I, iPtemp)) {
929  element = (element + 4) % 8; // turn down
930  }
931  else {
932  computeFreemanParameters((element + 3) % 8, diP);
933  iPtemp = iP + diP;
934 
935  if (hasGoodLevel(I, iPtemp)) {
936  element = (element + 3) % 8; // turn diag right down
937  }
938  else {
939  // No neighbor with a good level
940  //
941  return false;
942  }
943  }
944  }
945  }
946  }
947  }
948  }
949  }
950  }
951  else {
952  return false;
953  }
954  return true;
955 }
956 
969 bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
970 {
971  if (!isInImage(I, iP))
972  return false;
973 
974  if (I((unsigned int)vpMath::round(iP.get_i()), (unsigned int)vpMath::round(iP.get_j())) > 0) {
975  return true;
976  }
977  else {
978  return false;
979  }
980 }
981 
992 bool vpMeNurbs::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
993 {
994  return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && iP.get_j() < I.getWidth());
995 }
996 
1014 void vpMeNurbs::computeFreemanParameters(unsigned int element, vpImagePoint &diP)
1015 {
1016  /*
1017  5 6 7
1018  \ | /
1019  \|/
1020  4 ------- 0
1021  /|\
1022  / | \
1023  3 2 1
1024  */
1025  switch (element) {
1026  case 0: // go right
1027  diP.set_ij(0, 1);
1028  break;
1029 
1030  case 1: // go right top
1031  diP.set_ij(1, 1);
1032  break;
1033 
1034  case 2: // go top
1035  diP.set_ij(1, 0);
1036  break;
1037 
1038  case 3:
1039  diP.set_ij(1, -1);
1040  break;
1041 
1042  case 4:
1043  diP.set_ij(0, -1);
1044  break;
1045 
1046  case 5:
1047  diP.set_ij(-1, -1);
1048  break;
1049 
1050  case 6:
1051  diP.set_ij(-1, 0);
1052  break;
1053 
1054  case 7:
1055  diP.set_ij(-1, 1);
1056  break;
1057  }
1058 }
1059 
1069 bool vpMeNurbs::farFromImageEdge(const vpImage<unsigned char> &I, const vpImagePoint &iP)
1070 {
1071  unsigned int height = I.getHeight();
1072  unsigned int width = I.getWidth();
1073  return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20);
1074 }
1075 
1076 void vpMeNurbs::display(const vpImage<unsigned char> &I, vpNurbs &n, const vpColor &color, unsigned int thickness)
1077 {
1078  double u = 0.0;
1079  vpImagePoint pt;
1080  while (u <= 1) {
1081  pt = n.computeCurvePoint(u);
1082  vpDisplay::displayCross(I, pt, 4, color, thickness);
1083  u += 0.01;
1084  }
1085 }
1086 
1087 void vpMeNurbs::display(const vpImage<vpRGBa> &I, vpNurbs &n, const vpColor &color, unsigned int thickness)
1088 {
1089  double u = 0.0;
1090  vpImagePoint pt;
1091  while (u <= 1) {
1092  pt = n.computeCurvePoint(u);
1093  vpDisplay::displayCross(I, pt, 4, color, thickness);
1094  u += 0.01;
1095  }
1096 }
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:315
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:305
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
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:811
void reSample(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:689
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:700
void display(const vpImage< unsigned char > &I, const vpColor &color, unsigned int thickness=1)
Definition: vpMeNurbs.cpp:857
void seekExtremitiesCanny(const vpImage< unsigned char > &I)
Definition: vpMeNurbs.cpp:428
void suppressPoints()
Definition: vpMeNurbs.cpp:268
void supressNearPoints()
Definition: vpMeNurbs.cpp:784
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
@ TOO_NEAR
Point not tracked anymore, since too near from its neighbor.
Definition: vpMeSite.h:90
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:83
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:247
double m_ifloat
Subpixel coordinates along i of a site.
Definition: vpMeSite.h:100
void setAlpha(const double &a)
Definition: vpMeSite.h:242
void init()
Definition: vpMeSite.cpp:55
vpMeSiteState getState() const
Definition: vpMeSite.h:266
int m_j
Integer coordinates along j of a site.
Definition: vpMeSite.h:98
int get_j() const
Definition: vpMeSite.h:186
int m_i
Integer coordinate along i of a site.
Definition: vpMeSite.h:96
double m_jfloat
Subpixel coordinates along j of a site.
Definition: vpMeSite.h:102
int get_i() const
Definition: vpMeSite.h:180
static double sqrDistance(const vpMeSite &S1, const vpMeSite &S2)
Definition: vpMeSite.h:349
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
Definition: vpMeSite.cpp:267
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:256
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:60
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
Definition: vpMeTracker.cpp:95
vpMeSite::vpMeSiteDisplayType m_selectDisplay
Moving-edges display type.
Definition: vpMeTracker.h:78
void track(const vpImage< unsigned char > &I)
vpMe * m_me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:68
bool outOfImage(int i, int j, int border, int nrows, int ncols)
std::list< vpMeSite > m_meList
Definition: vpMeTracker.h:66
Definition: vpMe.h:124
void setRange(const unsigned int &range)
Definition: vpMe.h:429
unsigned int getAngleStep() const
Definition: vpMe.h:207
int getPointsToTrack() const
Definition: vpMe.h:275
int getStrip() const
Definition: vpMe.h:296
unsigned int getMaskSize() const
Definition: vpMe.h:239
vpMatrix * getMask() const
Definition: vpMe.h:214
double getSampleStep() const
Definition: vpMe.h:289
unsigned int getRange() const
Definition: vpMe.h:282
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:524