Visual Servoing Platform  version 3.6.1 under development (2024-04-23)
vpMeEllipse.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 
31 #include <cmath> // std::fabs
32 #include <limits> // numeric_limits
33 #include <vector>
34 
35 #include <visp3/core/vpDebug.h>
36 #include <visp3/core/vpMatrixException.h>
37 #include <visp3/core/vpTrackingException.h>
38 #include <visp3/core/vpImagePoint.h>
39 #include <visp3/core/vpRobust.h>
40 #include <visp3/me/vpMe.h>
41 #include <visp3/me/vpMeEllipse.h>
42 
43 // --comment: define the VP_ME_ELLIPSE_REGULAR_SAMPLING flag
44 #ifndef VP_ME_ELLIPSE_REGULAR_SAMPLING
45 #define VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES
46 #endif
47 
49  : m_K(), m_iPc(), m_a(0.), m_b(0.), m_e(0.), m_iP1(), m_iP2(), m_alpha1(0), m_alpha2(2 * M_PI), m_ce(0.), m_se(0.), m_angleList(), m_m00(0.),
50  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.),
51  m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackCircle(false), m_trackArc(false), m_arcEpsilon(1e-6)
52 {
53  // Resize internal parameters vector
54  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
55  m_K.resize(6);
56  m_iP1.set_ij(0, 0);
57  m_iP2.set_ij(0, 0);
58 }
59 
61  : 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),
62  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),
63  m_se(me_ellipse.m_se), m_angleList(me_ellipse.m_angleList), m_m00(me_ellipse.m_m00),
64  m_thresholdWeight(me_ellipse.m_thresholdWeight),
65  m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
66  m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
67  m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
68  m_trackCircle(me_ellipse.m_trackCircle), m_trackArc(me_ellipse.m_trackArc), m_arcEpsilon(me_ellipse.m_arcEpsilon)
69 { }
70 
72 {
73  m_meList.clear();
74  m_angleList.clear();
75 }
76 
77 double vpMeEllipse::computeTheta(const vpImagePoint &iP) const
78 {
79  double u = iP.get_u();
80  double v = iP.get_v();
81 
82  return (computeTheta(u, v));
83 }
84 
85 double vpMeEllipse::computeTheta(double u, double v) const
86 {
87  double A = (m_K[0] * u) + (m_K[2] * v) + m_K[3];
88  double B = (m_K[1] * v) + (m_K[2] * u) + m_K[4];
89 
90  double theta = atan2(B, A); // Angle between the tangent and the u axis.
91  if (theta < 0) { // theta in [0;Pi] // FC : pourquoi ? pour me sans doute
92  theta += M_PI;
93  }
94  return theta;
95 }
96 
98 {
99  vpMeSite p_me;
100  vpImagePoint iP;
101  std::list<vpMeSite>::iterator end = m_meList.end();
102  for (std::list<vpMeSite>::iterator it = m_meList.begin(); it != end; ++it) {
103  p_me = *it;
104  // (i,j) frame used for vpMESite
105  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
106  p_me.m_alpha = computeTheta(iP);
107  *it = p_me;
108  }
109 }
110 
112 {
113  // Two versions are available. If you change from one version to the other
114  // one, do not forget to adapt, for a correct display of an arc
115  // of ellipse, vpMeEllipse::display() below and
116  // vp_display_display_ellipse() in modules/core/src/display/vpDisplay_impl.h
117  // so that the two extremities of the arc are correctly shown.
118 
119 #ifdef VP_ME_ELLIPSE_REGULAR_SAMPLING
120  // Version that gives a regular angular sampling on the ellipse, so less
121  // points at its extremities
122  double co = cos(angle);
123  double si = sin(angle);
124  double coef = m_a * m_b / sqrt((m_b * m_b * co * co) + (m_a * m_a * si * si));
125  double u = co * coef;
126  double v = si * coef;
127  iP.set_u((uc + (m_ce * u)) - (m_se * v));
128  iP.set_v(vc + (m_se * u) + (m_ce * v));
129 #elif defined(VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES)
130  // Version from "the two concentric circles" method that gives more points
131  // at the ellipse extremities for a regular angle sampling. It is better to
132  // display an ellipse, not necessarily to track it
133 
134  // (u,v) are the coordinates on the canonical centered ellipse;
135  double u = m_a * cos(angle);
136  double v = m_b * sin(angle);
137  // a rotation of e and a translation by (uc,vc) are done
138  // to get the coordinates of the point on the shifted ellipse
139  iP.set_uv((m_uc + (m_ce * u)) - (m_se * v), m_vc + (m_se * u) + (m_ce * v));
140 #endif
141 }
142 
144 {
145  // Two versions are available. If you change from one version to the other
146  // one, do not forget to change also the reciprocal function
147  // computePointOnEllipse() just above. Adapt also the display; see comment
148  // at the beginning of computePointOnEllipse()
149 
150 #ifdef VP_ME_ELLIPSE_REGULAR_SAMPLING
151  // Regular angle sampling method
152  double du = pt.get_u() - uc;
153  double dv = pt.get_v() - vc;
154  double ang = atan2(dv, du) - m_e;
155  if (ang > M_PI) {
156  ang -= 2.0 * M_PI;
157  }
158  else if (ang < -M_PI) {
159  ang += 2.0 * M_PI;
160  }
161 #ifdef VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES
162  // for the "two concentric circles method" starting from the previous one
163  // (just to remember the link between both methods:
164  // tan(theta_2cc) = a/b tan(theta_rs))
165 
166  double co = cos(ang);
167  double si = sin(ang);
168  double coeff = 1.0 / sqrt((m_b * m_b * co * co) + (m_a * m_a * si * si));
169  si *= m_a * coeff;
170  co *= m_b * coeff;
171  ang = atan2(si, co);
172 #endif
173 #elif defined(VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES)
174  // For the "two concentric circles" method starting from scratch
175  double du = pt.get_u() - m_uc;
176  double dv = pt.get_v() - m_vc;
177  double co = ((du * m_ce) + (dv * m_se)) / m_a;
178  double si = ((-du * m_se) + (dv * m_ce)) / m_b;
179  double angle = atan2(si, co);
180 #endif
181 
182  return angle;
183 }
184 
186 {
187  double num = m_n20 - m_n02;
188  double d = (num * num) + (4.0 * m_n11 * m_n11); // always >= 0
189  if (d <= std::numeric_limits<double>::epsilon()) {
190  m_e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
191  m_ce = 1.0;
192  m_se = 0.0;
193  m_a = (m_b = (2.0 * sqrt(m_n20))); // = sqrt(2.0*(n20+n02))
194  }
195  else { // real ellipse
196  m_e = atan2(2.0 * m_n11, num) / 2.0; // e in [-Pi/2 ; Pi/2]
197  m_ce = cos(m_e);
198  m_se = sin(m_e);
199 
200  d = sqrt(d); // d in sqrt always >= 0
201  num = m_n20 + m_n02;
202  m_a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
203  m_b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
204  }
205 }
206 
208 {
209  m_K[0] = m_n02;
210  m_K[1] = m_n20;
211  m_K[2] = -m_n11;
212  m_K[3] = (m_n11 * m_vc) - (m_n02 * m_uc);
213  m_K[4] = (m_n11 * m_uc) - (m_n20 * m_vc);
214  m_K[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)));
215 }
216 
218 {
219  m_n20 = 0.25 * ((m_a * m_a * m_ce * m_ce) + (m_b * m_b * m_se * m_se));
220  m_n11 = 0.25 * m_se * m_ce * ((m_a * m_a) - (m_b * m_b));
221  m_n02 = 0.25 * ((m_a * m_a * m_se * m_se) + (m_b * m_b * m_ce * m_ce));
222 }
223 
225 {
226  // Equations below from Chaumette PhD and TRO 2004 paper
227  double num = (m_K[0] * m_K[1]) - (m_K[2] * m_K[2]); // > 0 for an ellipse
228  if (num <= 0) {
229  throw(vpTrackingException(vpTrackingException::fatalError, "The points do not belong to an ellipse! num: %f", num));
230  }
231 
232  m_uc = ((m_K[2] * m_K[4]) - (m_K[1] * m_K[3])) / num;
233  m_vc = ((m_K[2] * m_K[3]) - (m_K[0] * m_K[4])) / num;
234  m_iPc.set_uv(m_uc, m_vc);
235 
236  double d = (((m_K[0] * m_uc * m_uc) + (m_K[1] * m_vc * m_vc) + (2.0 * m_K[2] * m_uc * m_vc)) - m_K[5]) / (4.0 * num);
237  m_n20 = m_K[1] * d; // always > 0
238  m_n11 = -m_K[2] * d;
239  m_n02 = m_K[0] * d; // always > 0
240 
242 
243  // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper)
244  d = m_n02 / m_K[0]; // fabs(K[0]) > 0
245  unsigned int Ksize = m_K.size();
246  for (unsigned int i = 0; i < Ksize; ++i) {
247  m_K[i] *= d;
248  }
249  if (vpDEBUG_ENABLE(3)) {
250  printParameters();
251  }
252 }
253 
255 {
256  std::cout << "K :" << m_K.t() << std::endl;
257  std::cout << "xc = " << m_uc << ", yc = " << m_vc << std::endl;
258  std::cout << "n20 = " << m_n20 << ", n11 = " << m_n11 << ", n02 = " << m_n02 << std::endl;
259  std::cout << "A = " << m_a << ", B = " << m_b << ", E (dg) " << vpMath::deg(m_e) << std::endl;
260 }
261 
262 void vpMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
263 {
264  // Warning: similar code in vpMbtMeEllipse::sample()
265  if (!m_me) {
266  throw(vpTrackingException(vpTrackingException::fatalError, "Moving edges on ellipse not initialized"));
267  }
268  // Delete old lists
269  m_meList.clear();
270  m_angleList.clear();
271 
272  int nbrows = static_cast<int>(I.getHeight());
273  int nbcols = static_cast<int>(I.getWidth());
274  // New version using distance for sampling
275  if (std::fabs(m_me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
276  std::cout << "Warning: In vpMeEllipse::sample() ";
277  std::cout << "function called with sample step = 0. We set it rather to 10 pixels";
278  // std::cout << "function called with sample step = 0, set to 10 dg";
279  m_me->setSampleStep(10.0);
280  }
281  // Perimeter of the ellipse using Ramanujan formula
282  double perim = M_PI * ((3.0 * (m_a + m_b)) - sqrt(((3.0 * m_a) + m_b) * (m_a + (3.0 * m_b))));
283  // Number of points for a complete ellipse
284  unsigned int nb_pt = static_cast<unsigned int>(floor(perim / m_me->getSampleStep()));
285  double incr = (2.0 * M_PI) / nb_pt;
286  // Compute of the expected density
287  if (!m_trackArc) { // number of points for a complete ellipse
288  m_expectedDensity = nb_pt;
289  }
290  else { // number of points for an arc of ellipse
291  m_expectedDensity *= static_cast<unsigned int>(floor((perim / m_me->getSampleStep()) * ((m_alpha2 - m_alpha1) / (2.0 * M_PI))));
292  }
293 
294  // Starting angle for sampling: new version to not start at 0
295  double ang = m_alpha1 + (incr / 2.0);
296 
297  // sample positions
298  for (unsigned int i = 0; i < m_expectedDensity; ++i) {
299  vpImagePoint iP;
300  computePointOnEllipse(ang, iP);
301  // If point is in the image, add to the sample list
302  // Check done in (i,j) frame)
303  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
304  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
305  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)
306  && inMeMaskCandidates(m_maskCandidates, is_uint, js_uint)) {
307  const unsigned int crossSize = 5;
308  vpDisplay::displayCross(I, iP, crossSize, vpColor::red);
309 
310  double theta = computeTheta(iP);
311  vpMeSite pix;
312  // (i,j) frame used for vpMeSite
313  pix.init(iP.get_i(), iP.get_j(), theta);
316  const double marginRatio = m_me->getThresholdMarginRatio();
317  double convolution = pix.convolution(I, m_me);
318  double contrastThreshold = fabs(convolution) * marginRatio;
319  pix.setContrastThreshold(contrastThreshold, *m_me);
320  m_meList.push_back(pix);
321  m_angleList.push_back(ang);
322  }
323  ang += incr;
324  }
325 
326  if (!doNotTrack) {
328  }
329 }
330 
332 {
333  if (!m_me) {
334  throw(vpTrackingException(vpTrackingException::fatalError, "Moving edges on ellipse tracking not initialized"));
335  }
336  unsigned int nb_pts_added = 0;
337  int nbrows = static_cast<int>(I.getHeight());
338  int nbcols = static_cast<int>(I.getWidth());
339 
340  unsigned int memory_range = m_me->getRange();
341  m_me->setRange(2);
342 
343  // Perimeter of the ellipse using Ramanujan formula
344  double perim = M_PI * ((3.0 * (m_a + m_b)) - sqrt(((3.0 * m_a) + m_b) * (m_a + (3.0 * m_b))));
345  // Number of points for a complete ellipse
346  unsigned int nb_pt = static_cast<unsigned int>(floor(perim / m_me->getSampleStep()));
347  double incr = (2.0 * M_PI) / nb_pt;
348 
349  // Detect holes and try to complete them
350  // In this option, the sample step is used to complete the holes as much as possible
351  std::list<double>::iterator angleList = m_angleList.begin();
352  std::list<vpMeSite>::iterator meList = m_meList.begin();
353  const double marginRatio = m_me->getThresholdMarginRatio();
354  double ang = *angleList;
355  ++angleList;
356  ++meList;
357 
358  while (meList != m_meList.end()) {
359  double nextang = *angleList;
360  if ((nextang - ang) > (2.0 * incr)) { // A hole exists
361  ang += incr; // next point to be checked
362  // adding only 1 point if hole of 1 point
363  while (ang < (nextang - incr)) {
364  vpImagePoint iP;
365  computePointOnEllipse(ang, iP);
366  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
367  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
368  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
369  double theta = computeTheta(iP);
370  vpMeSite pix;
371  pix.init(iP.get_i(), iP.get_j(), theta);
374  double convolution = pix.convolution(I, m_me);
375  double contrastThreshold = fabs(convolution) * marginRatio;
376  pix.setContrastThreshold(contrastThreshold, *m_me);
377  pix.track(I, m_me, false);
378  if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
379  ++nb_pts_added;
380  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
381  double new_ang = computeAngleOnEllipse(iP);
382  if ((new_ang - ang) > M_PI) {
383  new_ang -= 2.0 * M_PI;
384  }
385  else if ((ang - new_ang) > M_PI) {
386  new_ang += 2.0 * M_PI;
387  }
388  m_meList.insert(meList, pix);
389  m_angleList.insert(angleList, new_ang);
390  if (vpDEBUG_ENABLE(3)) {
391  const unsigned int crossSize = 5;
392  vpDisplay::displayCross(I, iP, crossSize, vpColor::blue);
393  }
394  }
395  }
396  ang += incr;
397  }
398  }
399  ang = nextang;
400  ++angleList;
401  ++meList;
402  }
403 
404  if (vpDEBUG_ENABLE(3)) {
405  if (nb_pts_added > 0) {
406  std::cout << "Number of added points from holes with angles: " << nb_pts_added << std::endl;
407  }
408  }
409 
410  // Add points in case two neighboring points are too far away
411  angleList = m_angleList.begin();
412  ang = *angleList;
413  meList = m_meList.begin();
414  vpMeSite pix1 = *meList;
415  ++angleList;
416  ++meList;
417  while (meList != m_meList.end()) {
418  double nextang = *angleList;
419  vpMeSite pix2 = *meList;
420  double dist = sqrt(((pix1.get_ifloat() - pix2.get_ifloat()) * (pix1.get_ifloat() - pix2.get_ifloat()))
421  + ((pix1.get_jfloat() - pix2.get_jfloat()) * (pix1.get_jfloat() - pix2.get_jfloat())));
422  // Only one point is added if two neighboring points are too far away
423  if (dist > (2.0 * m_me->getSampleStep())) {
424  ang = (nextang + ang) / 2.0; // point added at mid angle
425  vpImagePoint iP;
426  computePointOnEllipse(ang, iP);
427  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
428  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
429  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
430  double theta = computeTheta(iP);
431  vpMeSite pix;
432  pix.init(iP.get_i(), iP.get_j(), theta);
435  double convolution = pix.convolution(I, m_me);
436  double contrastThreshold = fabs(convolution) * marginRatio;
437  pix.setContrastThreshold(contrastThreshold, *m_me);
438  pix.track(I, m_me, false);
439  if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
440  ++nb_pts_added;
441  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
442  double new_ang = computeAngleOnEllipse(iP);
443  if ((new_ang - ang) > M_PI) {
444  new_ang -= 2.0 * M_PI;
445  }
446  else if ((ang - new_ang) > M_PI) {
447  new_ang += 2.0 * M_PI;
448  }
449  m_meList.insert(meList, pix);
450  m_angleList.insert(angleList, new_ang);
451  if (vpDEBUG_ENABLE(3)) {
452  const unsigned int crossSize = 5;
453  vpDisplay::displayCross(I, iP, crossSize, vpColor::blue);
454  }
455  }
456  }
457  }
458  ang = nextang;
459  pix1 = pix2;
460  ++angleList;
461  ++meList;
462  }
463 
464  if (vpDEBUG_ENABLE(3)) {
465  if (nb_pts_added > 0) {
466  std::cout << "Number of added points from holes : " << nb_pts_added << std::endl;
467  angleList = m_angleList.begin();
468  while (angleList != m_angleList.end()) {
469  ang = *angleList;
470  std::cout << "ang = " << vpMath::deg(ang) << std::endl;
471  ++angleList;
472  }
473  }
474  }
475 
476  // Try to fill the first extremity: from alpha_min - incr to alpha1 + incr/2
477  meList = m_meList.begin();
478  pix1 = *meList;
479  unsigned int nbpts = 0;
480  // Add - incr/2.0 to avoid being too close to 0
481  if ((m_alphamin - m_alpha1 - (incr / 2.0)) > 0.0) {
482  nbpts = static_cast<unsigned int>(floor((m_alphamin - m_alpha1 - (incr / 2.0)) / incr));
483  }
484  ang = m_alphamin - incr;
485  for (unsigned int i = 0; i < nbpts; ++i) {
486  vpImagePoint iP;
487  computePointOnEllipse(ang, iP);
488  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
489  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
490  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
491  double theta = computeTheta(iP);
492  vpMeSite pix;
493  pix.init(iP.get_i(), iP.get_j(), theta);
496  // --comment: pix dot setContrastThreshold of pix1 dot getContrastThreshold() comma *m_me
497  double convolution = pix.convolution(I, m_me);
498  double contrastThreshold = fabs(convolution) * marginRatio;
499  pix.setContrastThreshold(contrastThreshold, *m_me);
500  pix.track(I, m_me, false);
501  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
502  ++nb_pts_added;
503  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
504  double new_ang = computeAngleOnEllipse(iP);
505  if ((new_ang - ang) > M_PI) {
506  new_ang -= 2.0 * M_PI;
507  }
508  else if ((ang - new_ang) > M_PI) {
509  new_ang += 2.0 * M_PI;
510  }
511  m_meList.push_front(pix);
512  m_angleList.push_front(new_ang);
513  if (vpDEBUG_ENABLE(3)) {
514  const unsigned int crossSize = 5;
515  vpDisplay::displayCross(I, iP, crossSize, vpColor::blue);
516  std::cout << "Add extremity 1, ang = " << vpMath::deg(new_ang) << std::endl;
517  }
518  }
519  }
520  ang -= incr;
521  }
522 
523  if (vpDEBUG_ENABLE(3)) {
524  if (nb_pts_added > 0) {
525  std::cout << "Number of added points from holes and first extremity : " << nb_pts_added << std::endl;
526  }
527  }
528 
529  // Try to fill the second extremity: from alphamax + incr to alpha2 - incr/2
530  pix1 = m_meList.back();
531  nbpts = 0;
532  if ((m_alpha2 - (incr / 2.0) - m_alphamax) > 0.0) {
533  nbpts = static_cast<unsigned int>(floor((m_alpha2 - (incr / 2.0) - m_alphamax) / incr));
534  }
535 
536  ang = m_alphamax + incr;
537  for (unsigned int i = 0; i < nbpts; ++i) {
538  vpImagePoint iP;
539  computePointOnEllipse(ang, iP);
540  unsigned int is_uint = static_cast<unsigned int>(iP.get_i());
541  unsigned int js_uint = static_cast<unsigned int>(iP.get_j());
542  if ((!outOfImage(iP, 0, nbrows, nbcols)) && inRoiMask(m_mask, is_uint, js_uint)) {
543  double theta = computeTheta(iP);
544  vpMeSite pix;
545  pix.init(iP.get_i(), iP.get_j(), theta);
548  double convolution = pix.convolution(I, m_me);
549  double contrastThreshold = fabs(convolution) * marginRatio;
550  pix.setContrastThreshold(contrastThreshold, *m_me);
551  pix.track(I, m_me, false);
552  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
553  ++nb_pts_added;
554  iP.set_ij(pix.get_ifloat(), pix.get_jfloat());
555  double new_ang = computeAngleOnEllipse(iP);
556  if ((new_ang - ang) > M_PI) {
557  new_ang -= 2.0 * M_PI;
558  }
559  else if ((ang - new_ang) > M_PI) {
560  new_ang += 2.0 * M_PI;
561  }
562  m_meList.push_back(pix);
563  m_angleList.push_back(new_ang);
564  if (vpDEBUG_ENABLE(3)) {
565  const unsigned int crossSize = 5;
566  vpDisplay::displayCross(I, iP, crossSize, vpColor::blue);
567  std::cout << "Add extremity 2, ang = " << vpMath::deg(new_ang) << std::endl;
568  }
569  }
570  }
571  ang += incr;
572  }
573  m_me->setRange(memory_range);
574 
575  if (m_meList.size() != m_angleList.size()) {
576  // Should never occur
577  throw(vpTrackingException(vpTrackingException::fatalError, "Lists are not coherent in vpMeEllipse::plugHoles(): nb MEs %ld, nb ang %ld",
578  m_meList.size(), m_angleList.size()));
579  }
580 
581  if (vpDEBUG_ENABLE(3)) {
582  if (nb_pts_added > 0) {
583  std::cout << "In plugHoles(): nb of added points : " << nb_pts_added << std::endl;
584  }
585  }
586  return nb_pts_added;
587 }
588 
589 void vpMeEllipse::leastSquare(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
590 {
591  double um = I.getWidth() / 2.;
592  double vm = I.getHeight() / 2.;
593  unsigned int n = static_cast<unsigned int>(iP.size());
594 
595  if (m_trackCircle) { // we track a circle
596  const unsigned int circleDims = 3;
597  if (n < circleDims) {
598  throw(vpException(vpException::dimensionError, "Not enough points to compute the circle"));
599  }
600  // System A x = b to be solved by least squares
601  // with A = (u v 1), b = (u^2 + v^2) and x = (2xc, 2yc, r^2-xc^2-yc^2)
602 
603  vpMatrix A(n, 3);
604  vpColVector b(n);
605 
606  for (unsigned int k = 0; k < n; ++k) {
607  // normalization so that (u,v) in [-1;1]
608  double u = (iP[k].get_u() - um) / um;
609  double v = (iP[k].get_v() - vm) / um; // um here to not deform the circle
610  A[k][0] = u;
611  A[k][1] = v;
612  A[k][2] = 1.0;
613  b[k] = (u * u) + (v * v);
614  }
615  vpColVector x(3);
616  x = A.solveBySVD(b);
617  // A circle is a particular ellipse. Going from x for circle to K for ellipse
618  // using inverse normalization to go back to pixel values
619  double ratio = vm / um;
620  m_K[0] = (m_K[1] = 1.0 / (um * um));
621  m_K[2] = 0.0;
622  m_K[3] = -(1.0 + (x[0] / 2.0)) / um;
623  m_K[4] = -(ratio + (x[1] / 2.0)) / um;
624  m_K[5] = -x[2] + 1.0 + (ratio * ratio) + x[0] + (ratio * x[1]);
625  }
626  else { // we track an ellipse
627  if (n < 5) {
628  throw(vpException(vpException::dimensionError, "Not enough points to compute the ellipse"));
629  }
630  // Homogeneous system A x = 0 ; x is the nullspace of A
631  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
632  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
633 
634  // It would be a bad idea to solve the same system using A x = b where
635  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
636  // cannot consider the case where the origin belongs to the ellipse.
637  // Another possibility would be to consider K0+K1=1 which is always valid,
638  // leading to the system A x = b where
639  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
640 
641  vpMatrix A(n, 6);
642 
643  for (unsigned int k = 0; k < n; ++k) {
644  // Normalization so that (u,v) in [-1;1]
645  double u = (iP[k].get_u() - um) / um;
646  double v = (iP[k].get_v() - vm) / vm;
647  A[k][0] = u * u;
648  A[k][1] = v * v;
649  A[k][2] = 2.0 * u * v;
650  A[k][3] = 2.0 * u;
651  A[k][4] = 2.0 * v;
652  A[k][5] = 1.0;
653  }
654  vpMatrix KerA;
655  unsigned int dim = A.nullSpace(KerA, 1);
656  if (dim > 1) { // case with less than 5 independent points
657  throw(vpMatrixException(vpMatrixException::rankDeficient, "Linear system for computing the ellipse equation ill conditioned"));
658  }
659  unsigned int nbRows = m_K.getRows();
660  for (unsigned int i = 0; i < nbRows; ++i) {
661  m_K[i] = KerA[i][0];
662  }
663 
664  // inverse normalization
665  m_K[0] *= vm / um;
666  m_K[1] *= um / vm;
667  m_K[3] = (m_K[3] * vm) - (m_K[0] * um) - (m_K[2] * vm);
668  m_K[4] = (m_K[4] * um) - (m_K[1] * vm) - (m_K[2] * um);
669  m_K[5] = (m_K[5] * um * vm) - (m_K[0] * um * um) - (m_K[1] * vm * vm) - (2.0 * m_K[2] * um * vm) - (2.0 * m_K[3] * um) - (2.0 * m_K[4] * vm);
670  }
671  getParameters();
672 }
673 
675 {
676  double um = I.getWidth() / 2.;
677  double vm = I.getHeight() / 2.;
678 
679  const unsigned int nos = numberOfSignal();
680  unsigned int k = 0; // count the number of tracked MEs
681 
682  vpColVector w(nos);
683  w = 1.0;
684  // Note that the (nos-k) last rows of w are not used. Hopefully, this is not an issue.
685 
686  if (m_trackCircle) { // we track a circle
687  // System A x = b to be solved by least squares
688  // with A = (u v 1), b = (u^2 + v^2) and x = (2xc, 2yc, r^2-xc^2-yc^2)
689 
690  // Note that the (nos-k) last rows of A, b, xp and yp are not used.
691  // Hopefully, this is not an issue.
692  vpMatrix A(nos, 3);
693  vpColVector b(nos);
694 
695  // Useful to compute the weights in the robust estimation
696  vpColVector xp(nos), yp(nos);
697  std::list<vpMeSite>::const_iterator end = m_meList.end();
698 
699  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != end; ++it) {
700  vpMeSite p_me = *it;
701  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
702  // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
703  double u = (p_me.get_jfloat() - um) / um;
704  double v = (p_me.get_ifloat() - vm) / um; // um to not deform the circle
705  A[k][0] = u;
706  A[k][1] = v;
707  A[k][2] = 1.0;
708  b[k] = (u * u) + (v * v);
709  // Useful to compute the weights in the robust estimation
710  xp[k] = p_me.get_jfloat();
711  yp[k] = p_me.get_ifloat();
712 
713  ++k;
714  }
715  }
716 
717  const unsigned int minRequiredNbMe = 3;
718  if (k < minRequiredNbMe) {
719  throw(vpException(vpException::dimensionError, "Not enough moving edges %d / %d to track the circle ",
720  k, m_meList.size()));
721  }
722 
723  vpRobust r;
724  r.setMinMedianAbsoluteDeviation(1.0); // Image noise in pixels for the algebraic distance
725 
726  unsigned int iter = 0;
727  double var = 1.0;
728  vpColVector x(3);
729  vpMatrix DA(k, 3);
730  vpColVector Db(k);
731  vpColVector xg_prev(2);
732  xg_prev = -10.0;
733 
734  // stop after 4 it or if cog variation between 2 it is more than 1 pixel
735  const unsigned int maxNbIter = 4;
736  const unsigned int widthDA = DA.getCols();
737  while ((iter < maxNbIter) && (var > 0.1)) {
738  for (unsigned int i = 0; i < k; ++i) {
739  for (unsigned int j = 0; j < widthDA; ++j) {
740  DA[i][j] = w[i] * A[i][j];
741  }
742  Db[i] = w[i] * b[i];
743  }
744  x = DA.solveBySVD(Db);
745 
746  // A circle is a particular ellipse. Going from x for circle to K for ellipse
747  // using inverse normalization to go back to pixel values
748  double ratio = vm / um;
749  m_K[0] = (m_K[1] = 1.0 / (um * um));
750  m_K[2] = 0.0;
751  m_K[3] = -(1.0 + (x[0] / 2.0)) / um;
752  m_K[4] = -(ratio + (x[1] / 2.0)) / um;
753  m_K[5] = -x[2] + 1.0 + (ratio * ratio) + x[0] + (ratio * x[1]);
754 
755  getParameters();
756  vpColVector xg(2);
757  xg[0] = m_uc;
758  xg[1] = m_vc;
759  var = (xg - xg_prev).frobeniusNorm();
760  xg_prev = xg;
761 
762  vpColVector residu(k); // near to geometric distance in pixel
763  for (unsigned int i = 0; i < k; ++i) {
764  double x = xp[i];
765  double y = yp[i];
766  double sign = (m_K[0] * x * x) + (m_K[1] * y * y) + (2. * m_K[2] * x * y) + (2. * m_K[3] * x) + (2. * m_K[4] * y) + m_K[5];
767  vpImagePoint ip1, ip2;
768  ip1.set_uv(x, y);
769  double ang = computeAngleOnEllipse(ip1);
770  computePointOnEllipse(ang, ip2);
771  // residu = 0 if point is exactly on the ellipse, not otherwise
772  if (sign > 0) {
773  residu[i] = vpImagePoint::distance(ip1, ip2);
774  }
775  else {
776  residu[i] = -vpImagePoint::distance(ip1, ip2);
777  }
778  }
779  r.MEstimator(vpRobust::TUKEY, residu, w);
780 
781  ++iter;
782  }
783  }
784  else { // we track an ellipse
785 
786  // Homogeneous system A x = 0 ; x is the nullspace of A
787  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
788  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
789 
790  // It would be a bad idea to solve the same system using A x = b where
791  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
792  // cannot consider the case where the origin belongs to the ellipse.
793  // Another possibility would be to consider K0+K1=1 which is always valid,
794  // leading to the system A x = b where
795  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
796 
797  vpMatrix A(nos, 6);
798  // Useful to compute the weights in the robust estimation
799  vpColVector xp(nos), yp(nos);
800  std::list<vpMeSite>::const_iterator end = m_meList.end();
801 
802  for (std::list<vpMeSite>::const_iterator it = m_meList.begin(); it != end; ++it) {
803  vpMeSite p_me = *it;
804  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
805  // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
806  double u = (p_me.get_jfloat() - um) / um;
807  double v = (p_me.get_ifloat() - vm) / vm;
808  A[k][0] = u * u;
809  A[k][1] = v * v;
810  A[k][2] = 2.0 * u * v;
811  A[k][3] = 2.0 * u;
812  A[k][4] = 2.0 * v;
813  A[k][5] = 1.0;
814  // Useful to compute the weights in the robust estimation
815  xp[k] = p_me.get_jfloat();
816  yp[k] = p_me.get_ifloat();
817 
818  ++k;
819  }
820  }
821 
822  const unsigned int minRequiredMe = 5;
823  if (k < minRequiredMe) {
824  throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse"));
825  }
826 
827  vpRobust r;
828 
829  r.setMinMedianAbsoluteDeviation(1.0); // image noise in pixels for the geometrical distance
830  unsigned int iter = 0;
831  double var = 1.0;
832  vpMatrix DA(k, 6);
833  vpMatrix KerDA;
834  vpColVector xg_prev(2);
835  xg_prev = -10.0;
836 
837  // Stop after 4 iterations or if cog variation between 2 iterations is more than 0.1 pixel
838  const unsigned int maxIter = 4;
839  const unsigned int widthDA = DA.getCols();
840  while ((iter < maxIter) && (var > 0.1)) {
841  for (unsigned int i = 0; i < k; ++i) {
842  for (unsigned int j = 0; j < widthDA; ++j) {
843  DA[i][j] = w[i] * A[i][j];
844  }
845  }
846  unsigned int dim = DA.nullSpace(KerDA, 1);
847  if (dim > 1) { // case with less than 5 independent points
848  throw(vpMatrixException(vpMatrixException::rankDeficient, "Linear system for computing the ellipse equation ill conditioned"));
849  }
850 
851 
852  for (unsigned int i = 0; i < 6; ++i) {
853  m_K[i] = KerDA[i][0]; // norm(K) = 1
854  }
855 
856  // inverse normalization
857  m_K[0] *= vm / um;
858  m_K[1] *= um / vm;
859  m_K[3] = (m_K[3] * vm) - (m_K[0] * um) - (m_K[2] * vm);
860  m_K[4] = (m_K[4] * um) - (m_K[1] * vm) - (m_K[2] * um);
861  m_K[5] = (m_K[5] * um * vm) - (m_K[0] * um * um) - (m_K[1] * vm * vm) - (2.0 * m_K[2] * um * vm) - (2.0 * m_K[3] * um) - (2.0 * m_K[4] * vm);
862 
863  getParameters(); // since a, b, and e are used just after
864  vpColVector xg(2);
865  xg[0] = m_uc;
866  xg[1] = m_vc;
867  var = (xg - xg_prev).frobeniusNorm();
868  xg_prev = xg;
869 
870  vpColVector residu(k);
871  for (unsigned int i = 0; i < k; ++i) {
872  double x = xp[i];
873  double y = yp[i];
874  double sign = (m_K[0] * x * x) + (m_K[1] * y * y) + (2. * m_K[2] * x * y) + (2. * m_K[3] * x) + (2. * m_K[4] * y) + m_K[5];
875  vpImagePoint ip1, ip2;
876  ip1.set_uv(x, y);
877  double ang = computeAngleOnEllipse(ip1);
878  computePointOnEllipse(ang, ip2);
879  // residu = 0 if point is exactly on the ellipse, not otherwise
880  if (sign > 0) {
881  residu[i] = vpImagePoint::distance(ip1, ip2);
882  }
883  else {
884  residu[i] = -vpImagePoint::distance(ip1, ip2);
885  }
886  }
887  r.MEstimator(vpRobust::TUKEY, residu, w);
888 
889  ++iter;
890  }
891  } // end of case ellipse
892 
893  // Remove bad points and outliers from the lists
894  // Modify the angle to order the list
895  double previous_ang = -4.0 * M_PI;
896  k = 0;
897  std::list<double>::iterator angleList = m_angleList.begin();
898  std::list<vpMeSite>::iterator end = m_meList.end();
899  std::list<vpMeSite>::iterator meList = m_meList.begin();
900  while (meList != end) {
901  vpMeSite p_me = *meList;
902  if (p_me.getState() != vpMeSite::NO_SUPPRESSION) {
903  // points not selected as me
904  double ang = *angleList;
905  meList = m_meList.erase(meList);
906  angleList = m_angleList.erase(angleList);
907  if (vpDEBUG_ENABLE(3)) {
908  vpImagePoint iP;
909  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
910  printf("point %d not me i : %.0f , j : %0.f, ang = %lf\n", k, p_me.get_ifloat(), p_me.get_jfloat(), vpMath::deg(ang));
911  const unsigned int crossSize = 10;
912  vpDisplay::displayCross(I, iP, crossSize, vpColor::blue, 1);
913  }
914  }
915  else {
916  if (w[k] < m_thresholdWeight) { // outlier
917  double ang = *angleList;
918  meList = m_meList.erase(meList);
919  angleList = m_angleList.erase(angleList);
920  if (vpDEBUG_ENABLE(3)) {
921  vpImagePoint iP;
922  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
923  printf("point %d outlier i : %.0f , j : %0.f, ang = %lf, poids : %lf\n",
924  k, p_me.get_ifloat(), p_me.get_jfloat(), vpMath::deg(ang), w[k]);
925  const unsigned int crossSize = 10;
926  vpDisplay::displayCross(I, iP, crossSize, vpColor::cyan, 1);
927  }
928  }
929  else { // good point
930  double ang = *angleList;
931  vpImagePoint iP;
932  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
933  double new_ang = computeAngleOnEllipse(iP);
934  if ((new_ang - ang) > M_PI) {
935  new_ang -= 2.0 * M_PI;
936  }
937  else if ((ang - new_ang) > M_PI) {
938  new_ang += 2.0 * M_PI;
939  }
940  previous_ang = new_ang;
941  *angleList = new_ang;
942  ++meList;
943  ++angleList;
944  if (vpDEBUG_ENABLE(3)) {
945  printf("point %d inlier i : %.0f , j : %0.f, poids : %lf\n", k, p_me.get_ifloat(), p_me.get_jfloat(), w[k]);
946  const unsigned int crossSize = 10;
947  vpDisplay::displayCross(I, iP, crossSize, vpColor::cyan, 1);
948  }
949  }
950  ++k; // k contains good points and outliers (used for w[k])
951  }
952  }
953 
954  if (m_meList.size() != m_angleList.size()) {
955  // Should never occur
956  throw(vpTrackingException(vpTrackingException::fatalError, "Lists are not coherent in vpMeEllipse::leastSquareRobust(): nb MEs %ld, nb ang %ld",
957  m_meList.size(), m_angleList.size()));
958  }
959 
960  // Manage the list so that all new angles belong to [0;2Pi]
961  bool nbdeb = false;
962  std::list<double> finAngle;
963  finAngle.clear();
964  std::list<vpMeSite> finMe;
965  finMe.clear();
966  std::list<double>::iterator debutAngleList;
967  std::list<vpMeSite>::iterator debutMeList;
968  angleList = m_angleList.begin();
969  meList = m_meList.begin();
970  end = m_meList.end();
971  while (meList != end) {
972  vpMeSite p_me = *meList;
973  double ang = *angleList;
974 
975  // Move these ones to another list to be added at the end
976  if (ang < m_alpha1) {
977  ang += 2.0 * M_PI;
978  angleList = m_angleList.erase(angleList);
979  finAngle.push_back(ang);
980  meList = m_meList.erase(meList);
981  finMe.push_back(p_me);
982  }
983  // Moved at the beginning of the list
984  else if (ang > m_alpha2) {
985  ang -= 2.0 * M_PI;
986  angleList = m_angleList.erase(angleList);
987  meList = m_meList.erase(meList);
988  if (!nbdeb) {
989  m_angleList.push_front(ang);
990  debutAngleList = m_angleList.begin();
991  ++debutAngleList;
992 
993  m_meList.push_front(p_me);
994  debutMeList = m_meList.begin();
995  ++debutMeList;
996 
997  nbdeb = true;
998  }
999  else {
1000  debutAngleList = m_angleList.insert(debutAngleList, ang);
1001  ++debutAngleList;
1002  debutMeList = m_meList.insert(debutMeList, p_me);
1003  ++debutMeList;
1004  }
1005  }
1006  else {
1007  ++angleList;
1008  ++meList;
1009  }
1010  }
1011  // Fuse the lists
1012  angleList = m_angleList.end();
1013  m_angleList.splice(angleList, finAngle);
1014  meList = m_meList.end();
1015  m_meList.splice(meList, finMe);
1016 
1017  unsigned int numberOfGoodPoints = 0;
1018  previous_ang = -4.0 * M_PI;
1019 
1020  // Perimeter of the ellipse using Ramanujan formula
1021  double perim = M_PI * ((3.0 * (m_a + m_b)) - sqrt(((3.0 * m_a) + m_b) * (m_a + (3.0 * m_b))));
1022  unsigned int nb_pt = static_cast<unsigned int>(floor(perim / m_me->getSampleStep()));
1023  double incr = (2.0 * M_PI) / nb_pt;
1024  // Update of the expected density
1025  if (!m_trackArc) { // number of points for a complete ellipse
1026  m_expectedDensity = nb_pt;
1027  }
1028  else { // number of points for an arc of ellipse
1029  m_expectedDensity *= static_cast<unsigned int>(floor((perim / m_me->getSampleStep()) * ((m_alpha2 - m_alpha1) / (2.0 * M_PI))));
1030  }
1031 
1032  // Keep only the points in the interval [alpha1 ; alpha2]
1033  // and those that are not too close
1034  angleList = m_angleList.begin();
1035  end = m_meList.end();
1036  meList = m_meList.begin();
1037  while (meList != end) {
1038  vpMeSite p_me = *meList;
1039  double new_ang = *angleList;
1040  if ((new_ang >= m_alpha1) && (new_ang <= m_alpha2)) {
1041  if ((new_ang - previous_ang) >= (0.6 * incr)) {
1042  previous_ang = new_ang;
1043  ++numberOfGoodPoints;
1044  ++meList;
1045  ++angleList;
1046  if (vpDEBUG_ENABLE(3)) {
1047  vpImagePoint iP;
1048  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
1049  const unsigned int crossSize = 10;
1050  vpDisplay::displayCross(I, iP, crossSize, vpColor::red, 1);
1051  printf("In LQR: angle : %lf, i = %.0lf, j = %.0lf\n", vpMath::deg(new_ang), iP.get_i(), iP.get_j());
1052  }
1053  }
1054  else {
1055  meList = m_meList.erase(meList);
1056  angleList = m_angleList.erase(angleList);
1057  if (vpDEBUG_ENABLE(3)) {
1058  vpImagePoint iP;
1059  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
1060  const unsigned int crossSize = 10;
1061  vpDisplay::displayCross(I, iP, crossSize, vpColor::orange, 1);
1062  printf("too near : angle %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.get_ifloat(), p_me.get_jfloat());
1063  }
1064  }
1065  }
1066  else { // point not in the interval [alpha1 ; alpha2]
1067  meList = m_meList.erase(meList);
1068  angleList = m_angleList.erase(angleList);
1069  if (vpDEBUG_ENABLE(3)) {
1070  vpImagePoint iP;
1071  iP.set_ij(p_me.m_ifloat, p_me.m_jfloat);
1072  const unsigned int crossSize = 10;
1073  vpDisplay::displayCross(I, iP, crossSize, vpColor::green, 1);
1074  printf("not in interval: angle : %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.get_ifloat(), p_me.get_jfloat());
1075  }
1076  }
1077  }
1078 
1079  if ((m_meList.size() != numberOfGoodPoints) || (m_angleList.size() != numberOfGoodPoints)) {
1080  // Should never occur
1081  throw(vpTrackingException(vpTrackingException::fatalError, "Lists are not coherent at the end of vpMeEllipse::leastSquareRobust(): nb goog MEs %d and %ld, nb ang %ld",
1082  numberOfGoodPoints, m_meList.size(), m_angleList.size()));
1083  }
1084 
1085  // set extremities of the angle list
1086  m_alphamin = m_angleList.front();
1087  m_alphamax = m_angleList.back();
1088 
1089  if (vpDEBUG_ENABLE(3)) {
1090  printf("alphamin : %lf, alphamax : %lf\n", vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
1091  printf("Fin leastSquareRobust : nb pts ok = %d \n", numberOfGoodPoints);
1092  }
1093 
1094  return numberOfGoodPoints;
1095 }
1096 
1097 void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpColor &col, unsigned int thickness)
1098 {
1099  vpMeEllipse::displayEllipse(I, m_iPc, m_a, m_b, m_e, m_alpha1, m_alpha2, col, thickness);
1100 }
1101 
1102 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, bool trackCircle, bool trackArc)
1103 {
1104  unsigned int n = 5; // by default, 5 points for an ellipse
1105  const unsigned int nForCircle = 3;
1106  m_trackCircle = trackCircle;
1107  if (trackCircle) {
1108  n = nForCircle;
1109  }
1110  std::vector<vpImagePoint> iP(n);
1111  m_trackArc = trackArc;
1112 
1113  vpDisplay::flush(I);
1114 
1115  if (m_trackCircle) {
1116  if (m_trackArc) {
1117  std::cout << "First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
1118  }
1119  for (unsigned int k = 0; k < n; ++k) {
1120  std::cout << "Click point " << (k + 1) << "/" << n << " on the circle " << std::endl;
1121  vpDisplay::getClick(I, iP[k], true);
1122  const unsigned int crossSize = 10;
1123  vpDisplay::displayCross(I, iP[k], crossSize, vpColor::red);
1124  vpDisplay::flush(I);
1125  std::cout << iP[k] << std::endl;
1126  }
1127  }
1128  else {
1129  if (m_trackArc) {
1130  std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
1131  }
1132  for (unsigned int k = 0; k < n; ++k) {
1133  std::cout << "Click point " << (k + 1) << "/" << n << " on the ellipse " << std::endl;
1134  vpDisplay::getClick(I, iP[k], true);
1135  const unsigned int crossSize = 10;
1136  vpDisplay::displayCross(I, iP[k], crossSize, vpColor::red);
1137  vpDisplay::flush(I);
1138  std::cout << iP[k] << std::endl;
1139  }
1140  }
1141  initTracking(I, iP, trackCircle, trackArc);
1142 }
1143 
1144 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
1145 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, std::optional<std::vector<vpImagePoint>> &opt_ips, bool trackCircle, bool trackArc)
1146 #else
1147 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, std::vector<vpImagePoint> *opt_ips, bool trackCircle, bool trackArc)
1148 #endif
1149 {
1150 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
1151  if (!opt_ips.has_value())
1152 #else
1153  if (opt_ips == nullptr)
1154 #endif
1155  {
1156  initTracking(I, trackCircle, trackArc);
1157  return;
1158  }
1159 
1160  if (opt_ips->size() != 0) {
1161  initTracking(I, *opt_ips, trackCircle, trackArc);
1162  return;
1163  }
1164 
1165  unsigned int n = 5; // by default, 5 points for an ellipse
1166  const unsigned int nForCircle = 3;
1167  m_trackCircle = trackCircle;
1168  if (trackCircle) {
1169  n = nForCircle;
1170  }
1171  opt_ips->resize(n);
1172  m_trackArc = trackArc;
1173 
1174  vpDisplay::flush(I);
1175 
1176  if (m_trackCircle) {
1177  if (m_trackArc) {
1178  std::cout << "First and third points specify the extremities of the arc of circle (clockwise)" << std::endl;
1179  }
1180  for (unsigned int k = 0; k < n; ++k) {
1181  std::cout << "Click point " << (k + 1) << "/" << n << " on the circle " << std::endl;
1182  vpDisplay::getClick(I, (*opt_ips)[k], true);
1183  const unsigned int crossSize = 10;
1184  vpDisplay::displayCross(I, (*opt_ips)[k], crossSize, vpColor::red);
1185  vpDisplay::flush(I);
1186  std::cout << (*opt_ips)[k] << std::endl;
1187  }
1188  }
1189  else {
1190  if (m_trackArc) {
1191  std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
1192  }
1193  for (unsigned int k = 0; k < n; ++k) {
1194  std::cout << "Click point " << (k + 1) << "/" << n << " on the ellipse " << std::endl;
1195  vpDisplay::getClick(I, (*opt_ips)[k], true);
1196  const unsigned int crossSize = 10;
1197  vpDisplay::displayCross(I, (*opt_ips)[k], crossSize, vpColor::red);
1198  vpDisplay::flush(I);
1199  std::cout << (*opt_ips)[k] << std::endl;
1200  }
1201  }
1202  initTracking(I, *opt_ips, trackCircle, trackArc);
1203 }
1204 
1205 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP,
1206  bool trackCircle, bool trackArc)
1207 {
1208  m_trackArc = trackArc;
1209  m_trackCircle = trackCircle;
1210  // useful for sample(I):
1211  leastSquare(I, iP);
1212  if (trackArc) {
1213  // useful for track(I):
1214  m_iP1 = iP.front();
1215  m_iP2 = iP.back();
1216  // useful for sample(I):
1219  if ((m_alpha2 <= m_alpha1) || (std::fabs(m_alpha2 - m_alpha1) < m_arcEpsilon)) {
1220  m_alpha2 += 2.0 * M_PI;
1221  }
1222  }
1223  else {
1224  m_alpha1 = 0.0;
1225  m_alpha2 = 2.0 * M_PI;
1226  // useful for track(I):
1227  vpImagePoint ip;
1229  m_iP1 = (m_iP2 = ip);
1230  }
1231 
1232  sample(I);
1233  track(I);
1235  vpDisplay::flush(I);
1236 }
1237 
1239  const vpImagePoint *pt2, bool trackCircle)
1240 {
1241  m_trackCircle = trackCircle;
1242  if ((pt1 != nullptr) && (pt2 != nullptr)) {
1243  m_trackArc = true;
1244  }
1245  // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
1246  m_uc = param[0];
1247  m_vc = param[1];
1248  m_n20 = param[2];
1249  m_n11 = param[3];
1250  m_n02 = param[4];
1252  computeKiFromNij();
1253 
1254  if (m_trackArc && pt1 && pt2) {
1257  if ((m_alpha2 <= m_alpha1) || (std::fabs(m_alpha2 - m_alpha1) < m_arcEpsilon)) {
1258  m_alpha2 += 2.0 * M_PI;
1259  }
1260  // useful for track(I)
1261  m_iP1 = *pt1;
1262  m_iP2 = *pt2;
1263  }
1264  else {
1265  m_alpha1 = 0.0;
1266  m_alpha2 = 2.0 * M_PI;
1267  // useful for track(I)
1268  vpImagePoint ip;
1270  m_iP1 = (m_iP2 = ip);
1271  }
1272  // useful for display(I) so useless if no display before track(I)
1273  m_iPc.set_uv(m_uc, m_vc);
1274 
1275  sample(I);
1276  track(I);
1278  vpDisplay::flush(I);
1279 }
1280 
1287 {
1288  vpMeTracker::track(I);
1289 
1290  // recompute alpha1 and alpha2 in case they have been changed by setEndPoints()
1291  if (m_trackArc) {
1294  if ((m_alpha2 <= m_alpha1) || (std::fabs(m_alpha2 - m_alpha1) < m_arcEpsilon)) {
1295  m_alpha2 += 2.0 * M_PI;
1296  }
1297  }
1298  // Compute the ellipse parameters from the tracked points, manage the lists,
1299  // and update the expected density (
1301  if (vpDEBUG_ENABLE(3)) {
1302  printf("1st step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1305  }
1306 
1307  if (plugHoles(I) > 0) {
1308  m_numberOfGoodPoints = leastSquareRobust(I); // if new points have been added, recompute the ellipse parameters and manage again the lists
1309  if (vpDEBUG_ENABLE(3)) {
1310  printf("2nd step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n", m_numberOfGoodPoints, m_expectedDensity,
1312  }
1313  }
1314 
1315  const unsigned int minNbGoodPoints = 5;
1316  if (m_numberOfGoodPoints <= minNbGoodPoints) {
1317  if (vpDEBUG_ENABLE(3)) {
1318  printf("Before RESAMPLE !!! nb points %d \n", m_numberOfGoodPoints);
1319  printf("A click to continue \n");
1320  vpDisplay::flush(I);
1322  vpDisplay::display(I);
1323  }
1324  sample(I);
1325  vpMeTracker::track(I);
1326  leastSquareRobust(I);
1327  if (vpDEBUG_ENABLE(3)) {
1328  printf("nb of Good points %u, density %d %lf, alphamin %lf, alphamax\n",
1331  }
1332 
1333  // Stop in case of failure after resample
1334  if (m_numberOfGoodPoints <= minNbGoodPoints) {
1335  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Impossible to track the ellipse, not enough features"));
1336  }
1337  }
1338 
1339  if (vpDEBUG_ENABLE(3)) {
1340  printParameters();
1341  }
1342 
1343  // remet a jour l'angle delta pour chaque vpMeSite de la liste
1344  updateTheta();
1345  // not in getParameters since computed only once for each image
1346  m_m00 = M_PI * m_a * m_b;
1347 
1348  // Useful only for tracking an arc of ellipse, but done to give them a value
1351 
1352  if (vpDEBUG_ENABLE(3)) {
1353  display(I, vpColor::red);
1355  vpDisplay::flush(I);
1356  }
1357 }
1358 
1359 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1360 
1385 void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A,
1386  const double &B, const double &E, const double &smallalpha, const double &highalpha,
1387  const vpColor &color, unsigned int thickness)
1388 {
1389  vpMeEllipse::displayEllipse(I, center, A, B, E, smallalpha, highalpha, color, thickness);
1390 }
1391 
1419 void vpMeEllipse::display(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
1420  const double &E, const double &smallalpha, const double &highalpha,
1421  const vpColor &color, unsigned int thickness)
1422 {
1423  vpMeEllipse::displayEllipse(I, center, A, B, E, smallalpha, highalpha, color, thickness);
1424 }
1425 #endif // Deprecated
1426 
1427 
1428 void vpMeEllipse::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A,
1429  const double &B, const double &E, const double &smallalpha, const double &highalpha,
1430  const vpColor &color, unsigned int thickness)
1431 {
1432  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1433 }
1434 
1435 void vpMeEllipse::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
1436  const double &E, const double &smallalpha, const double &highalpha,
1437  const vpColor &color, unsigned int thickness)
1438 {
1439  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1440 }
unsigned int getCols() const
Definition: vpArray2D.h:327
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:339
unsigned int getRows() const
Definition: vpArray2D.h:337
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor cyan
Definition: vpColor.h:220
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 display(const vpImage< unsigned char > &I)
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)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
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
double get_u() const
Definition: vpImagePoint.h:136
void set_u(double u)
Definition: vpImagePoint.h:330
void set_uv(double u, double v)
Definition: vpImagePoint.h:352
void set_v(double v)
Definition: vpImagePoint.h:341
double get_i() const
Definition: vpImagePoint.h:114
double get_v() const
Definition: vpImagePoint.h:147
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static double deg(double rad)
Definition: vpMath.h:117
error that can be emitted by the vpMatrix class and its derivatives
@ rankDeficient
Rank deficient.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:2010
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6130
Class that tracks an ellipse using moving edges.
Definition: vpMeEllipse.h:94
double m_n20
Second order centered and normalized moments .
Definition: vpMeEllipse.h:455
double m_arcEpsilon
Epsilon value used to check if arc angles are the same.
Definition: vpMeEllipse.h:469
double m_e
Definition: vpMeEllipse.h:411
void computePointOnEllipse(const double angle, vpImagePoint &iP)
void display(const vpImage< unsigned char > &I, const vpColor &col, unsigned int thickness=1)
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
double m_alpha2
Definition: vpMeEllipse.h:429
virtual ~vpMeEllipse() vp_override
Definition: vpMeEllipse.cpp:71
vpImagePoint m_iPc
The coordinates of the ellipse center.
Definition: vpMeEllipse.h:403
double m_vc
Value of v coordinate of iPc.
Definition: vpMeEllipse.h:453
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
Definition: vpMeEllipse.h:463
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:451
bool m_trackCircle
Track a circle (true) or an ellipse (false).
Definition: vpMeEllipse.h:465
void computeAbeFromNij()
vpColVector m_K
Definition: vpMeEllipse.h:401
double m_m00
Ellipse area.
Definition: vpMeEllipse.h:437
double m_alphamin
Definition: vpMeEllipse.h:445
std::list< double > m_angleList
Stores the value in increasing order of the angle on the ellipse for each vpMeSite.
Definition: vpMeEllipse.h:435
bool m_trackArc
Track an arc of ellipse/circle (true) or a complete one (false).
Definition: vpMeEllipse.h:467
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:405
unsigned int leastSquareRobust(const vpImage< unsigned char > &I)
double m_alpha1
Definition: vpMeEllipse.h:424
double m_b
is the semi minor axis of the ellipse.
Definition: vpMeEllipse.h:407
double m_alphamax
Definition: vpMeEllipse.h:449
double computeTheta(const vpImagePoint &iP) const
Definition: vpMeEllipse.cpp:77
double m_se
Value of sin(e).
Definition: vpMeEllipse.h:433
double m_n02
Second order centered and normalized moments .
Definition: vpMeEllipse.h:459
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:461
void updateTheta()
Definition: vpMeEllipse.cpp:97
vpImagePoint m_iP2
Definition: vpMeEllipse.h:420
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false) vp_override
double m_n11
Second order centered and normalized moments .
Definition: vpMeEllipse.h:457
vpImagePoint m_iP1
Definition: vpMeEllipse.h:416
void track(const vpImage< unsigned char > &I)
double m_ce
Value of cos(e).
Definition: vpMeEllipse.h:431
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 m_thresholdWeight
Threshold on the weights for the robust least square.
Definition: vpMeEllipse.h:440
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:65
@ 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
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:212
double m_alpha
Angle of tangent at site.
Definition: vpMeSite.h:106
void init()
Definition: vpMeSite.cpp:55
vpMeSiteState getState() const
Definition: vpMeSite.h:266
double get_ifloat() const
Definition: vpMeSite.h:192
double m_jfloat
Subpixel coordinates along j of a site.
Definition: vpMeSite.h:102
double get_jfloat() const
Definition: vpMeSite.h:198
void setContrastThreshold(const double &thresh, const vpMe &me)
Definition: vpMeSite.h:286
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)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMeTracker.h:74
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
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:76
std::list< vpMeSite > m_meList
Definition: vpMeTracker.h:66
static bool inMeMaskCandidates(const vpImage< bool > *meMaskCandidates, unsigned int i, unsigned int j)
void setRange(const unsigned int &range)
Definition: vpMe.h:429
double getThresholdMarginRatio() const
Definition: vpMe.h:314
void setSampleStep(const double &sample_step)
Definition: vpMe.h:436
double getSampleStep() const
Definition: vpMe.h:289
unsigned int getRange() const
Definition: vpMe.h:282
Contains an M-estimator and various influence function.
Definition: vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:88
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:136
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:156
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ fatalError
Tracker fatal error.
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:524