Visual Servoing Platform  version 3.6.1 under development (2024-07-17)
vpMeEllipse.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 
31 #include <cmath> // std::fabs
32 #include <limits> // numeric_limits
33 #include <vector>
34 
35 #include <visp3/core/vpMatrixException.h>
36 #include <visp3/core/vpTrackingException.h>
37 #include <visp3/core/vpImagePoint.h>
38 #include <visp3/me/vpMe.h>
39 #include <visp3/me/vpMeEllipse.h>
40 
41 #ifndef VP_ME_ELLIPSE_REGULAR_SAMPLING
42 #define VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES
43 #endif
44 
46 
48  : m_K(), m_iPc(), m_a(0.), m_b(0.), m_e(0.), m_iP1(), m_iP2(), m_alpha1(0), m_ce(0.), m_se(0.), m_angleList(), m_m00(0.),
49  m_thresholdWeight(0.2), m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.),
50  m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackCircle(false), m_trackArc(false), m_arcEpsilon(1e-6)
51 {
52  const unsigned int val_2 = 2;
53  const unsigned int val_6 = 6;
54  m_alpha2 = val_2 * M_PI;
55  // Resize internal parameters vector
56  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
57  m_K.resize(val_6);
58  m_iP1.set_ij(0, 0);
59  m_iP2.set_ij(0, 0);
60 }
61 
63  : vpMeTracker(me_ellipse), m_K(me_ellipse.m_K), m_iPc(me_ellipse.m_iPc), m_a(me_ellipse.m_a), m_b(me_ellipse.m_b), m_e(me_ellipse.m_e),
64  m_iP1(me_ellipse.m_iP1), m_iP2(me_ellipse.m_iP2), m_alpha1(me_ellipse.m_alpha1), m_alpha2(me_ellipse.m_alpha2), m_ce(me_ellipse.m_ce),
65  m_se(me_ellipse.m_se), m_angleList(me_ellipse.m_angleList), m_m00(me_ellipse.m_m00),
66  m_thresholdWeight(me_ellipse.m_thresholdWeight),
67  m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
68  m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
69  m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
70  m_trackCircle(me_ellipse.m_trackCircle), m_trackArc(me_ellipse.m_trackArc), m_arcEpsilon(me_ellipse.m_arcEpsilon)
71 { }
72 
74 {
75  m_meList.clear();
76  m_angleList.clear();
77 }
78 
79 double vpMeEllipse::computeTheta(const vpImagePoint &iP) const
80 {
81  double u = iP.get_u();
82  double v = iP.get_v();
83 
84  return (computeTheta(u, v));
85 }
86 
87 double vpMeEllipse::computeTheta(double u, double v) const
88 {
89  double A = (m_K[0] * u) + (m_K[2] * v) + m_K[3];
90  double B = (m_K[1] * v) + (m_K[2] * u) + m_K[4];
91 
92  double theta = atan2(B, A); // Angle between the tangent and the u axis.
93  if (theta < 0) { // theta in [0;Pi] // FC : pourquoi ? pour me sans doute
94  theta += M_PI;
95  }
96  return theta;
97 }
98 
100 {
101  vpMeSite p_me;
102  vpImagePoint iP;
103  std::list<vpMeSite>::iterator end = m_meList.end();
104  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != end; ++it) {
105  p_me = *it;
106  // (i,j) frame used for vpMESite
107  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
108  p_me.m_alpha = computeTheta(iP);
109  *it = p_me;
110  }
111 }
112 
114 {
115  // Two versions are available. If you change from one version to the other
116  // one, do not forget to adapt, for a correct display of an arc
117  // of ellipse, vpMeEllipse::display() below and
118  // vp_display_display_ellipse() in modules/core/src/display/vpDisplay_impl.h
119  // so that the two extremities of the arc are correctly shown.
120 
121 #ifdef VP_ME_ELLIPSE_REGULAR_SAMPLING
122  // Version that gives a regular angular sampling on the ellipse, so less
123  // points at its extremities
124  double co = cos(angle);
125  double si = sin(angle);
126  double coef = m_a * m_b / sqrt((m_b * m_b * co * co) + (m_a * m_a * si * si));
127  double u = co * coef;
128  double v = si * coef;
129  iP.set_u((uc + (m_ce * u)) - (m_se * v));
130  iP.set_v(vc + (m_se * u) + (m_ce * v));
131 #elif defined(VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES)
132  // Version from "the two concentric circles" method that gives more points
133  // at the ellipse extremities for a regular angle sampling. It is better to
134  // display an ellipse, not necessarily to track it
135 
136  // (u,v) are the coordinates on the canonical centered ellipse;
137  double u = m_a * cos(angle);
138  double v = m_b * sin(angle);
139  // a rotation of e and a translation by (uc,vc) are done
140  // to get the coordinates of the point on the shifted ellipse
141  iP.set_uv((m_uc + (m_ce * u)) - (m_se * v), m_vc + (m_se * u) + (m_ce * v));
142 #endif
143 }
144 
146 {
147  // Two versions are available. If you change from one version to the other
148  // one, do not forget to change also the reciprocal function
149  // computePointOnEllipse() just above. Adapt also the display; see comment
150  // at the beginning of computePointOnEllipse()
151 
152 #ifdef VP_ME_ELLIPSE_REGULAR_SAMPLING
153  // Regular angle sampling method
154  double du = pt.get_u() - uc;
155  double dv = pt.get_v() - vc;
156  double ang = atan2(dv, du) - m_e;
157  if (ang > M_PI) {
158  ang -= 2.0 * M_PI;
159  }
160  else if (ang < -M_PI) {
161  ang += 2.0 * M_PI;
162  }
163 #ifdef VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES
164  // for the "two concentric circles method" starting from the previous one
165  // (just to remember the link between both methods:
166  // tan(theta_2cc) = a/b tan(theta_rs))
167 
168  double co = cos(ang);
169  double si = sin(ang);
170  double coeff = 1.0 / sqrt((m_b * m_b * co * co) + (m_a * m_a * si * si));
171  si *= m_a * coeff;
172  co *= m_b * coeff;
173  ang = atan2(si, co);
174 #endif
175 #elif defined(VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES)
176  // For the "two concentric circles" method starting from scratch
177  double du = pt.get_u() - m_uc;
178  double dv = pt.get_v() - m_vc;
179  double co = ((du * m_ce) + (dv * m_se)) / m_a;
180  double si = ((-du * m_se) + (dv * m_ce)) / m_b;
181  double angle = atan2(si, co);
182 #endif
183 
184  return angle;
185 }
186 
188 {
189  double num = m_n20 - m_n02;
190  double d = (num * num) + (4.0 * m_n11 * m_n11); // always >= 0
191  if (d <= std::numeric_limits<double>::epsilon()) {
192  m_e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
193  m_ce = 1.0;
194  m_se = 0.0;
195  m_a = (m_b = (2.0 * sqrt(m_n20))); // = sqrt(2.0*(n20+n02))
196  }
197  else { // real ellipse
198  m_e = atan2(2.0 * m_n11, num) / 2.0; // e in [-Pi/2 ; Pi/2]
199  m_ce = cos(m_e);
200  m_se = sin(m_e);
201 
202  d = sqrt(d); // d in sqrt always >= 0
203  num = m_n20 + m_n02;
204  m_a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
205  m_b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
206  }
207 }
208 
210 {
211  const unsigned int index_0 = 0;
212  const unsigned int index_1 = 1;
213  const unsigned int index_2 = 2;
214  const unsigned int index_3 = 3;
215  const unsigned int index_4 = 4;
216  const unsigned int index_5 = 5;
217  m_K[index_0] = m_n02;
218  m_K[index_1] = m_n20;
219  m_K[index_2] = -m_n11;
220  m_K[index_3] = (m_n11 * m_vc) - (m_n02 * m_uc);
221  m_K[index_4] = (m_n11 * m_uc) - (m_n20 * m_vc);
222  m_K[index_5] = (((m_n02 * m_uc * m_uc) + (m_n20 * m_vc * m_vc)) - (2.0 * m_n11 * m_uc * m_vc)) + (4.0 * ((m_n11 * m_n11) - (m_n20 * m_n02)));
223 }
224 
226 {
227  m_n20 = 0.25 * ((m_a * m_a * m_ce * m_ce) + (m_b * m_b * m_se * m_se));
228  m_n11 = 0.25 * m_se * m_ce * ((m_a * m_a) - (m_b * m_b));
229  m_n02 = 0.25 * ((m_a * m_a * m_se * m_se) + (m_b * m_b * m_ce * m_ce));
230 }
231 
233 {
234  const unsigned int index_0 = 0;
235  const unsigned int index_1 = 1;
236  const unsigned int index_2 = 2;
237  const unsigned int index_3 = 3;
238  const unsigned int index_4 = 4;
239  const unsigned int index_5 = 5;
240  // Equations below from Chaumette PhD and TRO 2004 paper
241  double num = (m_K[index_0] * m_K[index_1]) - (m_K[index_2] * m_K[index_2]); // > 0 for an ellipse
242  if (num <= 0) {
243  throw(vpTrackingException(vpTrackingException::fatalError, "The points do not belong to an ellipse! num: %f", num));
244  }
245 
246  m_uc = ((m_K[index_2] * m_K[index_4]) - (m_K[index_1] * m_K[index_3])) / num;
247  m_vc = ((m_K[index_2] * m_K[index_3]) - (m_K[index_0] * m_K[index_4])) / num;
248  m_iPc.set_uv(m_uc, m_vc);
249 
250  double d = (((m_K[index_0] * m_uc * m_uc) + (m_K[index_1] * m_vc * m_vc) + (2.0 * m_K[index_2] * m_uc * m_vc)) - m_K[index_5]) / (4.0 * num);
251  m_n20 = m_K[index_1] * d; // always > 0
252  m_n11 = -m_K[index_2] * d;
253  m_n02 = m_K[index_0] * d; // always > 0
254 
256 
257  // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper)
258  d = m_n02 / m_K[index_0]; // fabs(K[0]) > 0
259  unsigned int Ksize = m_K.size();
260  for (unsigned int i = 0; i < Ksize; ++i) {
261  m_K[i] *= d;
262  }
263 }
264 
266 {
267  std::cout << "K :" << m_K.t() << std::endl;
268  std::cout << "xc = " << m_uc << ", yc = " << m_vc << std::endl;
269  std::cout << "n20 = " << m_n20 << ", n11 = " << m_n11 << ", n02 = " << m_n02 << std::endl;
270  std::cout << "A = " << m_a << ", B = " << m_b << ", E (dg) " << vpMath::deg(m_e) << std::endl;
271 }
272 
273 void vpMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
274 {
275  // Warning: similar code in vpMbtMeEllipse::sample()
276  if (!m_me) {
277  throw(vpTrackingException(vpTrackingException::fatalError, "Moving edges on ellipse not initialized"));
278  }
279  // Delete old lists
280  m_meList.clear();
281  m_angleList.clear();
282 
283  int nbrows = static_cast<int>(I.getHeight());
284  int nbcols = static_cast<int>(I.getWidth());
285  // New version using distance for sampling
286  if (std::fabs(m_me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
287  std::cout << "Warning: In vpMeEllipse::sample() ";
288  std::cout << "function called with sample step = 0. We set it rather to 10 pixels";
289  // std::cout << "function called with sample step = 0, set to 10 dg";
290  m_me->setSampleStep(10.0);
291  }
292  // Perimeter of the ellipse using Ramanujan formula
293  double perim = M_PI * ((3.0 * (m_a + m_b)) - sqrt(((3.0 * m_a) + m_b) * (m_a + (3.0 * m_b))));
294  // Number of points for a complete ellipse
295  unsigned int nb_pt = static_cast<unsigned int>(floor(perim / m_me->getSampleStep()));
296  double incr = (2.0 * M_PI) / nb_pt;
297  // Compute of the expected density
298  if (!m_trackArc) { // number of points for a complete ellipse
299  m_expectedDensity = nb_pt;
300  }
301  else { // number of points for an arc of ellipse
302  m_expectedDensity *= static_cast<unsigned int>(floor((perim / m_me->getSampleStep()) * ((m_alpha2 - m_alpha1) / (2.0 * M_PI))));
303  }
304 
305  // Starting angle for sampling: new version to not start at 0
306  double ang = m_alpha1 + (incr / 2.0);
307 
308  // sample positions
309  for (unsigned int i = 0; i < m_expectedDensity; ++i) {
310  vpImagePoint iP;
311  computePointOnEllipse(ang, iP);
312  // If point is in the image, add to the sample list
313  // Check done in (i,j) frame)
314  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
315  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
316  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)
317  && inMeMaskCandidates(m_maskCandidates, is_uint, js_uint)) {
318  const unsigned int crossSize = 5;
319  vpDisplay::displayCross(I, iP, crossSize, vpColor::red);
320 
321  double theta = computeTheta(iP);
322  vpMeSite pix;
323  // (i,j) frame used for vpMeSite
324  pix.init(iP.get_i(), iP.get_j(), theta);
327  const double marginRatio = m_me->getThresholdMarginRatio();
328  double convolution = pix.convolution(I, m_me);
329  double contrastThreshold = fabs(convolution) * marginRatio;
330  pix.setContrastThreshold(contrastThreshold, *m_me);
331  m_meList.push_back(pix);
332  m_angleList.push_back(ang);
333  }
334  ang += incr;
335  }
336 
337  if (!doNotTrack) {
339  }
340 }
341 
343 {
344  if (!m_me) {
345  throw(vpTrackingException(vpTrackingException::fatalError, "Moving edges on ellipse tracking not initialized"));
346  }
347  unsigned int nb_pts_added = 0;
348  int nbrows = static_cast<int>(I.getHeight());
349  int nbcols = static_cast<int>(I.getWidth());
350  const unsigned int range_default = 2;
351 
352  unsigned int memory_range = m_me->getRange();
353  m_me->setRange(range_default);
354 
355  // Perimeter of the ellipse using Ramanujan formula
356  double perim = M_PI * ((3.0 * (m_a + m_b)) - sqrt(((3.0 * m_a) + m_b) * (m_a + (3.0 * m_b))));
357  // Number of points for a complete ellipse
358  unsigned int nb_pt = static_cast<unsigned int>(floor(perim / m_me->getSampleStep()));
359  double incr = (2.0 * M_PI) / nb_pt;
360 
361  // Detect holes and try to complete them
362  // In this option, the sample step is used to complete the holes as much as possible
363  std::list<double>::iterator angleList = m_angleList.begin();
364  std::list<vpMeSite>::iterator meList = m_meList.begin();
365  const double marginRatio = m_me->getThresholdMarginRatio();
366  double ang = *angleList;
367  ++angleList;
368  ++meList;
369 
370  while (meList != m_meList.end()) {
371  double nextang = *angleList;
372  if ((nextang - ang) > (2.0 * incr)) { // A hole exists
373  ang += incr; // next point to be checked
374  // adding only 1 point if hole of 1 point
375  while (ang < (nextang - incr)) {
376  vpImagePoint iP;
377  computePointOnEllipse(ang, iP);
378  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
379  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
380  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
381  double theta = computeTheta(iP);
382  vpMeSite pix;
383  pix.init(iP.get_i(), iP.get_j(), theta);
386  double convolution = pix.convolution(I, m_me);
387  double contrastThreshold = fabs(convolution) * marginRatio;
388  pix.setContrastThreshold(contrastThreshold, *m_me);
389  pix.track(I, m_me, false);
390  if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
391  ++nb_pts_added;
392  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
393  double new_ang = computeAngleOnEllipse(iP);
394  if ((new_ang - ang) > M_PI) {
395  new_ang -= 2.0 * M_PI;
396  }
397  else if ((ang - new_ang) > M_PI) {
398  new_ang += 2.0 * M_PI;
399  }
400  m_meList.insert(meList, pix);
401  m_angleList.insert(angleList, new_ang);
402  }
403  }
404  ang += incr;
405  }
406  }
407  ang = nextang;
408  ++angleList;
409  ++meList;
410  }
411 
412  // Add points in case two neighboring points are too far away
413  angleList = m_angleList.begin();
414  ang = *angleList;
415  meList = m_meList.begin();
416  vpMeSite pix1 = *meList;
417  ++angleList;
418  ++meList;
419  while (meList != m_meList.end()) {
420  double nextang = *angleList;
421  vpMeSite pix2 = *meList;
422  double dist = sqrt(((pix1.get_ifloat() - pix2.get_ifloat()) * (pix1.get_ifloat() - pix2.get_ifloat()))
423  + ((pix1.get_jfloat() - pix2.get_jfloat()) * (pix1.get_jfloat() - pix2.get_jfloat())));
424  // Only one point is added if two neighboring points are too far away
425  if (dist > (2.0 * m_me->getSampleStep())) {
426  ang = (nextang + ang) / 2.0; // point added at mid angle
427  vpImagePoint iP;
428  computePointOnEllipse(ang, iP);
429  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
430  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
431  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
432  double theta = computeTheta(iP);
433  vpMeSite pix;
434  pix.init(iP.get_i(), iP.get_j(), theta);
437  double convolution = pix.convolution(I, m_me);
438  double contrastThreshold = fabs(convolution) * marginRatio;
439  pix.setContrastThreshold(contrastThreshold, *m_me);
440  pix.track(I, m_me, false);
441  if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
442  ++nb_pts_added;
443  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
444  double new_ang = computeAngleOnEllipse(iP);
445  if ((new_ang - ang) > M_PI) {
446  new_ang -= 2.0 * M_PI;
447  }
448  else if ((ang - new_ang) > M_PI) {
449  new_ang += 2.0 * M_PI;
450  }
451  m_meList.insert(meList, pix);
452  m_angleList.insert(angleList, new_ang);
453  }
454  }
455  }
456  ang = nextang;
457  pix1 = pix2;
458  ++angleList;
459  ++meList;
460  }
461 
462  // Try to fill the first extremity: from alpha_min - incr to alpha1 + incr/2
463  meList = m_meList.begin();
464  pix1 = *meList;
465  unsigned int nbpts = 0;
466  // Add - incr/2.0 to avoid being too close to 0
467  if ((m_alphamin - m_alpha1 - (incr / 2.0)) > 0.0) {
468  nbpts = static_cast<unsigned int>(floor((m_alphamin - m_alpha1 - (incr / 2.0)) / incr));
469  }
470  ang = m_alphamin - incr;
471  for (unsigned int i = 0; i < nbpts; ++i) {
472  vpImagePoint iP;
473  computePointOnEllipse(ang, iP);
474  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
475  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
476  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
477  double theta = computeTheta(iP);
478  vpMeSite pix;
479  pix.init(iP.get_i(), iP.get_j(), theta);
482  // --comment: pix dot setContrastThreshold of pix1 dot getContrastThreshold() comma *m_me
483  double convolution = pix.convolution(I, m_me);
484  double contrastThreshold = fabs(convolution) * marginRatio;
485  pix.setContrastThreshold(contrastThreshold, *m_me);
486  pix.track(I, m_me, false);
487  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
488  ++nb_pts_added;
489  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
490  double new_ang = computeAngleOnEllipse(iP);
491  if ((new_ang - ang) > M_PI) {
492  new_ang -= 2.0 * M_PI;
493  }
494  else if ((ang - new_ang) > M_PI) {
495  new_ang += 2.0 * M_PI;
496  }
497  m_meList.push_front(pix);
498  m_angleList.push_front(new_ang);
499  }
500  }
501  ang -= incr;
502  }
503 
504  // Try to fill the second extremity: from alphamax + incr to alpha2 - incr/2
505  pix1 = m_meList.back();
506  nbpts = 0;
507  if ((m_alpha2 - (incr / 2.0) - m_alphamax) > 0.0) {
508  nbpts = static_cast<unsigned int>(floor((m_alpha2 - (incr / 2.0) - m_alphamax) / incr));
509  }
510 
511  ang = m_alphamax + incr;
512  for (unsigned int i = 0; i < nbpts; ++i) {
513  vpImagePoint iP;
514  computePointOnEllipse(ang, iP);
515  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
516  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
517  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
518  double theta = computeTheta(iP);
519  vpMeSite pix;
520  pix.init(iP.get_i(), iP.get_j(), theta);
523  double convolution = pix.convolution(I, m_me);
524  double contrastThreshold = fabs(convolution) * marginRatio;
525  pix.setContrastThreshold(contrastThreshold, *m_me);
526  pix.track(I, m_me, false);
527  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
528  ++nb_pts_added;
529  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
530  double new_ang = computeAngleOnEllipse(iP);
531  if ((new_ang - ang) > M_PI) {
532  new_ang -= 2.0 * M_PI;
533  }
534  else if ((ang - new_ang) > M_PI) {
535  new_ang += 2.0 * M_PI;
536  }
537  m_meList.push_back(pix);
538  m_angleList.push_back(new_ang);
539  }
540  }
541  ang += incr;
542  }
543  m_me->setRange(memory_range);
544 
545  if (m_meList.size() != m_angleList.size()) {
546  // Should never occur
547  throw(vpTrackingException(vpTrackingException::fatalError, "Lists are not coherent in vpMeEllipse::plugHoles(): nb MEs %ld, nb ang %ld",
548  m_meList.size(), m_angleList.size()));
549  }
550 
551  return nb_pts_added;
552 }
553 
554 void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpColor &col, unsigned int thickness)
555 {
556  vpMeEllipse::displayEllipse(I, m_iPc, m_a, m_b, m_e, m_alpha1, m_alpha2, col, thickness);
557 }
558 
559 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, bool trackCircle, bool trackArc)
560 {
561  unsigned int n = 5; // by default, 5 points for an ellipse
562  const unsigned int nForCircle = 3;
563  m_trackCircle = trackCircle;
564  if (trackCircle) {
565  n = nForCircle;
566  }
567  std::vector<vpImagePoint> iP(n);
568  m_trackArc = trackArc;
569 
570  vpDisplay::flush(I);
571 
572  if (m_trackCircle) {
573  if (m_trackArc) {
574  std::cout << "First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
575  }
576  for (unsigned int k = 0; k < n; ++k) {
577  std::cout << "Click point " << (k + 1) << "/" << n << " on the circle " << std::endl;
578  vpDisplay::getClick(I, iP[k], true);
579  const unsigned int crossSize = 10;
580  vpDisplay::displayCross(I, iP[k], crossSize, vpColor::red);
581  vpDisplay::flush(I);
582  std::cout << iP[k] << std::endl;
583  }
584  }
585  else {
586  if (m_trackArc) {
587  std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
588  }
589  for (unsigned int k = 0; k < n; ++k) {
590  std::cout << "Click point " << (k + 1) << "/" << n << " on the ellipse " << std::endl;
591  vpDisplay::getClick(I, iP[k], true);
592  const unsigned int crossSize = 10;
593  vpDisplay::displayCross(I, iP[k], crossSize, vpColor::red);
594  vpDisplay::flush(I);
595  std::cout << iP[k] << std::endl;
596  }
597  }
598  initTracking(I, iP, trackCircle, trackArc);
599 }
600 
601 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
602 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, std::optional<std::vector<vpImagePoint>> &opt_ips, bool trackCircle, bool trackArc)
603 #else
604 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, std::vector<vpImagePoint> *opt_ips, bool trackCircle, bool trackArc)
605 #endif
606 {
607 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
608  if (!opt_ips.has_value())
609 #else
610  if (opt_ips == nullptr)
611 #endif
612  {
613  initTracking(I, trackCircle, trackArc);
614  return;
615  }
616 
617  if (opt_ips->size() != 0) {
618  initTracking(I, *opt_ips, trackCircle, trackArc);
619  return;
620  }
621 
622  unsigned int n = 5; // by default, 5 points for an ellipse
623  const unsigned int nForCircle = 3;
624  m_trackCircle = trackCircle;
625  if (trackCircle) {
626  n = nForCircle;
627  }
628  opt_ips->resize(n);
629  m_trackArc = trackArc;
630 
631  vpDisplay::flush(I);
632 
633  if (m_trackCircle) {
634  if (m_trackArc) {
635  std::cout << "First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
636  }
637  for (unsigned int k = 0; k < n; ++k) {
638  std::cout << "Click point " << (k + 1) << "/" << n << " on the circle " << std::endl;
639  vpDisplay::getClick(I, (*opt_ips)[k], true);
640  const unsigned int crossSize = 10;
641  vpDisplay::displayCross(I, (*opt_ips)[k], crossSize, vpColor::red);
642  vpDisplay::flush(I);
643  std::cout << (*opt_ips)[k] << std::endl;
644  }
645  }
646  else {
647  if (m_trackArc) {
648  std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
649  }
650  for (unsigned int k = 0; k < n; ++k) {
651  std::cout << "Click point " << (k + 1) << "/" << n << " on the ellipse " << std::endl;
652  vpDisplay::getClick(I, (*opt_ips)[k], true);
653  const unsigned int crossSize = 10;
654  vpDisplay::displayCross(I, (*opt_ips)[k], crossSize, vpColor::red);
655  vpDisplay::flush(I);
656  std::cout << (*opt_ips)[k] << std::endl;
657  }
658  }
659  initTracking(I, *opt_ips, trackCircle, trackArc);
660 }
661 
662 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP,
663  bool trackCircle, bool trackArc)
664 {
665  m_trackArc = trackArc;
666  m_trackCircle = trackCircle;
667  // useful for sample(I):
668  leastSquare(I, iP);
669  if (trackArc) {
670  // useful for track(I):
671  m_iP1 = iP.front();
672  m_iP2 = iP.back();
673  // useful for sample(I):
676  if ((m_alpha2 <= m_alpha1) || (std::fabs(m_alpha2 - m_alpha1) < m_arcEpsilon)) {
677  m_alpha2 += 2.0 * M_PI;
678  }
679  }
680  else {
681  m_alpha1 = 0.0;
682  m_alpha2 = 2.0 * M_PI;
683  // useful for track(I):
684  vpImagePoint ip;
686  m_iP1 = (m_iP2 = ip);
687  }
688 
689  sample(I);
690  track(I);
692  vpDisplay::flush(I);
693 }
694 
696  const vpImagePoint *pt2, bool trackCircle)
697 {
698  const unsigned int index_0 = 0;
699  const unsigned int index_1 = 1;
700  const unsigned int index_2 = 2;
701  const unsigned int index_3 = 3;
702  const unsigned int index_4 = 4;
703 
704  m_trackCircle = trackCircle;
705  if ((pt1 != nullptr) && (pt2 != nullptr)) {
706  m_trackArc = true;
707  }
708  // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
709  m_uc = param[index_0];
710  m_vc = param[index_1];
711  m_n20 = param[index_2];
712  m_n11 = param[index_3];
713  m_n02 = param[index_4];
716 
717  if (m_trackArc && pt1 && pt2) {
720  if ((m_alpha2 <= m_alpha1) || (std::fabs(m_alpha2 - m_alpha1) < m_arcEpsilon)) {
721  m_alpha2 += 2.0 * M_PI;
722  }
723  // useful for track(I)
724  m_iP1 = *pt1;
725  m_iP2 = *pt2;
726  }
727  else {
728  m_alpha1 = 0.0;
729  m_alpha2 = 2.0 * M_PI;
730  // useful for track(I)
731  vpImagePoint ip;
733  m_iP1 = (m_iP2 = ip);
734  }
735  // useful for display(I) so useless if no display before track(I)
736  m_iPc.set_uv(m_uc, m_vc);
737 
738  sample(I);
739  track(I);
741  vpDisplay::flush(I);
742 }
743 
750 {
752 
753  // recompute alpha1 and alpha2 in case they have been changed by setEndPoints()
754  if (m_trackArc) {
757  if ((m_alpha2 <= m_alpha1) || (std::fabs(m_alpha2 - m_alpha1) < m_arcEpsilon)) {
758  m_alpha2 += 2.0 * M_PI;
759  }
760  }
761  // Compute the ellipse parameters from the tracked points, manage the lists,
762  // and update the expected density (
764 
765  if (plugHoles(I) > 0) {
766  m_numberOfGoodPoints = leastSquareRobust(I); // if new points have been added, recompute the ellipse parameters and manage again the lists
767  }
768 
769  const unsigned int minNbGoodPoints = 5;
770  if (m_numberOfGoodPoints <= minNbGoodPoints) {
771  sample(I);
774 
775  // Stop in case of failure after resample
776  if (m_numberOfGoodPoints <= minNbGoodPoints) {
777  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Impossible to track the ellipse, not enough features"));
778  }
779  }
780 
781  // remet a jour l'angle delta pour chaque vpMeSite de la liste
782  updateTheta();
783  // not in getParameters since computed only once for each image
784  m_m00 = M_PI * m_a * m_b;
785 
786  // Useful only for tracking an arc of ellipse, but done to give them a value
789 }
790 
791 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
792 
817 void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A,
818  const double &B, const double &E, const double &smallalpha, const double &highalpha,
819  const vpColor &color, unsigned int thickness)
820 {
821  vpMeEllipse::displayEllipse(I, center, A, B, E, smallalpha, highalpha, color, thickness);
822 }
823 
851 void vpMeEllipse::display(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
852  const double &E, const double &smallalpha, const double &highalpha,
853  const vpColor &color, unsigned int thickness)
854 {
855  vpMeEllipse::displayEllipse(I, center, A, B, E, smallalpha, highalpha, color, thickness);
856 }
857 #endif // Deprecated
858 
859 
860 void vpMeEllipse::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A,
861  const double &B, const double &E, const double &smallalpha, const double &highalpha,
862  const vpColor &color, unsigned int thickness)
863 {
864  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
865 }
866 
867 void vpMeEllipse::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
868  const double &E, const double &smallalpha, const double &highalpha,
869  const vpColor &color, unsigned int thickness)
870 {
871  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
872 }
873 
874 END_VISP_NAMESPACE
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:217
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
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)
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
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:320
double get_u() const
Definition: vpImagePoint.h:136
void set_u(double u)
Definition: vpImagePoint.h:335
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
void set_v(double v)
Definition: vpImagePoint.h:346
double get_i() const
Definition: vpImagePoint.h:114
double get_v() const
Definition: vpImagePoint.h:147
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static double deg(double rad)
Definition: vpMath.h:119
Class that tracks an ellipse using moving edges.
Definition: vpMeEllipse.h:96
double m_n20
Second order centered and normalized moments .
Definition: vpMeEllipse.h:463
double m_arcEpsilon
Epsilon value used to check if arc angles are the same.
Definition: vpMeEllipse.h:477
double m_e
Definition: vpMeEllipse.h:419
void computePointOnEllipse(const double angle, vpImagePoint &iP)
void display(const vpImage< unsigned char > &I, const vpColor &col, unsigned int thickness=1)
double m_alpha2
Definition: vpMeEllipse.h:437
vpImagePoint m_iPc
The coordinates of the ellipse center.
Definition: vpMeEllipse.h:411
double m_vc
Value of v coordinate of iPc.
Definition: vpMeEllipse.h:461
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
Definition: vpMeEllipse.h:471
void computeKiFromNij()
unsigned int plugHoles(const vpImage< unsigned char > &I)
void computeNijFromAbe()
void getParameters()
void printParameters() const
double m_uc
Value of u coordinate of iPc.
Definition: vpMeEllipse.h:459
bool m_trackCircle
Track a circle (true) or an ellipse (false).
Definition: vpMeEllipse.h:473
void computeAbeFromNij()
vpColVector m_K
Definition: vpMeEllipse.h:409
double m_m00
Ellipse area.
Definition: vpMeEllipse.h:445
double m_alphamin
Definition: vpMeEllipse.h:453
std::list< double > m_angleList
Stores the value in increasing order of the angle on the ellipse for each vpMeSite.
Definition: vpMeEllipse.h:443
bool m_trackArc
Track an arc of ellipse/circle (true) or a complete one (false).
Definition: vpMeEllipse.h:475
void initTracking(const vpImage< unsigned char > &I, bool trackCircle=false, bool trackArc=false)
double m_a
is the semi major axis of the ellipse.
Definition: vpMeEllipse.h:413
unsigned int leastSquareRobust(const vpImage< unsigned char > &I)
double m_alpha1
Definition: vpMeEllipse.h:432
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
double m_b
is the semi minor axis of the ellipse.
Definition: vpMeEllipse.h:415
double m_alphamax
Definition: vpMeEllipse.h:457
double computeTheta(const vpImagePoint &iP) const
Definition: vpMeEllipse.cpp:79
double m_se
Value of sin(e).
Definition: vpMeEllipse.h:441
double m_n02
Second order centered and normalized moments .
Definition: vpMeEllipse.h:467
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:469
void updateTheta()
Definition: vpMeEllipse.cpp:99
vpImagePoint m_iP2
Definition: vpMeEllipse.h:428
double m_n11
Second order centered and normalized moments .
Definition: vpMeEllipse.h:465
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) VP_OVERRIDE
vpImagePoint m_iP1
Definition: vpMeEllipse.h:424
void track(const vpImage< unsigned char > &I)
virtual ~vpMeEllipse() VP_OVERRIDE
Definition: vpMeEllipse.cpp:73
double m_ce
Value of cos(e).
Definition: vpMeEllipse.h:439
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &A, const double &B, const double &E, const double &smallalpha, const double &highalpha, const vpColor &color=vpColor::green, unsigned int thickness=1)
double computeAngleOnEllipse(const vpImagePoint &pt) const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:68
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:86
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:250
double m_ifloat
Subpixel coordinates along i of a site.
Definition: vpMeSite.h:103
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:214
void init()
Definition: vpMeSite.cpp:57
double m_alpha
Angle of tangent at site.
Definition: vpMeSite.h:109
vpMeSiteState getState() const
Definition: vpMeSite.h:269
double get_ifloat() const
Definition: vpMeSite.h:195
double m_jfloat
Subpixel coordinates along j of a site.
Definition: vpMeSite.h:105
double get_jfloat() const
Definition: vpMeSite.h:201
void setContrastThreshold(const double &thresh, const vpMe &me)
Definition: vpMeSite.h:289
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
Definition: vpMeSite.cpp:269
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:259
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:62
void initTracking(const vpImage< unsigned char > &I)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMeTracker.h:319
vpMeSite::vpMeSiteDisplayType m_selectDisplay
Moving-edges display type.
Definition: vpMeTracker.h:323
void track(const vpImage< unsigned char > &I)
vpMe * m_me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:313
static bool inRoiMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
void display(const vpImage< unsigned char > &I)
bool outOfImage(int i, int j, int border, int nrows, int ncols)
const vpImage< bool > * m_maskCandidates
Mask used to determine candidate points for initialization in an image.
Definition: vpMeTracker.h:321
std::list< vpMeSite > m_meList
Definition: vpMeTracker.h:311
static bool inMeMaskCandidates(const vpImage< bool > *meMaskCandidates, unsigned int i, unsigned int j)
void setRange(const unsigned int &range)
Definition: vpMe.h:415
double getThresholdMarginRatio() const
Definition: vpMe.h:300
void setSampleStep(const double &sample_step)
Definition: vpMe.h:422
double getSampleStep() const
Definition: vpMe.h:275
unsigned int getRange() const
Definition: vpMe.h:268
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ fatalError
Tracker fatal error.