Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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.),
56  iP1(), iP2(), alpha1(0), alpha2(2 * M_PI),
57  ce(0.), se(0.), angle(), m00(0.),
58  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
59  mu11(0.), mu20(0.), mu02(0.),
60  m10(0.), m01(0.),
61  m11(0.), m02(0.), m20(0.),
62  #endif
63  thresholdWeight(0.2),
64  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
65  expecteddensity(0.),
66  #endif
67  m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.),
68  m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackArc(false), m_arcEpsilon(1e-6)
69 {
70  // Resize internal parameters vector
71  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
72  K.resize(6);
73  iP1.set_ij(0,0);
74  iP2.set_ij(0,0);
75 }
76 
81  : vpMeTracker(me_ellipse),
82  K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
83  iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2),
84  ce(me_ellipse.ce), se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
85  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
86  mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02),
87  m10(me_ellipse.m10), m01(me_ellipse.m01), m11(me_ellipse.m11),
88  m02(me_ellipse.m02), m20(me_ellipse.m20),
89  #endif
90  thresholdWeight(me_ellipse.thresholdWeight),
91  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
92  expecteddensity(me_ellipse.expecteddensity),
93  #endif
94  m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax),
95  m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
96  m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
98 {
99 }
100 
105 {
106  list.clear();
107  angle.clear();
108 }
109 
118 {
119  double u = iP.get_u();
120  double v = iP.get_v();
121 
122  return (computeTheta(u, v));
123 }
124 
132 double vpMeEllipse::computeTheta(double u, double v) const
133 {
134  double A = K[0] * u + K[2] * v + K[3];
135  double B = K[1] * v + K[2] * u + K[4];
136 
137  double theta = atan2(B, A); // Angle between the tangent and the u axis.
138  if (theta < 0) { // theta in [0;Pi] // FC : pourquoi ? pour me sans doute
139  theta += M_PI;
140  }
141  return (theta);
142 }
143 
150 {
151  vpMeSite p_me;
152  vpImagePoint iP;
153  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
154  p_me = *it;
155  // (i,j) frame used for vpMESite
156  iP.set_ij(p_me.ifloat, p_me.jfloat);
157  p_me.alpha = computeTheta(iP);
158  *it = p_me;
159  }
160 }
161 
170 {
171  // Two versions are available. If you change from one version to the other
172  // one, do not forget to change also the reciprocical function
173  // computeAngleOnEllipse() just below and, for a correct display of an arc
174  // of ellipse, adapt vpMeEllipse::display() below and
175  // vp_display_display_ellipse() in modules/core/src/display/vpDisplay_impl.h
176  // so that the two extremities of the arc are correctly shown.
177 
178  // Version that gives a regular angular sampling on the ellipse, so less
179  // points at its extremities
180  /*
181  double co = cos(angle);
182  double si = sin(angle);
183  double coef = a * b / sqrt(b * b * co * co + a * a * si * si);
184  double u = co * coef;
185  double v = si * coef;
186  iP.set_u(uc + ce * u - se * v);
187  iP.set_v(vc + se * u + ce * v);
188  */
189 
190  // Version from "the two concentric circles" method that gives more points
191  // at the ellipse extremities for a regular angle sampling. It is better to
192  // display an ellipse, not necessarily to track it
193 
194  // (u,v) are the coordinates on the canonical centered ellipse;
195  double u = a * cos(angle);
196  double v = b * sin(angle);
197  // a rotation of e and a translation by (uc,vc) are done
198  // to get the coordinates of the point on the shifted ellipse
199  iP.set_uv(m_uc + ce * u - se * v, m_vc + se * u + ce * v);
200 }
201 
208 {
209  // Two versions are available. If you change from one version to the other
210  // one, do not forget to change also the reciprocical function
211  // computePointOnEllipse() just above. Adapt also the display; see comment
212  // at the beginning of computePointOnEllipse()
213 
214  // Regular angle smapling method
215  /*
216  double du = pt.get_u() - uc;
217  double dv = pt.get_v() - vc;
218  double ang = atan2(dv,du) - e;
219  if (ang > M_PI) {
220  ang -= 2.0 * M_PI;
221  }
222  else if (ang < -M_PI) {
223  ang += 2.0 * M_PI;
224  }
225  */
226  // for the "two concentric circles method" starting from the previous one
227  // (just to remember the link between both methods:
228  // tan(theta_2cc) = a/b tan(theta_rs))
229  /*
230  double co = cos(ang);
231  double si = sin(ang);
232  double coeff = 1.0/sqrt(b*b*co*co+a*a*si*si);
233  si *= a*coeff;
234  co *= b*coeff;
235  ang = atan2(si,co);
236  */
237  // For the "two concentric circles" method starting from scratch
238  double du = pt.get_u() - m_uc;
239  double dv = pt.get_v() - m_vc;
240  double co = (du * ce + dv * se)/a;
241  double si = (- du * se + dv * ce)/b;
242  double angle = atan2(si,co);
243 
244  return(angle);
245 }
246 
254 {
255  double num = m_n20 - m_n02;
256  double d = num * num + 4.0 * m_n11 * m_n11; // always >= 0
257  if (d <= std::numeric_limits<double>::epsilon()) {
258  e = 0.0; // case n20 = n02 and n11 = 0 : circle, e undefined
259  ce = 1.0;
260  se = 0.0;
261  a = b = 2.0*sqrt(m_n20); // = sqrt(2.0*(n20+n02))
262  }
263  else { // real ellipse
264  e = atan2(2.0*m_n11, num)/2.0; // e in [-Pi/2 ; Pi/2]
265  ce = cos(e);
266  se = sin(e);
267 
268  d = sqrt(d); // d in sqrt always >= 0
269  num = m_n20 + m_n02;
270  a = sqrt(2.0*(num + d)); // term in sqrt always > 0
271  b = sqrt(2.0*(num - d)); // term in sqrt always > 0
272  }
273 }
274 
282 {
283  K[0] = m_n02;
284  K[1] = m_n20;
285  K[2] = -m_n11;
286  K[3] = m_n11 * m_vc - m_n02 * m_uc;
287  K[4] = m_n11 * m_uc - m_n20 * m_vc;
288  K[5] = m_n02 * m_uc * m_uc + m_n20 * m_vc * m_vc - 2.0 * m_n11 * m_uc * m_vc
289  + 4.0 * (m_n11 * m_n11 - m_n20 * m_n02);
290 }
291 
298 {
299  m_n20 = 0.25 * (a * a * ce * ce + b * b * se * se);
300  m_n11 = 0.25 * se * ce * (a * a - b * b);
301  m_n02 = 0.25 * (a * a * se * se + b * b * ce * ce);
302 }
303 
314 {
315  // Equations below from Chaumette PhD and TRO 2004 paper
316  double num = K[0] * K[1] - K[2] * K[2]; // > 0 for an ellipse
317  if (num <= 0) {
318  throw(vpException(vpException::fatalError, "The points do not belong to an ellipse!"));
319  }
320 
321  m_uc = (K[2] * K[4] - K[1] * K[3]) / num;
322  m_vc = (K[2] * K[3] - K[0] * K[4]) / num;
323  iPc.set_uv(m_uc, m_vc);
324 
325  double d = (K[0] * m_uc * m_uc + K[1] * m_vc * m_vc + 2.0 * K[2] * m_uc * m_vc - K[5])
326  / (4.0 * num);
327  m_n20 = K[1] * d; // always > 0
328  m_n11 = -K[2] * d;
329  m_n02 = K[0] * d; // always > 0
330 
332 
333  // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper)
334  d = m_n02/K[0]; // fabs(K[0]) > 0
335  for (unsigned int i=0; i < 6; i++) {
336  K[i] *= d;
337  }
338  if (vpDEBUG_ENABLE(3)) {
339  printParameters();
340  }
341 }
342 
348 {
349  std::cout << "K :" << K.t() << std::endl;
350  std::cout << "xc = " << m_uc << ", yc = " << m_vc << std::endl;
351  std::cout << "n20 = " << m_n20 << ", n11 = " << m_n11 << ", n02 = " << m_n02 <<std::endl;
352  std::cout << "A = " << a << ", B = " << b << ", E (dg) " << vpMath::deg(e) <<std::endl;
353 }
354 
367 void vpMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
368 {
369  // Warning: similar code in vpMbtMeEllipse::sample()
370  if (!me) {
371  throw(vpException(vpException::fatalError, "Moving edges on ellipse not initialized"));
372  }
373  // Delete old lists
374  list.clear();
375  angle.clear();
376 
377  int nbrows = static_cast<int>(I.getHeight());
378  int nbcols = static_cast<int>(I.getWidth());
379 
380  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
381  std::cout << "In vpMeEllipse::sample: ";
382  std::cout << "function called with sample step = 0, set to 10 dg";
383  me->setSampleStep(10.0);
384  }
385  double incr = vpMath::rad(me->getSampleStep()); // angle increment
386  // alpha2 - alpha1 = 2 * M_PI for a complete ellipse
387  m_expectedDensity = static_cast<unsigned int>(floor((alpha2 - alpha1) / incr));
388 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
389  expecteddensity = static_cast<double>(m_expectedDensity);
390 #endif
391 
392  // starting angle for sampling
393  double ang = alpha1 + ((alpha2 - alpha1) - static_cast<double>(m_expectedDensity) * incr)/2.0;
394  // sample positions
395  for (unsigned int i = 0; i < m_expectedDensity; i++) {
396  vpImagePoint iP;
397  computePointOnEllipse(ang,iP);
398  // If point is in the image, add to the sample list
399  // Check done in (i,j) frame)
400  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
402 
403  double theta = computeTheta(iP);
404  vpMeSite pix;
405  // (i,j) frame used for vpMeSite
406  pix.init(iP.get_i(), iP.get_j(), theta);
409  list.push_back(pix);
410  angle.push_back(ang);
411  }
412  ang += incr;
413  }
414  if (!doNotTrack) {
416  }
417 }
418 
431 {
432  if (!me) {
433  throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
434  }
435  unsigned int nb_pts_added = 0;
436  int nbrows = static_cast<int>(I.getHeight());
437  int nbcols = static_cast<int>(I.getWidth());
438 
439  unsigned int memory_range = me->getRange();
440  me->setRange(2);
441  double memory_mu1 = me->getMu1();
442  me->setMu1(0.5);
443  double memory_mu2 = me->getMu2();
444  me->setMu2(0.5);
445 
446  double incr = vpMath::rad(me->getSampleStep());
447  // Detect holes and try to complete them
448  // FC : Currently only one point is looked at the middle of each hole
449  // (to avoid multiple insertions that are time consuming).
450  // A different choice could be done.
451  std::list<double>::iterator angleList = angle.begin();
452  double ang = *angleList;
453  for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
454  ++angleList;
455  ++meList;
456  double nextang = *angleList;
457  // The minimal size of a hole (1 point lost for sure).
458  // could be increased to reduce time processing
459  if ((nextang - ang) > 1.6 * incr) {
460  ang = (nextang + ang) / 2.0; // mid angle
461  vpImagePoint iP;
462  computePointOnEllipse(ang,iP);
463  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
464  double theta = computeTheta(iP);
465  vpMeSite pix;
466  pix.init(iP.get_i(), iP.get_j(), theta);
469  pix.track(I, me, false);
470  if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
471  nb_pts_added ++;
472  iP.set_ij(pix.ifloat, pix.jfloat);
473  double new_ang = computeAngleOnEllipse(iP);
474  if ((new_ang - ang) > M_PI) {
475  new_ang -= 2.0 * M_PI;
476  }
477  else if ((ang - new_ang) > M_PI) {
478  new_ang += 2.0 * M_PI;
479  }
480  list.insert(meList,pix);
481  angle.insert(angleList,new_ang);
482  if (vpDEBUG_ENABLE(3)) {
484  }
485  }
486  }
487  }
488  ang = nextang;
489  }
490 
491  // Try to fill the first extremity: from alpha_min - incr to alpha1
492  unsigned int nbpts = static_cast<unsigned int>(floor((m_alphamin-alpha1)/incr));
493  ang = m_alphamin - incr;
494  for (unsigned int i = 0; i < nbpts; i++) {
495  vpImagePoint iP;
496  computePointOnEllipse(ang ,iP);
497  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
498  double theta = computeTheta(iP);
499  vpMeSite pix;
500  pix.init(iP.get_i(), iP.get_j(), theta);
503  pix.track(I, me, false);
504  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
505  nb_pts_added ++;
506  iP.set_ij(pix.ifloat, pix.jfloat);
507  double new_ang = computeAngleOnEllipse(iP);
508  if ((new_ang - ang) > M_PI) {
509  new_ang -= 2.0 * M_PI;
510  }
511  else if ((ang - new_ang) > M_PI) {
512  new_ang += 2.0 * M_PI;
513  }
514  list.push_front(pix);
515  angle.push_front(new_ang);
516  if (vpDEBUG_ENABLE(3)) {
518  }
519  }
520  }
521  ang -= incr;
522  }
523 
524  // Try to fill the second extremity: from alphamax + incr to alpha2
525  nbpts = static_cast<unsigned int>(floor((alpha2-m_alphamax)/incr));
526  ang = m_alphamax + incr;
527  for (unsigned int i = 0; i < nbpts; i++) {
528  vpImagePoint iP;
529  computePointOnEllipse(ang,iP);
530  if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
531  double theta = computeTheta(iP);
532  vpMeSite pix;
533  pix.init(iP.get_i(), iP.get_j(), theta);
536  pix.track(I, me, false);
537  if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
538  nb_pts_added ++;
539  iP.set_ij(pix.ifloat, pix.jfloat);
540  double new_ang = computeAngleOnEllipse(iP);
541  if ((new_ang - ang) > M_PI) {
542  new_ang -= 2.0 * M_PI;
543  }
544  else if ((ang - new_ang) > M_PI) {
545  new_ang += 2.0 * M_PI;
546  }
547  list.push_back(pix);
548  angle.push_back(new_ang);
549  if (vpDEBUG_ENABLE(3)) {
551  }
552  }
553  }
554  ang += incr;
555  }
556  me->setRange(memory_range);
557  me->setMu1(memory_mu1);
558  me->setMu2(memory_mu2);
559 
560  if (vpDEBUG_ENABLE(3)) {
561  printf("In plugHoles(): nb of added points : %d\n", nb_pts_added);
562  }
563  return nb_pts_added;
564 }
565 
573 void vpMeEllipse::leastSquare(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
574 {
575  // Homogeneous system A x = 0 ; x is the nullspace of A
576  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
577  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
578 
579  // It would be a bad idea to solve the same system using A x = b where
580  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
581  // cannot consider the case where the origin belongs to the ellipse.
582  // Another possibility would be to consider K0+K1=1 which is always valid,
583  // leading to the system A x = b where
584  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
585 
586  double um = I.getWidth() / 2.;
587  double vm = I.getHeight() / 2.;
588  unsigned int n = static_cast<unsigned int>(iP.size());
589 
590  vpMatrix A(n, 6);
591 
592  for (unsigned int k = 0; k < n; k++) {
593  // normalization so that (u,v) in [-1;1]
594  double u = (iP[k].get_u() - um) / um;
595  double v = (iP[k].get_v() - vm) / vm;
596  A[k][0] = u * u;
597  A[k][1] = v * v;
598  A[k][2] = 2.0 * u * v;
599  A[k][3] = 2.0 * u;
600  A[k][4] = 2.0 * v;
601  A[k][5] = 1.0;
602  }
603  vpMatrix KerA;
604  unsigned int dim = A.nullSpace(KerA, 1);
605  if (dim > 1) { // case with less than 5 independent points
606  // FC : should create a rankError exception
607  throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
608  }
609  // the term um*vm is for counterbalancing the bad conditioning of the
610  // inverse normalization below
611  for (unsigned int i = 0; i < 6 ; i++) K[i] = um * vm * KerA[i][0];
612 
613  // Inverse normalization to go back to pixel values
614  K[0] /= um * um;
615  K[1] /= vm * vm;
616  K[2] /= um * vm;
617  K[3] = K[3]/um - K[0] * um - K[2] * vm;
618  K[4] = K[4]/vm - K[1] * vm - K[2] * um;
619  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;
620 
621  getParameters();
622 }
623 
632 {
633  // Homogeneous system A x = 0 ; x is the nullspace of A
634  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
635  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T
636 
637  // It would be a bad idea to solve the same system using A x = b where
638  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
639  // cannot consider the case where the origin belongs to the ellipse.
640  // Another possibility would be to consider K0+K1=1 which is always valid,
641  // leading to the system A x = b where
642  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T
643 
644  const unsigned int nos = numberOfSignal();
645 
646  // Note that the (nos-k) last rows of A, xp and yp are not used.
647  // Hopefully, this is not an issue.
648 
649  vpMatrix A(nos, 6);
650  // Useful to compute the weights in the robust estimation
651  vpColVector xp(nos), yp(nos);
652 
653  unsigned int k = 0;
654  double um = I.getWidth() / 2.;
655  double vm = I.getHeight() / 2.;
656  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
657  vpMeSite p_me = *it;
658  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
659  // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
660  double u = (p_me.jfloat - um) / um;
661  double v = (p_me.ifloat - vm) / vm;
662  A[k][0] = u * u;
663  A[k][1] = v * v;
664  A[k][2] = 2.0 * u * v;
665  A[k][3] = 2.0 * u;
666  A[k][4] = 2.0 * v;
667  A[k][5] = 1.0;
668  // Useful to compute the weights in the robust estimation
669  xp[k] = p_me.jfloat;
670  yp[k] = p_me.ifloat;
671 
672  k++;
673  }
674  }
675  if (k < 5) {
676  throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse"));
677  }
678 
679  vpRobust r;
680  // r.setThreshold(0.02); // Old version where this threshold was highly
681  // sensitive since the residues do not represent the Euclidean distance
682  // from the point to the ellipse
683  r.setMinMedianAbsoluteDeviation(1.0); // image noise in pixels for the geometrical distance
684  vpColVector w(k);
685  w = 1.0;
686  unsigned int iter = 0;
687  double var = 1.0;
688  vpColVector Kprev(6);
689  vpMatrix DA(k, 6);
690  vpMatrix KerDA;
691 
692  // stop after 4 it or if variation of K between 2 iterations is more than 0.1 %
693  while ((iter < 4) && (var > 0.001)) {
694  for (unsigned int i = 0; i < k ; i++) {
695  for (unsigned int j = 0; j < 6 ; j++) {
696  DA[i][j] = w[i] * A[i][j];
697  }
698  }
699  unsigned int dim = DA.nullSpace(KerDA, 1);
700  if (dim > 1) { // case with less than 5 independent points
701  // FC : should create a rankError exception
702  throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
703  }
704 
705  for (unsigned int i=0; i<6 ; i++) K[i] = KerDA[i][0]; // norm(K) = 1
706  var = (K-Kprev).frobeniusNorm();
707  Kprev = K;
708  // the term um*vm is for counterbalancing the bad conditioning of the
709  // inverse normalization just below
710  K *= (um * vm);
711  // vpColVector residu(k); // old version for considering the algebraic distance
712  // residu = A * K;
713  // Better version considering the geometric distance
714  // Inverse normalization to go back to pixels
715  K[0] /= um * um;
716  K[1] /= vm * vm;
717  K[2] /= um * vm;
718  K[3] = K[3]/um - K[0] * um - K[2] * vm;
719  K[4] = K[4]/vm - K[1] * vm - K[2] * um;
720  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;
721  getParameters(); // since a, b, and e are used just after
722 
723  vpColVector residu(k);
724  for (unsigned int i=0; i < k ; i++) {
725  vpImagePoint ip1, ip2;
726  ip1.set_uv(xp[i],yp[i]);
727  double ang = computeAngleOnEllipse(ip1);
728  computePointOnEllipse(ang, ip2);
729  // residu = 0 if point is exactly on the ellipse, not otherwise
730  residu[i] = vpImagePoint::distance(ip1, ip2);
731  }
732  // end of new version
733  r.MEstimator(vpRobust::TUKEY, residu, w);
734  iter++;
735  }
736  /* FC : for old version with algebraic distance
737  // Inverse normalization to go back to pixels
738  K[0] /= um * um;
739  K[1] /= vm * vm;
740  K[2] /= um * vm;
741  K[3] = K[3]/um - K[0] * um - K[2] * vm;
742  K[4] = K[4]/vm - K[1] * vm - K[2] * um;
743  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;
744  getParameters();
745  */
746  double previous_ang = - 4.0 * M_PI;
747  double incr = vpMath::rad(me->getSampleStep());
748  // Remove bad points, too near points, and outliers from the lists
749  k = m_numberOfGoodPoints = 0;
750  std::list<double>::iterator angleList = angle.begin();
751  for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
752  vpMeSite p_me = *meList;
753  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
754  if (w[k] > thresholdWeight) { // inlier
755  // Management of the angle to keep only the points in the interval
756  // [alpha1 ; alpha2]
757  double ang = *angleList;
758  vpImagePoint iP;
759  iP.set_ij(p_me.ifloat,p_me.jfloat);
760  double new_ang = computeAngleOnEllipse(iP);
761  if ((new_ang - ang) > M_PI) {
762  new_ang -= 2.0 * M_PI;
763  }
764  else if ((ang - new_ang) > M_PI) {
765  new_ang += 2.0 * M_PI;
766  }
767  if ((new_ang >= alpha1) && (new_ang <= alpha2) ) {
768  // good point if not too near from the previous one in the list
769  // if so, udate of its angle
770  if ((new_ang - previous_ang) >= (0.6 * incr)) {
771  *angleList = previous_ang = new_ang;
773  ++meList;
774  ++angleList;
775  if (vpDEBUG_ENABLE(3)) {
776  vpDisplay::displayCross(I, iP, 10, vpColor::red, 1);
777  printf("In LQR: angle : %lf, i = %.0lf, j = %.0lf\n",
778  vpMath::deg(new_ang), iP.get_i(), iP.get_j());
779  }
780  }
781  else {
782  if (vpDEBUG_ENABLE(3)) {
784  printf("too near : angle %lf, i %.0f , j : %0.f\n",
785  vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
786  }
787  meList = list.erase(meList);
788  angleList = angle.erase(angleList);
789  }
790  }
791  else { // point not in the interval [alpha1 ; alpha2]
792  if (vpDEBUG_ENABLE(3)) {
794  printf("not in interval: angle : %lf, i %.0f , j : %0.f\n",
795  vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
796  }
797  meList = list.erase(meList);
798  angleList = angle.erase(angleList);
799  }
800  }
801  else { // outlier
802  if (vpDEBUG_ENABLE(3)) {
803  vpImagePoint iP;
804  iP.set_ij(p_me.ifloat,p_me.jfloat);
805  printf("point %d outlier i : %.0f , j : %0.f, poids : %lf\n",
806  k, p_me.ifloat, p_me.jfloat, w[k]);
807  vpDisplay::displayCross(I, iP, 10, vpColor::cyan, 1);
808  }
809  meList = list.erase(meList);
810  angleList = angle.erase(angleList);
811  }
812  k++;
813  }
814  else { // points not selected as me
815  meList = list.erase(meList);
816  angleList = angle.erase(angleList);
817  if (vpDEBUG_ENABLE(3)) {
818  vpImagePoint iP;
819  iP.set_ij(p_me.ifloat,p_me.jfloat);
820  printf("point not me i : %.0f , j : %0.f\n", p_me.ifloat, p_me.jfloat);
821  vpDisplay::displayCross(I, iP, 10, vpColor::blue, 1);
822  }
823  }
824  }
825  // set extremities of the angle list
826  m_alphamin = angle.front();
827  m_alphamax = angle.back();
828 
829  if (vpDEBUG_ENABLE(3)) {
830  printf("alphamin : %lf, alphamax : %lf\n", vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
831  printf("dans leastSquareRobust : nb pts ok = %d \n", m_numberOfGoodPoints);
832  }
833 }
834 
845 {
846  vpMeEllipse::display(I, iPc, a, b, e, alpha1, alpha2, col);
847 }
848 
862 {
863  const unsigned int n = 5;
864  std::vector<vpImagePoint> iP(n);
865  m_trackArc = trackArc;
866 
867  if (m_trackArc) {
868  std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
869  }
870  for (unsigned int k = 0; k < n; k++) {
871  std::cout << "Click point " << k + 1 << "/" << n << " on the ellipse " << std::endl;
872  vpDisplay::getClick(I, iP[k], true);
873  vpDisplay::displayCross(I, iP[k], 10, vpColor::red);
874  vpDisplay::flush(I);
875  std::cout << iP[k] << std::endl;
876  }
877  initTracking(I, iP, trackArc);
878 }
879 
896 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP, bool trackArc)
897 {
898  m_trackArc = trackArc;
899  // useful for sample(I):
900  leastSquare(I, iP);
901  if (trackArc) {
902  // useful for track(I):
903  iP1 = iP.front();
904  iP2 = iP.back();
905  // useful for sample(I):
908  if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
909  alpha2 += 2.0 * M_PI;
910  }
911  }
912  else {
913  alpha1 = 0.0;
914  alpha2 = 2 * M_PI;
915  // useful for track(I):
916  vpImagePoint ip;
918  iP1 = iP2 = ip;
919  }
920 
921  sample(I);
922  track(I);
924  vpDisplay::flush(I);
925 }
926 
942 {
943  if (pt1 != NULL && pt2 != NULL) {
944  m_trackArc = true;
945  }
946  // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
947  m_uc = param[0];
948  m_vc = param[1];
949  m_n20 = param[2];
950  m_n11 = param[3];
951  m_n02 = param[4];
954 
955  if (m_trackArc) {
958  if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
959  alpha2 += 2.0 * M_PI;
960  }
961  // useful for track(I)
962  iP1 = *pt1;
963  iP2 = *pt2;
964  }
965  else {
966  alpha1 = 0.0;
967  alpha2 = 2.0 * M_PI;
968  // useful for track(I)
969  vpImagePoint ip;
971  iP1 = iP2 = ip;
972  }
973  // useful for display(I) so useless if no display before track(I)
974  iPc.set_uv(m_uc, m_vc);
975 
976  sample(I);
977  track(I);
979  vpDisplay::flush(I);
980 }
981 
988 {
990 
991  // recompute alpha1 and alpha2 in case they have been changed by setEndPoints()
992  if (m_trackArc) {
995  if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
996  alpha2 += 2.0 * M_PI;
997  }
998  }
999  // Compute the ellipse parameters from the tracked points and manage the lists
1000  leastSquareRobust(I);
1001  if (vpDEBUG_ENABLE(3)) {
1002  printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1005  }
1006 
1007  // Try adding points at the extremities and in the holes if needed
1008  if (m_numberOfGoodPoints < m_expectedDensity) { // at least one point has been lost
1009  if (plugHoles(I) > 0) {
1010  leastSquareRobust(I); // if new points have been added, recompute the ellipse parameters and manage again the lists
1011  }
1012  }
1013  if (vpDEBUG_ENABLE(3)) {
1014  printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
1017  }
1018 
1019  // resample if needed in case of unsufficient number of points or
1020  // bad repartition
1021  // FC : (thresholds ad hoc and sensitive...)
1022  if ( ((3 * m_numberOfGoodPoints) < m_expectedDensity) || (m_numberOfGoodPoints <= 5) || ( (m_alphamax - m_alphamin) < vpMath::rad(120.0) ) ) {
1023  if (vpDEBUG_ENABLE(3)) {
1024  printf("Before RESAMPLE !!! nb points %d \n", m_numberOfGoodPoints);
1025  printf("A click to continue \n");
1026  vpDisplay::flush(I);
1028  vpDisplay::display(I);
1029  }
1030 
1031  sample(I);
1032  vpMeTracker::track(I);
1033  leastSquareRobust(I);
1034  if (vpDEBUG_ENABLE(3)) {
1035  printf("nb of Good points %u, density %d, alphamax %lf, alphamin %lf\n",
1037  }
1038 
1039  // Stop in case of failure after resample
1040  if ( ((3 * m_numberOfGoodPoints) < m_expectedDensity) || (m_numberOfGoodPoints <= 5) || ( (m_alphamax - m_alphamin) < vpMath::rad(120.0) ) ) {
1041  // FC : dimensionError pas vraiment le bon terme...
1042  throw(vpException(vpException::dimensionError, "Impossible to track the ellipse"));
1043  }
1044  }
1045 
1046  if (vpDEBUG_ENABLE(3)) {
1047  printParameters();
1048  }
1049  // remet a jour l'angle delta pour chaque vpMeSite de la liste
1050  updateTheta();
1051  // not in getParameters since computed only once for each image
1052  m00 = M_PI * a * b;
1053 
1054  // Useful only for tracking an arc of ellipse, but done to give them a value
1057 
1058 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1059  computeMoments();
1060 #endif
1061 
1062  if (vpDEBUG_ENABLE(3)) {
1063  display(I, vpColor::red);
1065  vpDisplay::flush(I);
1066  }
1067 }
1068 
1069 
1070 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1071 
1074 void vpMeEllipse::computeMoments()
1075 {
1076  // m00 = M_PI * a * b; // moved in track(I)
1077 
1078  m10 = m_uc * m00;
1079  m01 = m_vc * m00;
1080 
1081  mu20 = m_n20 * m00;
1082  mu11 = m_n11 * m00;
1083  mu02 = m_n02 * m00;
1084 
1085  m20 = mu20 + m10 * m_uc;
1086  m11 = mu11 + m10 * m_vc;
1087  m02 = mu02 + m01 * m_vc;
1088 }
1089 
1104  double a_p, double b_p, double e_p, double alpha1_p, double alpha2_p)
1105 {
1106  m_trackArc = true;
1107  // useful for sample(I): uc, vc, a, b, e, Ki, alpha1, alpha2
1108  m_uc = center_p.get_u();
1109  m_vc = center_p.get_v();
1110  a = a_p;
1111  b = b_p;
1112  e = e_p;
1113  ce = cos(e);
1114  se = sin(e);
1116  computeKiFromNij();
1117 
1118  alpha1 = alpha1_p;
1119  alpha2 = alpha2_p;
1120  if (alpha2 < alpha1) {
1121  alpha2 += 2 * M_PI;
1122  }
1123  // useful for track(I)
1124  vpImagePoint ip;
1126  iP1 = ip;
1128  iP2 = ip;
1129  // currently not used after, but done to be complete
1130  // would be needed for displaying the ellipse here
1131  iPc = center_p;
1132 
1133  sample(I);
1134  track(I);
1136  vpDisplay::flush(I);
1137 }
1138 
1152 {
1153  std::vector<vpImagePoint> v_iP(n);
1154 
1155  for (unsigned int i = 0; i < n; i++) {
1156  v_iP[i] = iP[i];
1157  }
1158  initTracking(I, v_iP);
1159 }
1160 
1164 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, unsigned int n, unsigned *i, unsigned *j)
1165 {
1166  std::vector<vpImagePoint> v_iP(n);
1167 
1168  for (unsigned int k=0; k < n; k++) {
1169  v_iP[k].set_ij(i[k],j[k]);
1170  }
1171  initTracking(I, v_iP);
1172 }
1173 #endif // Deprecated
1174 
1198  const vpImagePoint &center, const double &A, const double &B, const double &E,
1199  const double &smallalpha, const double &highalpha,
1200  const vpColor &color, unsigned int thickness)
1201 {
1202  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1203 }
1204 
1229  const vpImagePoint &center, const double &A, const double &B, const double &E,
1230  const double &smallalpha, const double &highalpha,
1231  const vpColor &color, unsigned int thickness)
1232 {
1233  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
1234 }
double a
is the semimajor axis of the ellipse.
Definition: vpMeEllipse.h:365
void track(const vpImage< unsigned char > &I)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
double m01
Definition: vpMeEllipse.h:403
double m_alphamin
Definition: vpMeEllipse.h:419
double get_i() const
Definition: vpImagePoint.h:203
double expecteddensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:413
vpMeSiteState getState() const
Definition: vpMeSite.h:190
bool m_trackArc
Track an arc of ellipse (true) or a complete one (false).
Definition: vpMeEllipse.h:435
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
double mu02
Definition: vpMeEllipse.h:401
void init()
Definition: vpMeSite.cpp:66
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
double m_n11
Definition: vpMeEllipse.h:429
double jfloat
Definition: vpMeSite.h:89
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538
double m_arcEpsilon
Epsilon value used to check if arc angles are the same.
Definition: vpMeEllipse.h:437
unsigned int numberOfSignal()
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
double thresholdWeight
Threshold on the weights for the robust least square.
Definition: vpMeEllipse.h:409
void set_uv(double u, double v)
Definition: vpImagePoint.h:247
virtual void sample(const vpImage< unsigned char > &I, bool doNotTrack=false)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void setSampleStep(const double &s)
Definition: vpMe.h:278
double m_uc
Value of u coordinate of iPc.
Definition: vpMeEllipse.h:425
vpImagePoint iP1
Definition: vpMeEllipse.h:377
double alpha
Definition: vpMeSite.h:93
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that tracks an ellipse using moving edges.
Definition: vpMeEllipse.h:97
vpRowVector t() const
double mu20
Definition: vpMeEllipse.h:401
static const vpColor green
Definition: vpColor.h:220
double computeAngleOnEllipse(const vpImagePoint &pt) const
static void flush(const vpImage< unsigned char > &I)
double b
is the semiminor axis of the ellipse.
Definition: vpMeEllipse.h:367
double se
Value of sin(e).
Definition: vpMeEllipse.h:394
void computeAbeFromNij()
void setMu1(const double &mu_1)
Definition: vpMe.h:241
double getMu1() const
Definition: vpMe.h:155
static const vpColor red
Definition: vpColor.h:217
void initTracking(const vpImage< unsigned char > &I, bool trackArc=false)
static const vpColor orange
Definition: vpColor.h:227
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6335
double ifloat
Definition: vpMeSite.h:89
double computeTheta(const vpImagePoint &iP) const
virtual ~vpMeEllipse()
double m20
Definition: vpMeEllipse.h:405
double get_u() const
Definition: vpImagePoint.h:262
std::list< vpMeSite > list
Definition: vpMeTracker.h:78
vpImagePoint iP2
Definition: vpMeEllipse.h:381
Point used by the tracker.
Definition: vpMeSite.h:78
static const vpColor cyan
Definition: vpColor.h:226
double getSampleStep() const
Definition: vpMe.h:285
int outOfImage(int i, int j, int half, int row, int cols)
vpImagePoint iPc
The coordinates of the ellipse center.
Definition: vpMeEllipse.h:363
double m11
Second order raw moments.
Definition: vpMeEllipse.h:405
double alpha1
Definition: vpMeEllipse.h:385
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
static void display(const vpImage< unsigned char > &I)
double get_j() const
Definition: vpImagePoint.h:214
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
unsigned int m_numberOfGoodPoints
Number of correct points tracked along the ellipse.
Definition: vpMeEllipse.h:433
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)
void display(const vpImage< unsigned char > &I, vpColor col)
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
double m_alphamax
Definition: vpMeEllipse.h:423
double mu11
Second order centered moments.
Definition: vpMeEllipse.h:401
double m10
First order raw moments.
Definition: vpMeEllipse.h:403
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:65
vpColVector K
Definition: vpMeEllipse.h:361
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
double m00
Ellipse area.
Definition: vpMeEllipse.h:398
static double rad(double deg)
Definition: vpMath.h:110
std::list< double > angle
Stores the value in increasing order of the angle on the ellipse for each vpMeSite ...
Definition: vpMeEllipse.h:396
static int round(double x)
Definition: vpMath.h:247
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void computeKiFromNij()
void setMu2(const double &mu_2)
Definition: vpMe.h:248
void computePointOnEllipse(const double ang, vpImagePoint &iP)
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
static double deg(double rad)
Definition: vpMath.h:103
double m02
Definition: vpMeEllipse.h:405
unsigned int getHeight() const
Definition: vpImage.h:188
unsigned int m_expectedDensity
Expected number of points to track along the ellipse.
Definition: vpMeEllipse.h:431
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void updateTheta()
void leastSquareRobust(const vpImage< unsigned char > &I)
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:80
unsigned int plugHoles(const vpImage< unsigned char > &I)
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
double getMu2() const
Definition: vpMe.h:161
Tukey influence function.
Definition: vpRobust.h:93
void initTracking(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
double alpha2
Definition: vpMeEllipse.h:390
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
void setRange(const unsigned int &r)
Definition: vpMe.h:271
double m_vc
Value of v coordinate of iPc.
Definition: vpMeEllipse.h:427
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:90
void leastSquare(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &iP)
unsigned int getWidth() const
Definition: vpImage.h:246
void getParameters()
unsigned int getRange() const
Definition: vpMe.h:179
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
void printParameters() const
double get_v() const
Definition: vpImagePoint.h:273
void computeNijFromAbe()
double ce
Value of cos(e).
Definition: vpMeEllipse.h:392
double m_n02
Definition: vpMeEllipse.h:429
static const vpColor blue
Definition: vpColor.h:223
double m_n20
Second order centered and normalized moments.
Definition: vpMeEllipse.h:429