ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpMbEdgeTracker.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbEdgeTracker.cpp 4337 2013-07-23 13:57:53Z ayol $
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 
104 }
105 
110 {
111  vpMbtDistanceLine *l ;
113 
114  for (unsigned int i = 0; i < lines.size(); i += 1){
115  if(scales[i]){
116  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
117  l = *it;
118  if (l!=NULL){
119  delete l ;
120  }
121  l = NULL ;
122  }
123 
124  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
125  c = *it;
126  if (c!=NULL){
127  delete c ;
128  }
129  c = NULL ;
130  }
131 
132  lines[i].clear();
133  cylinders[i].clear();
134  }
135  }
137 }
138 
144 void
146 {
147  this->me = me;
148 
149  for (unsigned int i = 0; i < scales.size(); i += 1){
150  if(scales[i]){
151  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
152  (*it)->setMovingEdge(&(this->me)) ;
153  }
154 
156  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
157  cy = *it;
158  cy->setMovingEdge(&(this->me)) ;
159  }
160  }
161  }
162 }
163 
171 void
173 {
174  useOgre = v;
175  if(useOgre){
176 #ifndef VISP_HAVE_OGRE
177  useOgre = false;
178  std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
179 #endif
180  }
181 }
182 
191 void
193 {
194  double residu_1 =1e3;
195  double r =1e3-1;
196  vpMatrix LTL;
197  vpColVector LTR;
198 
199  // compute the interaction matrix and its pseudo inverse
200  vpMbtDistanceLine *l ;
202 
203  vpColVector w;
204  vpColVector weighted_error;
205  vpColVector factor;
206 
207  unsigned int iter = 0;
208 
209  //Nombre de moving edges
210  unsigned int nbrow = 0;
211  unsigned int nberrors_lines = 0;
212  unsigned int nberrors_cylinders = 0;
213 
214  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
215  l = *it;
216  nbrow += l->nbFeature ;
217  nberrors_lines+=l->nbFeature;
219  }
220 
221  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
222  cy = *it;
223  nbrow += cy->nbFeature ;
224  nberrors_cylinders += cy->nbFeature ;
226  }
227 
228  if (nbrow==0){
229  vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
230  throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
231  }
232 
233  vpMatrix L(nbrow,6), Lp;
234 
235  // compute the error vector
236  vpColVector error(nbrow);
237  unsigned int nerror = error.getRows();
238  vpColVector v ;
239 
240  double limite = 3; //Une limite de 3 pixels
241  limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
242 
243  //Parametre pour la premiere phase d'asservissement
244  double e_prev = 0, e_cur, e_next;
245  bool reloop = true;
246  double count = 0;
247 
248  /*** First phase ***/
249  while ( reloop == true && iter<10)
250  {
251  if(iter==0)
252  {
253  weighted_error.resize(nerror) ;
254  w.resize(nerror);
255  w = 0;
256  factor.resize(nerror);
257  factor = 1;
258  }
259 
260  count = 0;
261 
262  unsigned int n = 0;
263  reloop = false;
264  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
265  l = *it;
267 
268  double fac = 1;
269  if (iter == 0)
270  {
271  for(std::list<int>::const_iterator it = l->Lindex_polygon.begin(); it!=l->Lindex_polygon.end(); ++it){
272  int index = *it;
273  if (l->hiddenface->isAppearing((unsigned int)index))
274  {
275  fac = 0.2;
276  break;
277  }
278  if(l->closeToImageBorder(_I, 10))
279  {
280  fac = 0.1;
281  break;
282  }
283  }
284  }
285 
286  std::list<vpMeSite>::const_iterator itListLine;
287  if (iter == 0 && l->meline != NULL)
288  itListLine = l->meline->getMeList().begin();
289 
290  for (unsigned int i=0 ; i < l->nbFeature ; i++)
291  {
292  for (unsigned int j=0; j < 6 ; j++)
293  {
294  L[n+i][j] = l->L[i][j]; //On remplit la matrice d'interaction globale
295  }
296  error[n+i] = l->error[i]; //On remplit la matrice d'erreur
297 
298  if (error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
299 
300  w[n+i] = 0;
301 
302  if (iter == 0)
303  {
304  factor[n+i] = fac;
305  vpMeSite site = *itListLine;
306  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
307  ++itListLine;
308  }
309 
310  //If pour la premiere extremite des moving edges
311  if (i == 0)
312  {
313  e_cur = l->error[0];
314  if (l->nbFeature > 1)
315  {
316  e_next = l->error[1];
317  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
318  {
319  w[n+i] = 1/*0.5*/;
320  }
321  e_prev = e_cur;
322  }
323  else w[n+i] = 1;
324  }
325 
326  //If pour la derniere extremite des moving edges
327  else if(i == l->nbFeature-1)
328  {
329  e_cur = l->error[i];
330  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
331  {
332  w[n+i] += 1/*0.5*/;
333  }
334  }
335 
336  else
337  {
338  e_cur = l->error[i];
339  e_next = l->error[i+1];
340  if ( fabs(e_cur - e_prev) < limite )
341  {
342  w[n+i] += 0.5;
343  }
344  if ( fabs(e_cur - e_next) < limite )
345  {
346  w[n+i] += 0.5;
347  }
348  e_prev = e_cur;
349  }
350  }
351 
352  n+= l->nbFeature ;
353  }
354 
355 
356  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
357  cy = *it;
359  double fac = 1.0;
360 
361  std::list<vpMeSite>::const_iterator itCyl1;
362  std::list<vpMeSite>::const_iterator itCyl2;
363  if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)){
364  itCyl1 = cy->meline1->getMeList().begin();
365  itCyl2 = cy->meline2->getMeList().begin();
366  }
367 
368  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
369  for(unsigned int j=0; j < 6 ; j++){
370  L[n+i][j] = cy->L[i][j]; //On remplit la matrice d'interaction globale
371  }
372  error[n+i] = cy->error[i]; //On remplit la matrice d'erreur
373 
374  if (error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
375 
376  w[n+i] = 0;
377 
378  if (iter == 0)
379  {
380  factor[n+i] = fac;
381  vpMeSite site;
382  if(i<cy->nbFeaturel1) {
383  site= *itCyl1;
384  ++itCyl1;
385  }
386  else{
387  site= *itCyl2;
388  ++itCyl2;
389  }
390  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
391  }
392 
393  //If pour la premiere extremite des moving edges
394  if (i == 0)
395  {
396  e_cur = cy->error[0];
397  if (cy->nbFeature > 1)
398  {
399  e_next = cy->error[1];
400  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
401  {
402  w[n+i] = 1/*0.5*/;
403  }
404  e_prev = e_cur;
405  }
406  else w[n+i] = 1;
407  }
408  if (i == cy->nbFeaturel1)
409  {
410  e_cur = cy->error[i];
411  if (cy->nbFeaturel2 > 1)
412  {
413  e_next = cy->error[i+1];
414  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
415  {
416  w[n+i] = 1/*0.5*/;
417  }
418  e_prev = e_cur;
419  }
420  else w[n+i] = 1;
421  }
422 
423  //If pour la derniere extremite des moving edges
424  else if(i == cy->nbFeaturel1-1)
425  {
426  e_cur = cy->error[i];
427  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
428  {
429  w[n+i] += 1/*0.5*/;
430  }
431  }
432  //If pour la derniere extremite des moving edges
433  else if(i == cy->nbFeature-1)
434  {
435  e_cur = cy->error[i];
436  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
437  {
438  w[n+i] += 1/*0.5*/;
439  }
440  }
441 
442  else
443  {
444  e_cur = cy->error[i];
445  e_next = cy->error[i+1];
446  if ( fabs(e_cur - e_prev) < limite ){
447  w[n+i] += 0.5;
448  }
449  if ( fabs(e_cur - e_next) < limite ){
450  w[n+i] += 0.5;
451  }
452  e_prev = e_cur;
453  }
454  }
455 
456  n+= cy->nbFeature ;
457  }
458 
459  count = count / (double)nbrow;
460  if (count < 0.85){
461  reloop = true;
462  }
463 
464  double num=0;
465  double den=0;
466 
467  double wi ; double eri ;
468  for(unsigned int i = 0; i < nerror; i++){
469  wi = w[i]*factor[i];
470  eri = error[i];
471  num += wi*vpMath::sqr(eri);
472  den += wi ;
473 
474  weighted_error[i] = wi*eri ;
475  }
476 
477  if((iter==0) || compute_interaction){
478  for (unsigned int i=0 ; i < nerror ; i++){
479  for (unsigned int j=0 ; j < 6 ; j++){
480  L[i][j] = w[i]*factor[i]*L[i][j] ;
481  }
482  }
483  }
484 
485  LTL = L.AtA();
486  computeJTR(L, weighted_error, LTR);
487  v = -0.7*LTL.pseudoInverse(LTL.getRows()*DBL_EPSILON)*LTR;
489 
490  iter++;
491  }
492 // cout << "\t First minimization in " << iter << " iteration " << endl ;
493 
494 /*** Second phase ***/
495 
496  vpRobust robust_lines(nberrors_lines);
497  vpRobust robust_cylinders(nberrors_cylinders);
498  robust_lines.setIteration(0) ;
499  robust_cylinders.setIteration(0) ;
500  iter = 0;
501  vpColVector w_lines(nberrors_lines);
502  vpColVector w_cylinders(nberrors_cylinders);
503  vpColVector error_lines(nberrors_lines);
504  vpColVector error_cylinders(nberrors_cylinders);
505 
506  vpColVector error_vec;
507  vpColVector W_true;
508  vpMatrix L_true;
509 
510  while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
511  {
512  unsigned int n = 0 ;
513  unsigned int nlines = 0;
514  unsigned int ncylinders = 0;
515  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
516  l = *it;
518  for (unsigned int i=0 ; i < l->nbFeature ; i++){
519  for (unsigned int j=0; j < 6 ; j++){
520  L[n+i][j] = l->L[i][j];
521  error[n+i] = l->error[i];
522  error_lines[nlines+i] = error[n+i];
523  }
524  }
525  n+= l->nbFeature;
526  nlines+= l->nbFeature;
527  }
528 
529  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
530  cy = *it;
532  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
533  for(unsigned int j=0; j < 6 ; j++){
534  L[n+i][j] = cy->L[i][j];
535  error[n+i] = cy->error[i];
536  error_cylinders[ncylinders+i] = error[n+i];
537  }
538  }
539 
540  n+= cy->nbFeature ;
541  ncylinders+= cy->nbFeature ;
542  }
543 
544  if(iter==0)
545  {
546  weighted_error.resize(nerror);
547  w.resize(nerror);
548  w = 1;
549  w_lines.resize(nberrors_lines);
550  w_lines = 1;
551  w_cylinders.resize(nberrors_cylinders);
552  w_cylinders = 1;
553 
554  robust_lines.setThreshold(2/cam.get_px());
555  robust_cylinders.setThreshold(2/cam.get_px());
556  if(nberrors_lines > 0)
557  robust_lines.MEstimator(vpRobust::TUKEY, error_lines,w_lines);
558  if(nberrors_cylinders > 0){
559  robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
560  }
561  }
562  else
563  {
564  robust_lines.setIteration(iter);
565  robust_cylinders.setIteration(iter);
566  if(nberrors_lines > 0)
567  robust_lines.MEstimator(vpRobust::TUKEY, error_lines, w_lines);
568  if(nberrors_cylinders > 0){
569  robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
570  }
571  }
572 
573  unsigned int cpt = 0;
574  while(cpt<nbrow){
575  if(cpt<nberrors_lines){
576  w[cpt] = w_lines[cpt];
577  }
578  else{
579  w[cpt] = w_cylinders[cpt-nberrors_lines];
580  }
581  cpt++;
582  }
583 
584  residu_1 = r;
585 
586  double num=0;
587  double den=0;
588  double wi;
589  double eri;
590 
591  L_true = L;
592  W_true = vpColVector(nerror);
593 
594  for(unsigned int i=0; i<nerror; i++){
595  wi = w[i]*factor[i];
596  W_true[i] = wi*wi;
597  eri = error[i];
598  num += wi*vpMath::sqr(eri);
599  den += wi;
600 
601  weighted_error[i] = wi*eri ;
602  }
603 
604  r = sqrt(num/den); //Le critere d'arret prend en compte le poids
605 
606  if((iter==0)|| compute_interaction){
607  for (unsigned int i=0 ; i < nerror ; i++){
608  for (unsigned int j=0 ; j < 6 ; j++){
609  L[i][j] = w[i]*factor[i]*L[i][j];
610  }
611  }
612  }
613 
614  LTL = L.AtA();
615  computeJTR(L, weighted_error, LTR);
616  v = -lambda*LTL.pseudoInverse(LTL.getRows()*DBL_EPSILON)*LTR;
618 
619  iter++;
620  }
621 
622  if(computeCovariance){
623  vpMatrix D; //Should be the M.diag(wi) * M.diag(wi).transpose() = (M.diag(wi^2)) which is more efficient
624  D.diag(W_true);
626  }
627 
628  unsigned int n =0 ;
629  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
630  l = *it;
631  {
632  double wmean = 0 ;
633  std::list<vpMeSite>::iterator itListLine;
634  if (l->nbFeature > 0) itListLine = l->meline->getMeList().begin();
635 
636  for (unsigned int i=0 ; i < l->nbFeature ; i++){
637  wmean += w[n+i] ;
638  vpMeSite p = *itListLine;
639  if (w[n+i] < 0.5){
641 
642  *itListLine = p;
643  }
644 
645  ++itListLine;
646  }
647  n+= l->nbFeature ;
648 
649  if (l->nbFeature!=0)
650  wmean /= l->nbFeature ;
651  else
652  wmean = 1;
653 
654  l->setMeanWeight(wmean);
655 
656  if (wmean < 0.8)
657  l->Reinit = true;
658  }
659  }
660 
661 
662  // Same thing with cylinders as with lines
663  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
664  cy = *it;
665  double wmean = 0 ;
666  std::list<vpMeSite>::iterator itListCyl1;
667  std::list<vpMeSite>::iterator itListCyl2;
668  if (cy->nbFeature > 0){
669  itListCyl1 = cy->meline1->getMeList().begin();
670  itListCyl2 = cy->meline2->getMeList().begin();
671  }
672 
673  wmean = 0;
674  for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
675  wmean += w[n+i] ;
676  vpMeSite p = *itListCyl1;
677  if (w[n+i] < 0.5){
679 
680  *itListCyl1 = p;
681  }
682 
683  ++itListCyl1;
684  }
685 
686  if (cy->nbFeaturel1!=0)
687  wmean /= cy->nbFeaturel1 ;
688  else
689  wmean = 1;
690 
691  cy->setMeanWeight1(wmean);
692 
693  if (wmean < 0.8){
694  cy->Reinit = true;
695  }
696 
697  wmean = 0;
698  for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
699  wmean += w[n+i] ;
700  vpMeSite p = *itListCyl2;
701  if (w[n+i] < 0.5){
703 
704  *itListCyl2 = p;
705  }
706 
707  ++itListCyl2;
708  }
709 
710  if (cy->nbFeaturel2!=0)
711  wmean /= cy->nbFeaturel2 ;
712  else
713  wmean = 1;
714 
715  cy->setMeanWeight2(wmean);
716 
717  if (wmean < 0.8){
718  cy->Reinit = true;
719  }
720 
721  n+= cy->nbFeature ;
722  }
723 }
724 
730 void
732 {
733  int nbExpectedPoint = 0;
734  int nbGoodPoint = 0;
735  int nbBadPoint = 0;
736 
737  vpMbtDistanceLine *l ;
738  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
739  l = *it;
740  if (l->isVisible() && l->meline != NULL)
741  {
742  nbExpectedPoint += (int)l->meline->expecteddensity;
743  for(std::list<vpMeSite>::const_iterator it=l->meline->getMeList().begin(); it!=l->meline->getMeList().end(); ++it){
744  vpMeSite pix = *it;
745  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
746  else nbBadPoint++;
747  }
748  }
749  }
750 
751 
753  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
754  cy = *it;
755  if (cy->meline1 !=NULL && cy->meline2 != NULL)
756  {
757  nbExpectedPoint += (int)cy->meline1->expecteddensity;
758  for(std::list<vpMeSite>::const_iterator it=cy->meline1->getMeList().begin(); it!=cy->meline1->getMeList().end(); ++it){
759  vpMeSite pix = *it;
760  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
761  else nbBadPoint++;
762  }
763  nbExpectedPoint += (int)cy->meline2->expecteddensity;
764  for(std::list<vpMeSite>::const_iterator it=cy->meline2->getMeList().begin(); it!=cy->meline2->getMeList().end(); ++it){
765  vpMeSite pix = *it;
766  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
767  else nbBadPoint++;
768  }
769  }
770  }
771 
772  //if (nbGoodPoint < percentageGdPt *(nbGoodPoint+nbBadPoint) || nbExpectedPoint < 2)
773  // Compare the number of good points with the min between the number of expected points and number of points that are tracked
774  if ( ( (nbGoodPoint < percentageGdPt *nbExpectedPoint) && (nbGoodPoint < percentageGdPt *(nbGoodPoint+nbBadPoint)) ) // Modif FS
775  || nbExpectedPoint < 2)
776  {
777  throw vpTrackingException(vpTrackingException::fatalError, "Not enough points to track the object");
778  }
779 }
780 
781 
789 void
791 {
792  initPyramid(I, Ipyramid);
793 
794 // for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
795  unsigned int lvl = (unsigned int)scales.size();
796  do{
797  lvl--;
798  if(scales[lvl]){
799  vpHomogeneousMatrix cMo_1 = cMo;
800  try
801  {
802  downScale(lvl);
803 
804  try
805  {
806  trackMovingEdge(*Ipyramid[lvl]);
807  }
808  catch(...)
809  {
810  vpTRACE("Error in moving edge tracking") ;
811  throw ;
812  }
813 
814  // initialize the vector that contains the error and the matrix that contains
815  // the interaction matrix
816  vpMbtDistanceLine *l ;
817  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
818  l = *it;
819  if (l->isVisible()){
821  }
822  }
823 
825  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
826  cy = *it;
828  }
829 
830  try
831  {
832  computeVVS(*Ipyramid[lvl]);
833  }
834  catch(...)
835  {
836  vpTRACE("Error in computeVVS") ;
837  throw vpException(vpException::fatalError, "Error in computeVVS");
838  }
839 
840  try
841  {
842  testTracking();
843  }
844  catch(...)
845  {
846  throw vpTrackingException(vpTrackingException::fatalError, "test Tracking fail");
847  }
848 
849  if (displayFeatures)
850  {
851  if(lvl == 0){
852  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
853  l = *it;
854  if (l->isVisible()){
855  l->displayMovingEdges(I);
856  }
857  }
858 
859  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
860  cy = *it;
861  cy->displayMovingEdges(I);
862  }
863  }
864  }
865 
866  try
867  {
868  updateMovingEdge(I);
869  }
870  catch(...)
871  {
872  vpTRACE("Error in moving edge updating") ;
873  throw ;
874  }
875 
876  // Looking for new visible face
877  bool newvisibleface = false ;
878  visibleFace(I, cMo, newvisibleface) ;
879  initMovingEdge(I,cMo) ;
880 
881  // Reinit the moving edge for the lines which need it.
883  upScale(lvl);
884  }
885  catch(...)
886  {
887  if(lvl != 0){
888  cMo = cMo_1;
889  reInitLevel(lvl);
890  upScale(lvl);
891  }
892  else{
893  upScale(lvl);
894  throw ;
895  }
896  }
897  }
898  } while(lvl != 0);
899 
901 }
902 
909 {
910  bool a = false;
911 
912 #ifdef VISP_HAVE_OGRE
913  if(useOgre){
914  if(!faces.isOgreInitialised())
916  faces.initOgre(cam);
917  }
918 #endif
919 
920 
921  initPyramid(I, Ipyramid);
922  visibleFace(I, cMo, a);
923  unsigned int i = (unsigned int)scales.size();
924 
925  do {
926  i--;
927  if(scales[i]){
928  downScale(i);
930  upScale(i);
931  }
932  } while(i != 0);
933 
935 }
936 
946 void
948 {
949  cMo = cdMo;
950 
952  lines[scaleLevel].front() ;
953  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
954  l = *it;
955  if(l->meline != NULL){
956  delete l->meline;
957  l->meline = NULL;
958  }
959  }
960 
961  init(I);
962 }
963 
975 void
976 vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
977 {
978  vpMbEdgeTracker::loadConfigFile(configFile.c_str());
979 }
980 
1033 void
1034 vpMbEdgeTracker::loadConfigFile(const char* configFile)
1035 {
1036 #ifdef VISP_HAVE_XML2
1037  vpMbtXmlParser xmlp;
1038 
1039  xmlp.setCameraParameters(cam);
1040  xmlp.setMovingEdge(me);
1043 
1044  try{
1045  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1046  xmlp.parse(configFile);
1047  }
1048  catch(...){
1049  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
1050  throw vpException(vpException::ioError, "problem to parse configuration file.");
1051  }
1052 
1053  vpCameraParameters camera;
1054  vpMe meParser;
1055  xmlp.getCameraParameters(camera);
1056  xmlp.getMe(meParser);
1057 
1058  setCameraParameters(camera);
1059  setMovingEdge(meParser);
1062 
1063  if(xmlp.hasNearClippingDistance())
1065 
1066  if(xmlp.hasFarClippingDistance())
1068 
1069  if(xmlp.getFovClipping())
1071 #else
1072  vpTRACE("You need the libXML2 to read the config file %s", configFile);
1073 #endif
1074 }
1075 
1076 
1087 void
1089  const vpColor& col,
1090  const unsigned int thickness, const bool displayFullModel)
1091 {
1092  vpMbtDistanceLine *l ;
1093 
1094  for (unsigned int i = 0; i < scales.size(); i += 1){
1095  if(scales[i]){
1096  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1097  l = *it;
1098  l->display(I,cMo, cam, col, thickness, displayFullModel);
1099  }
1100 
1101  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1102  (*it)->display(I, cMo, cam, col, thickness);
1103  }
1104 
1105  break ; //displaying model on one scale only
1106  }
1107  }
1108 
1109 #ifdef VISP_HAVE_OGRE
1110  if(useOgre)
1111  faces.displayOgre(cMo);
1112 #endif
1113 }
1114 
1125 void
1127  const vpColor& col,
1128  const unsigned int thickness, const bool displayFullModel)
1129 {
1130  vpMbtDistanceLine *l ;
1131 
1132  for (unsigned int i = 0; i < scales.size(); i += 1){
1133  if(scales[i]){
1134  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1135  l = *it;
1136  l->display(I, cMo, cam, col, thickness, displayFullModel) ;
1137  }
1138 
1139  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1140  (*it)->display(I, cMo, cam, col, thickness) ;
1141  }
1142 
1143  break ; //displaying model on one scale only
1144  }
1145  }
1146 
1147 #ifdef VISP_HAVE_OGRE
1148  if(useOgre)
1149  faces.displayOgre(cMo);
1150 #endif
1151 }
1152 
1153 
1161 void
1163 {
1164  vpMbtDistanceLine *l ;
1165 
1166  lines[scaleLevel].front() ;
1167  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1168  l = *it;
1169  bool isvisible = false ;
1170 
1171  for(std::list<int>::const_iterator it=l->Lindex_polygon.begin(); it!=l->Lindex_polygon.end(); ++it){
1172  int index = *it;
1173  if (index ==-1) isvisible =true ;
1174  else
1175  {
1176  if (l->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
1177  }
1178  }
1179 
1180  //Si la ligne n'appartient a aucune face elle est tout le temps visible
1181  if (l->Lindex_polygon.empty()) isvisible = true;
1182 
1183  if (isvisible)
1184  {
1185  l->setVisible(true) ;
1186  if (l->meline==NULL)
1187  {
1188 // std::cout << "init me line "<< l->getIndex() << std::endl ;
1189  l->initMovingEdge(I, _cMo) ;
1190  }
1191  }
1192  else
1193  {
1194  l->setVisible(false) ;
1195  if (l->meline!=NULL) delete l->meline;
1196  l->meline=NULL;
1197  }
1198  }
1199 
1200 
1201  vpMbtDistanceCylinder *cy ;
1202  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1203  cy = *it;
1204  if (cy->meline1==NULL || cy->meline2==NULL){
1205  cy->initMovingEdge(I, _cMo) ;
1206  }
1207  }
1208 }
1209 
1210 
1216 void
1218 {
1219  vpMbtDistanceLine *l ;
1220  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1221  l = *it;
1222  if(l->isVisible() == true){
1223  if(l->meline == NULL){
1224  l->initMovingEdge(I, cMo);
1225  }
1226  l->trackMovingEdge(I, cMo) ;
1227  }
1228  }
1229 
1231  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1232  cy = *it;
1233  if(cy->meline1 == NULL || cy->meline2 == NULL){
1234  cy->initMovingEdge(I, cMo);
1235  }
1236  cy->trackMovingEdge(I, cMo) ;
1237  }
1238 }
1239 
1240 
1246 void
1248 {
1249  vpMbtDistanceLine *l ;
1250  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1251  l = *it;
1252  l->updateMovingEdge(I, cMo) ;
1253  if (l->nbFeature == 0 && l->isVisible()){
1254  l->Reinit = true;
1255  }
1256  }
1257 
1258 
1259  vpMbtDistanceCylinder *cy ;
1260  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1261  cy = *it;
1262  cy->updateMovingEdge(I, cMo) ;
1263  if(cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0){
1264  cy->Reinit = true;
1265  }
1266  }
1267 }
1268 
1269 
1278 void
1280 {
1281  vpMbtDistanceLine *l ;
1282  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1283  l = *it;
1284  if (l->Reinit && l->isVisible())
1285  l->reinitMovingEdge(I, _cMo);
1286  }
1287 
1289  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1290  cy = *it;
1291  if (cy->Reinit)
1292  cy->reinitMovingEdge(I, _cMo);
1293  }
1294 }
1295 
1296 
1306 bool samePoint(const vpPoint &P1, const vpPoint &P2, double threshold=1e-5)
1307 {
1308  double d = vpMath::sqr(P1.get_oX() - P2.get_oX())+
1309  vpMath::sqr(P1.get_oY() - P2.get_oY())+
1310  vpMath::sqr(P1.get_oZ() - P2.get_oZ()) ;
1311  if (d < threshold)
1312  return true ;
1313  else
1314  return false ;
1315 }
1316 
1317 
1328 void
1329 vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygone, std::string name)
1330 {
1331  //suppress line already in the model
1332 
1333  bool already_here = false ;
1334  vpMbtDistanceLine *l ;
1335 
1336  for (unsigned int i = 0; i < scales.size(); i += 1){
1337  if(scales[i]){
1338  downScale(i);
1339  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1340  l = *it;
1341  if((samePoint(*(l->p1),P1) && samePoint(*(l->p2),P2)) ||
1342  (samePoint(*(l->p1),P2) && samePoint(*(l->p2),P1)) ){
1343  already_here = true ;
1344  l->Lindex_polygon.push_back(polygone);
1345  l->hiddenface = &faces ;
1346  }
1347  }
1348 
1349  if (!already_here){
1350  l = new vpMbtDistanceLine ;
1351 
1352  l->setCameraParameters(cam) ;
1353  l->buildFrom(P1,P2) ;
1354  l->Lindex_polygon.push_back(polygone);
1355  l->setMovingEdge(&me) ;
1356  l->hiddenface = &faces ;
1357  l->setIndex(nline) ;
1358  l->setName(name);
1359 
1362 
1363  if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
1365 
1366  if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
1368 
1369  nline +=1 ;
1370  lines[i].push_back(l);
1371  }
1372  upScale(i);
1373  }
1374  }
1375 }
1376 
1382 void
1383 vpMbEdgeTracker::removeLine(const std::string& name)
1384 {
1385  vpMbtDistanceLine *l;
1386 
1387  for(unsigned int i=0; i<scales.size(); i++){
1388  if(scales[i]){
1389  for(std::list<vpMbtDistanceLine*>::iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1390  l = *it;
1391  if (name.compare(l->getName()) == 0){
1392  lines[i].erase(it);
1393  break;
1394  }
1395  }
1396  }
1397  }
1398 }
1399 
1400 
1401 
1402 
1411 void
1412 vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, const std::string& name)
1413 {
1414  bool already_here = false ;
1415  vpMbtDistanceCylinder *cy ;
1416 
1417  for (unsigned int i = 0; i < scales.size(); i += 1){
1418  if(scales[i]){
1419  downScale(i);
1420  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1421  cy = *it;
1422  if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
1423  (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
1424  already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
1425  }
1426  }
1427 
1428  if (!already_here){
1429  cy = new vpMbtDistanceCylinder ;
1430 
1431  cy->setCameraParameters(cam);
1432  cy->buildFrom(P1, P2, r);
1433  cy->setMovingEdge(&me);
1434  cy->setIndex(ncylinder);
1435  cy->setName(name);
1436  ncylinder +=1;
1437  cylinders[i].push_back(cy);
1438  }
1439  upScale(i);
1440  }
1441  }
1442 }
1443 
1444 
1450 void
1451 vpMbEdgeTracker::removeCylinder(const std::string& name)
1452 {
1454 
1455  for(unsigned int i=0; i<scales.size(); i++){
1456  if(scales[i]){
1457  for(std::list<vpMbtDistanceCylinder*>::iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1458  cy = *it;
1459  if (name.compare(cy->getName()) == 0){
1460  cylinders[i].erase(it);
1461  break;
1462  }
1463  }
1464  }
1465  }
1466 }
1467 
1473 void
1475 {
1476  p.setIndex(index_polygon) ;
1477  faces.addPolygon(&p) ;
1478 
1479  unsigned int nbpt = p.getNbPoint() ;
1480  if(nbpt > 0){
1481  for (unsigned int i=0 ; i < nbpt-1 ; i++)
1482  addLine(p.p[i], p.p[i+1], index_polygon) ;
1483  addLine(p.p[nbpt-1], p.p[0], index_polygon) ;
1484  }
1485 
1486  index_polygon++ ;
1487 }
1488 
1489 
1490 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1491 
1502 void
1503 vpMbEdgeTracker::visibleFace(const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
1504 {
1505  unsigned int n ;
1506 
1507  if(!useOgre)
1508  n = faces.setVisible(_cMo) ;
1509  else{
1510 #ifdef VISP_HAVE_OGRE
1511  bool changed = false;
1512  n = faces.setVisibleOgre(_cMo, vpMath::rad(70), vpMath::rad(70), changed);
1513 #else
1514  n = faces.setVisible(_cMo) ;
1515 #endif
1516  }
1517 
1518 // cout << "visible face " << n << endl ;
1519  if (n > nbvisiblepolygone)
1520  {
1521  //cout << "une nouvelle face est visible " << endl ;
1522  newvisibleline = true ;
1523  }
1524  else
1525  newvisibleline = false ;
1526 
1527  nbvisiblepolygone= n ;
1528 }
1529 #endif //VISP_BUILD_DEPRECATED_FUNCTIONS
1530 
1541 void
1543  const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
1544 {
1545  unsigned int n ;
1546  bool changed = false;
1547 
1548  if(!useOgre)
1549  n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), changed) ;
1550  else{
1551 #ifdef VISP_HAVE_OGRE
1552  n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
1553 #else
1554  n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), changed) ;
1555 #endif
1556  }
1557 
1558 // cout << "visible face " << n << endl ;
1559  if (n > nbvisiblepolygone)
1560  {
1561  //cout << "une nouvelle face est visible " << endl ;
1562  newvisibleline = true ;
1563  }
1564  else
1565  newvisibleline = false ;
1566 
1567  nbvisiblepolygone= n ;
1568 }
1569 
1570 
1579 void
1581 {
1582  std::string model(file);
1583  vpMbTracker::loadModel(model);
1584 }
1585 
1594 void
1595 vpMbEdgeTracker::loadModel(const std::string &file)
1596 {
1597  vpMbTracker::loadModel(file);
1598 }
1599 
1608 void
1609 vpMbEdgeTracker::initFaceFromCorners(const std::vector<vpPoint>& _corners, const unsigned int _indexFace)
1610 {
1611  vpMbtPolygon *polygon = NULL;
1612  polygon = new vpMbtPolygon;
1613  polygon->setNbPoint((unsigned int)_corners.size());
1614  polygon->setIndex((int)_indexFace);
1615  for(unsigned int j = 0; j < _corners.size(); j++) {
1616  polygon->addPoint(j, _corners[j]);
1617  }
1618  addPolygon(*polygon);
1619 
1620  delete polygon;
1621  polygon = NULL;
1622 }
1623 
1633 void
1634 vpMbEdgeTracker::initCylinder(const vpPoint& _p1, const vpPoint _p2, const double _radius, const unsigned int _indexCylinder)
1635 {
1636  if(_indexCylinder != 0){
1637  ncylinder = _indexCylinder;
1638  }
1639  addCylinder(_p1, _p2, _radius);
1640 }
1641 
1647 void
1649 {
1650  this->cMo.setIdentity();
1651  vpMbtDistanceLine *l;
1653 
1654  for (unsigned int i = 0; i < scales.size(); i += 1){
1655  if(scales[i]){
1656  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1657  l = *it;
1658  if (l!=NULL) delete l ;
1659  l = NULL ;
1660  }
1661 
1662  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1663  cy = *it;
1664  if (cy!=NULL) delete cy;
1665  cy = NULL;
1666  }
1667  lines[i].clear();
1668  cylinders[i].clear();
1669  }
1670  }
1671 
1672  faces.reset();
1673 
1674  index_polygon =0;
1676  nline = 0;
1677  ncylinder = 0;
1678  lambda = 1;
1679  nbvisiblepolygone = 0;
1680  percentageGdPt = 0.4;
1681 
1682  angleAppears = vpMath::rad(95);
1685 
1686  // reinitialization of the scales.
1687  this->setScales(scales);
1688 }
1689 
1690 
1691 
1699 void
1701 {
1702  resetTracker();
1703  loadModel(cad_name);
1704  initFromPose(I, cMo);
1705 }
1706 
1717 unsigned int
1718 vpMbEdgeTracker::getNbPoints(const unsigned int level) const
1719 {
1720  if((level > scales.size()) || !scales[level]){
1721  throw vpException(vpException::dimensionError, "Level is not used");
1722  }
1723 
1724  unsigned int nbGoodPoints = 0;
1725  vpMbtDistanceLine *l ;
1726  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[level].begin(); it!=lines[level].end(); ++it){
1727  l = *it;
1728  if (l->isVisible() && l->meline != NULL)
1729  {
1730  for(std::list<vpMeSite>::const_iterator it=l->meline->getMeList().begin(); it!=l->meline->getMeList().end(); ++it){
1731  if (it->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
1732  }
1733  }
1734  }
1735 
1736  vpMbtDistanceCylinder *cy ;
1737  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[level].begin(); it!=cylinders[level].end(); ++it){
1738  cy = *it;
1739  if (cy->meline1 != NULL || cy->meline2 != NULL)
1740  {
1741  for(std::list<vpMeSite>::const_iterator it=cy->meline1->getMeList().begin(); it!=cy->meline1->getMeList().end(); ++it){
1742  if (it->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
1743  }
1744  for(std::list<vpMeSite>::const_iterator it=cy->meline2->getMeList().begin(); it!=cy->meline2->getMeList().end(); ++it){
1745  if (it->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
1746  }
1747  }
1748  }
1749 
1750  return nbGoodPoints;
1751 }
1752 
1753 
1763 vpMbtPolygon*
1764 vpMbEdgeTracker::getPolygon(const unsigned int index)
1765 {
1766  if(index >= static_cast<unsigned int>(faces.size()) ){
1767  throw vpException(vpException::dimensionError, "index out of range");
1768  }
1769 
1770  return faces[index];
1771 }
1772 
1778 unsigned int
1780 {
1781  return static_cast<unsigned int>(faces.size());
1782 }
1783 
1804 void
1805 vpMbEdgeTracker::setScales(const std::vector<bool>& scales)
1806 {
1807  unsigned int nbActivatedLevels = 0;
1808  for (unsigned int i = 0; i < scales.size(); i += 1){
1809  if(scales[i]){
1810  nbActivatedLevels++;
1811  }
1812  }
1813  if((scales.size() < 1) || (nbActivatedLevels == 0)){
1814  vpERROR_TRACE(" !! WARNING : must use at least one level for the tracking. Use the global one");
1815  this->scales.resize(0);
1816  this->scales.push_back(true);
1817  lines.resize(1);
1818  lines[0].clear();
1819  cylinders.resize(1);
1820  cylinders[0].clear();
1821  }
1822  else{
1823  this->scales = scales;
1824  lines.resize(scales.size());
1825  cylinders.resize(scales.size());
1826  for (unsigned int i = 0; i < lines.size(); i += 1){
1827  lines[i].clear();
1828  cylinders[i].clear();
1829  }
1830  }
1831 }
1832 
1838 void
1840 {
1841  if( (clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING && dist <= distNearClip)
1842  vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
1843  else if ( dist < 0 )
1844  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
1845  else{
1846  distFarClip = dist;
1848  vpMbtDistanceLine *l;
1849 
1850  for (unsigned int i = 0; i < scales.size(); i += 1){
1851  if(scales[i]){
1852  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1853  l = *it;
1855  }
1856  }
1857  }
1858  }
1859 }
1860 
1866 void
1868 {
1869  if( (clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING && dist >= distFarClip)
1870  vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
1871  else if ( dist < 0 )
1872  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
1873  else{
1874  distNearClip = dist;
1876  vpMbtDistanceLine *l;
1877 
1878  for (unsigned int i = 0; i < scales.size(); i += 1){
1879  if(scales[i]){
1880  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1881  l = *it;
1883  }
1884  }
1885  }
1886  }
1887 }
1888 
1896 void
1897 vpMbEdgeTracker::setClipping(const unsigned int &flags)
1898 {
1899  clippingFlag = flags;
1900 
1901  vpMbtDistanceLine *l;
1902 
1903  for (unsigned int i = 0; i < scales.size(); i += 1){
1904  if(scales[i]){
1905  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1906  l = *it;
1908  }
1909  }
1910  }
1911 }
1912 
1927 void
1929 {
1930  _pyramid.resize(scales.size());
1931 
1932  if(scales[0]){
1933  _pyramid[0] = &_I;
1934  }
1935  else{
1936  _pyramid[0] = NULL;
1937  }
1938 
1939  for(unsigned int i=1; i<_pyramid.size(); i += 1){
1940  if(scales[i]){
1941  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
1942  vpImage<unsigned char>* I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
1943 #ifdef VISP_HAVE_OPENCV
1944  IplImage* vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
1945  vpI0->imageData = (char*)(_I.bitmap);
1946  IplImage* vpI = cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
1947  cvResize(vpI0, vpI, CV_INTER_NN);
1948  vpImageConvert::convert(vpI, *I);
1949  cvReleaseImage(&vpI);
1950  vpI0->imageData = NULL;
1951  cvReleaseImageHeader(&vpI0);
1952 #else
1953  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale){
1954  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale){
1955  (*I)[k][l] = _I[ii][jj];
1956  }
1957  }
1958 #endif
1959  _pyramid[i] = I;
1960  }
1961  else{
1962  _pyramid[i] = NULL;
1963  }
1964  }
1965 }
1966 
1973 void
1975 {
1976  if(_pyramid.size() > 0){
1977  _pyramid[0] = NULL;
1978  for (unsigned int i = 1; i < _pyramid.size(); i += 1){
1979  if(_pyramid[i] != NULL){
1980  delete _pyramid[i];
1981  _pyramid[i] = NULL;
1982  }
1983  }
1984  _pyramid.resize(0);
1985  }
1986 }
1987 
1998 void
1999 vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level)
2000 {
2001  if(level > scales.size() || !scales[level]){
2002  std::ostringstream oss;
2003  oss << level;
2004  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2005  throw vpException(vpException::dimensionError, errorMsg);
2006  }
2007 
2008  linesList = lines[level];
2009 }
2010 
2011 
2022 void
2023 vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level)
2024 {
2025  if(level > scales.size() || !scales[level]){
2026  std::ostringstream oss;
2027  oss << level;
2028  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2029  throw vpException(vpException::dimensionError, errorMsg);
2030  }
2031 
2032  cylindersList = cylinders[level];
2033 }
2034 
2035 
2042 void
2043 vpMbEdgeTracker::downScale(const unsigned int _scale)
2044 {
2045  const double ratio = pow(2., (int)_scale);
2046  scaleLevel = _scale;
2047 
2048  vpMatrix K = cam.get_K();
2049 
2050  K[0][0] /= ratio;
2051  K[1][1] /= ratio;
2052  K[0][2] /= ratio;
2053  K[1][2] /= ratio;
2054 
2056 }
2057 
2064 void
2065 vpMbEdgeTracker::upScale(const unsigned int _scale)
2066 {
2067  const double ratio = pow(2., (int)_scale);
2068  scaleLevel = 0;
2069 
2070  vpMatrix K = cam.get_K();
2071 
2072  K[0][0] *= ratio;
2073  K[1][1] *= ratio;
2074  K[0][2] *= ratio;
2075  K[1][2] *= ratio;
2076 
2078 }
2079 
2087 void
2088 vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2089 {
2090  unsigned int scaleLevel_1 = scaleLevel;
2091  scaleLevel = _lvl;
2092 
2093  vpMbtDistanceLine *l;
2094  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
2095  l = *it;
2096  l->reinitMovingEdge(*Ipyramid[_lvl], cMo);
2097  }
2098 
2099 
2101  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
2102  cy = *it;
2103  cy->reinitMovingEdge(*Ipyramid[_lvl], cMo);
2104  }
2105 
2106  trackMovingEdge(*Ipyramid[_lvl]);
2107  updateMovingEdge(*Ipyramid[_lvl]);
2108  scaleLevel = scaleLevel_1;
2109 }
2110