ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpMbEdgeTracker.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbEdgeTracker.cpp 4108 2013-02-06 15:15:34Z 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  * Make the complete tracking of an object by using its CAD model
36  *
37  * Authors:
38  * Nicolas Melchior
39  * Romain Tallonneau
40  * Eric Marchand
41  *
42  *****************************************************************************/
43 
51 #include <visp/vpDebug.h>
52 #include <visp/vpPose.h>
53 #include <visp/vpExponentialMap.h>
54 #include <visp/vpPixelMeterConversion.h>
55 #include <visp/vpImageIo.h>
56 #include <visp/vpRobust.h>
57 #include <visp/vpDisplayOpenCV.h>
58 #include <visp/vpDisplayX.h>
59 #include <visp/vpDisplayGDI.h>
60 #include <visp/vpMatrixException.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpException.h>
63 #include <visp/vpTrackingException.h>
64 #include <visp/vpMbEdgeTracker.h>
65 #include <visp/vpMbtDistanceLine.h>
66 #include <visp/vpMbtXmlParser.h>
67 #include <visp/vpMbtPolygon.h>
68 
69 #include <limits>
70 #include <string>
71 #include <sstream>
72 #include <float.h>
73 
78 {
79  index_polygon =0;
81  nline = 0;
82  ncylinder = 0;
83  lambda = 1;
85  percentageGdPt = 0.4;
86  computeCovariance = false;
87 
88  lines.resize(1);
89  cylinders.resize(1);
90  scales.resize(1);
91  scales[0] = true;
92  lines[0].clear();
93  cylinders[0].clear();
94  Ipyramid.resize(0);
95 
96 #ifdef VISP_HAVE_OGRE
97  faces.getOgreContext()->setWindowName("MBT Edge");
98 #endif
99  useOgre = false;
100 
103 }
104 
109 {
110  vpMbtDistanceLine *l ;
112 
113  for (unsigned int i = 0; i < lines.size(); i += 1){
114  if(scales[i]){
115  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
116  l = *it;
117  if (l!=NULL){
118  delete l ;
119  }
120  l = NULL ;
121  }
122 
123  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
124  c = *it;
125  if (c!=NULL){
126  delete c ;
127  }
128  c = NULL ;
129  }
130 
131  lines[i].clear();
132  cylinders[i].clear();
133  }
134  }
136 }
137 
143 void
145 {
146  this->me = me;
147 
148  for (unsigned int i = 0; i < scales.size(); i += 1){
149  if(scales[i]){
150  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
151  (*it)->setMovingEdge(&(this->me)) ;
152  }
153 
155  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
156  cy = *it;
157  cy->setMovingEdge(&(this->me)) ;
158  }
159  }
160  }
161 }
162 
170 void
172 {
173  useOgre = v;
174  if(useOgre){
175 #ifndef VISP_HAVE_OGRE
176  useOgre = false;
177  std::cout << "WARNING: ViSP dosen't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
178 #endif
179  }
180 }
181 
190 void
192 {
193  double residu_1 =1e3;
194  double r =1e3-1;
195  vpMatrix LTL;
196  vpColVector LTR;
197 
198  // compute the interaction matrix and its pseudo inverse
199  vpMbtDistanceLine *l ;
201 
202  vpColVector w;
203  vpColVector weighted_error;
204  vpColVector factor;
205 
206  unsigned int iter = 0;
207 
208  //Nombre de moving edges
209  unsigned int nbrow = 0;
210  unsigned int nberrors_lines = 0;
211  unsigned int nberrors_cylinders = 0;
212 
213  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
214  l = *it;
215  nbrow += l->nbFeature ;
216  nberrors_lines+=l->nbFeature;
218  }
219 
220  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
221  cy = *it;
222  nbrow += cy->nbFeature ;
223  nberrors_cylinders += cy->nbFeature ;
225  }
226 
227  if (nbrow==0){
228  vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
229  throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
230  }
231 
232  vpMatrix L(nbrow,6), Lp;
233 
234  // compute the error vector
235  vpColVector error(nbrow);
236  unsigned int nerror = error.getRows();
237  vpColVector v ;
238 
239  double limite = 3; //Une limite de 3 pixels
240  limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
241 
242  //Parametre pour la premiere phase d'asservissement
243  double e_prev = 0, e_cur, e_next;
244  bool reloop = true;
245  double count = 0;
246 
247  /*** First phase ***/
248  while ( reloop == true && iter<10)
249  {
250  if(iter==0)
251  {
252  weighted_error.resize(nerror) ;
253  w.resize(nerror);
254  w = 0;
255  factor.resize(nerror);
256  factor = 1;
257  }
258 
259  count = 0;
260 
261  unsigned int n = 0;
262  reloop = false;
263  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
264  l = *it;
266 
267  double fac = 1;
268  if (iter == 0)
269  {
270  for(std::list<int>::const_iterator it = l->Lindex_polygon.begin(); it!=l->Lindex_polygon.end(); ++it){
271  int index = *it;
272  if (l->hiddenface->isAppearing((unsigned int)index))
273  {
274  fac = 0.2;
275  break;
276  }
277  if(l->closeToImageBorder(_I, 10))
278  {
279  fac = 0.1;
280  break;
281  }
282  }
283  }
284 
285  std::list<vpMeSite>::const_iterator itListLine;
286  if (iter == 0 && l->meline != NULL)
287  itListLine = l->meline->getMeList().begin();
288 
289  for (unsigned int i=0 ; i < l->nbFeature ; i++)
290  {
291  for (unsigned int j=0; j < 6 ; j++)
292  {
293  L[n+i][j] = l->L[i][j]; //On remplit la matrice d'interaction globale
294  }
295  error[n+i] = l->error[i]; //On remplit la matrice d'erreur
296 
297  if (error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
298 
299  w[n+i] = 0;
300 
301  if (iter == 0)
302  {
303  factor[n+i] = fac;
304  vpMeSite site = *itListLine;
305  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
306  ++itListLine;
307  }
308 
309  //If pour la premiere extremite des moving edges
310  if (i == 0)
311  {
312  e_cur = l->error[0];
313  if (l->nbFeature > 1)
314  {
315  e_next = l->error[1];
316  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
317  {
318  w[n+i] = 1/*0.5*/;
319  }
320  e_prev = e_cur;
321  }
322  else w[n+i] = 1;
323  }
324 
325  //If pour la derniere extremite des moving edges
326  else if(i == l->nbFeature-1)
327  {
328  e_cur = l->error[i];
329  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
330  {
331  w[n+i] += 1/*0.5*/;
332  }
333  }
334 
335  else
336  {
337  e_cur = l->error[i];
338  e_next = l->error[i+1];
339  if ( fabs(e_cur - e_prev) < limite )
340  {
341  w[n+i] += 0.5;
342  }
343  if ( fabs(e_cur - e_next) < limite )
344  {
345  w[n+i] += 0.5;
346  }
347  e_prev = e_cur;
348  }
349  }
350 
351  n+= l->nbFeature ;
352  }
353 
354 
355  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
356  cy = *it;
358  double fac = 1.0;
359 
360  std::list<vpMeSite>::const_iterator itCyl1;
361  std::list<vpMeSite>::const_iterator itCyl2;
362  if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)){
363  itCyl1 = cy->meline1->getMeList().begin();
364  itCyl2 = cy->meline2->getMeList().begin();
365  }
366 
367  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
368  for(unsigned int j=0; j < 6 ; j++){
369  L[n+i][j] = cy->L[i][j]; //On remplit la matrice d'interaction globale
370  }
371  error[n+i] = cy->error[i]; //On remplit la matrice d'erreur
372 
373  if (error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
374 
375  w[n+i] = 0;
376 
377  if (iter == 0)
378  {
379  factor[n+i] = fac;
380  vpMeSite site;
381  if(i<cy->nbFeaturel1) {
382  site= *itCyl1;
383  ++itCyl1;
384  }
385  else{
386  site= *itCyl2;
387  ++itCyl2;
388  }
389  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
390  }
391 
392  //If pour la premiere extremite des moving edges
393  if (i == 0)
394  {
395  e_cur = cy->error[0];
396  if (cy->nbFeature > 1)
397  {
398  e_next = cy->error[1];
399  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
400  {
401  w[n+i] = 1/*0.5*/;
402  }
403  e_prev = e_cur;
404  }
405  else w[n+i] = 1;
406  }
407  if (i == cy->nbFeaturel1)
408  {
409  e_cur = cy->error[i];
410  if (cy->nbFeaturel2 > 1)
411  {
412  e_next = cy->error[i+1];
413  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
414  {
415  w[n+i] = 1/*0.5*/;
416  }
417  e_prev = e_cur;
418  }
419  else w[n+i] = 1;
420  }
421 
422  //If pour la derniere extremite des moving edges
423  else if(i == cy->nbFeaturel1-1)
424  {
425  e_cur = cy->error[i];
426  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
427  {
428  w[n+i] += 1/*0.5*/;
429  }
430  }
431  //If pour la derniere extremite des moving edges
432  else if(i == cy->nbFeature-1)
433  {
434  e_cur = cy->error[i];
435  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
436  {
437  w[n+i] += 1/*0.5*/;
438  }
439  }
440 
441  else
442  {
443  e_cur = cy->error[i];
444  e_next = cy->error[i+1];
445  if ( fabs(e_cur - e_prev) < limite ){
446  w[n+i] += 0.5;
447  }
448  if ( fabs(e_cur - e_next) < limite ){
449  w[n+i] += 0.5;
450  }
451  e_prev = e_cur;
452  }
453  }
454 
455  n+= cy->nbFeature ;
456  }
457 
458  count = count / (double)nbrow;
459  if (count < 0.85){
460  reloop = true;
461  }
462 
463  double num=0;
464  double den=0;
465 
466  double wi ; double eri ;
467  for(unsigned int i = 0; i < nerror; i++){
468  wi = w[i]*factor[i];
469  eri = error[i];
470  num += wi*vpMath::sqr(eri);
471  den += wi ;
472 
473  weighted_error[i] = wi*eri ;
474  }
475 
476  if((iter==0) || compute_interaction){
477  for (unsigned int i=0 ; i < nerror ; i++){
478  for (unsigned int j=0 ; j < 6 ; j++){
479  L[i][j] = w[i]*factor[i]*L[i][j] ;
480  }
481  }
482  }
483 
484  LTL = L.AtA();
485  computeJTR(L, weighted_error, LTR);
486  v = -0.7*LTL.pseudoInverse(LTL.getRows()*DBL_EPSILON)*LTR;
488 
489  iter++;
490  }
491 // cout << "\t First minimization in " << iter << " iteration " << endl ;
492 
493 /*** Second phase ***/
494 
495  vpRobust robust_lines(nberrors_lines);
496  vpRobust robust_cylinders(nberrors_cylinders);
497  robust_lines.setIteration(0) ;
498  robust_cylinders.setIteration(0) ;
499  iter = 0;
500  vpColVector w_lines(nberrors_lines);
501  vpColVector w_cylinders(nberrors_cylinders);
502  vpColVector error_lines(nberrors_lines);
503  vpColVector error_cylinders(nberrors_cylinders);
504 
505  vpColVector error_vec;
506  vpColVector W_true;
507  vpMatrix L_true;
508 
509  while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
510  {
511  unsigned int n = 0 ;
512  unsigned int nlines = 0;
513  unsigned int ncylinders = 0;
514  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
515  l = *it;
517  for (unsigned int i=0 ; i < l->nbFeature ; i++){
518  for (unsigned int j=0; j < 6 ; j++){
519  L[n+i][j] = l->L[i][j];
520  error[n+i] = l->error[i];
521  error_lines[nlines+i] = error[n+i];
522  }
523  }
524  n+= l->nbFeature;
525  nlines+= l->nbFeature;
526  }
527 
528  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
529  cy = *it;
531  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
532  for(unsigned int j=0; j < 6 ; j++){
533  L[n+i][j] = cy->L[i][j];
534  error[n+i] = cy->error[i];
535  error_cylinders[ncylinders+i] = error[n+i];
536  }
537  }
538 
539  n+= cy->nbFeature ;
540  ncylinders+= cy->nbFeature ;
541  }
542 
543  if(iter==0)
544  {
545  weighted_error.resize(nerror);
546  w.resize(nerror);
547  w = 1;
548  w_lines.resize(nberrors_lines);
549  w_lines = 1;
550  w_cylinders.resize(nberrors_cylinders);
551  w_cylinders = 1;
552 
553  robust_lines.setThreshold(2/cam.get_px());
554  robust_cylinders.setThreshold(2/cam.get_px());
555  if(nberrors_lines > 0)
556  robust_lines.MEstimator(vpRobust::TUKEY, error_lines,w_lines);
557  if(nberrors_cylinders > 0){
558  robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
559  }
560  }
561  else
562  {
563  robust_lines.setIteration(iter);
564  robust_cylinders.setIteration(iter);
565  if(nberrors_lines > 0)
566  robust_lines.MEstimator(vpRobust::TUKEY, error_lines, w_lines);
567  if(nberrors_cylinders > 0){
568  robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
569  }
570  }
571 
572  unsigned int cpt = 0;
573  while(cpt<nbrow){
574  if(cpt<nberrors_lines){
575  w[cpt] = w_lines[cpt];
576  }
577  else{
578  w[cpt] = w_cylinders[cpt-nberrors_lines];
579  }
580  cpt++;
581  }
582 
583  residu_1 = r;
584 
585  double num=0;
586  double den=0;
587  double wi;
588  double eri;
589 
590  L_true = L;
591  W_true = vpColVector(nerror);
592 
593  for(unsigned int i=0; i<nerror; i++){
594  wi = w[i]*factor[i];
595  W_true[i] = wi*wi;
596  eri = error[i];
597  num += wi*vpMath::sqr(eri);
598  den += wi;
599 
600  weighted_error[i] = wi*eri ;
601  }
602 
603  r = sqrt(num/den); //Le critere d'arret prend en compte le poids
604 
605  if((iter==0)|| compute_interaction){
606  for (unsigned int i=0 ; i < nerror ; i++){
607  for (unsigned int j=0 ; j < 6 ; j++){
608  L[i][j] = w[i]*factor[i]*L[i][j];
609  }
610  }
611  }
612 
613  LTL = L.AtA();
614  computeJTR(L, weighted_error, LTR);
615  v = -lambda*LTL.pseudoInverse(LTL.getRows()*DBL_EPSILON)*LTR;
617 
618  iter++;
619  }
620 
621  if(computeCovariance){
622  vpMatrix D; //Should be the M.diag(wi) * M.diag(wi).transpose() = (M.diag(wi^2)) which is more efficient
623  D.diag(W_true);
625  }
626 
627  unsigned int n =0 ;
628  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
629  l = *it;
630  {
631  double wmean = 0 ;
632  std::list<vpMeSite>::iterator itListLine;
633  if (l->nbFeature > 0) itListLine = l->meline->getMeList().begin();
634 
635  for (unsigned int i=0 ; i < l->nbFeature ; i++){
636  wmean += w[n+i] ;
637  vpMeSite p = *itListLine;
638  if (w[n+i] < 0.5){
640 
641  *itListLine = p;
642  }
643 
644  ++itListLine;
645  }
646  n+= l->nbFeature ;
647 
648  if (l->nbFeature!=0)
649  wmean /= l->nbFeature ;
650  else
651  wmean = 1;
652 
653  l->setMeanWeight(wmean);
654 
655  if (wmean < 0.8)
656  l->Reinit = true;
657  }
658  }
659 
660 
661  // Same thing with cylinders as with lines
662  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
663  cy = *it;
664  double wmean = 0 ;
665  std::list<vpMeSite>::iterator itListCyl1;
666  std::list<vpMeSite>::iterator itListCyl2;
667  if (cy->nbFeature > 0){
668  itListCyl1 = cy->meline1->getMeList().begin();
669  itListCyl2 = cy->meline2->getMeList().begin();
670  }
671 
672  wmean = 0;
673  for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
674  wmean += w[n+i] ;
675  vpMeSite p = *itListCyl1;
676  if (w[n+i] < 0.5){
678 
679  *itListCyl1 = p;
680  }
681 
682  ++itListCyl1;
683  }
684 
685  if (cy->nbFeaturel1!=0)
686  wmean /= cy->nbFeaturel1 ;
687  else
688  wmean = 1;
689 
690  cy->setMeanWeight1(wmean);
691 
692  if (wmean < 0.8){
693  cy->Reinit = true;
694  }
695 
696  wmean = 0;
697  for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
698  wmean += w[n+i] ;
699  vpMeSite p = *itListCyl2;
700  if (w[n+i] < 0.5){
702 
703  *itListCyl2 = p;
704  }
705 
706  ++itListCyl2;
707  }
708 
709  if (cy->nbFeaturel2!=0)
710  wmean /= cy->nbFeaturel2 ;
711  else
712  wmean = 1;
713 
714  cy->setMeanWeight2(wmean);
715 
716  if (wmean < 0.8){
717  cy->Reinit = true;
718  }
719 
720  n+= cy->nbFeature ;
721  }
722 }
723 
729 void
731 {
732  int nbExpectedPoint = 0;
733  int nbGoodPoint = 0;
734  int nbBadPoint = 0;
735 
736  vpMbtDistanceLine *l ;
737  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
738  l = *it;
739  if (l->isVisible() && l->meline != NULL)
740  {
741  nbExpectedPoint += (int)l->meline->expecteddensity;
742  for(std::list<vpMeSite>::const_iterator it=l->meline->getMeList().begin(); it!=l->meline->getMeList().end(); ++it){
743  vpMeSite pix = *it;
744  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
745  else nbBadPoint++;
746  }
747  }
748  }
749 
750 
752  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
753  cy = *it;
754  if (cy->meline1 !=NULL && cy->meline2 != NULL)
755  {
756  nbExpectedPoint += (int)cy->meline1->expecteddensity;
757  for(std::list<vpMeSite>::const_iterator it=cy->meline1->getMeList().begin(); it!=cy->meline1->getMeList().end(); ++it){
758  vpMeSite pix = *it;
759  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
760  else nbBadPoint++;
761  }
762  nbExpectedPoint += (int)cy->meline2->expecteddensity;
763  for(std::list<vpMeSite>::const_iterator it=cy->meline2->getMeList().begin(); it!=cy->meline2->getMeList().end(); ++it){
764  vpMeSite pix = *it;
765  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
766  else nbBadPoint++;
767  }
768  }
769  }
770 
771  //if (nbGoodPoint < percentageGdPt *(nbGoodPoint+nbBadPoint) || nbExpectedPoint < 2)
772  // Compare the number of good points with the min between the number of expected points and number of points that are tracked
773  if ( ( (nbGoodPoint < percentageGdPt *nbExpectedPoint) && (nbGoodPoint < percentageGdPt *(nbGoodPoint+nbBadPoint)) ) // Modif FS
774  || nbExpectedPoint < 2)
775  {
776  throw vpTrackingException(vpTrackingException::fatalError, "Not enough points to track the object");
777  }
778 }
779 
780 
788 void
790 {
791  initPyramid(I, Ipyramid);
792 
793 // for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
794  unsigned int lvl = scales.size();
795  do{
796  lvl--;
797  if(scales[lvl]){
798  vpHomogeneousMatrix cMo_1 = cMo;
799  try
800  {
801  downScale(lvl);
802 
803  try
804  {
805  trackMovingEdge(*Ipyramid[lvl]);
806  }
807  catch(...)
808  {
809  vpTRACE("Error in moving edge tracking") ;
810  throw ;
811  }
812 
813  // initialize the vector that contains the error and the matrix that contains
814  // the interaction matrix
815  vpMbtDistanceLine *l ;
816  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
817  l = *it;
818  if (l->isVisible()){
820  }
821  }
822 
824  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
825  cy = *it;
827  }
828 
829  try
830  {
831  computeVVS(*Ipyramid[lvl]);
832  }
833  catch(...)
834  {
835  vpTRACE("Error in computeVVS") ;
836  throw vpException(vpException::fatalError, "Error in computeVVS");
837  }
838 
839  try
840  {
841  testTracking();
842  }
843  catch(...)
844  {
845  throw vpTrackingException(vpTrackingException::fatalError, "test Tracking fail");
846  }
847 
848  if (displayFeatures)
849  {
850  if(lvl == 0){
851  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
852  l = *it;
853  if (l->isVisible()){
854  l->displayMovingEdges(I);
855  }
856  }
857 
858  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
859  cy = *it;
860  cy->displayMovingEdges(I);
861  }
862  }
863  }
864 
865  try
866  {
867  updateMovingEdge(I);
868  }
869  catch(...)
870  {
871  vpTRACE("Error in moving edge updating") ;
872  throw ;
873  }
874 
875  // Looking for new visible face
876  bool newvisibleface = false ;
877  visibleFace(I, cMo, newvisibleface) ;
878  initMovingEdge(I,cMo) ;
879 
880  // Reinit the moving edge for the lines which need it.
882  upScale(lvl);
883  }
884  catch(...)
885  {
886  if(lvl != 0){
887  cMo = cMo_1;
888  reInitLevel(lvl);
889  upScale(lvl);
890  }
891  else{
892  upScale(lvl);
893  throw ;
894  }
895  }
896  }
897  } while(lvl != 0);
898 
900 }
901 
908 {
909  bool a = false;
910 
911 #ifdef VISP_HAVE_OGRE
912  if(useOgre){
913  if(!faces.isOgreInitialised())
914  faces.initOgre(cam);
915  }
916 #endif
917 
918 
919  initPyramid(I, Ipyramid);
920  visibleFace(I, cMo, a);
921  unsigned int i=scales.size();
922 
923  do {
924  i--;
925  if(scales[i]){
926  downScale(i);
928  upScale(i);
929  }
930  } while(i != 0);
931 
933 }
934 
944 void
946 {
947  cMo = cdMo;
948 
950  lines[scaleLevel].front() ;
951  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
952  l = *it;
953  if(l->meline != NULL){
954  delete l->meline;
955  l->meline = NULL;
956  }
957  }
958 
959  init(I);
960 }
961 
973 void
974 vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
975 {
976  vpMbEdgeTracker::loadConfigFile(configFile.c_str());
977 }
978 
1026 void
1027 vpMbEdgeTracker::loadConfigFile(const char* configFile)
1028 {
1029 #ifdef VISP_HAVE_XML2
1030  vpMbtXmlParser xmlp;
1031 
1032  xmlp.setCameraParameters(cam);
1033  xmlp.setMovingEdge(me);
1036 
1037  try{
1038  std::cout << " *********** Parsing XML for MbEdge Tracker ************ " << std::endl;
1039  xmlp.parse(configFile);
1040  }
1041  catch(...){
1042  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
1043  throw vpException(vpException::ioError, "problem to parse configuration file.");
1044  }
1045 
1046  vpCameraParameters camera;
1047  vpMe meParser;
1048  xmlp.getCameraParameters(camera);
1049  xmlp.getMe(meParser);
1050 
1051  setCameraParameters(camera);
1052  setMovingEdge(meParser);
1055 #else
1056  vpTRACE("You need the libXML2 to read the config file %s", configFile);
1057 #endif
1058 }
1059 
1060 
1071 void
1073  const vpColor& col,
1074  const unsigned int thickness, const bool displayFullModel)
1075 {
1076  vpMbtDistanceLine *l ;
1077 
1078  for (unsigned int i = 0; i < scales.size(); i += 1){
1079  if(scales[i]){
1080  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1081  l = *it;
1082  l->display(I,cMo, cam, col, thickness, displayFullModel);
1083  }
1084 
1085  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1086  (*it)->display(I, cMo, cam, col, thickness);
1087  }
1088 
1089  break ; //displaying model on one scale only
1090  }
1091  }
1092 
1093 #ifdef VISP_HAVE_OGRE
1094  if(useOgre)
1095  faces.displayOgre(cMo);
1096 #endif
1097 }
1098 
1109 void
1111  const vpColor& col,
1112  const unsigned int thickness, const bool displayFullModel)
1113 {
1114  vpMbtDistanceLine *l ;
1115 
1116  for (unsigned int i = 0; i < scales.size(); i += 1){
1117  if(scales[i]){
1118  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1119  l = *it;
1120  l->display(I, cMo, cam, col, thickness, displayFullModel) ;
1121  }
1122 
1123  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1124  (*it)->display(I, cMo, cam, col, thickness) ;
1125  }
1126 
1127  break ; //displaying model on one scale only
1128  }
1129  }
1130 
1131 #ifdef VISP_HAVE_OGRE
1132  if(useOgre)
1133  faces.displayOgre(cMo);
1134 #endif
1135 }
1136 
1137 
1145 void
1147 {
1148  vpMbtDistanceLine *l ;
1149 
1150  lines[scaleLevel].front() ;
1151  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1152  l = *it;
1153  bool isvisible = false ;
1154 
1155  for(std::list<int>::const_iterator it=l->Lindex_polygon.begin(); it!=l->Lindex_polygon.end(); ++it){
1156  int index = *it;
1157  if (index ==-1) isvisible =true ;
1158  else
1159  {
1160  if (l->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
1161  }
1162  }
1163 
1164  //Si la ligne n'appartient a aucune face elle est tout le temps visible
1165  if (l->Lindex_polygon.empty()) isvisible = true;
1166 
1167  if (isvisible)
1168  {
1169  l->setVisible(true) ;
1170  if (l->meline==NULL)
1171  {
1172 // std::cout << "init me line "<< l->getIndex() << std::endl ;
1173  l->initMovingEdge(I, _cMo) ;
1174  }
1175  }
1176  else
1177  {
1178  l->setVisible(false) ;
1179  if (l->meline!=NULL) delete l->meline;
1180  l->meline=NULL;
1181  }
1182  }
1183 
1184 
1185  vpMbtDistanceCylinder *cy ;
1186  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1187  cy = *it;
1188  if (cy->meline1==NULL || cy->meline2==NULL){
1189  cy->initMovingEdge(I, _cMo) ;
1190  }
1191  }
1192 }
1193 
1194 
1200 void
1202 {
1203  vpMbtDistanceLine *l ;
1204  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1205  l = *it;
1206  if(l->isVisible() == true){
1207  if(l->meline == NULL){
1208  l->initMovingEdge(I, cMo);
1209  }
1210  l->trackMovingEdge(I, cMo) ;
1211  }
1212  }
1213 
1215  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1216  cy = *it;
1217  if(cy->meline1 == NULL || cy->meline2 == NULL){
1218  cy->initMovingEdge(I, cMo);
1219  }
1220  cy->trackMovingEdge(I, cMo) ;
1221  }
1222 }
1223 
1224 
1230 void
1232 {
1233  vpMbtDistanceLine *l ;
1234  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1235  l = *it;
1236  l->updateMovingEdge(I, cMo) ;
1237  if (l->nbFeature == 0 && l->isVisible()){
1238  l->Reinit = true;
1239  }
1240  }
1241 
1242 
1243  vpMbtDistanceCylinder *cy ;
1244  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1245  cy = *it;
1246  cy->updateMovingEdge(I, cMo) ;
1247  if(cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0){
1248  cy->Reinit = true;
1249  }
1250  }
1251 }
1252 
1253 
1262 void
1264 {
1265  vpMbtDistanceLine *l ;
1266  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1267  l = *it;
1268  if (l->Reinit && l->isVisible())
1269  l->reinitMovingEdge(I, _cMo);
1270  }
1271 
1273  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1274  cy = *it;
1275  if (cy->Reinit)
1276  cy->reinitMovingEdge(I, _cMo);
1277  }
1278 }
1279 
1280 
1290 bool samePoint(const vpPoint &P1, const vpPoint &P2, double threshold=1e-5)
1291 {
1292  double d = vpMath::sqr(P1.get_oX() - P2.get_oX())+
1293  vpMath::sqr(P1.get_oY() - P2.get_oY())+
1294  vpMath::sqr(P1.get_oZ() - P2.get_oZ()) ;
1295  if (d < threshold)
1296  return true ;
1297  else
1298  return false ;
1299 }
1300 
1301 
1312 void
1313 vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygone, std::string name)
1314 {
1315  //suppress line already in the model
1316 
1317  bool already_here = false ;
1318  vpMbtDistanceLine *l ;
1319 
1320  for (unsigned int i = 0; i < scales.size(); i += 1){
1321  if(scales[i]){
1322  downScale(i);
1323  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1324  l = *it;
1325  if((samePoint(*(l->p1),P1) && samePoint(*(l->p2),P2)) ||
1326  (samePoint(*(l->p1),P2) && samePoint(*(l->p2),P1)) ){
1327  already_here = true ;
1328  l->Lindex_polygon.push_back(polygone);
1329  l->hiddenface = &faces ;
1330  }
1331  }
1332 
1333  if (!already_here){
1334  l = new vpMbtDistanceLine ;
1335 
1336  l->setCameraParameters(cam) ;
1337  l->buildFrom(P1,P2) ;
1338  l->Lindex_polygon.push_back(polygone);
1339  l->setMovingEdge(&me) ;
1340  l->hiddenface = &faces ;
1341  l->setIndex(nline) ;
1342  l->setName(name);
1343  nline +=1 ;
1344  lines[i].push_back(l);
1345  }
1346  upScale(i);
1347  }
1348  }
1349 }
1350 
1356 void
1357 vpMbEdgeTracker::removeLine(const std::string& name)
1358 {
1359  vpMbtDistanceLine *l;
1360 
1361  for(unsigned int i=0; i<scales.size(); i++){
1362  if(scales[i]){
1363  for(std::list<vpMbtDistanceLine*>::iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1364  l = *it;
1365  if (name.compare(l->getName()) == 0){
1366  lines[i].erase(it);
1367  break;
1368  }
1369  }
1370  }
1371  }
1372 }
1373 
1374 
1375 
1376 
1385 void
1386 vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, const std::string& name)
1387 {
1388  bool already_here = false ;
1389  vpMbtDistanceCylinder *cy ;
1390 
1391  for (unsigned int i = 0; i < scales.size(); i += 1){
1392  if(scales[i]){
1393  downScale(i);
1394  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1395  cy = *it;
1396  if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
1397  (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
1398  already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
1399  }
1400  }
1401 
1402  if (!already_here){
1403  cy = new vpMbtDistanceCylinder ;
1404 
1405  cy->setCameraParameters(cam);
1406  cy->buildFrom(P1, P2, r);
1407  cy->setMovingEdge(&me);
1408  cy->setIndex(ncylinder);
1409  cy->setName(name);
1410  ncylinder +=1;
1411  cylinders[i].push_back(cy);
1412  }
1413  upScale(i);
1414  }
1415  }
1416 }
1417 
1418 
1424 void
1425 vpMbEdgeTracker::removeCylinder(const std::string& name)
1426 {
1428 
1429  for(unsigned int i=0; i<scales.size(); i++){
1430  if(scales[i]){
1431  for(std::list<vpMbtDistanceCylinder*>::iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1432  cy = *it;
1433  if (name.compare(cy->getName()) == 0){
1434  cylinders[i].erase(it);
1435  break;
1436  }
1437  }
1438  }
1439  }
1440 }
1441 
1447 void
1449 {
1450  p.setIndex(index_polygon) ;
1451  faces.addPolygon(&p) ;
1452 
1453  unsigned int nbpt = p.getNbPoint() ;
1454  if(nbpt > 0){
1455  for (unsigned int i=0 ; i < nbpt-1 ; i++)
1456  addLine(p.p[i], p.p[i+1], index_polygon) ;
1457  addLine(p.p[nbpt-1], p.p[0], index_polygon) ;
1458  }
1459 
1460  index_polygon++ ;
1461 }
1462 
1463 
1473 void
1474 vpMbEdgeTracker::visibleFace(const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
1475 {
1476  unsigned int n ;
1477 
1478  if(!useOgre)
1479  n = faces.setVisible(_cMo) ;
1480  else{
1481 #ifdef VISP_HAVE_OGRE
1482  bool changed = false;
1483  n = faces.setVisibleOgre(_cMo, vpMath::rad(70), vpMath::rad(70), changed);
1484 #else
1485  n = faces.setVisible(_cMo) ;
1486 #endif
1487  }
1488 
1489 // cout << "visible face " << n << endl ;
1490  if (n > nbvisiblepolygone)
1491  {
1492  //cout << "une nouvelle face est visible " << endl ;
1493  newvisibleline = true ;
1494  }
1495  else
1496  newvisibleline = false ;
1497 
1498  nbvisiblepolygone= n ;
1499 }
1500 
1511 void
1513  #ifdef VISP_HAVE_OGRE
1514  _I
1515  #endif
1516  , const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
1517 {
1518  unsigned int n ;
1519 
1520  if(!useOgre)
1521  n = faces.setVisible(_cMo) ;
1522  else{
1523 #ifdef VISP_HAVE_OGRE
1524  bool changed = false;
1525  n = faces.setVisibleOgre(_I, cam, _cMo, vpMath::rad(70), vpMath::rad(70), changed);
1526 #else
1527  n = faces.setVisible(_cMo) ;
1528 #endif
1529  }
1530 
1531 // cout << "visible face " << n << endl ;
1532  if (n > nbvisiblepolygone)
1533  {
1534  //cout << "une nouvelle face est visible " << endl ;
1535  newvisibleline = true ;
1536  }
1537  else
1538  newvisibleline = false ;
1539 
1540  nbvisiblepolygone= n ;
1541 }
1542 
1543 
1552 void
1554 {
1555  std::string model(file);
1556  vpMbTracker::loadModel(model);
1557 }
1558 
1567 void
1568 vpMbEdgeTracker::loadModel(const std::string &file)
1569 {
1570  vpMbTracker::loadModel(file);
1571 }
1572 
1581 void
1582 vpMbEdgeTracker::initFaceFromCorners(const std::vector<vpPoint>& _corners, const unsigned int _indexFace)
1583 {
1584  vpMbtPolygon *polygon = NULL;
1585  polygon = new vpMbtPolygon;
1586  polygon->setNbPoint(_corners.size());
1587  polygon->setIndex((int)_indexFace);
1588  for(unsigned int j = 0; j < _corners.size(); j++) {
1589  polygon->addPoint(j, _corners[j]);
1590  }
1591  addPolygon(*polygon);
1592 
1593  delete polygon;
1594  polygon = NULL;
1595 }
1596 
1606 void
1607 vpMbEdgeTracker::initCylinder(const vpPoint& _p1, const vpPoint _p2, const double _radius, const unsigned int _indexCylinder)
1608 {
1609  if(_indexCylinder != 0){
1610  ncylinder = _indexCylinder;
1611  }
1612  addCylinder(_p1, _p2, _radius);
1613 }
1614 
1620 void
1622 {
1623  this->cMo.setIdentity();
1624  vpMbtDistanceLine *l;
1626 
1627  for (unsigned int i = 0; i < scales.size(); i += 1){
1628  if(scales[i]){
1629  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1630  l = *it;
1631  if (l!=NULL) delete l ;
1632  l = NULL ;
1633  }
1634 
1635  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1636  cy = *it;
1637  if (cy!=NULL) delete cy;
1638  cy = NULL;
1639  }
1640  lines[i].clear();
1641  cylinders[i].clear();
1642  }
1643  }
1644 
1645  faces.reset();
1646 
1647  index_polygon =0;
1649  nline = 0;
1650  ncylinder = 0;
1651  lambda = 1;
1652  nbvisiblepolygone = 0;
1653  percentageGdPt = 0.4;
1654 
1655  // reinitialisation of the scales.
1656  this->setScales(scales);
1657 }
1658 
1659 
1660 
1668 void
1670 {
1671  resetTracker();
1672  loadModel(cad_name);
1673  initFromPose(I, cMo);
1674 }
1675 
1686 unsigned int
1687 vpMbEdgeTracker::getNbPoints(const unsigned int level)
1688 {
1689  if((level > scales.size()) || !scales[level]){
1690  throw vpException(vpException::dimensionError, "Level is not used");
1691  }
1692 
1693  unsigned int nbGoodPoints = 0;
1694  vpMbtDistanceLine *l ;
1695  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[level].begin(); it!=lines[level].end(); ++it){
1696  l = *it;
1697  if (l->isVisible() && l->meline != NULL)
1698  {
1699  for(std::list<vpMeSite>::const_iterator it=l->meline->getMeList().begin(); it!=l->meline->getMeList().end(); ++it){
1700  if (it->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
1701  }
1702  }
1703  }
1704 
1705  vpMbtDistanceCylinder *cy ;
1706  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[level].begin(); it!=cylinders[level].end(); ++it){
1707  cy = *it;
1708  if (cy->meline1 != NULL || cy->meline2 != NULL)
1709  {
1710  for(std::list<vpMeSite>::const_iterator it=cy->meline1->getMeList().begin(); it!=cy->meline1->getMeList().end(); ++it){
1711  if (it->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
1712  }
1713  for(std::list<vpMeSite>::const_iterator it=cy->meline2->getMeList().begin(); it!=cy->meline2->getMeList().end(); ++it){
1714  if (it->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
1715  }
1716  }
1717  }
1718 
1719  return nbGoodPoints;
1720 }
1721 
1722 
1732 vpMbtPolygon*
1733 vpMbEdgeTracker::getPolygon(const unsigned int index)
1734 {
1735  if(index >= static_cast<unsigned int>(faces.size()) ){
1736  throw vpException(vpException::dimensionError, "index out of range");
1737  }
1738 
1739  return faces[index];
1740 }
1741 
1747 unsigned int
1749 {
1750  return static_cast<unsigned int>(faces.size());
1751 }
1752 
1773 void
1774 vpMbEdgeTracker::setScales(const std::vector<bool>& scales)
1775 {
1776  unsigned int nbActivatedLevels = 0;
1777  for (unsigned int i = 0; i < scales.size(); i += 1){
1778  if(scales[i]){
1779  nbActivatedLevels++;
1780  }
1781  }
1782  if((scales.size() < 1) || (nbActivatedLevels == 0)){
1783  vpERROR_TRACE(" !! WARNING : must use at least one level for the tracking. Use the global one");
1784  this->scales.resize(0);
1785  this->scales.push_back(true);
1786  lines.resize(1);
1787  lines[0].clear();
1788  cylinders.resize(1);
1789  cylinders[0].clear();
1790  }
1791  else{
1792  this->scales = scales;
1793  lines.resize(scales.size());
1794  cylinders.resize(scales.size());
1795  for (unsigned int i = 0; i < lines.size(); i += 1){
1796  lines[i].clear();
1797  cylinders[i].clear();
1798  }
1799  }
1800 }
1801 
1816 void
1818 {
1819  _pyramid.resize(scales.size());
1820 
1821  if(scales[0]){
1822  _pyramid[0] = &_I;
1823  }
1824  else{
1825  _pyramid[0] = NULL;
1826  }
1827 
1828  for(unsigned int i=1; i<_pyramid.size(); i += 1){
1829  if(scales[i]){
1830  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
1831  vpImage<unsigned char>* I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
1832 #ifdef VISP_HAVE_OPENCV
1833  IplImage* vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
1834  vpI0->imageData = (char*)(_I.bitmap);
1835  IplImage* vpI = cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
1836  cvResize(vpI0, vpI, CV_INTER_NN);
1837  vpImageConvert::convert(vpI, *I);
1838  cvReleaseImage(&vpI);
1839  vpI0->imageData = NULL;
1840  cvReleaseImageHeader(&vpI0);
1841 #else
1842  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale){
1843  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale){
1844  (*I)[k][l] = _I[ii][jj];
1845  }
1846  }
1847 #endif
1848  _pyramid[i] = I;
1849  }
1850  else{
1851  _pyramid[i] = NULL;
1852  }
1853  }
1854 }
1855 
1862 void
1864 {
1865  if(_pyramid.size() > 0){
1866  _pyramid[0] = NULL;
1867  for (unsigned int i = 1; i < _pyramid.size(); i += 1){
1868  if(_pyramid[i] != NULL){
1869  delete _pyramid[i];
1870  _pyramid[i] = NULL;
1871  }
1872  }
1873  _pyramid.resize(0);
1874  }
1875 }
1876 
1887 void
1888 vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level)
1889 {
1890  if(level > scales.size() || !scales[level]){
1891  std::ostringstream oss;
1892  oss << level;
1893  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
1894  throw vpException(vpException::dimensionError, errorMsg);
1895  }
1896 
1897  linesList = lines[level];
1898 }
1899 
1900 
1911 void
1912 vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level)
1913 {
1914  if(level > scales.size() || !scales[level]){
1915  std::ostringstream oss;
1916  oss << level;
1917  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
1918  throw vpException(vpException::dimensionError, errorMsg);
1919  }
1920 
1921  cylindersList = cylinders[level];
1922 }
1923 
1924 
1931 void
1932 vpMbEdgeTracker::downScale(const unsigned int _scale)
1933 {
1934  const double ratio = pow(2., (int)_scale);
1935  scaleLevel = _scale;
1936 
1937  vpMatrix K = cam.get_K();
1938 
1939  K[0][0] /= ratio;
1940  K[1][1] /= ratio;
1941  K[0][2] /= ratio;
1942  K[1][2] /= ratio;
1943 
1945 }
1946 
1953 void
1954 vpMbEdgeTracker::upScale(const unsigned int _scale)
1955 {
1956  const double ratio = pow(2., (int)_scale);
1957  scaleLevel = 0;
1958 
1959  vpMatrix K = cam.get_K();
1960 
1961  K[0][0] *= ratio;
1962  K[1][1] *= ratio;
1963  K[0][2] *= ratio;
1964  K[1][2] *= ratio;
1965 
1967 }
1968 
1976 void
1977 vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
1978 {
1979  unsigned int scaleLevel_1 = scaleLevel;
1980  scaleLevel = _lvl;
1981 
1982  vpMbtDistanceLine *l;
1983  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1984  l = *it;
1985  l->reinitMovingEdge(*Ipyramid[_lvl], cMo);
1986  }
1987 
1988 
1990  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1991  cy = *it;
1992  cy->reinitMovingEdge(*Ipyramid[_lvl], cMo);
1993  }
1994 
1995  trackMovingEdge(*Ipyramid[_lvl]);
1996  updateMovingEdge(*Ipyramid[_lvl]);
1997  scaleLevel = scaleLevel_1;
1998 }
1999