ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpMeEllipse.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMeEllipse.cpp 4062 2013-01-09 10:30:06Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Moving edges.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
43 
44 #include <visp/vpMeEllipse.h>
45 
46 #include <visp/vpMe.h>
47 #include <visp/vpRobust.h>
48 #include <visp/vpTrackingException.h>
49 #include <visp/vpDebug.h>
50 #include <visp/vpImagePoint.h>
51 
52 #include <cmath> // std::fabs
53 #include <limits> // numeric_limits
61 void
62 computeTheta(double &theta, vpColVector &K, vpImagePoint iP)
63 {
64 
65  double i = iP.get_i();
66  double j = iP.get_j();
67 
68  double A = 2*i+2*K[1]*j + 2*K[2] ;
69  double B = 2*K[0]*j + 2*K[1]*i + 2*K[3];
70 
71  theta = atan2(A,B) ; //Angle between the tangente and the i axis.
72 
73  while (theta > M_PI) { theta -= M_PI ; }
74  while (theta < 0) { theta += M_PI ; }
75 }
76 
77 
82 {
83  vpCDEBUG(1) << "begin vpMeEllipse::vpMeEllipse() " << std::endl ;
84 
85  // redimensionnement du vecteur de parametre
86  // i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
87 
88  circle = false ;
89  K.resize(5) ;
90 
91  alpha1 = 0 ;
92  alpha2 = 2*M_PI ;
93 
94  //j1 = j2 = i1 = i2 = 0 ;
95  iP1.set_i(0);
96  iP1.set_j(0);
97  iP2.set_i(0);
98  iP2.set_j(0);
99 
100  m00 = m01 = m10 = m11 = m20 = m02 = mu11 = mu20 = mu02 = 0;
101 
102  thresholdWeight = 0.2;
103 
104  vpCDEBUG(1) << "end vpMeEllipse::vpMeEllipse() " << std::endl ;
105 }
106 
111 {
112  K = meellipse.K;
113  iPc = meellipse.iPc;
114  a = meellipse.a;
115  b = meellipse.b;
116  e = meellipse.e;
117 
118  iP1 = meellipse.iP1;
119  iP2 = meellipse.iP2;
120  alpha1 = meellipse.alpha1;
121  alpha2 = meellipse.alpha2;
122  ce = meellipse.ce;
123  se = meellipse.se;
124 
125  angle = meellipse.angle;
126 
127  m00 = meellipse.m00;
128  mu11 = meellipse.mu11;
129  mu20 = meellipse.mu20;
130  mu02 = meellipse.mu02;
131  m10 = meellipse.m10;
132  m01 = meellipse.m01;
133  m11 = meellipse.m11;
134  m02 = meellipse.m02;
135  m20 = meellipse.m20;
136  thresholdWeight = meellipse.thresholdWeight;
137 }
138 
143 {
144  vpCDEBUG(1) << "begin vpMeEllipse::~vpMeEllipse() " << std::endl ;
145 
146  list.clear();
147  angle.clear();
148 
149  vpCDEBUG(1) << "end vpMeEllipse::~vpMeEllipse() " << std::endl ;
150 }
151 
152 
160 void
161 vpMeEllipse::sample(const vpImage<unsigned char> & I)
162 {
163  vpCDEBUG(1) <<"begin vpMeEllipse::sample() : "<<std::endl ;
164 
165  int height = (int)I.getHeight() ;
166  int width = (int)I.getWidth() ;
167 
168  double n_sample;
169 
170  //if (me->getSampleStep()==0)
171  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
172  {
173  std::cout << "In vpMeEllipse::sample: " ;
174  std::cout << "function called with sample step = 0" ;
175  //return fatalError ;
176  }
177 
178  double j, i;//, j11, i11;
179  vpImagePoint iP11;
180  j = i = 0.0 ;
181 
182  double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
183  vpColor col = vpColor::red ;
184  getParameters() ;
185 
186 
187  // Delete old list
188  list.clear();
189 
190  angle.clear();
191 
192  // sample positions
193 
194  double k = alpha1 ;
195  while (k<alpha2)
196  {
197 
198 // j = a *cos(k) ; // equation of an ellipse
199 // i = b *sin(k) ; // equation of an ellipse
200 
201  j = a *sin(k) ; // equation of an ellipse
202  i = b *cos(k) ; // equation of an ellipse
203 
204  // (i,j) are the coordinates on the origin centered ellipse ;
205  // a rotation by "e" and a translation by (xci,jc) are done
206  // to get the coordinates of the point on the shifted ellipse
207 // iP11.set_j( iPc.get_j() + ce *j - se *i );
208 // iP11.set_i( iPc.get_i() -( se *j + ce *i) );
209 
210  iP11.set_j( iPc.get_j() + ce *j + se *i );
211  iP11.set_i( iPc.get_i() - se *j + ce *i );
212 
213  vpDisplay::displayCross(I, iP11, 5, col) ;
214 
215  double theta ;
216  computeTheta(theta, K, iP11) ;
217 
218  // If point is in the image, add to the sample list
219  if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
220  {
221  vpMeSite pix ;
222  pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
225 
226  if(vpDEBUG_ENABLE(3))
227  {
229  }
230  list.push_back(pix);
231  angle.push_back(k);
232  }
233  k += incr ;
234 
235  }
237 
238  n_sample = list.size() ;
239 
240  vpCDEBUG(1) << "end vpMeEllipse::sample() : " ;
241  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
242 }
243 
244 
256 void
257 vpMeEllipse::reSample(const vpImage<unsigned char> &I)
258 {
259  unsigned int n = numberOfSignal() ;
260  double expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
261  if ((double)n<0.9*expecteddensity){
262  sample(I) ;
263  }
264 }
265 
266 
273 void
274 vpMeEllipse::getParameters()
275 {
276 
277  double k[6] ;
278  for (unsigned int i=0 ; i < 5 ; i++)
279  k[i+1] = K[i] ;
280  k[0] = 1 ;
281 
282  double d = k[2]*k[2] - k[0]*k[1];
283 
284 
285  iPc.set_i( (k[1] * k[3] - k[2] * k[4]) / d );
286  iPc.set_j( (k[0] * k[4] - k[2] * k[3]) / d );
287 
288  double sq = sqrt(vpMath::sqr(k[1]-k[0]) + 4.0*vpMath::sqr(k[2])) ;
289  if (circle ==true)
290  {
291  e = 0 ;
292  }
293  else
294  {
295  e = (k[1] - k[0] + sq) / (2.0*k[2]);
296  e = (-1/e) ;
297 
298  e = atan(e) ;
299  }
300 
301  if(e < 0.0) e += M_PI ;
302 
303  ce = cos(e) ;
304  se = sin(e) ;
305 
306  double num = 2.0*(k[0]*iPc.get_i()*iPc.get_i() + 2.0*k[2]*iPc.get_j()*iPc.get_i() + k[1]*iPc.get_j()*iPc.get_j() - k[5]) ;
307  double a2 = num / (k[0] + k[1] + sq ) ;
308  double b2 = num / (k[0] + k[1] - sq ) ;
309 
310  a = sqrt( a2 ) ;
311  b = sqrt( b2 ) ;
312 
313 }
314 
318 void
320 {
321  std::cout << "K" << std::endl ;
322  std::cout << K.t() ;
323  std::cout << iPc << std::endl ;
324 }
325 
334 void
335 vpMeEllipse::computeAngle(vpImagePoint pt1, vpImagePoint pt2)
336 {
337 
338  getParameters() ;
339  double j1, i1, j11, i11;
340  j1 = i1 = 0.0 ;
341 
342  int number_of_points = 2000 ;
343  double incr = 2 * M_PI / number_of_points ; // angle increment
344 
345  double dmin1 = 1e6 ;
346  double dmin2 = 1e6 ;
347 
348  double k = 0 ;
349  while(k < 2*M_PI) {
350 
351 // j1 = a *cos(k) ; // equation of an ellipse
352 // i1 = b *sin(k) ; // equation of an ellipse
353 
354  j1 = a *sin(k) ; // equation of an ellipse
355  i1 = b *cos(k) ; // equation of an ellipse
356 
357  // (i1,j1) are the coordinates on the origin centered ellipse ;
358  // a rotation by "e" and a translation by (xci,jc) are done
359  // to get the coordinates of the point on the shifted ellipse
360 // j11 = iPc.get_j() + ce *j1 - se *i1 ;
361 // i11 = iPc.get_i() -( se *j1 + ce *i1) ;
362 
363  j11 = iPc.get_j() + ce *j1 + se *i1 ;
364  i11 = iPc.get_i() - se *j1 + ce *i1 ;
365 
366  double d = vpMath::sqr(pt1.get_i()-i11) + vpMath::sqr(pt1.get_j()-j11) ;
367  if (d < dmin1)
368  {
369  dmin1 = d ;
370  alpha1 = k ;
371  }
372  d = vpMath::sqr(pt2.get_i()-i11) + vpMath::sqr(pt2.get_j()-j11) ;
373  if (d < dmin2)
374  {
375  dmin2 = d ;
376  alpha2 = k ;
377  }
378  k += incr ;
379  }
380  //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
381 
382  if (alpha2 <alpha1)
383  {
384 // double alphatmp = alpha2;
385 // alpha2 = alpha1;
386 // alpha1 = alphatmp;
387  alpha2 += 2 * M_PI;
388  }
389 
390  //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
391 
392 
393 }
394 
395 
401 void
402 vpMeEllipse::updateTheta()
403 {
404  vpMeSite p;
405  double theta;
406  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
407  p = *it;
408  vpImagePoint iP;
409  iP.set_i(p.ifloat);
410  iP.set_j(p.jfloat);
411  computeTheta(theta, K, iP) ;
412  p.alpha = theta ;
413  *it = p;
414  }
415 }
416 
420 void
421 vpMeEllipse::suppressPoints()
422 {
423  // Loop through list of sites to track
424  std::list<vpMeSite>::iterator itList = list.begin();
425  for(std::list<double>::iterator it=angle.begin(); it!=angle.end(); ){
426  vpMeSite s = *itList;//current reference pixel
428  {
429  itList = list.erase(itList) ;
430  it = angle.erase(it);
431  }
432  else
433  {
434  ++itList;
435  ++it;
436  }
437  }
438 }
439 
440 
447 void
448 vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
449 {
450  int rows = (int)I.getHeight() ;
451  int cols = (int)I.getWidth() ;
452 
453  vpImagePoint ip;
454 
455  unsigned int memory_range = me->getRange() ;
456  me->setRange(2);
457 
458  double memory_mu1 = me->getMu1();
459  me->setMu1(0.5);
460 
461  double memory_mu2 = me->getMu2();
462  me->setMu2(0.5);
463 
464  double incr = vpMath::rad(2.0) ;
465 
466  if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
467  {
468  vpMeSite P;
469  double k = alpha1;
470  double i1,j1;
471 
472  for (unsigned int i=0 ; i < 3 ; i++)
473  {
474  k -= incr;
475  //while ( k < -M_PI ) { k+=2*M_PI; }
476 
477  i1 = b *cos(k) ; // equation of an ellipse
478  j1 = a *sin(k) ; // equation of an ellipse
479  P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
480  P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
481 
482  if(!outOfImage(P.i, P.j, 5, rows, cols))
483  {
484  P.track(I,me,false) ;
485 
487  {
488  list.push_back(P);
489  angle.push_back(k);
490  if (vpDEBUG_ENABLE(3)) {
491  ip.set_i( P.i );
492  ip.set_j( P.j );
493 
495  }
496  }
497  else {
498  if (vpDEBUG_ENABLE(3)) {
499  ip.set_i( P.i );
500  ip.set_j( P.j );
502  }
503  }
504  }
505  }
506 
507  k = alpha2;
508 
509  for (unsigned int i=0 ; i < 3 ; i++)
510  {
511  k += incr;
512  //while ( k > M_PI ) { k-=2*M_PI; }
513 
514  i1 = b *cos(k) ; // equation of an ellipse
515  j1 = a *sin(k) ; // equation of an ellipse
516  P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
517  P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
518 
519  if(!outOfImage(P.i, P.j, 5, rows, cols))
520  {
521  P.track(I,me,false) ;
522 
524  {
525  list.push_back(P);
526  angle.push_back(k);
527  if (vpDEBUG_ENABLE(3)) {
528  ip.set_i( P.i );
529  ip.set_j( P.j );
530 
532  }
533  }
534  else {
535  if (vpDEBUG_ENABLE(3)) {
536  ip.set_i( P.i );
537  ip.set_j( P.j );
539  }
540  }
541  }
542  }
543  }
544 
545  suppressPoints() ;
546 
547  me->setRange(memory_range);
548  me->setMu1(memory_mu1);
549  me->setMu2(memory_mu2);
550 }
551 
552 
556 void
557 vpMeEllipse::setExtremities()
558 {
559  double alphamin = +1e6;
560  double alphamax = -1e6;
561  double imin = 0;
562  double jmin = 0;
563  double imax = 0;
564  double jmax = 0;
565 
566  // Loop through list of sites to track
567  std::list<double>::const_iterator itAngle = angle.begin();
568 
569  for(std::list<vpMeSite>::const_iterator itList=list.begin(); itList!=list.end(); ++itList){
570  vpMeSite s = *itList;//current reference pixel
571  double alpha = *itAngle;
572  if (alpha < alphamin)
573  {
574  alphamin = alpha;
575  imin = s.ifloat ;
576  jmin = s.jfloat ;
577  }
578 
579  if (alpha > alphamax)
580  {
581  alphamax = alpha;
582  imax = s.ifloat ;
583  jmax = s.jfloat ;
584  }
585  ++itAngle;
586  }
587 
588  alpha1 = alphamin;
589  alpha2 = alphamax;
590  iP1.set_ij(imin,jmin);
591  iP2.set_ij(imax,jmax);
592 }
593 
594 
600 void
601 vpMeEllipse::leastSquare()
602 {
603  // Construction du systeme Ax=b
605  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
606  unsigned int i ;
607 
608  vpMeSite p ;
609 
610  unsigned int iter =0 ;
612  vpRobust r(numberOfSignal()) ;
613  r.setThreshold(2);
614  r.setIteration(0) ;
616  D.setIdentity() ;
617  vpMatrix DA, DAmemory ;
618  vpColVector DAx ;
620  w =1 ;
621  unsigned int nos_1 = numberOfSignal() ;
622 
623  if (list.size() < 3)
624  {
625  vpERROR_TRACE("Not enough point") ;
627  "not enough point")) ;
628  }
629 
630  if (circle ==false)
631  {
632  vpMatrix A(numberOfSignal(),5) ;
633  vpColVector x(5);
634 
635  unsigned int k =0 ;
636  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
637  p = *it;
639  {
640 
641  A[k][0] = vpMath::sqr(p.jfloat) ;
642  A[k][1] = 2 * p.ifloat * p.jfloat ;
643  A[k][2] = 2 * p.ifloat ;
644  A[k][3] = 2 * p.jfloat ;
645  A[k][4] = 1 ;
646 
647  b[k] = - vpMath::sqr(p.ifloat) ;
648  k++ ;
649  }
650  }
651 
652  while (iter < 4 )
653  {
654  DA = D*A ;
655  vpMatrix DAp ;
656 
657  x = DA.pseudoInverse(1e-26) *D*b ;
658 
659  vpColVector residu(nos_1);
660  residu = b - A*x;
661  r.setIteration(iter) ;
662  r.MEstimator(vpRobust::TUKEY,residu,w) ;
663 
664  k = 0;
665  for (i=0 ; i < nos_1 ; i++)
666  {
667  D[k][k] =w[k] ;
668  k++;
669  }
670  iter++;
671  }
672 
673  k =0 ;
674  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
675  p = *it;
677  {
678  if (w[k] < thresholdWeight)
679  {
681 
682  *it = p;
683  }
684  k++ ;
685  }
686  }
687  for(i = 0; i < 5; i ++)
688  K[i] = x[i];
689  }
690 
691  else
692  {
693  vpMatrix A(numberOfSignal(),3) ;
694  vpColVector x(3);
695 
696  unsigned int k =0 ;
697  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
698  p = *it;
700  {
701 
702  A[k][0] = 2* p.ifloat ;
703  A[k][1] = 2 * p.jfloat ;
704  A[k][2] = 1 ;
705 
706  b[k] = - vpMath::sqr(p.ifloat) - vpMath::sqr(p.jfloat) ;
707  k++ ;
708  }
709  }
710 
711  while (iter < 4 )
712  {
713  DA = D*A ;
714  vpMatrix DAp ;
715 
716  x = DA.pseudoInverse(1e-26) *D*b ;
717 
718  vpColVector residu(nos_1);
719  residu = b - A*x;
720  r.setIteration(iter) ;
721  r.MEstimator(vpRobust::TUKEY,residu,w) ;
722 
723  k = 0;
724  for (i=0 ; i < nos_1 ; i++)
725  {
726  D[k][k] =w[k];
727  k++;
728  }
729  iter++;
730  }
731 
732  k =0 ;
733  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
734  p = *it;
736  {
737  if (w[k] < thresholdWeight)
738  {
740 
741  *it = p;
742  }
743  k++ ;
744  }
745  }
746  for(i = 0; i < 3; i ++)
747  K[i+2] = x[i];
748  }
749  getParameters() ;
750 }
751 
752 
762 void
764 {
766 }
767 
768 
775 void
777 {
778  vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
779 
780  const unsigned int n=5 ;
781  vpImagePoint iP[n];
782 
783  for (unsigned int k =0 ; k < n ; k++)
784  {
785  std::cout << "Click points "<< k+1 <<"/" << n ;
786  std::cout << " on the ellipse in the trigonometric order" <<std::endl ;
787  vpDisplay::getClick(I, iP[k], true);
788  std::cout << iP[k] << std::endl;
789  }
790 
791  iP1 = iP[0];
792  iP2 = iP[n-1];
793 
794  initTracking(I, n, iP) ;
795 }
796 
797 
808 void
809 vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
810  vpImagePoint *iP)
811 {
812  vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
813 
814  if (circle==false)
815  {
816  vpMatrix A(n,5) ;
817  vpColVector b(n) ;
818  vpColVector x(5) ;
819 
820  // Construction du systeme Ax=b
822  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
823 
824  for (unsigned int k =0 ; k < n ; k++)
825  {
826  A[k][0] = vpMath::sqr(iP[k].get_j()) ;
827  A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
828  A[k][2] = 2* iP[k].get_i() ;
829  A[k][3] = 2* iP[k].get_j() ;
830  A[k][4] = 1 ;
831 
832  b[k] = - vpMath::sqr(iP[k].get_i()) ;
833  }
834 
835  K = A.pseudoInverse(1e-26)*b ;
836  std::cout << K << std::endl;
837  }
838  else
839  {
840  vpMatrix A(n,3) ;
841  vpColVector b(n) ;
842  vpColVector x(3) ;
843 
844  vpColVector Kc(3) ;
845  for (unsigned int k =0 ; k < n ; k++)
846  {
847  A[k][0] = 2* iP[k].get_i() ;
848  A[k][1] = 2* iP[k].get_j() ;
849  A[k][2] = 1 ;
850 
851  b[k] = - vpMath::sqr(iP[k].get_i()) - vpMath::sqr(iP[k].get_j()) ;
852  }
853 
854  Kc = A.pseudoInverse(1e-26)*b ;
855  K[0] = 1 ;
856  K[1] = 0 ;
857  K[2] = Kc[0] ;
858  K[3] = Kc[1] ;
859  K[4] = Kc[2] ;
860 
861  std::cout << K << std::endl;
862  }
863 
864  iP1 = iP[0];
865  iP2 = iP[n-1];
866 
867  getParameters() ;
868  computeAngle(iP1, iP2) ;
869  display(I, vpColor::green) ;
870 
871  sample(I) ;
872 
873  // 2. On appelle ce qui n'est pas specifique
874  {
876  }
877 
878 
879  try{
880  track(I) ;
881  }
882  catch(...)
883  {
884  vpERROR_TRACE("Error caught") ;
885  throw ;
886  }
888  vpDisplay::flush(I) ;
889 
890 }
891 
897 void
899 {
900  vpCDEBUG(1) <<"begin vpMeEllipse::track()"<<std::endl ;
901 
902  static int iter =0 ;
903  // 1. On fait ce qui concerne les ellipse (peut etre vide)
904  {
905  }
906 
907  //vpDisplay::display(I) ;
908  // 2. On appelle ce qui n'est pas specifique
909  {
910 
911  try{
912  vpMeTracker::track(I) ;
913  }
914  catch(...)
915  {
916  vpERROR_TRACE("Error caught") ;
917  throw ;
918  }
919  // std::cout << "number of signals " << numberOfSignal() << std::endl ;
920  }
921 
922  // 3. On revient aux ellipses
923  {
924  // Estimation des parametres de la droite aux moindres carre
925  suppressPoints() ;
926  setExtremities() ;
927 
928 
929  try{
930  leastSquare() ; }
931  catch(...)
932  {
933  vpERROR_TRACE("Error caught") ;
934  throw ;
935  }
936 
937  seekExtremities(I) ;
938 
939  setExtremities() ;
940 
941  try
942  {
943  leastSquare() ;
944  }
945  catch(...)
946  {
947  vpERROR_TRACE("Error caught") ;
948  throw ;
949  }
950 
951  // suppression des points rejetes par la regression robuste
952  suppressPoints() ;
953  setExtremities() ;
954 
955  //reechantillonage si necessaire
956  reSample(I) ;
957 
958  // remet a jour l'angle delta pour chaque point de la liste
959 
960  updateTheta() ;
961 
962  computeMoments();
963 
964  // Remise a jour de delta dans la liste de site me
965  if (vpDEBUG_ENABLE(2))
966  {
967  display(I,vpColor::red) ;
969  vpDisplay::flush(I) ;
970  }
971 // computeAngle(iP1, iP2) ;
972 //
973 // if (iter%5==0)
974 // {
975 // sample(I) ;
976 // try{
977 // leastSquare() ; }
978 // catch(...)
979 // {
980 // vpERROR_TRACE("Error caught") ;
981 // throw ;
982 // }
983 // computeAngle(iP1, iP2) ;
984 // }
985 // seekExtremities(I) ;
986 //
987 // vpMeTracker::display(I) ;
988 // // vpDisplay::flush(I) ;
989 //
990 // // remet a jour l'angle theta pour chaque point de la liste
991 // updateTheta() ;
992 
993  }
994 
995  iter++ ;
996 
997 
998  vpCDEBUG(1) << "end vpMeEllipse::track()"<<std::endl ;
999 
1000 }
1001 
1002 
1008 void
1009 vpMeEllipse::computeMoments()
1010 {
1011  double tane = tan(-1/e);
1012  m00 = M_PI*a*b;
1013  m10 = m00*iPc.get_i();
1014  m01 = m00*iPc.get_j();
1015  m20 = m00*(a*a+b*b*tane*tane)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_i();
1016  m02 = m00*(a*a*tane*tane+b*b)/(4*(1+tane*tane))+m00*iPc.get_j()*iPc.get_j();
1017  m11 = m00*tane*(a*a-b*b)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_j();
1018  mu11 = m11 - iPc.get_j()*m10;
1019  mu02 = m02 - iPc.get_j()*m01;
1020  mu20 = m20 - iPc.get_i()*m10;
1021 }
1022 
1023 
1024 
1025 
1026 
1027 
1028 
1029 
1030 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1031 
1035 void
1036 vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1,
1037  int ip2, int jp2, double &_alpha2)
1038 {
1039 
1040  getParameters() ;
1041  double j1, i1, j11, i11;
1042  j1 = i1 = 0.0 ;
1043 
1044  int number_of_points = 2000 ;
1045  double incr = 2 * M_PI / number_of_points ; // angle increment
1046 
1047  double dmin1 = 1e6 ;
1048  double dmin2 = 1e6 ;
1049 
1050  double k = -M_PI ;
1051  while(k < M_PI) {
1052 
1053  j1 = a *cos(k) ; // equation of an ellipse
1054  i1 = b *sin(k) ; // equation of an ellipse
1055 
1056  // (i1,j1) are the coordinates on the origin centered ellipse ;
1057  // a rotation by "e" and a translation by (xci,jc) are done
1058  // to get the coordinates of the point on the shifted ellipse
1059  j11 = iPc.get_j() + ce *j1 - se *i1 ;
1060  i11 = iPc.get_i() -( se *j1 + ce *i1) ;
1061 
1062  double d = vpMath::sqr(ip1-i11) + vpMath::sqr(jp1-j11) ;
1063  if (d < dmin1)
1064  {
1065  dmin1 = d ;
1066  alpha1 = k ;
1067  _alpha1 = k ;
1068  }
1069  d = vpMath::sqr(ip2-i11) + vpMath::sqr(jp2-j11) ;
1070  if (d < dmin2)
1071  {
1072  dmin2 = d ;
1073  alpha2 = k ;
1074  _alpha2 = k ;
1075  }
1076  k += incr ;
1077  }
1078 
1079  if (alpha2 <alpha1) alpha2 += 2*M_PI ;
1080 
1081  vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
1082 
1083 }
1084 
1085 
1089 void
1090 vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2)
1091 {
1092 
1093  double a1, a2 ;
1094  computeAngle(ip1,jp1,a1, ip2, jp2,a2) ;
1095 }
1096 
1097 
1098 void
1099 vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
1100  unsigned *i, unsigned *j)
1101 {
1102  vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
1103 
1104  if (circle==false)
1105  {
1106  vpMatrix A(n,5) ;
1107  vpColVector b(n) ;
1108  vpColVector x(5) ;
1109 
1110  // Construction du systeme Ax=b
1112  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
1113 
1114  for (unsigned int k =0 ; k < n ; k++)
1115  {
1116  A[k][0] = vpMath::sqr(j[k]) ;
1117  A[k][1] = 2* i[k] * j[k] ;
1118  A[k][2] = 2* i[k] ;
1119  A[k][3] = 2* j[k] ;
1120  A[k][4] = 1 ;
1121 
1122  b[k] = - vpMath::sqr(i[k]) ;
1123  }
1124 
1125  K = A.pseudoInverse(1e-26)*b ;
1126  std::cout << K << std::endl;
1127  }
1128  else
1129  {
1130  vpMatrix A(n,3) ;
1131  vpColVector b(n) ;
1132  vpColVector x(3) ;
1133 
1134  vpColVector Kc(3) ;
1135  for (unsigned int k =0 ; k < n ; k++)
1136  {
1137  A[k][0] = 2* i[k] ;
1138  A[k][1] = 2* j[k] ;
1139 
1140  A[k][2] = 1 ;
1141  b[k] = - vpMath::sqr(i[k]) - vpMath::sqr(j[k]) ;
1142  }
1143 
1144  Kc = A.pseudoInverse(1e-26)*b ;
1145  K[0] = 1 ;
1146  K[1] = 0 ;
1147  K[2] = Kc[0] ;
1148  K[3] = Kc[1] ;
1149  K[4] = Kc[2] ;
1150 
1151  std::cout << K << std::endl;
1152  }
1153  iP1.set_i( i[0] );
1154  iP1.set_j( j[0] );
1155  iP2.set_i( i[n-1] );
1156  iP2.set_j( j[n-1] );
1157 
1158  getParameters() ;
1159  computeAngle(iP1, iP2) ;
1160  display(I, vpColor::green) ;
1161 
1162  sample(I) ;
1163 
1164  // 2. On appelle ce qui n'est pas specifique
1165  {
1167  }
1168 
1169 
1170  try{
1171  track(I) ;
1172  }
1173  catch(...)
1174  {
1175  vpERROR_TRACE("Error caught") ;
1176  throw ;
1177  }
1179  vpDisplay::flush(I) ;
1180 
1181 }
1182 #endif // Deprecated
1183 
1205  const double &A, const double &B, const double &E,
1206  const double & smallalpha, const double &highalpha,
1207  vpColor color)
1208 {
1209  double j1, i1;
1210  vpImagePoint iP11;
1211  double j2, i2;
1212  vpImagePoint iP22;
1213  j1 = j2 = i1 = i2 = 0 ;
1214 
1215  double incr = vpMath::rad(2) ; // angle increment
1216 
1217  vpDisplay::displayCross(I,center,20,vpColor::red) ;
1218 
1219  double k = smallalpha ;
1220  while (k+incr<highalpha)
1221  {
1222  j1 = A *cos(k) ; // equation of an ellipse
1223  i1 = B *sin(k) ; // equation of an ellipse
1224 
1225  j2 = A *cos(k+incr) ; // equation of an ellipse
1226  i2 = B *sin(k+incr) ; // equation of an ellipse
1227 
1228  // (i1,j1) are the coordinates on the origin centered ellipse ;
1229  // a rotation by "e" and a translation by (xci,jc) are done
1230  // to get the coordinates of the point on the shifted ellipse
1231  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1232  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1233  // to get the coordinates of the point on the shifted ellipse
1234  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1235  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1236 
1237  vpDisplay::displayLine(I, iP11, iP22, color, 3) ;
1238 
1239  k += incr ;
1240  }
1241 
1242  j1 = A *cos(smallalpha) ; // equation of an ellipse
1243  i1 = B *sin(smallalpha) ; // equation of an ellipse
1244 
1245  j2 = A *cos(highalpha) ; // equation of an ellipse
1246  i2 = B *sin(highalpha) ; // equation of an ellipse
1247 
1248  // (i1,j1) are the coordinates on the origin centered ellipse ;
1249  // a rotation by "e" and a translation by (xci,jc) are done
1250  // to get the coordinates of the point on the shifted ellipse
1251  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1252  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1253  // to get the coordinates of the point on the shifted ellipse
1254  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1255  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1256 
1257  vpDisplay::displayLine(I, center, iP11, vpColor::red, 3) ;
1258  vpDisplay::displayLine(I, center, iP22, vpColor::blue, 3) ;
1259 }
1260 
1282  const double &A, const double &B, const double &E,
1283  const double & smallalpha, const double &highalpha,
1284  vpColor color)
1285 {
1286  double j1, i1;
1287  vpImagePoint iP11;
1288  double j2, i2;
1289  vpImagePoint iP22;
1290  j1 = j2 = i1 = i2 = 0 ;
1291 
1292  double incr = vpMath::rad(2) ; // angle increment
1293 
1294  vpDisplay::displayCross(I,center,20,vpColor::red) ;
1295 
1296  double k = smallalpha ;
1297  while (k+incr<highalpha)
1298  {
1299  j1 = A *cos(k) ; // equation of an ellipse
1300  i1 = B *sin(k) ; // equation of an ellipse
1301 
1302  j2 = A *cos(k+incr) ; // equation of an ellipse
1303  i2 = B *sin(k+incr) ; // equation of an ellipse
1304 
1305  // (i1,j1) are the coordinates on the origin centered ellipse ;
1306  // a rotation by "e" and a translation by (xci,jc) are done
1307  // to get the coordinates of the point on the shifted ellipse
1308  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1309  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1310  // to get the coordinates of the point on the shifted ellipse
1311  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1312  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1313 
1314  vpDisplay::displayLine(I, iP11, iP22, color, 3) ;
1315 
1316  k += incr ;
1317  }
1318 
1319  j1 = A *cos(smallalpha) ; // equation of an ellipse
1320  i1 = B *sin(smallalpha) ; // equation of an ellipse
1321 
1322  j2 = A *cos(highalpha) ; // equation of an ellipse
1323  i2 = B *sin(highalpha) ; // equation of an ellipse
1324 
1325  // (i1,j1) are the coordinates on the origin centered ellipse ;
1326  // a rotation by "e" and a translation by (xci,jc) are done
1327  // to get the coordinates of the point on the shifted ellipse
1328  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1329  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1330  // to get the coordinates of the point on the shifted ellipse
1331  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1332  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1333 
1334  vpDisplay::displayLine(I, center, iP11, vpColor::red, 3) ;
1335  vpDisplay::displayLine(I, center, iP22, vpColor::blue, 3) ;
1336 }