ViSP  2.8.0
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 
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:234
void setCameraParameters(const vpCameraParameters &_cam)
void setCameraParameters(const vpCameraParameters &cam)
double distFarClip
Distance for near clipping.
unsigned int ncylinder
Index of the cylinder to add, and total number of polygon extracted so far.
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void displayMovingEdges(const vpImage< unsigned char > &I)
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:118
void initFromCalibrationMatrix(const vpMatrix &_K)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setFarClippingDistance(const double &dist)
Definition: vpMbtPolygon.h:205
void setMovingEdge(const vpMe &me)
double getAngleAppear() const
void setClipping(const unsigned int &flags)
Definition: vpMbtPolygon.h:198
double getNearClippingDistance() const
void setVisible(bool _isvisible)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void upScale(const unsigned int _scale)
std::string getName() const
bool hasNearClippingDistance() const
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:138
bool isAppearing(const unsigned int i)
unsigned int getWidth() const
Definition: vpImage.h:159
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(const double wmean)
void computeVVS(const vpImage< unsigned char > &_I)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
void init(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
#define vpTRACE
Definition: vpDebug.h:401
double lambda
The gain of the virtual visual servoing stage.
unsigned int scaleLevel
Current scale level used. This attribute must not be modified outside of the downScale() and upScale(...
std::list< int > Lindex_polygon
Index of the faces which contain the line.
Type * bitmap
points toward the bitmap
Definition: vpImage.h:120
void setIdentity()
Basic initialisation (identity)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:76
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:108
vpPoint * p1
The first extremity.
double distNearClip
Distance for near clipping.
void displayMovingEdges(const vpImage< unsigned char > &I)
virtual ~vpMbEdgeTracker()
void setAngleDisappear(const double &adisappear)
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:129
void getMe(vpMe &_ecm) const
void initOgre(vpCameraParameters _cam=vpCameraParameters())
error that can be emited by ViSP classes.
Definition: vpException.h:75
void computeJTR(const vpMatrix &J, const vpColVector &R, vpMatrix &JTR)
Manage a cylinder used in the model-based tracker.
void reInitLevel(const unsigned int _lvl)
Contains predetermined masks for sites and holds moving edges tracking parameters.
Definition: vpMe.h:70
Manage the line of a polygon used in the model-based tracker.
void setName(const std::string &name)
bool hasFarClippingDistance() const
unsigned int nbFeature
The number of moving edges.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
std::vector< const vpImage< unsigned char > * > Ipyramid
Pyramid of image associated to the current image. This pyramid is computed in the init() and in the t...
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0)
void setIndex(const unsigned int i)
double getAngleDisappear() const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void removeCylinder(const std::string &name)
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:116
double angleDisappears
Angle used to detect a face disappearance.
void loadConfigFile(const std::string &configFile)
vpMe me
The moving edges parameters.
void updateMovingEdge(const vpImage< unsigned char > &I)
std::vector< std::list< vpMbtDistanceLine * > > lines
Vector of list of all the lines tracked (each line is linked to a list of moving edges). Each element of the vector is for a scale (element 0 = level 0 = no subsampling).
unsigned int getNbPoints(const unsigned int level=0) const
void setNearClippingDistance(const double &dist)
Definition: vpMbtPolygon.h:220
vpPoint * p1
The first extremity on the axe.
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
bool useOgre
Use Ogre3d for visibility tests.
unsigned int nbFeature
The number of moving edges.
void downScale(const unsigned int _scale)
Class that defines what is a point.
Definition: vpPoint.h:65
double expecteddensity
Definition: vpMbtMeLine.h:74
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
Definition: vpMeSite.h:202
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness=1, const bool displayFullModel=false)
void addLine(vpPoint &p1, vpPoint &p2, int polygone=-1, std::string name="")
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:106
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
virtual void setNearClippingDistance(const double &dist)
unsigned int setVisibleOgre(const vpImage< unsigned char > &_I, const vpCameraParameters &_cam, const vpHomogeneousMatrix &_cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
unsigned int nbFeaturel1
The number of moving edges on line 1.
int index_polygon
Index of the polygon to add, and total number of polygon extracted so far. Cannot be unsigned because...
void addPoint(const unsigned int n, const vpPoint &P)
vpAROgre * getOgreContext()
unsigned int setVisible(const vpImage< unsigned char > &_I, const vpCameraParameters &_cam, const vpHomogeneousMatrix &_cMo, const double &angle, bool &changed)
void setMeanWeight(const double wmean)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
void getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0)
Error that can be emited by the vpTracker class and its derivates.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:67
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
vpPoint * p2
The second extremity.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
static double sqr(double x)
Definition: vpMath.h:106
virtual void initCylinder(const vpPoint &_p1, const vpPoint _p2, const double _radius, const unsigned int _indexCylinder=0)
vpColVector error
The error vector.
void setCameraParameters(const vpCameraParameters &cam)
bool isVisible() const
Generic class defining intrinsic camera parameters.
virtual void loadModel(const std::string &modelFile)
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:131
void setIndex(const unsigned int i)
double percentageGdPt
Percentage of good points over total number of points below which tracking is supposed to have failed...
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
bool getFovClipping() const
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
double angleAppears
Angle used to detect a face appearance.
virtual void setFarClippingDistance(const double &dist)
virtual void initFaceFromCorners(const std::vector< vpPoint > &_corners, const unsigned int _indexFace=-1)
std::list< vpMeSite > & getMeList()
Definition: vpMeTracker.h:161
void reInitModel(const vpImage< unsigned char > &I, const char *cad_name, const vpHomogeneousMatrix &cMo)
void addPolygon(PolygonType *p)
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:189
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
double getFarClippingDistance() const
double get_px() const
static double rad(double deg)
Definition: vpMath.h:100
void parse(const char *filename)
void setMeanWeight1(const double wmean)
void diag(const vpColVector &A)
Definition: vpMatrix.cpp:2523
virtual void setOgreVisibilityTest(const bool &v)
unsigned int size() const
void setScales(const std::vector< bool > &_scales)
vpPoint * p2
The second extremity on the axe.
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:127
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void getCameraParameters(vpCameraParameters &_cam) const
vpMatrix get_K() const
static double deg(double rad)
Definition: vpMath.h:93
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:120
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setName(const std::string name)
static vpHomogeneousMatrix direct(const vpColVector &v)
void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, const std::string &name="")
bool isVisible(const unsigned int i)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:63
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
void displayOgre(const vpHomogeneousMatrix &_cMo)
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
unsigned int getHeight() const
Definition: vpImage.h:150
unsigned int getNbPolygon() const
vpColVector error
The error vector.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1812
bool Reinit
Indicates if the line has to be reinitialized.
virtual void setCameraParameters(const vpCameraParameters &cam)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
double radius
The radius of the cylinder.
unsigned int getNbPoint() const
Definition: vpMbtPolygon.h:155
unsigned int nline
Index of the polygon to add, and total number of polygon extracted so far.
void addPolygon(vpMbtPolygon &p)
void trackMovingEdge(const vpImage< unsigned char > &I)
unsigned int clippingFlag
Flags specifying which clipping to used.
void loadModel(const std::string &cad_name)
virtual void setClipping(const unsigned int &flags)
unsigned int nbFeaturel2
The number of moving edges on line 2.
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:128
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
static int sign(double x)
Definition: vpMath.h:248
virtual void setNbPoint(const unsigned int nb)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void removeLine(const std::string &name)
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:122
vpMatrix L
The interaction matrix.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void setMovingEdge(const vpMe &_ecm)
void setAngleAppear(const double &aappear)
std::string getName() const
vpMbtMeLine * meline
The moving edge container.
vpMbtPolygon * getPolygon(const unsigned int index)
void buildFrom(vpPoint &_p1, vpPoint &_p2)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94
virtual void setIndex(const int i)
Definition: vpMbtPolygon.h:212
vpPoint * p
corners in the object frame
Definition: vpMbtPolygon.h:95