Visual Servoing Platform  version 3.2.1 under development (2019-10-21) under development (2019-10-21)
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 
51 void computeTheta(double &theta, vpColVector &K, const vpImagePoint &iP);
52 
61 void computeTheta(double &theta, vpColVector &K, const vpImagePoint &iP)
62 {
63  double i = iP.get_i();
64  double j = iP.get_j();
65 
66  double A = 2 * i + 2 * K[1] * j + 2 * K[2];
67  double B = 2 * K[0] * j + 2 * K[1] * i + 2 * K[3];
68 
69  theta = atan2(A, B); // Angle between the tangente and the i axis.
70 
71  while (theta > M_PI) {
72  theta -= M_PI;
73  }
74  while (theta < 0) {
75  theta += M_PI;
76  }
77 }
78 
83  : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.),
84  mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.), thresholdWeight(0.2), expecteddensity(0.)
85 {
86  // redimensionnement du vecteur de parametre
87  // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
88 
89  K.resize(5);
90 
91  // j1 = j2 = i1 = i2 = 0;
92  iP1.set_i(0);
93  iP1.set_j(0);
94  iP2.set_i(0);
95  iP2.set_j(0);
96 }
97 
102  : vpMeTracker(meellipse), K(meellipse.K), iPc(meellipse.iPc), a(0.), b(0.), e(0.), iP1(meellipse.iP1),
103  iP2(meellipse.iP2), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(meellipse.angle), m00(0.), mu11(0.),
104  mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.), thresholdWeight(0.2), expecteddensity(0.)
105 {
106  a = meellipse.a;
107  b = meellipse.b;
108  e = meellipse.e;
109 
110  alpha1 = meellipse.alpha1;
111  alpha2 = meellipse.alpha2;
112  ce = meellipse.ce;
113  se = meellipse.se;
114 
115  m00 = meellipse.m00;
116  mu11 = meellipse.mu11;
117  mu20 = meellipse.mu20;
118  mu02 = meellipse.mu02;
119  m10 = meellipse.m10;
120  m01 = meellipse.m01;
121  m11 = meellipse.m11;
122  m02 = meellipse.m02;
123  m20 = meellipse.m20;
124  thresholdWeight = meellipse.thresholdWeight;
125 
126  expecteddensity = meellipse.expecteddensity;
127 }
128 
133 {
134  list.clear();
135  angle.clear();
136 }
137 
150 void vpMeEllipse::sample(const vpImage<unsigned char> &I, const bool doNotTrack)
151 {
152  (void)doNotTrack;
153  if (!me) {
154  throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
155  }
156 
157  int height = (int)I.getHeight();
158  int width = (int)I.getWidth();
159 
160  // if (me->getSampleStep()==0)
161  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
162  std::cout << "In vpMeEllipse::sample: ";
163  std::cout << "function called with sample step = 0";
164  // return fatalError;
165  }
166 
167  vpImagePoint iP11;
168 
169  double incr = vpMath::rad(me->getSampleStep()); // angle increment en degree
170  vpColor col = vpColor::red;
171  getParameters();
172 
173  // Delete old list
174  list.clear();
175 
176  angle.clear();
177 
178  // sample positions
179  double k = alpha1;
180  while (k < alpha2) {
181  double j = a * sin(k); // equation of an ellipse
182  double i = b * cos(k); // equation of an ellipse
183 
184  // (i,j) are the coordinates on the origin centered ellipse;
185  // a rotation by "e" and a translation by (xci,jc) are done
186  // to get the coordinates of the point on the shifted ellipse
187  iP11.set_j(iPc.get_j() + ce * j + se * i);
188  iP11.set_i(iPc.get_i() - se * j + ce * i);
189 
190  vpDisplay::displayCross(I, iP11, 5, col);
191 
192  double theta;
193  computeTheta(theta, K, iP11);
194 
195  // If point is in the image, add to the sample list
196  if (!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width)) {
197  vpMeSite pix;
198  pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta);
201 
202  if (vpDEBUG_ENABLE(3)) {
204  }
205  list.push_back(pix);
206  angle.push_back(k);
207  }
208  k += incr;
209  }
211 }
212 
228 void vpMeEllipse::reSample(const vpImage<unsigned char> &I)
229 {
230  if (!me) {
231  throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
232  }
233 
234  unsigned int n = numberOfSignal();
236  if ((double)n < 0.5 * expecteddensity) {
237  sample(I);
239  }
240 }
241 
251 void vpMeEllipse::getParameters()
252 {
253  double k[6];
254  for (unsigned int i = 0; i < 5; i++)
255  k[i + 1] = K[i];
256  k[0] = 1;
257 
258  double d = k[2] * k[2] - k[0] * k[1];
259 
260  iPc.set_i((k[1] * k[3] - k[2] * k[4]) / d);
261  iPc.set_j((k[0] * k[4] - k[2] * k[3]) / d);
262 
263  double sq = sqrt(vpMath::sqr(k[1] - k[0]) + 4.0 * vpMath::sqr(k[2]));
264 
265  if (std::fabs(k[2]) <= std::numeric_limits<double>::epsilon()) {
266  e = 0;
267  } else {
268  e = (k[1] - k[0] + sq) / (2.0 * k[2]);
269  e = (-1 / e);
270  }
271 
272  e = atan(e);
273 
274  if (e < 0.0)
275  e += M_PI;
276 
277  ce = cos(e);
278  se = sin(e);
279 
280  double num = 2.0 * (k[0] * iPc.get_i() * iPc.get_i() + 2.0 * k[2] * iPc.get_j() * iPc.get_i() +
281  k[1] * iPc.get_j() * iPc.get_j() - k[5]);
282  double a2 = num / (k[0] + k[1] + sq);
283  double b2 = num / (k[0] + k[1] - sq);
284 
285  a = sqrt(a2);
286  b = sqrt(b2);
287 }
288 
294 {
295  std::cout << "K" << std::endl;
296  std::cout << K.t();
297  std::cout << iPc << std::endl;
298 }
299 
309 void vpMeEllipse::computeAngle(const vpImagePoint &pt1, const vpImagePoint &pt2)
310 {
311  getParameters();
312 
313  int number_of_points = 2000;
314  double incr = 2 * M_PI / number_of_points; // angle increment
315 
316  double dmin1 = 1e6;
317  double dmin2 = 1e6;
318 
319  double k = 0;
320  while (k < 2 * M_PI) {
321 
322  double j1 = a * sin(k); // equation of an ellipse
323  double i1 = b * cos(k); // equation of an ellipse
324 
325  // (i1,j1) are the coordinates on the origin centered ellipse;
326  // a rotation by "e" and a translation by (xci,jc) are done
327  // to get the coordinates of the point on the shifted ellipse
328 
329  double j11 = iPc.get_j() + ce * j1 + se * i1;
330  double i11 = iPc.get_i() - se * j1 + ce * i1;
331 
332  double d = vpMath::sqr(pt1.get_i() - i11) + vpMath::sqr(pt1.get_j() - j11);
333  if (d < dmin1) {
334  dmin1 = d;
335  alpha1 = k;
336  }
337  d = vpMath::sqr(pt2.get_i() - i11) + vpMath::sqr(pt2.get_j() - j11);
338  if (d < dmin2) {
339  dmin2 = d;
340  alpha2 = k;
341  }
342  k += incr;
343  }
344 
345  if (alpha2 < alpha1)
346  alpha2 += 2 * M_PI;
347  // else if (alpha2 == alpha1)
348  else if (std::fabs(alpha2 - alpha1) < std::fabs(alpha1) * std::numeric_limits<double>::epsilon())
349  alpha2 += 2 * M_PI;
350 }
351 
357 void vpMeEllipse::updateTheta()
358 {
359  vpMeSite p_me;
360  double theta;
361  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
362  p_me = *it;
363  vpImagePoint iP;
364  iP.set_i(p_me.ifloat);
365  iP.set_j(p_me.jfloat);
366  computeTheta(theta, K, iP);
367  p_me.alpha = theta;
368  *it = p_me;
369  }
370 }
371 
376 void vpMeEllipse::suppressPoints()
377 {
378  // Loop through list of sites to track
379  std::list<vpMeSite>::iterator itList = list.begin();
380  for (std::list<double>::iterator it = angle.begin(); it != angle.end();) {
381  vpMeSite s = *itList; // current reference pixel
382  if (s.getState() != vpMeSite::NO_SUPPRESSION) {
383  itList = list.erase(itList);
384  it = angle.erase(it);
385  } else {
386  ++itList;
387  ++it;
388  }
389  }
390 }
391 
403 void vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
404 {
405  if (!me) {
406  throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
407  }
408 
409  int rows = (int)I.getHeight();
410  int cols = (int)I.getWidth();
411 
412  vpImagePoint ip;
413 
414  unsigned int memory_range = me->getRange();
415  me->setRange(2);
416 
417  double memory_mu1 = me->getMu1();
418  me->setMu1(0.5);
419 
420  double memory_mu2 = me->getMu2();
421  me->setMu2(0.5);
422 
423  double incr = vpMath::rad(2.0);
424 
425  if (alpha2 - alpha1 < 2 * M_PI - vpMath::rad(6.0)) {
426  vpMeSite P;
427  double k = alpha1;
428  double i1, j1;
429 
430  for (unsigned int i = 0; i < 3; i++) {
431  k -= incr;
432  // while ( k < -M_PI ) { k+=2*M_PI; }
433 
434  i1 = b * cos(k); // equation of an ellipse
435  j1 = a * sin(k); // equation of an ellipse
436  P.ifloat = iPc.get_i() - se * j1 + ce * i1;
437  P.i = (int)P.ifloat;
438  P.jfloat = iPc.get_j() + ce * j1 + se * i1;
439  P.j = (int)P.jfloat;
440 
441  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
442  P.track(I, me, false);
443 
444  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
445  list.push_back(P);
446  angle.push_back(k);
447  if (vpDEBUG_ENABLE(3)) {
448  ip.set_i(P.i);
449  ip.set_j(P.j);
450 
452  }
453  } else {
454  if (vpDEBUG_ENABLE(3)) {
455  ip.set_i(P.i);
456  ip.set_j(P.j);
458  }
459  }
460  }
461  }
462 
463  k = alpha2;
464 
465  for (unsigned int i = 0; i < 3; i++) {
466  k += incr;
467  // while ( k > M_PI ) { k-=2*M_PI; }
468 
469  i1 = b * cos(k); // equation of an ellipse
470  j1 = a * sin(k); // equation of an ellipse
471  P.ifloat = iPc.get_i() - se * j1 + ce * i1;
472  P.i = (int)P.ifloat;
473  P.jfloat = iPc.get_j() + ce * j1 + se * i1;
474  P.j = (int)P.jfloat;
475 
476  if (!outOfImage(P.i, P.j, 5, rows, cols)) {
477  P.track(I, me, false);
478 
479  if (P.getState() == vpMeSite::NO_SUPPRESSION) {
480  list.push_back(P);
481  angle.push_back(k);
482  if (vpDEBUG_ENABLE(3)) {
483  ip.set_i(P.i);
484  ip.set_j(P.j);
485 
487  }
488  } else {
489  if (vpDEBUG_ENABLE(3)) {
490  ip.set_i(P.i);
491  ip.set_j(P.j);
493  }
494  }
495  }
496  }
497  }
498 
499  suppressPoints();
500 
501  me->setRange(memory_range);
502  me->setMu1(memory_mu1);
503  me->setMu2(memory_mu2);
504 }
505 
510 void vpMeEllipse::setExtremities()
511 {
512  double alphamin = +1e6;
513  double alphamax = -1e6;
514  double imin = 0;
515  double jmin = 0;
516  double imax = 0;
517  double jmax = 0;
518 
519  // Loop through list of sites to track
520  std::list<double>::const_iterator itAngle = angle.begin();
521 
522  for (std::list<vpMeSite>::const_iterator itList = list.begin(); itList != list.end(); ++itList) {
523  vpMeSite s = *itList; // current reference pixel
524  double alpha = *itAngle;
525  if (alpha < alphamin) {
526  alphamin = alpha;
527  imin = s.ifloat;
528  jmin = s.jfloat;
529  }
530 
531  if (alpha > alphamax) {
532  alphamax = alpha;
533  imax = s.ifloat;
534  jmax = s.jfloat;
535  }
536  ++itAngle;
537  }
538 
539  alpha1 = alphamin;
540  alpha2 = alphamax;
541  iP1.set_ij(imin, jmin);
542  iP2.set_ij(imax, jmax);
543 }
544 
550 void vpMeEllipse::leastSquare()
551 {
552  // Construction du systeme Ax=b
553  // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
554  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
555  unsigned int i;
556 
557  vpMeSite p_me;
558 
559  unsigned int iter = 0;
562  r.setThreshold(2);
563  r.setIteration(0);
565  D.eye();
566  vpMatrix DA, DAmemory;
567  vpColVector DAx;
569  w = 1;
570  unsigned int nos_1 = numberOfSignal();
571 
572  if (list.size() < 3) {
573  throw(vpException(vpException::dimensionError, "Not enought moving edges to track the ellipse"));
574  }
575 
576  vpMatrix A(numberOfSignal(), 5);
577  vpColVector x(5);
578 
579  unsigned int k = 0;
580  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
581  p_me = *it;
582  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
583  A[k][0] = vpMath::sqr(p_me.jfloat);
584  A[k][1] = 2 * p_me.ifloat * p_me.jfloat;
585  A[k][2] = 2 * p_me.ifloat;
586  A[k][3] = 2 * p_me.jfloat;
587  A[k][4] = 1;
588 
589  b_[k] = -vpMath::sqr(p_me.ifloat);
590  k++;
591  }
592  }
593 
594  while (iter < 4) {
595  DA = D * A;
596  vpMatrix DAp;
597 
598  x = DA.pseudoInverse(1e-26) * D * b_;
599 
600  vpColVector residu(nos_1);
601  residu = b_ - A * x;
602  r.setIteration(iter);
603  r.MEstimator(vpRobust::TUKEY, residu, w);
604 
605  k = 0;
606  for (i = 0; i < nos_1; i++) {
607  D[k][k] = w[k];
608  k++;
609  }
610  iter++;
611  }
612 
613  k = 0;
614  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
615  p_me = *it;
616  if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
617  if (w[k] < thresholdWeight) {
619 
620  *it = p_me;
621  }
622  k++;
623  }
624  }
625  for (i = 0; i < 5; i++)
626  K[i] = x[i];
627 
628  getParameters();
629 }
630 
641 {
642  vpMeEllipse::display(I, iPc, a, b, e, alpha1, alpha2, col);
643 }
644 
654 {
655  const unsigned int n = 5;
656  vpImagePoint iP[n];
657 
658  for (unsigned int k = 0; k < n; k++) {
659  std::cout << "Click points " << k + 1 << "/" << n;
660  std::cout << " on the ellipse in the trigonometric order" << std::endl;
661  vpDisplay::getClick(I, iP[k], true);
663  vpDisplay::flush(I);
664  std::cout << iP[k] << std::endl;
665  }
666 
667  iP1 = iP[0];
668  iP2 = iP[n - 1];
669 
670  initTracking(I, n, iP);
671 }
672 
685 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n, vpImagePoint *iP)
686 {
687  vpMatrix A(n, 5);
688  vpColVector b_(n);
689  vpColVector x(5);
690 
691  // Construction du systeme Ax=b
692  // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
693  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
694 
695  for (unsigned int k = 0; k < n; k++) {
696  A[k][0] = vpMath::sqr(iP[k].get_j());
697  A[k][1] = 2 * iP[k].get_i() * iP[k].get_j();
698  A[k][2] = 2 * iP[k].get_i();
699  A[k][3] = 2 * iP[k].get_j();
700  A[k][4] = 1;
701 
702  b_[k] = -vpMath::sqr(iP[k].get_i());
703  }
704 
705  K = A.pseudoInverse(1e-26) * b_;
706 
707  iP1 = iP[0];
708  iP2 = iP[n - 1];
709 
710  getParameters();
711 
712  computeAngle(iP1, iP2);
713 
715 
717  sample(I);
718 
720 
721  track(I);
722 
724  vpDisplay::flush(I);
725 }
726 
739 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
740 {
741  unsigned int n = (unsigned int)(iP.size());
742  vpMatrix A(n, 5);
743  vpColVector b_(n);
744  vpColVector x(5);
745 
746  // Construction du systeme Ax=b
747  // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
748  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
749 
750  for (unsigned int k = 0; k < n; k++) {
751  A[k][0] = vpMath::sqr(iP[k].get_j());
752  A[k][1] = 2 * iP[k].get_i() * iP[k].get_j();
753  A[k][2] = 2 * iP[k].get_i();
754  A[k][3] = 2 * iP[k].get_j();
755  A[k][4] = 1;
756 
757  b_[k] = -vpMath::sqr(iP[k].get_i());
758  }
759 
760  K = A.pseudoInverse(1e-26) * b_;
761 
762  iP1 = iP[0];
763  iP2 = iP[n - 1];
764 
765  getParameters();
766 
767  computeAngle(iP1, iP2);
768 
770 
772  sample(I);
773 
775 
776  track(I);
777 
779  vpDisplay::flush(I);
780 }
781 
782 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p,
783  double e_p, double low_alpha, double high_alpha)
784 {
785  iPc = ic;
786  a = a_p;
787  b = b_p;
788  e = e_p;
789  alpha1 = low_alpha;
790  alpha2 = high_alpha;
791 
792  if (alpha2 < alpha1)
793  alpha2 += 2 * M_PI;
794 
795  ce = cos(e);
796  se = sin(e);
797 
799  sample(I);
800 
802 
803  track(I);
804 
806  vpDisplay::flush(I);
807 }
808 
815 {
817 
818  // Estimation des parametres de la droite aux moindres carre
819  suppressPoints();
820  setExtremities();
821 
822  leastSquare();
823  seekExtremities(I);
824  setExtremities();
825  leastSquare();
826 
827  // suppression des points rejetes par la regression robuste
828  suppressPoints();
829  setExtremities();
830 
831  // reechantillonage si necessaire
832  reSample(I);
833 
834  // remet a jour l'angle delta pour chaque point de la liste
835 
836  updateTheta();
837 
838  computeMoments();
839 
840  // Remise a jour de delta dans la liste de site me
841  if (vpDEBUG_ENABLE(2)) {
842  display(I, vpColor::red);
844  vpDisplay::flush(I);
845  }
846 }
847 
855 void vpMeEllipse::computeMoments()
856 {
857  double tane = tan(-1 / e);
858  m00 = M_PI * a * b;
859  m10 = m00 * iPc.get_i();
860  m01 = m00 * iPc.get_j();
861  m20 = m00 * (a * a + b * b * tane * tane) / (4 * (1 + tane * tane)) + m00 * iPc.get_i() * iPc.get_i();
862  m02 = m00 * (a * a * tane * tane + b * b) / (4 * (1 + tane * tane)) + m00 * iPc.get_j() * iPc.get_j();
863  m11 = m00 * tane * (a * a - b * b) / (4 * (1 + tane * tane)) + m00 * iPc.get_i() * iPc.get_j();
864  mu11 = m11 - iPc.get_j() * m10;
865  mu02 = m02 - iPc.get_j() * m01;
866  mu20 = m20 - iPc.get_i() * m10;
867 }
868 
869 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
870 
874 void vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1, int ip2, int jp2, double &_alpha2)
875 {
876  getParameters();
877 
878  int number_of_points = 2000;
879  double incr = 2 * M_PI / number_of_points; // angle increment
880 
881  double dmin1 = 1e6;
882  double dmin2 = 1e6;
883 
884  double k = -M_PI;
885  while (k < M_PI) {
886 
887  double j1 = a * cos(k); // equation of an ellipse
888  double i1 = b * sin(k); // equation of an ellipse
889 
890  // (i1,j1) are the coordinates on the origin centered ellipse;
891  // a rotation by "e" and a translation by (xci,jc) are done
892  // to get the coordinates of the point on the shifted ellipse
893  double j11 = iPc.get_j() + ce * j1 - se * i1;
894  double i11 = iPc.get_i() - (se * j1 + ce * i1);
895 
896  double d = vpMath::sqr(ip1 - i11) + vpMath::sqr(jp1 - j11);
897  if (d < dmin1) {
898  dmin1 = d;
899  alpha1 = k;
900  _alpha1 = k;
901  }
902  d = vpMath::sqr(ip2 - i11) + vpMath::sqr(jp2 - j11);
903  if (d < dmin2) {
904  dmin2 = d;
905  alpha2 = k;
906  _alpha2 = k;
907  }
908  k += incr;
909  }
910 
911  if (alpha2 < alpha1)
912  alpha2 += 2 * M_PI;
913 
914  vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl;
915 }
916 
920 void vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2)
921 {
922 
923  double a1, a2;
924  computeAngle(ip1, jp1, a1, ip2, jp2, a2);
925 }
926 
927 void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n, unsigned *i, unsigned *j)
928 {
929  vpMatrix A(n, 5);
930  vpColVector b_(n);
931  vpColVector x(5);
932 
933  // Construction du systeme Ax=b
935  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
936 
937  for (unsigned int k = 0; k < n; k++) {
938  A[k][0] = vpMath::sqr(j[k]);
939  A[k][1] = 2 * i[k] * j[k];
940  A[k][2] = 2 * i[k];
941  A[k][3] = 2 * j[k];
942  A[k][4] = 1;
943 
944  b_[k] = -vpMath::sqr(i[k]);
945  }
946 
947  K = A.pseudoInverse(1e-26) * b_;
948  // std::cout << K << std::endl;
949 
950  iP1.set_i(i[0]);
951  iP1.set_j(j[0]);
952  iP2.set_i(i[n - 1]);
953  iP2.set_j(j[n - 1]);
954 
955  getParameters();
956  computeAngle(iP1, iP2);
958 
959  sample(I);
960 
961  // 2. On appelle ce qui n'est pas specifique
962  {
964  }
965 
966  track(I);
967 
969  vpDisplay::flush(I);
970 }
971 #endif // Deprecated
972 
996 void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpImagePoint &center, const double &A, const double &B,
997  const double &E, const double &smallalpha, const double &highalpha, const vpColor &color,
998  unsigned int thickness)
999 {
1000  double j1, i1;
1001  vpImagePoint iP11;
1002  double j2, i2;
1003  vpImagePoint iP22;
1004  j1 = j2 = i1 = i2 = 0;
1005 
1006  double incr = vpMath::rad(2); // angle increment
1007 
1008  vpDisplay::displayCross(I, center, 20, vpColor::red, thickness);
1009 
1010  double k = smallalpha;
1011  while (k + incr < highalpha) {
1012  j1 = A * cos(k); // equation of an ellipse
1013  i1 = B * sin(k); // equation of an ellipse
1014 
1015  j2 = A * cos(k + incr); // equation of an ellipse
1016  i2 = B * sin(k + incr); // equation of an ellipse
1017 
1018  // (i1,j1) are the coordinates on the origin centered ellipse;
1019  // a rotation by "e" and a translation by (xci,jc) are done
1020  // to get the coordinates of the point on the shifted ellipse
1021  iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
1022  iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
1023  // to get the coordinates of the point on the shifted ellipse
1024  iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
1025  iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
1026 
1027  vpDisplay::displayLine(I, iP11, iP22, color, thickness);
1028 
1029  k += incr;
1030  }
1031 
1032  j1 = A * cos(smallalpha); // equation of an ellipse
1033  i1 = B * sin(smallalpha); // equation of an ellipse
1034 
1035  j2 = A * cos(highalpha); // equation of an ellipse
1036  i2 = B * sin(highalpha); // equation of an ellipse
1037 
1038  // (i1,j1) are the coordinates on the origin centered ellipse;
1039  // a rotation by "e" and a translation by (xci,jc) are done
1040  // to get the coordinates of the point on the shifted ellipse
1041  iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
1042  iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
1043  // to get the coordinates of the point on the shifted ellipse
1044  iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
1045  iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
1046 
1047  vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness);
1048  vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness);
1049 }
1050 
1074 void vpMeEllipse::display(const vpImage<vpRGBa> &I, const vpImagePoint &center, const double &A, const double &B,
1075  const double &E, const double &smallalpha, const double &highalpha, const vpColor &color,
1076  unsigned int thickness)
1077 {
1078  double j1, i1;
1079  vpImagePoint iP11;
1080  double j2, i2;
1081  vpImagePoint iP22;
1082  j1 = j2 = i1 = i2 = 0;
1083 
1084  double incr = vpMath::rad(2); // angle increment
1085 
1086  vpDisplay::displayCross(I, center, 20, vpColor::red, thickness);
1087 
1088  double k = smallalpha;
1089  while (k + incr < highalpha) {
1090  j1 = A * cos(k); // equation of an ellipse
1091  i1 = B * sin(k); // equation of an ellipse
1092 
1093  j2 = A * cos(k + incr); // equation of an ellipse
1094  i2 = B * sin(k + incr); // equation of an ellipse
1095 
1096  // (i1,j1) are the coordinates on the origin centered ellipse;
1097  // a rotation by "e" and a translation by (xci,jc) are done
1098  // to get the coordinates of the point on the shifted ellipse
1099  iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
1100  iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
1101  // to get the coordinates of the point on the shifted ellipse
1102  iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
1103  iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
1104 
1105  vpDisplay::displayLine(I, iP11, iP22, color, thickness);
1106 
1107  k += incr;
1108  }
1109 
1110  j1 = A * cos(smallalpha); // equation of an ellipse
1111  i1 = B * sin(smallalpha); // equation of an ellipse
1112 
1113  j2 = A * cos(highalpha); // equation of an ellipse
1114  i2 = B * sin(highalpha); // equation of an ellipse
1115 
1116  // (i1,j1) are the coordinates on the origin centered ellipse;
1117  // a rotation by "e" and a translation by (xci,jc) are done
1118  // to get the coordinates of the point on the shifted ellipse
1119  iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
1120  iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
1121  // to get the coordinates of the point on the shifted ellipse
1122  iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
1123  iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
1124 
1125  vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness);
1126  vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness);
1127 }
double a
is the semiminor axis of the ellipse.
Definition: vpMeEllipse.h:282
void track(const vpImage< unsigned char > &I)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2081
double m01
Definition: vpMeEllipse.h:313
double get_i() const
Definition: vpImagePoint.h:204
double expecteddensity
Expected number of me to track along the ellipse.
Definition: vpMeEllipse.h:319
vpMeSiteState getState() const
Definition: vpMeSite.h:189
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void initTracking(const vpImage< unsigned char > &I)
double mu02
Definition: vpMeEllipse.h:311
void init()
Definition: vpMeSite.cpp:66
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
double jfloat
Definition: vpMeSite.h:88
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:538
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 for the robust least square.
Definition: vpMeEllipse.h:317
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
vpImagePoint iP1
Definition: vpMeEllipse.h:293
double alpha
Definition: vpMeSite.h:92
int i
Definition: vpMeSite.h:86
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that tracks an ellipse moving edges.
Definition: vpMeEllipse.h:106
double mu20
Definition: vpMeEllipse.h:311
static const vpColor green
Definition: vpColor.h:183
static void flush(const vpImage< unsigned char > &I)
static int round(const double x)
Definition: vpMath.h:241
double b
is the semimajor axis of the ellipse.
Definition: vpMeEllipse.h:284
double se
Value of sin(e).
Definition: vpMeEllipse.h:305
void setMu1(const double &mu_1)
Definition: vpMe.h:241
double getMu1() const
Definition: vpMe.h:155
static const vpColor red
Definition: vpColor.h:180
double ifloat
Definition: vpMeSite.h:88
virtual ~vpMeEllipse()
double m20
Definition: vpMeEllipse.h:315
void set_i(const double ii)
Definition: vpImagePoint.h:167
std::list< vpMeSite > list
Definition: vpMeTracker.h:74
vpImagePoint iP2
Definition: vpMeEllipse.h:297
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:280
double m11
Second order raw moments.
Definition: vpMeEllipse.h:315
double alpha1
The smallest angle.
Definition: vpMeEllipse.h:299
static double sqr(double x)
Definition: vpMath.h:114
void printParameters()
double get_j() const
Definition: vpImagePoint.h:215
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:138
void display(const vpImage< unsigned char > &I, vpColor col)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
void track(const vpImage< unsigned char > &I)
Track sampled pixels.
double mu11
Second order central moments.
Definition: vpMeEllipse.h:311
double m10
First order raw moments.
Definition: vpMeEllipse.h:313
Contains abstract elements for a Distance to Feature type feature.
Definition: vpMeTracker.h:65
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:175
double m00
Surface.
Definition: vpMeEllipse.h:309
static double rad(double deg)
Definition: vpMath.h:108
std::list< double > angle
Stores the value of the angle for each vpMeSite.
Definition: vpMeEllipse.h:307
void set_j(const double jj)
Definition: vpImagePoint.h:178
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void setMu2(const double &mu_2)
Definition: vpMe.h:248
int j
Definition: vpMeSite.h:86
double m02
Definition: vpMeEllipse.h:315
unsigned int getHeight() const
Definition: vpImage.h:186
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void track(const vpImage< unsigned char > &im, const vpMe *me, const bool test_contraste=true)
Definition: vpMeSite.cpp:355
vpMe * me
Moving edges initialisation parameters.
Definition: vpMeTracker.h:76
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
double getMu2() const
Definition: vpMe.h:161
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:88
double alpha2
The highest angle.
Definition: vpMeEllipse.h:301
void setRange(const unsigned int &r)
Definition: vpMe.h:271
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
vpMeSite::vpMeSiteDisplayType selectDisplay
Definition: vpMeTracker.h:83
unsigned int getWidth() const
Definition: vpImage.h:244
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:109
unsigned int getRange() const
Definition: vpMe.h:179
void eye()
Definition: vpMatrix.cpp:440
virtual void display(const vpImage< unsigned char > &I, vpColor col)=0
double ce
Value of cos(e).
Definition: vpMeEllipse.h:303
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:189
static const vpColor blue
Definition: vpColor.h:186