ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpMeEllipse.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMeEllipse.cpp 4303 2013-07-04 14:14:00Z 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 
163 void
164 vpMeEllipse::sample(const vpImage<unsigned char> & I)
165 {
166  vpCDEBUG(1) <<"begin vpMeEllipse::sample() : "<<std::endl ;
167 
168  if (!me) {
169  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
171  "Moving edges not initialized")) ;
172  }
173 
174  int height = (int)I.getHeight() ;
175  int width = (int)I.getWidth() ;
176 
177  double n_sample;
178 
179  //if (me->getSampleStep()==0)
180  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
181  {
182  std::cout << "In vpMeEllipse::sample: " ;
183  std::cout << "function called with sample step = 0" ;
184  //return fatalError ;
185  }
186 
187  double j, i;//, j11, i11;
188  vpImagePoint iP11;
189  j = i = 0.0 ;
190 
191  double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
192  vpColor col = vpColor::red ;
193  getParameters() ;
194 
195  // Delete old list
196  list.clear();
197 
198  angle.clear();
199 
200  // sample positions
201  double k = alpha1 ;
202  while (k<alpha2)
203  {
204 // j = a *cos(k) ; // equation of an ellipse
205 // i = b *sin(k) ; // equation of an ellipse
206 
207  j = a *sin(k) ; // equation of an ellipse
208  i = b *cos(k) ; // equation of an ellipse
209 
210  // (i,j) are the coordinates on the origin centered ellipse ;
211  // a rotation by "e" and a translation by (xci,jc) are done
212  // to get the coordinates of the point on the shifted ellipse
213 // iP11.set_j( iPc.get_j() + ce *j - se *i );
214 // iP11.set_i( iPc.get_i() -( se *j + ce *i) );
215 
216  iP11.set_j( iPc.get_j() + ce *j + se *i );
217  iP11.set_i( iPc.get_i() - se *j + ce *i );
218 
219  vpDisplay::displayCross(I, iP11, 5, col) ;
220 
221  double theta ;
222  computeTheta(theta, K, iP11) ;
223 
224  // If point is in the image, add to the sample list
225  if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
226  {
227  vpMeSite pix ;
228  pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
231 
232  if(vpDEBUG_ENABLE(3))
233  {
235  }
236  list.push_back(pix);
237  angle.push_back(k);
238  }
239  k += incr ;
240 
241  }
243 
244  n_sample = (unsigned int)list.size() ;
245 
246  vpCDEBUG(1) << "end vpMeEllipse::sample() : " ;
247  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
248 }
249 
250 
265 void
266 vpMeEllipse::reSample(const vpImage<unsigned char> &I)
267 {
268  if (!me) {
269  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
271  "Moving edges not initialized")) ;
272  }
273 
274  unsigned int n = numberOfSignal() ;
275  double expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
276  if ((double)n<0.9*expecteddensity){
277  sample(I) ;
278  }
279 }
280 
281 
288 void
289 vpMeEllipse::getParameters()
290 {
291 
292  double k[6] ;
293  for (unsigned int i=0 ; i < 5 ; i++)
294  k[i+1] = K[i] ;
295  k[0] = 1 ;
296 
297  double d = k[2]*k[2] - k[0]*k[1];
298 
299 
300  iPc.set_i( (k[1] * k[3] - k[2] * k[4]) / d );
301  iPc.set_j( (k[0] * k[4] - k[2] * k[3]) / d );
302 
303  double sq = sqrt(vpMath::sqr(k[1]-k[0]) + 4.0*vpMath::sqr(k[2])) ;
304  if (circle ==true)
305  {
306  e = 0 ;
307  }
308  else
309  {
310  e = (k[1] - k[0] + sq) / (2.0*k[2]);
311  e = (-1/e) ;
312 
313  e = atan(e) ;
314  }
315 
316  if(e < 0.0) e += M_PI ;
317 
318  ce = cos(e) ;
319  se = sin(e) ;
320 
321  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]) ;
322  double a2 = num / (k[0] + k[1] + sq ) ;
323  double b2 = num / (k[0] + k[1] - sq ) ;
324 
325  a = sqrt( a2 ) ;
326  b = sqrt( b2 ) ;
327 
328 }
329 
333 void
335 {
336  std::cout << "K" << std::endl ;
337  std::cout << K.t() ;
338  std::cout << iPc << std::endl ;
339 }
340 
349 void
350 vpMeEllipse::computeAngle(vpImagePoint pt1, vpImagePoint pt2)
351 {
352 
353  getParameters() ;
354  double j1, i1, j11, i11;
355  j1 = i1 = 0.0 ;
356 
357  int number_of_points = 2000 ;
358  double incr = 2 * M_PI / number_of_points ; // angle increment
359 
360  double dmin1 = 1e6 ;
361  double dmin2 = 1e6 ;
362 
363  double k = 0 ;
364  while(k < 2*M_PI) {
365 
366 // j1 = a *cos(k) ; // equation of an ellipse
367 // i1 = b *sin(k) ; // equation of an ellipse
368 
369  j1 = a *sin(k) ; // equation of an ellipse
370  i1 = b *cos(k) ; // equation of an ellipse
371 
372  // (i1,j1) are the coordinates on the origin centered ellipse ;
373  // a rotation by "e" and a translation by (xci,jc) are done
374  // to get the coordinates of the point on the shifted ellipse
375 // j11 = iPc.get_j() + ce *j1 - se *i1 ;
376 // i11 = iPc.get_i() -( se *j1 + ce *i1) ;
377 
378  j11 = iPc.get_j() + ce *j1 + se *i1 ;
379  i11 = iPc.get_i() - se *j1 + ce *i1 ;
380 
381  double d = vpMath::sqr(pt1.get_i()-i11) + vpMath::sqr(pt1.get_j()-j11) ;
382  if (d < dmin1)
383  {
384  dmin1 = d ;
385  alpha1 = k ;
386  }
387  d = vpMath::sqr(pt2.get_i()-i11) + vpMath::sqr(pt2.get_j()-j11) ;
388  if (d < dmin2)
389  {
390  dmin2 = d ;
391  alpha2 = k ;
392  }
393  k += incr ;
394  }
395  //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
396 
397  if (alpha2 <alpha1)
398  {
399 // double alphatmp = alpha2;
400 // alpha2 = alpha1;
401 // alpha1 = alphatmp;
402  alpha2 += 2 * M_PI;
403  }
404 
405  //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
406 
407 
408 }
409 
410 
416 void
417 vpMeEllipse::updateTheta()
418 {
419  vpMeSite p;
420  double theta;
421  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
422  p = *it;
423  vpImagePoint iP;
424  iP.set_i(p.ifloat);
425  iP.set_j(p.jfloat);
426  computeTheta(theta, K, iP) ;
427  p.alpha = theta ;
428  *it = p;
429  }
430 }
431 
435 void
436 vpMeEllipse::suppressPoints()
437 {
438  // Loop through list of sites to track
439  std::list<vpMeSite>::iterator itList = list.begin();
440  for(std::list<double>::iterator it=angle.begin(); it!=angle.end(); ){
441  vpMeSite s = *itList;//current reference pixel
443  {
444  itList = list.erase(itList) ;
445  it = angle.erase(it);
446  }
447  else
448  {
449  ++itList;
450  ++it;
451  }
452  }
453 }
454 
455 
465 void
466 vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
467 {
468  if (!me) {
469  vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
471  "Moving edges not initialized")) ;
472  }
473 
474  int rows = (int)I.getHeight() ;
475  int cols = (int)I.getWidth() ;
476 
477  vpImagePoint ip;
478 
479  unsigned int memory_range = me->getRange() ;
480  me->setRange(2);
481 
482  double memory_mu1 = me->getMu1();
483  me->setMu1(0.5);
484 
485  double memory_mu2 = me->getMu2();
486  me->setMu2(0.5);
487 
488  double incr = vpMath::rad(2.0) ;
489 
490  if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
491  {
492  vpMeSite P;
493  double k = alpha1;
494  double i1,j1;
495 
496  for (unsigned int i=0 ; i < 3 ; i++)
497  {
498  k -= incr;
499  //while ( k < -M_PI ) { k+=2*M_PI; }
500 
501  i1 = b *cos(k) ; // equation of an ellipse
502  j1 = a *sin(k) ; // equation of an ellipse
503  P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
504  P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
505 
506  if(!outOfImage(P.i, P.j, 5, rows, cols))
507  {
508  P.track(I,me,false) ;
509 
511  {
512  list.push_back(P);
513  angle.push_back(k);
514  if (vpDEBUG_ENABLE(3)) {
515  ip.set_i( P.i );
516  ip.set_j( P.j );
517 
519  }
520  }
521  else {
522  if (vpDEBUG_ENABLE(3)) {
523  ip.set_i( P.i );
524  ip.set_j( P.j );
526  }
527  }
528  }
529  }
530 
531  k = alpha2;
532 
533  for (unsigned int i=0 ; i < 3 ; i++)
534  {
535  k += incr;
536  //while ( k > M_PI ) { k-=2*M_PI; }
537 
538  i1 = b *cos(k) ; // equation of an ellipse
539  j1 = a *sin(k) ; // equation of an ellipse
540  P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
541  P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
542 
543  if(!outOfImage(P.i, P.j, 5, rows, cols))
544  {
545  P.track(I,me,false) ;
546 
548  {
549  list.push_back(P);
550  angle.push_back(k);
551  if (vpDEBUG_ENABLE(3)) {
552  ip.set_i( P.i );
553  ip.set_j( P.j );
554 
556  }
557  }
558  else {
559  if (vpDEBUG_ENABLE(3)) {
560  ip.set_i( P.i );
561  ip.set_j( P.j );
563  }
564  }
565  }
566  }
567  }
568 
569  suppressPoints() ;
570 
571  me->setRange(memory_range);
572  me->setMu1(memory_mu1);
573  me->setMu2(memory_mu2);
574 }
575 
576 
580 void
581 vpMeEllipse::setExtremities()
582 {
583  double alphamin = +1e6;
584  double alphamax = -1e6;
585  double imin = 0;
586  double jmin = 0;
587  double imax = 0;
588  double jmax = 0;
589 
590  // Loop through list of sites to track
591  std::list<double>::const_iterator itAngle = angle.begin();
592 
593  for(std::list<vpMeSite>::const_iterator itList=list.begin(); itList!=list.end(); ++itList){
594  vpMeSite s = *itList;//current reference pixel
595  double alpha = *itAngle;
596  if (alpha < alphamin)
597  {
598  alphamin = alpha;
599  imin = s.ifloat ;
600  jmin = s.jfloat ;
601  }
602 
603  if (alpha > alphamax)
604  {
605  alphamax = alpha;
606  imax = s.ifloat ;
607  jmax = s.jfloat ;
608  }
609  ++itAngle;
610  }
611 
612  alpha1 = alphamin;
613  alpha2 = alphamax;
614  iP1.set_ij(imin,jmin);
615  iP2.set_ij(imax,jmax);
616 }
617 
618 
624 void
625 vpMeEllipse::leastSquare()
626 {
627  // Construction du systeme Ax=b
629  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
630  unsigned int i ;
631 
632  vpMeSite p ;
633 
634  unsigned int iter =0 ;
636  vpRobust r(numberOfSignal()) ;
637  r.setThreshold(2);
638  r.setIteration(0) ;
640  D.setIdentity() ;
641  vpMatrix DA, DAmemory ;
642  vpColVector DAx ;
644  w =1 ;
645  unsigned int nos_1 = numberOfSignal() ;
646 
647  if (list.size() < 3)
648  {
649  vpERROR_TRACE("Not enough point") ;
651  "not enough point")) ;
652  }
653 
654  if (circle ==false)
655  {
656  vpMatrix A(numberOfSignal(),5) ;
657  vpColVector x(5);
658 
659  unsigned int k =0 ;
660  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
661  p = *it;
663  {
664 
665  A[k][0] = vpMath::sqr(p.jfloat) ;
666  A[k][1] = 2 * p.ifloat * p.jfloat ;
667  A[k][2] = 2 * p.ifloat ;
668  A[k][3] = 2 * p.jfloat ;
669  A[k][4] = 1 ;
670 
671  b[k] = - vpMath::sqr(p.ifloat) ;
672  k++ ;
673  }
674  }
675 
676  while (iter < 4 )
677  {
678  DA = D*A ;
679  vpMatrix DAp ;
680 
681  x = DA.pseudoInverse(1e-26) *D*b ;
682 
683  vpColVector residu(nos_1);
684  residu = b - A*x;
685  r.setIteration(iter) ;
686  r.MEstimator(vpRobust::TUKEY,residu,w) ;
687 
688  k = 0;
689  for (i=0 ; i < nos_1 ; i++)
690  {
691  D[k][k] =w[k] ;
692  k++;
693  }
694  iter++;
695  }
696 
697  k =0 ;
698  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
699  p = *it;
701  {
702  if (w[k] < thresholdWeight)
703  {
705 
706  *it = p;
707  }
708  k++ ;
709  }
710  }
711  for(i = 0; i < 5; i ++)
712  K[i] = x[i];
713  }
714 
715  else
716  {
717  vpMatrix A(numberOfSignal(),3) ;
718  vpColVector x(3);
719 
720  unsigned int k =0 ;
721  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
722  p = *it;
724  {
725 
726  A[k][0] = 2* p.ifloat ;
727  A[k][1] = 2 * p.jfloat ;
728  A[k][2] = 1 ;
729 
730  b[k] = - vpMath::sqr(p.ifloat) - vpMath::sqr(p.jfloat) ;
731  k++ ;
732  }
733  }
734 
735  while (iter < 4 )
736  {
737  DA = D*A ;
738  vpMatrix DAp ;
739 
740  x = DA.pseudoInverse(1e-26) *D*b ;
741 
742  vpColVector residu(nos_1);
743  residu = b - A*x;
744  r.setIteration(iter) ;
745  r.MEstimator(vpRobust::TUKEY,residu,w) ;
746 
747  k = 0;
748  for (i=0 ; i < nos_1 ; i++)
749  {
750  D[k][k] =w[k];
751  k++;
752  }
753  iter++;
754  }
755 
756  k =0 ;
757  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
758  p = *it;
760  {
761  if (w[k] < thresholdWeight)
762  {
764 
765  *it = p;
766  }
767  k++ ;
768  }
769  }
770  for(i = 0; i < 3; i ++)
771  K[i+2] = x[i];
772  }
773  getParameters() ;
774 }
775 
776 
786 void
788 {
790 }
791 
792 
799 void
801 {
802  vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
803 
804  const unsigned int n=5 ;
805  vpImagePoint iP[n];
806 
807  for (unsigned int k =0 ; k < n ; k++)
808  {
809  std::cout << "Click points "<< k+1 <<"/" << n ;
810  std::cout << " on the ellipse in the trigonometric order" <<std::endl ;
811  vpDisplay::getClick(I, iP[k], true);
813  vpDisplay::flush(I);
814  std::cout << iP[k] << std::endl;
815  }
816 
817  iP1 = iP[0];
818  iP2 = iP[n-1];
819 
820  initTracking(I, n, iP) ;
821 }
822 
823 
834 void
835 vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
836  vpImagePoint *iP)
837 {
838  vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
839 
840  if (circle==false)
841  {
842  vpMatrix A(n,5) ;
843  vpColVector b(n) ;
844  vpColVector x(5) ;
845 
846  // Construction du systeme Ax=b
848  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
849 
850  for (unsigned int k =0 ; k < n ; k++)
851  {
852  A[k][0] = vpMath::sqr(iP[k].get_j()) ;
853  A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
854  A[k][2] = 2* iP[k].get_i() ;
855  A[k][3] = 2* iP[k].get_j() ;
856  A[k][4] = 1 ;
857 
858  b[k] = - vpMath::sqr(iP[k].get_i()) ;
859  }
860 
861  K = A.pseudoInverse(1e-26)*b ;
862  std::cout << K << std::endl;
863  }
864  else
865  {
866  vpMatrix A(n,3) ;
867  vpColVector b(n) ;
868  vpColVector x(3) ;
869 
870  vpColVector Kc(3) ;
871  for (unsigned int k =0 ; k < n ; k++)
872  {
873  A[k][0] = 2* iP[k].get_i() ;
874  A[k][1] = 2* iP[k].get_j() ;
875  A[k][2] = 1 ;
876 
877  b[k] = - vpMath::sqr(iP[k].get_i()) - vpMath::sqr(iP[k].get_j()) ;
878  }
879 
880  Kc = A.pseudoInverse(1e-26)*b ;
881  K[0] = 1 ;
882  K[1] = 0 ;
883  K[2] = Kc[0] ;
884  K[3] = Kc[1] ;
885  K[4] = Kc[2] ;
886 
887  std::cout << K << std::endl;
888  }
889 
890  iP1 = iP[0];
891  iP2 = iP[n-1];
892 
893  getParameters() ;
894  computeAngle(iP1, iP2) ;
895  display(I, vpColor::green) ;
896 
897  sample(I) ;
898 
899  // 2. On appelle ce qui n'est pas specifique
900  {
902  }
903 
904 
905  try{
906  track(I) ;
907  }
908  catch(...)
909  {
910  vpERROR_TRACE("Error caught") ;
911  throw ;
912  }
914  vpDisplay::flush(I) ;
915 
916 }
917 
923 void
925 {
926  vpCDEBUG(1) <<"begin vpMeEllipse::track()"<<std::endl ;
927 
928  static int iter =0 ;
929  // 1. On fait ce qui concerne les ellipse (peut etre vide)
930  {
931  }
932 
933  //vpDisplay::display(I) ;
934  // 2. On appelle ce qui n'est pas specifique
935  {
936 
937  try{
938  vpMeTracker::track(I) ;
939  }
940  catch(...)
941  {
942  vpERROR_TRACE("Error caught") ;
943  throw ;
944  }
945  // std::cout << "number of signals " << numberOfSignal() << std::endl ;
946  }
947 
948  // 3. On revient aux ellipses
949  {
950  // Estimation des parametres de la droite aux moindres carre
951  suppressPoints() ;
952  setExtremities() ;
953 
954 
955  try{
956  leastSquare() ; }
957  catch(...)
958  {
959  vpERROR_TRACE("Error caught") ;
960  throw ;
961  }
962 
963  seekExtremities(I) ;
964 
965  setExtremities() ;
966 
967  try
968  {
969  leastSquare() ;
970  }
971  catch(...)
972  {
973  vpERROR_TRACE("Error caught") ;
974  throw ;
975  }
976 
977  // suppression des points rejetes par la regression robuste
978  suppressPoints() ;
979  setExtremities() ;
980 
981  //reechantillonage si necessaire
982  reSample(I) ;
983 
984  // remet a jour l'angle delta pour chaque point de la liste
985 
986  updateTheta() ;
987 
988  computeMoments();
989 
990  // Remise a jour de delta dans la liste de site me
991  if (vpDEBUG_ENABLE(2))
992  {
993  display(I,vpColor::red) ;
995  vpDisplay::flush(I) ;
996  }
997 // computeAngle(iP1, iP2) ;
998 //
999 // if (iter%5==0)
1000 // {
1001 // sample(I) ;
1002 // try{
1003 // leastSquare() ; }
1004 // catch(...)
1005 // {
1006 // vpERROR_TRACE("Error caught") ;
1007 // throw ;
1008 // }
1009 // computeAngle(iP1, iP2) ;
1010 // }
1011 // seekExtremities(I) ;
1012 //
1013 // vpMeTracker::display(I) ;
1014 // // vpDisplay::flush(I) ;
1015 //
1016 // // remet a jour l'angle theta pour chaque point de la liste
1017 // updateTheta() ;
1018 
1019  }
1020 
1021  iter++ ;
1022 
1023 
1024  vpCDEBUG(1) << "end vpMeEllipse::track()"<<std::endl ;
1025 
1026 }
1027 
1028 
1034 void
1035 vpMeEllipse::computeMoments()
1036 {
1037  double tane = tan(-1/e);
1038  m00 = M_PI*a*b;
1039  m10 = m00*iPc.get_i();
1040  m01 = m00*iPc.get_j();
1041  m20 = m00*(a*a+b*b*tane*tane)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_i();
1042  m02 = m00*(a*a*tane*tane+b*b)/(4*(1+tane*tane))+m00*iPc.get_j()*iPc.get_j();
1043  m11 = m00*tane*(a*a-b*b)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_j();
1044  mu11 = m11 - iPc.get_j()*m10;
1045  mu02 = m02 - iPc.get_j()*m01;
1046  mu20 = m20 - iPc.get_i()*m10;
1047 }
1048 
1049 
1050 
1051 
1052 
1053 
1054 
1055 
1056 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1057 
1061 void
1062 vpMeEllipse::computeAngle(int ip1, int jp1, double &_alpha1,
1063  int ip2, int jp2, double &_alpha2)
1064 {
1065 
1066  getParameters() ;
1067  double j1, i1, j11, i11;
1068  j1 = i1 = 0.0 ;
1069 
1070  int number_of_points = 2000 ;
1071  double incr = 2 * M_PI / number_of_points ; // angle increment
1072 
1073  double dmin1 = 1e6 ;
1074  double dmin2 = 1e6 ;
1075 
1076  double k = -M_PI ;
1077  while(k < M_PI) {
1078 
1079  j1 = a *cos(k) ; // equation of an ellipse
1080  i1 = b *sin(k) ; // equation of an ellipse
1081 
1082  // (i1,j1) are the coordinates on the origin centered ellipse ;
1083  // a rotation by "e" and a translation by (xci,jc) are done
1084  // to get the coordinates of the point on the shifted ellipse
1085  j11 = iPc.get_j() + ce *j1 - se *i1 ;
1086  i11 = iPc.get_i() -( se *j1 + ce *i1) ;
1087 
1088  double d = vpMath::sqr(ip1-i11) + vpMath::sqr(jp1-j11) ;
1089  if (d < dmin1)
1090  {
1091  dmin1 = d ;
1092  alpha1 = k ;
1093  _alpha1 = k ;
1094  }
1095  d = vpMath::sqr(ip2-i11) + vpMath::sqr(jp2-j11) ;
1096  if (d < dmin2)
1097  {
1098  dmin2 = d ;
1099  alpha2 = k ;
1100  _alpha2 = k ;
1101  }
1102  k += incr ;
1103  }
1104 
1105  if (alpha2 <alpha1) alpha2 += 2*M_PI ;
1106 
1107  vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
1108 
1109 }
1110 
1111 
1115 void
1116 vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2)
1117 {
1118 
1119  double a1, a2 ;
1120  computeAngle(ip1,jp1,a1, ip2, jp2,a2) ;
1121 }
1122 
1123 
1124 void
1125 vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
1126  unsigned *i, unsigned *j)
1127 {
1128  vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
1129 
1130  if (circle==false)
1131  {
1132  vpMatrix A(n,5) ;
1133  vpColVector b(n) ;
1134  vpColVector x(5) ;
1135 
1136  // Construction du systeme Ax=b
1138  // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
1139 
1140  for (unsigned int k =0 ; k < n ; k++)
1141  {
1142  A[k][0] = vpMath::sqr(j[k]) ;
1143  A[k][1] = 2* i[k] * j[k] ;
1144  A[k][2] = 2* i[k] ;
1145  A[k][3] = 2* j[k] ;
1146  A[k][4] = 1 ;
1147 
1148  b[k] = - vpMath::sqr(i[k]) ;
1149  }
1150 
1151  K = A.pseudoInverse(1e-26)*b ;
1152  std::cout << K << std::endl;
1153  }
1154  else
1155  {
1156  vpMatrix A(n,3) ;
1157  vpColVector b(n) ;
1158  vpColVector x(3) ;
1159 
1160  vpColVector Kc(3) ;
1161  for (unsigned int k =0 ; k < n ; k++)
1162  {
1163  A[k][0] = 2* i[k] ;
1164  A[k][1] = 2* j[k] ;
1165 
1166  A[k][2] = 1 ;
1167  b[k] = - vpMath::sqr(i[k]) - vpMath::sqr(j[k]) ;
1168  }
1169 
1170  Kc = A.pseudoInverse(1e-26)*b ;
1171  K[0] = 1 ;
1172  K[1] = 0 ;
1173  K[2] = Kc[0] ;
1174  K[3] = Kc[1] ;
1175  K[4] = Kc[2] ;
1176 
1177  std::cout << K << std::endl;
1178  }
1179  iP1.set_i( i[0] );
1180  iP1.set_j( j[0] );
1181  iP2.set_i( i[n-1] );
1182  iP2.set_j( j[n-1] );
1183 
1184  getParameters() ;
1185  computeAngle(iP1, iP2) ;
1186  display(I, vpColor::green) ;
1187 
1188  sample(I) ;
1189 
1190  // 2. On appelle ce qui n'est pas specifique
1191  {
1193  }
1194 
1195 
1196  try{
1197  track(I) ;
1198  }
1199  catch(...)
1200  {
1201  vpERROR_TRACE("Error caught") ;
1202  throw ;
1203  }
1205  vpDisplay::flush(I) ;
1206 
1207 }
1208 #endif // Deprecated
1209 
1231  const double &A, const double &B, const double &E,
1232  const double & smallalpha, const double &highalpha,
1233  vpColor color)
1234 {
1235  double j1, i1;
1236  vpImagePoint iP11;
1237  double j2, i2;
1238  vpImagePoint iP22;
1239  j1 = j2 = i1 = i2 = 0 ;
1240 
1241  double incr = vpMath::rad(2) ; // angle increment
1242 
1243  vpDisplay::displayCross(I,center,20,vpColor::red) ;
1244 
1245  double k = smallalpha ;
1246  while (k+incr<highalpha)
1247  {
1248  j1 = A *cos(k) ; // equation of an ellipse
1249  i1 = B *sin(k) ; // equation of an ellipse
1250 
1251  j2 = A *cos(k+incr) ; // equation of an ellipse
1252  i2 = B *sin(k+incr) ; // equation of an ellipse
1253 
1254  // (i1,j1) are the coordinates on the origin centered ellipse ;
1255  // a rotation by "e" and a translation by (xci,jc) are done
1256  // to get the coordinates of the point on the shifted ellipse
1257  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1258  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1259  // to get the coordinates of the point on the shifted ellipse
1260  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1261  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1262 
1263  vpDisplay::displayLine(I, iP11, iP22, color, 3) ;
1264 
1265  k += incr ;
1266  }
1267 
1268  j1 = A *cos(smallalpha) ; // equation of an ellipse
1269  i1 = B *sin(smallalpha) ; // equation of an ellipse
1270 
1271  j2 = A *cos(highalpha) ; // equation of an ellipse
1272  i2 = B *sin(highalpha) ; // equation of an ellipse
1273 
1274  // (i1,j1) are the coordinates on the origin centered ellipse ;
1275  // a rotation by "e" and a translation by (xci,jc) are done
1276  // to get the coordinates of the point on the shifted ellipse
1277  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1278  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1279  // to get the coordinates of the point on the shifted ellipse
1280  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1281  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1282 
1283  vpDisplay::displayLine(I, center, iP11, vpColor::red, 3) ;
1284  vpDisplay::displayLine(I, center, iP22, vpColor::blue, 3) ;
1285 }
1286 
1308  const double &A, const double &B, const double &E,
1309  const double & smallalpha, const double &highalpha,
1310  vpColor color)
1311 {
1312  double j1, i1;
1313  vpImagePoint iP11;
1314  double j2, i2;
1315  vpImagePoint iP22;
1316  j1 = j2 = i1 = i2 = 0 ;
1317 
1318  double incr = vpMath::rad(2) ; // angle increment
1319 
1320  vpDisplay::displayCross(I,center,20,vpColor::red) ;
1321 
1322  double k = smallalpha ;
1323  while (k+incr<highalpha)
1324  {
1325  j1 = A *cos(k) ; // equation of an ellipse
1326  i1 = B *sin(k) ; // equation of an ellipse
1327 
1328  j2 = A *cos(k+incr) ; // equation of an ellipse
1329  i2 = B *sin(k+incr) ; // equation of an ellipse
1330 
1331  // (i1,j1) are the coordinates on the origin centered ellipse ;
1332  // a rotation by "e" and a translation by (xci,jc) are done
1333  // to get the coordinates of the point on the shifted ellipse
1334  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1335  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1336  // to get the coordinates of the point on the shifted ellipse
1337  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1338  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1339 
1340  vpDisplay::displayLine(I, iP11, iP22, color, 3) ;
1341 
1342  k += incr ;
1343  }
1344 
1345  j1 = A *cos(smallalpha) ; // equation of an ellipse
1346  i1 = B *sin(smallalpha) ; // equation of an ellipse
1347 
1348  j2 = A *cos(highalpha) ; // equation of an ellipse
1349  i2 = B *sin(highalpha) ; // equation of an ellipse
1350 
1351  // (i1,j1) are the coordinates on the origin centered ellipse ;
1352  // a rotation by "e" and a translation by (xci,jc) are done
1353  // to get the coordinates of the point on the shifted ellipse
1354  iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
1355  iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
1356  // to get the coordinates of the point on the shifted ellipse
1357  iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
1358  iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
1359 
1360  vpDisplay::displayLine(I, center, iP11, vpColor::red, 3) ;
1361  vpDisplay::displayLine(I, center, iP22, vpColor::blue, 3) ;
1362 }