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