Visual Servoing Platform  version 3.5.1 under development (2022-12-02)
vpMeEllipse.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Moving edges.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/me/vpMeEllipse.h>
40 
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpImagePoint.h>
44 #include <visp3/core/vpRobust.h>
45 #include <visp3/me/vpMe.h>
46 
47 #include <cmath> // std::fabs
48 #include <limits> // numeric_limits
49 #include <vector>
50 
55  : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.),
56 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
57  mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
58 #endif
59  thresholdWeight(0.2),
60 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
61  expecteddensity(0.),
62 #endif
63  m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.), m_expectedDensity(0),
64  m_numberOfGoodPoints(0), m_trackArc(false), m_arcEpsilon(1e-6)
65 {
66  // Resize internal parameters vector
67  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
68  K.resize(6);
69  iP1.set_ij(0, 0);
70  iP2.set_ij(0, 0);
71 }
72 
77  : vpMeTracker(me_ellipse), K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
78  iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2), ce(me_ellipse.ce),
79  se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
80 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
81  mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02), m10(me_ellipse.m10), m01(me_ellipse.m01),
82  m11(me_ellipse.m11), m02(me_ellipse.m02), m20(me_ellipse.m20),
83 #endif
84  thresholdWeight(me_ellipse.thresholdWeight),
85 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
86  expecteddensity(me_ellipse.expecteddensity),
87 #endif
88  m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax), m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
89  m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
90  m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints),
91  m_trackArc(me_ellipse.m_trackArc)
92 {
93 }
94 
99 {
100  list.clear();
101  angle.clear();
102 }
103 
112 {
113  double u = iP.get_u();
114  double v = iP.get_v();
115 
116  return (computeTheta(u, v));
117 }
118 
126 double vpMeEllipse::computeTheta(double u, double v) const
127 {
128  double A = K[0] * u + K[2] * v + K[3];
129  double B = K[1] * v + K[2] * u + K[4];
130 
131  double theta = atan2(B, A); // Angle between the tangent and the u axis.
132  if (theta < 0) { // theta in [0;Pi] // FC : pourquoi ? pour me sans doute
133  theta += M_PI;
134  }
135  return (theta);
136 }
137 
144 {
145  vpMeSite p_me;
146  vpImagePoint iP;
147  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
148  p_me = *it;
149  // (i,j) frame used for vpMESite
150  iP.set_ij(p_me.ifloat, p_me.jfloat);
151  p_me.alpha = computeTheta(iP);
152  *it = p_me;
153  }
154 }
155 
164 {
165  // Two versions are available. If you change from one version to the other
166  // one, do not forget to change also the reciprocical function
167  // computeAngleOnEllipse() just below and, for a correct display of an arc
168  // of ellipse, adapt vpMeEllipse::display() below and
169  // vp_display_display_ellipse() in modules/core/src/display/vpDisplay_impl.h
170  // so that the two extremities of the arc are correctly shown.
171 
172  // Version that gives a regular angular sampling on the ellipse, so less
173  // points at its extremities
174  /*
175  double co = cos(angle);
176  double si = sin(angle);
177  double coef = a * b / sqrt(b * b * co * co + a * a * si * si);
178  double u = co * coef;
179  double v = si * coef;
180  iP.set_u(uc + ce * u - se * v);
181  iP.set_v(vc + se * u + ce * v);
182  */
183 
184  // Version from "the two concentric circles" method that gives more points
185  // at the ellipse extremities for a regular angle sampling. It is better to
186  // display an ellipse, not necessarily to track it
187 
188  // (u,v) are the coordinates on the canonical centered ellipse;
189  double u = a * cos(angle);
190  double v = b * sin(angle);
191  // a rotation of e and a translation by (uc,vc) are done
192  // to get the coordinates of the point on the shifted ellipse
193  iP.set_uv(m_uc + ce * u - se * v, m_vc + se * u + ce * v);
194 }
195 
202 {
203  // Two versions are available. If you change from one version to the other
204  // one, do not forget to change also the reciprocical function
205  // computePointOnEllipse() just above. Adapt also the display; see comment
206  // at the beginning of computePointOnEllipse()
207 
208  // Regular angle smapling method
209  /*
210  double du = pt.get_u() - uc;
211  double dv = pt.get_v() - vc;
212  double ang = atan2(dv,du) - e;
213  if (ang > M_PI) {
214  ang -= 2.0 * M_PI;
215  }
216  else if (ang < -M_PI) {
217  ang += 2.0 * M_PI;
218  }
219  */
220  // for the "two concentric circles method" starting from the previous one
221  // (just to remember the link between both methods:
222  // tan(theta_2cc) = a/b tan(theta_rs))
223  /*
224  double co = cos(ang);
225  double si = sin(ang);
226  double coeff = 1.0/sqrt(b*b*co*co+a*a*si*si);
227  si *= a*coeff;
228  co *= b*coeff;
229  ang = atan2(si,co);
230  */
231  // For the "two concentric circles" method starting from scratch
232  double du = pt.get_u() - m_uc;
233  double dv = pt.get_v() - m_vc;
234  double co = (du * ce + dv * se) / a;
235  double si = (-du * se + dv * ce) / b;
236  double angle = atan2(si, co);
237 
238  return (angle);
239 }
240 
248 {
249  double num = m_n20 - m_n02;
250  double d = num * num + 4.0 * m_n11 * m_n11; // always >= 0
251  if (d <= std::numeric_limits<double>::epsilon()) {
252  e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
253  ce = 1.0;
254  se = 0.0;
255  a = b = 2.0 * sqrt(m_n20); // = sqrt(2.0*(n20+n02))
256  } else { // real ellipse
257  e = atan2(2.0 * m_n11, num) / 2.0; // e in [-Pi/2 ; Pi/2]
258  ce = cos(e);
259  se = sin(e);
260 
261  d = sqrt(d); // d in sqrt always >= 0
262  num = m_n20 + m_n02;
263  a = sqrt(2.0 * (num + d)); // term in sqrt always > 0
264  b = sqrt(2.0 * (num - d)); // term in sqrt always > 0
265  }
266 }
267 
275 {
276  K[0] = m_n02;
277  K[1] = m_n20;
278  K[2] = -m_n11;
279  K[3] = m_n11 * m_vc - m_n02 * m_uc;
280  K[4] = m_n11 * m_uc - m_n20 * m_vc;
281  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);
282 }
283 
290 {
291  m_n20 = 0.25 * (a * a * ce * ce + b * b * se * se);
292  m_n11 = 0.25 * se * ce * (a * a - b * b);
293  m_n02 = 0.25 * (a * a * se * se + b * b * ce * ce);
294 }
295 
306 {
307  // Equations below from Chaumette PhD and TRO 2004 paper
308  double num = K[0] * K[1] - K[2] * K[2]; // > 0 for an ellipse
309  if (num <= 0) {
310  throw(vpException(vpException::fatalError, "The points do not belong to an ellipse!"));
311  }
312 
313  m_uc = (K[2] * K[4] - K[1] * K[3]) / num;
314  m_vc = (K[2] * K[3] - K[0] * K[4]) / num;
315  iPc.set_uv(m_uc, m_vc);
316 
317  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);
318  m_n20 = K[1] * d; // always > 0
319  m_n11 = -K[2] * d;
320  m_n02 = K[0] * d; // always > 0
321 
323 
324  // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper)
325  d = m_n02 / K[0]; // fabs(K[0]) > 0
326  for (unsigned int i = 0; i < 6; i++) {
327  K[i] *= d;
328  }
329  if (vpDEBUG_ENABLE(3)) {
330  printParameters();
331  }
332 }
333 
339 {
340  std::cout << "K :" << K.t() << std::endl;
341  std::cout << "xc = " << m_uc << ", yc = " << m_vc << std::endl;
342  std::cout << "n20 = " << m_n20 << ", n11 = " << m_n11 << ", n02 = " << m_n02 << std::endl;
343  std::cout << "A = " << a << ", B = " << b << ", E (dg) " << vpMath::deg(e) << std::endl;
344 }
345 
358 void vpMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
359 {
360  // Warning: similar code in vpMbtMeEllipse::sample()
361  if (!me) {
362  throw(vpException(vpException::fatalError, "Moving edges on ellipse not initialized"));
363  }
364  // Delete old lists
365  list.clear();
366  angle.clear();
367 
368  int nbrows = static_cast<int>(I.getHeight());
369  int nbcols = static_cast<int>(I.getWidth());
370 
371  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
372  std::cout << "In vpMeEllipse::sample: ";
373  std::cout << "function called with sample step = 0, set to 10 dg";
374  me->setSampleStep(10.0);
375  }
376  double incr = vpMath::rad(me->getSampleStep()); // angle increment
377  // alpha2 - alpha1 = 2 * M_PI for a complete ellipse
378  m_expectedDensity = static_cast<unsigned int>(floor((alpha2 - alpha1) / incr));
379 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
380  expecteddensity = static_cast<double>(m_expectedDensity);
381 #endif
382 
383  // starting angle for sampling
384  double ang = alpha1 + ((alpha2 - alpha1) - static_cast<double>(m_expectedDensity) * incr) / 2.0;
385  // sample positions
386  for (unsigned int i = 0; i < m_expectedDensity; i++) {
387  vpImagePoint iP;
388  computePointOnEllipse(ang, iP);
389  // If point is in the image, add to the sample list
390  // Check done in (i,j) frame)
391  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
393 
394  double theta = computeTheta(iP);
395  vpMeSite pix;
396  // (i,j) frame used for vpMeSite
397  pix.init(iP.get_i(), iP.get_j(), theta);
400  list.push_back(pix);
401  angle.push_back(ang);
402  }
403  ang += incr;
404  }
405  if (!doNotTrack) {
407  }
408 }
409 
422 {
423  if (!me) {
424  throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
425  }
426  unsigned int nb_pts_added = 0;
427  int nbrows = static_cast<int>(I.getHeight());
428  int nbcols = static_cast<int>(I.getWidth());
429 
430  unsigned int memory_range = me->getRange();
431  me->setRange(2);
432  double memory_mu1 = me->getMu1();
433  me->setMu1(0.5);
434  double memory_mu2 = me->getMu2();
435  me->setMu2(0.5);
436 
437  double incr = vpMath::rad(me->getSampleStep());
438  // Detect holes and try to complete them
439  // FC : Currently only one point is looked at the middle of each hole
440  // (to avoid multiple insertions that are time consuming).
441  // A different choice could be done.
442  std::list<double>::iterator angleList = angle.begin();
443  double ang = *angleList;
444  for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
445  ++angleList;
446  ++meList;
447  double nextang = *angleList;
448  // The minimal size of a hole (1 point lost for sure).
449  // could be increased to reduce time processing
450  if ((nextang - ang) > 1.6 * incr) {
451  ang = (nextang + ang) / 2.0; // mid angle
452  vpImagePoint iP;
453  computePointOnEllipse(ang, iP);
454  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
455  double theta = computeTheta(iP);
456  vpMeSite pix;
457  pix.init(iP.get_i(), iP.get_j(), theta);
460  pix.track(I, me, false);
461  if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
462  nb_pts_added++;
463  iP.set_ij(pix.ifloat, pix.jfloat);
464  double new_ang = computeAngleOnEllipse(iP);
465  if ((new_ang - ang) > M_PI) {
466  new_ang -= 2.0 * M_PI;
467  } else if ((ang - new_ang) > M_PI) {
468  new_ang += 2.0 * M_PI;
469  }
470  list.insert(meList, pix);
471  angle.insert(angleList, new_ang);
472  if (vpDEBUG_ENABLE(3)) {
474  }
475  }
476  }
477  }
478  ang = nextang;
479  }
480 
481  // Try to fill the first extremity: from alpha_min - incr to alpha1
482  unsigned int nbpts = static_cast<unsigned int>(floor((m_alphamin - alpha1) / incr));
483  ang = m_alphamin - incr;
484  for (unsigned int i = 0; i < nbpts; i++) {
485  vpImagePoint iP;
486  computePointOnEllipse(ang, iP);
487  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
488  double theta = computeTheta(iP);
489  vpMeSite pix;
490  pix.init(iP.get_i(), iP.get_j(), theta);
493  pix.track(I, me, false);
494  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
495  nb_pts_added++;
496  iP.set_ij(pix.ifloat, pix.jfloat);
497  double new_ang = computeAngleOnEllipse(iP);
498  if ((new_ang - ang) > M_PI) {
499  new_ang -= 2.0 * M_PI;
500  } else if ((ang - new_ang) > M_PI) {
501  new_ang += 2.0 * M_PI;
502  }
503  list.push_front(pix);
504  angle.push_front(new_ang);
505  if (vpDEBUG_ENABLE(3)) {
507  }
508  }
509  }
510  ang -= incr;
511  }
512 
513  // Try to fill the second extremity: from alphamax + incr to alpha2
514  nbpts = static_cast<unsigned int>(floor((alpha2 - m_alphamax) / incr));
515  ang = m_alphamax + incr;
516  for (unsigned int i = 0; i < nbpts; i++) {
517  vpImagePoint iP;
518  computePointOnEllipse(ang, iP);
519  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
520  double theta = computeTheta(iP);
521  vpMeSite pix;
522  pix.init(iP.get_i(), iP.get_j(), theta);
525  pix.track(I, me, false);
526  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
527  nb_pts_added++;
528  iP.set_ij(pix.ifloat, pix.jfloat);
529  double new_ang = computeAngleOnEllipse(iP);
530  if ((new_ang - ang) > M_PI) {
531  new_ang -= 2.0 * M_PI;
532  } else if ((ang - new_ang) > M_PI) {
533  new_ang += 2.0 * M_PI;
534  }
535  list.push_back(pix);
536  angle.push_back(new_ang);
537  if (vpDEBUG_ENABLE(3)) {
539  }
540  }
541  }
542  ang += incr;
543  }
544  me->setRange(memory_range);
545  me->setMu1(memory_mu1);
546  me->setMu2(memory_mu2);
547 
548  if (vpDEBUG_ENABLE(3)) {
549  printf("In plugHoles(): nb of added points : %d\n", nb_pts_added);
550  }
551  return nb_pts_added;
552 }
553 
561 void vpMeEllipse::leastSquare(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
562 {
563  // Homogeneous system A x = 0 ; x is the nullspace of A
564  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
565  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
566 
567  // It would be a bad idea to solve the same system using A x = b where
568  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
569  // cannot consider the case where the origin belongs to the ellipse.
570  // Another possibility would be to consider K0+K1=1 which is always valid,
571  // leading to the system A x = b where
572  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
573 
574  double um = I.getWidth() / 2.;
575  double vm = I.getHeight() / 2.;
576  unsigned int n = static_cast<unsigned int>(iP.size());
577 
578  vpMatrix A(n, 6);
579 
580  for (unsigned int k = 0; k < n; k++) {
581  // normalization so that (u,v) in [-1;1]
582  double u = (iP[k].get_u() - um) / um;
583  double v = (iP[k].get_v() - vm) / vm;
584  A[k][0] = u * u;
585  A[k][1] = v * v;
586  A[k][2] = 2.0 * u * v;
587  A[k][3] = 2.0 * u;
588  A[k][4] = 2.0 * v;
589  A[k][5] = 1.0;
590  }
591  vpMatrix KerA;
592  unsigned int dim = A.nullSpace(KerA, 1);
593  if (dim > 1) { // case with less than 5 independent points
594  // FC : should create a rankError exception
595  throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
596  }
597  // the term um*vm is for counterbalancing the bad conditioning of the
598  // inverse normalization below
599  for (unsigned int i = 0; i < 6; i++)
600  K[i] = um * vm * KerA[i][0];
601 
602  // Inverse normalization to go back to pixel values
603  K[0] /= um * um;
604  K[1] /= vm * vm;
605  K[2] /= um * vm;
606  K[3] = K[3] / um - K[0] * um - K[2] * vm;
607  K[4] = K[4] / vm - K[1] * vm - K[2] * um;
608  K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
609 
610  getParameters();
611 }
612 
621 {
622  // Homogeneous system A x = 0 ; x is the nullspace of A
623  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
624  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
625 
626  // It would be a bad idea to solve the same system using A x = b where
627  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
628  // cannot consider the case where the origin belongs to the ellipse.
629  // Another possibility would be to consider K0+K1=1 which is always valid,
630  // leading to the system A x = b where
631  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
632 
633  const unsigned int nos = numberOfSignal();
634 
635  // Note that the (nos-k) last rows of A, xp and yp are not used.
636  // Hopefully, this is not an issue.
637 
638  vpMatrix A(nos, 6);
639  // Useful to compute the weights in the robust estimation
640  vpColVector xp(nos), yp(nos);
641 
642  unsigned int k = 0;
643  double um = I.getWidth() / 2.;
644  double vm = I.getHeight() / 2.;
645  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
646  vpMeSite p_me = *it;
647  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
648  // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
649  double u = (p_me.jfloat - um) / um;
650  double v = (p_me.ifloat - vm) / vm;
651  A[k][0] = u * u;
652  A[k][1] = v * v;
653  A[k][2] = 2.0 * u * v;
654  A[k][3] = 2.0 * u;
655  A[k][4] = 2.0 * v;
656  A[k][5] = 1.0;
657  // Useful to compute the weights in the robust estimation
658  xp[k] = p_me.jfloat;
659  yp[k] = p_me.ifloat;
660 
661  k++;
662  }
663  }
664  if (k < 5) {
665  throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse"));
666  }
667 
668  vpRobust r;
669  // r.setThreshold(0.02); // Old version where this threshold was highly
670  // sensitive since the residues do not represent the Euclidean distance
671  // from the point to the ellipse
672  r.setMinMedianAbsoluteDeviation(1.0); // image noise in pixels for the geometrical distance
673  vpColVector w(k);
674  w = 1.0;
675  unsigned int iter = 0;
676  double var = 1.0;
677  vpColVector Kprev(6);
678  vpMatrix DA(k, 6);
679  vpMatrix KerDA;
680 
681  // stop after 4 it or if variation of K between 2 iterations is more than 0.1 %
682  while ((iter < 4) && (var > 0.001)) {
683  for (unsigned int i = 0; i < k; i++) {
684  for (unsigned int j = 0; j < 6; j++) {
685  DA[i][j] = w[i] * A[i][j];
686  }
687  }
688  unsigned int dim = DA.nullSpace(KerDA, 1);
689  if (dim > 1) { // case with less than 5 independent points
690  // FC : should create a rankError exception
691  throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
692  }
693 
694  for (unsigned int i = 0; i < 6; i++)
695  K[i] = KerDA[i][0]; // norm(K) = 1
696  var = (K - Kprev).frobeniusNorm();
697  Kprev = K;
698  // the term um*vm is for counterbalancing the bad conditioning of the
699  // inverse normalization just below
700  K *= (um * vm);
701  // vpColVector residu(k); // old version for considering the algebraic distance
702  // residu = A * K;
703  // Better version considering the geometric distance
704  // Inverse normalization to go back to pixels
705  K[0] /= um * um;
706  K[1] /= vm * vm;
707  K[2] /= um * vm;
708  K[3] = K[3] / um - K[0] * um - K[2] * vm;
709  K[4] = K[4] / vm - K[1] * vm - K[2] * um;
710  K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
711  getParameters(); // since a, b, and e are used just after
712 
713  vpColVector residu(k);
714  for (unsigned int i = 0; i < k; i++) {
715  vpImagePoint ip1, ip2;
716  ip1.set_uv(xp[i], yp[i]);
717  double ang = computeAngleOnEllipse(ip1);
718  computePointOnEllipse(ang, ip2);
719  // residu = 0 if point is exactly on the ellipse, not otherwise
720  residu[i] = vpImagePoint::distance(ip1, ip2);
721  }
722  // end of new version
723  r.MEstimator(vpRobust::TUKEY, residu, w);
724  iter++;
725  }
726  /* FC : for old version with algebraic distance
727  // Inverse normalization to go back to pixels
728  K[0] /= um * um;
729  K[1] /= vm * vm;
730  K[2] /= um * vm;
731  K[3] = K[3]/um - K[0] * um - K[2] * vm;
732  K[4] = K[4]/vm - K[1] * vm - K[2] * um;
733  K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
734  getParameters();
735  */
736  double previous_ang = -4.0 * M_PI;
737  double incr = vpMath::rad(me->getSampleStep());
738  // Remove bad points, too near points, and outliers from the lists
739  k = m_numberOfGoodPoints = 0;
740  std::list<double>::iterator angleList = angle.begin();
741  for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
742  vpMeSite p_me = *meList;
743  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
744  if (w[k] > thresholdWeight) { // inlier
745  // Management of the angle to keep only the points in the interval
746  // [alpha1 ; alpha2]
747  double ang = *angleList;
748  vpImagePoint iP;
749  iP.set_ij(p_me.ifloat, p_me.jfloat);
750  double new_ang = computeAngleOnEllipse(iP);
751  if ((new_ang - ang) > M_PI) {
752  new_ang -= 2.0 * M_PI;
753  } else if ((ang - new_ang) > M_PI) {
754  new_ang += 2.0 * M_PI;
755  }
756  if ((new_ang >= alpha1) && (new_ang <= alpha2)) {
757  // good point if not too near from the previous one in the list
758  // if so, udate of its angle
759  if ((new_ang - previous_ang) >= (0.6 * incr)) {
760  *angleList = previous_ang = new_ang;
762  ++meList;
763  ++angleList;
764  if (vpDEBUG_ENABLE(3)) {
765  vpDisplay::displayCross(I, iP, 10, vpColor::red, 1);
766  printf("In LQR: angle : %lf, i = %.0lf, j = %.0lf\n", vpMath::deg(new_ang), iP.get_i(), iP.get_j());
767  }
768  } else {
769  if (vpDEBUG_ENABLE(3)) {
771  printf("too near : angle %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
772  }
773  meList = list.erase(meList);
774  angleList = angle.erase(angleList);
775  }
776  } else { // point not in the interval [alpha1 ; alpha2]
777  if (vpDEBUG_ENABLE(3)) {
779  printf("not in interval: angle : %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
780  }
781  meList = list.erase(meList);
782  angleList = angle.erase(angleList);
783  }
784  } else { // outlier
785  if (vpDEBUG_ENABLE(3)) {
786  vpImagePoint iP;
787  iP.set_ij(p_me.ifloat, p_me.jfloat);
788  printf("point %d outlier i : %.0f , j : %0.f, poids : %lf\n", k, p_me.ifloat, p_me.jfloat, w[k]);
789  vpDisplay::displayCross(I, iP, 10, vpColor::cyan, 1);
790  }
791  meList = list.erase(meList);
792  angleList = angle.erase(angleList);
793  }
794  k++;
795  } else { // points not selected as me
796  meList = list.erase(meList);
797  angleList = angle.erase(angleList);
798  if (vpDEBUG_ENABLE(3)) {
799  vpImagePoint iP;
800  iP.set_ij(p_me.ifloat, p_me.jfloat);
801  printf("point not me i : %.0f , j : %0.f\n", p_me.ifloat, p_me.jfloat);
802  vpDisplay::displayCross(I, iP, 10, vpColor::blue, 1);
803  }
804  }
805  }
806  // set extremities of the angle list
807  m_alphamin = angle.front();
808  m_alphamax = angle.back();
809 
810  if (vpDEBUG_ENABLE(3)) {
811  printf("alphamin : %lf, alphamax : %lf\n", vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
812  printf("dans leastSquareRobust : nb pts ok = %d \n", m_numberOfGoodPoints);
813  }
814 }
815 
826 {
827  vpMeEllipse::display(I, iPc, a, b, e, alpha1, alpha2, col);
828 }
829 
843 {
844  const unsigned int n = 5;
845  std::vector<vpImagePoint> iP(n);
846  m_trackArc = trackArc;
847 
848  if (m_trackArc) {
849  std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
850  }
851  for (unsigned int k = 0; k < n; k++) {
852  std::cout << "Click point " << k + 1 << "/" << n << " on the ellipse " << std::endl;
853  vpDisplay::getClick(I, iP[k], true);
854  vpDisplay::displayCross(I, iP[k], 10, vpColor::red);
855  vpDisplay::flush(I);
856  std::cout << iP[k] << std::endl;
857  }
858  initTracking(I, iP, trackArc);
859 }
860 
877 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP, bool trackArc)
878 {
879  m_trackArc = trackArc;
880  // useful for sample(I):
881  leastSquare(I, iP);
882  if (trackArc) {
883  // useful for track(I):
884  iP1 = iP.front();
885  iP2 = iP.back();
886  // useful for sample(I):
889  if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
890  alpha2 += 2.0 * M_PI;
891  }
892  } else {
893  alpha1 = 0.0;
894  alpha2 = 2 * M_PI;
895  // useful for track(I):
896  vpImagePoint ip;
898  iP1 = iP2 = ip;
899  }
900 
901  sample(I);
902  track(I);
904  vpDisplay::flush(I);
905 }
906 
922  const vpImagePoint *pt2)
923 {
924  if (pt1 != NULL && pt2 != NULL) {
925  m_trackArc = true;
926  }
927  // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
928  m_uc = param[0];
929  m_vc = param[1];
930  m_n20 = param[2];
931  m_n11 = param[3];
932  m_n02 = param[4];
935 
936  if (m_trackArc) {
939  if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
940  alpha2 += 2.0 * M_PI;
941  }
942  // useful for track(I)
943  iP1 = *pt1;
944  iP2 = *pt2;
945  } else {
946  alpha1 = 0.0;
947  alpha2 = 2.0 * M_PI;
948  // useful for track(I)
949  vpImagePoint ip;
951  iP1 = iP2 = ip;
952  }
953  // useful for display(I) so useless if no display before track(I)
954  iPc.set_uv(m_uc, m_vc);
955 
956  sample(I);
957  track(I);
959  vpDisplay::flush(I);
960 }
961 
968 {
970 
971  // recompute alpha1 and alpha2 in case they have been changed by setEndPoints()
972  if (m_trackArc) {
975  if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
976  alpha2 += 2.0 * M_PI;
977  }
978  }
979  // Compute the ellipse parameters from the tracked points and manage the lists
981  if (vpDEBUG_ENABLE(3)) {
982  printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n", m_numberOfGoodPoints, m_expectedDensity,
984  }
985 
986  // Try adding points at the extremities and in the holes if needed
987  if (m_numberOfGoodPoints < m_expectedDensity) { // at least one point has been lost
988  if (plugHoles(I) > 0) {
990  I); // if new points have been added, recompute the ellipse parameters and manage again the lists
991  }
992  }
993  if (vpDEBUG_ENABLE(3)) {
994  printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n", m_numberOfGoodPoints, m_expectedDensity,
996  }
997 
998  // resample if needed in case of unsufficient number of points or
999  // bad repartition
1000  // FC : (thresholds ad hoc and sensitive...)
1002  ((m_alphamax - m_alphamin) < vpMath::rad(120.0))) {
1003  if (vpDEBUG_ENABLE(3)) {
1004  printf("Before RESAMPLE !!! nb points %d \n", m_numberOfGoodPoints);
1005  printf("A click to continue \n");
1006  vpDisplay::flush(I);
1008  vpDisplay::display(I);
1009  }
1010 
1011  sample(I);
1012  vpMeTracker::track(I);
1013  leastSquareRobust(I);
1014  if (vpDEBUG_ENABLE(3)) {
1015  printf("nb of Good points %u, density %d, alphamax %lf, alphamin %lf\n", m_numberOfGoodPoints, m_expectedDensity,
1017  }
1018 
1019  // Stop in case of failure after resample
1021  ((m_alphamax - m_alphamin) < vpMath::rad(120.0))) {
1022  // FC : dimensionError pas vraiment le bon terme...
1023  throw(vpException(vpException::dimensionError, "Impossible to track the ellipse"));
1024  }
1025  }
1026 
1027  if (vpDEBUG_ENABLE(3)) {
1028  printParameters();
1029  }
1030  // remet a jour l'angle delta pour chaque vpMeSite de la liste
1031  updateTheta();
1032  // not in getParameters since computed only once for each image
1033  m00 = M_PI * a * b;
1034 
1035  // Useful only for tracking an arc of ellipse, but done to give them a value
1038 
1039 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1040  computeMoments();
1041 #endif
1042 
1043  if (vpDEBUG_ENABLE(3)) {
1044  display(I, vpColor::red);
1046  vpDisplay::flush(I);
1047  }
1048 }
1049 
1050 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1054 void vpMeEllipse::computeMoments()
1055 {
1056  // m00 = M_PI * a * b; // moved in track(I)
1057 
1058  m10 = m_uc * m00;
1059  m01 = m_vc * m00;
1060 
1061  mu20 = m_n20 * m00;
1062  mu11 = m_n11 * m00;
1063  mu02 = m_n02 * m00;
1064 
1065  m20 = mu20 + m10 * m_uc;
1066  m11 = mu11 + m10 * m_vc;
1067  m02 = mu02 + m01 * m_vc;
1068 }
1069 
1083 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &center_p, double a_p, double b_p,
1084  double e_p, double alpha1_p, double alpha2_p)
1085 {
1086  m_trackArc = true;
1087  // useful for sample(I): uc, vc, a, b, e, Ki, alpha1, alpha2
1088  m_uc = center_p.get_u();
1089  m_vc = center_p.get_v();
1090  a = a_p;
1091  b = b_p;
1092  e = e_p;
1093  ce = cos(e);
1094  se = sin(e);
1096  computeKiFromNij();
1097 
1098  alpha1 = alpha1_p;
1099  alpha2 = alpha2_p;
1100  if (alpha2 < alpha1) {
1101  alpha2 += 2 * M_PI;
1102  }
1103  // useful for track(I)
1104  vpImagePoint ip;
1106  iP1 = ip;
1108  iP2 = ip;
1109  // currently not used after, but done to be complete
1110  // would be needed for displaying the ellipse here
1111  iPc = center_p;
1112 
1113  sample(I);
1114  track(I);
1116  vpDisplay::flush(I);
1117 }
1118 
1132 {
1133  std::vector<vpImagePoint> v_iP(n);
1134 
1135  for (unsigned int i = 0; i < n; i++) {
1136  v_iP[i] = iP[i];
1137  }
1138  initTracking(I, v_iP);
1139 }
1140 
1144 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, unsigned int n, unsigned *i, unsigned *j)
1145 {
1146  std::vector<vpImagePoint> v_iP(n);
1147 
1148  for (unsigned int k = 0; k < n; k++) {
1149  v_iP[k].set_ij(i[k], j[k]);
1150  }
1151  initTracking(I, v_iP);
1152 }
1153 #endif // Deprecated
1154 
1177 void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A, const double &B,
1178  const double &E, const double &smallalpha, const double &highalpha, const vpColor &color,
1179  unsigned int thickness)
1180 {
1181  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1182 }
1183 
1207 void vpMeEllipse::display(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
1208  const double &E, const double &smallalpha, const double &highalpha, const vpColor &color,
1209  unsigned int thickness)
1210 {
1211  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1212 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor orange
Definition: vpColor.h:227
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
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 emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
double get_j() const
Definition: vpImagePoint.h:132
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:320
double get_u() const
Definition: vpImagePoint.h:143
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
double get_i() const
Definition: vpImagePoint.h:121
double get_v() const
Definition: vpImagePoint.h:154
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double rad(double deg)
Definition: vpMath.h:117
static int round(double x)
Definition: vpMath.h:318
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6323
Class that tracks an ellipse using moving edges.
Definition: vpMeEllipse.h:98
double m_n20
Second order centered and normalized moments.
Definition: vpMeEllipse.h:430
double m_arcEpsilon
Epsilon value used to check if arc angles are the same.
Definition: vpMeEllipse.h:438
double m02
Definition: vpMeEllipse.h:406
double a
is the semimajor axis of the ellipse.
Definition: vpMeEllipse.h:366
double se
Value of sin(e).
Definition: vpMeEllipse.h:395
double mu11
Second order centered moments.
Definition: vpMeEllipse.h:402
double m10
First order raw moments.
Definition: vpMeEllipse.h:404
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
vpImagePoint iP2
Definition: vpMeEllipse.h:382
void leastSquareRobust(const vpImage< unsigned char > &I)
vpColVector K
Definition: vpMeEllipse.h:362
void display(const vpImage< unsigned char > &I, vpColor col)
double m_vc
Value of v coordinate of iPc.
Definition: vpMeEllipse.h:428
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
Definition: vpMeEllipse.h:434
vpImagePoint iPc
The coordinates of the ellipse center.
Definition: vpMeEllipse.h:364
double m20
Definition: vpMeEllipse.h:406
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:397
double b
is the semiminor axis of the ellipse.
Definition: vpMeEllipse.h:368
void getParameters()
double expecteddensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:414
void printParameters() const
double m_uc
Value of u coordinate of iPc.
Definition: vpMeEllipse.h:426
double m01
Definition: vpMeEllipse.h:404
void computeAbeFromNij()
double ce
Value of cos(e).
Definition: vpMeEllipse.h:393
double m00
Ellipse area.
Definition: vpMeEllipse.h:399
double m_alphamin
Definition: vpMeEllipse.h:420
void initTracking(const vpImage< unsigned char > &I, bool trackArc=false)
double m11
Second order raw moments.
Definition: vpMeEllipse.h:406
double mu20
Definition: vpMeEllipse.h:402
double mu02
Definition: vpMeEllipse.h:402
bool m_trackArc
Track an arc of ellipse (true) or a complete one (false).
Definition: vpMeEllipse.h:436
void computePointOnEllipse(const double ang, vpImagePoint &iP)
double m_alphamax
Definition: vpMeEllipse.h:424
double computeTheta(const vpImagePoint &iP) const
double m_n02
Definition: vpMeEllipse.h:430
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:432
void updateTheta()
virtual ~vpMeEllipse()
Definition: vpMeEllipse.cpp:98
double alpha2
Definition: vpMeEllipse.h:391
vpImagePoint iP1
Definition: vpMeEllipse.h:378
double m_n11
Definition: vpMeEllipse.h:430
void track(const vpImage< unsigned char > &I)
double alpha1
Definition: vpMeEllipse.h:386
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false)
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:72
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
double ifloat
Definition: vpMeSite.h:89
double alpha
Definition: vpMeSite.h:93
void init()
Definition: vpMeSite.cpp:66
double jfloat
Definition: vpMeSite.h:89
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:66
void initTracking(const vpImage< unsigned char > &I)
unsigned int numberOfSignal()
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
int outOfImage(int i, int j, int half, int row, int cols)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:80
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
void setMu1(const double &mu_1)
Definition: vpMe.h:241
void setSampleStep(const double &s)
Definition: vpMe.h:278
void setRange(const unsigned int &r)
Definition: vpMe.h:271
double getMu1() const
Definition: vpMe.h:155
double getMu2() const
Definition: vpMe.h:161
void setMu2(const double &mu_2)
Definition: vpMe.h:248
double getSampleStep() const
Definition: vpMe.h:285
unsigned int getRange() const
Definition: vpMe.h:179
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538