Visual Servoing Platform  version 3.0.0
vpMbEdgeTracker.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Make the complete tracking of an object by using its CAD model
32  *
33  * Authors:
34  * Nicolas Melchior
35  * Romain Tallonneau
36  * Eric Marchand
37  *
38  *****************************************************************************/
39 
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/vision/vpPose.h>
47 #include <visp3/core/vpExponentialMap.h>
48 #include <visp3/core/vpPixelMeterConversion.h>
49 #include <visp3/core/vpRobust.h>
50 #include <visp3/core/vpMatrixException.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpException.h>
53 #include <visp3/core/vpTrackingException.h>
54 #include <visp3/mbt/vpMbEdgeTracker.h>
55 #include <visp3/mbt/vpMbtDistanceLine.h>
56 #include <visp3/mbt/vpMbtXmlParser.h>
57 #include <visp3/core/vpPolygon3D.h>
58 #include <visp3/core/vpVelocityTwistMatrix.h>
59 
60 #include <limits>
61 #include <string>
62 #include <sstream>
63 #include <float.h>
64 #include <map>
65 
66 bool samePoint(const vpPoint &P1, const vpPoint &P2, double threshold);
67 
72  : compute_interaction(1), lambda(1), me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0),
73  nbvisiblepolygone(0), percentageGdPt(0.4), scales(1),
74  Ipyramid(0), scaleLevel(0)
75 {
78 
79  scales[0] = true;
80 
81 #ifdef VISP_HAVE_OGRE
82  faces.getOgreContext()->setWindowName("MBT Edge");
83 #endif
84 }
85 
90 {
94 
95  for (unsigned int i = 0; i < scales.size(); i += 1){
96  if(scales[i]){
97  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
98  l = *it;
99  if (l!=NULL){
100  delete l ;
101  }
102  l = NULL ;
103  }
104 
105  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
106  cy = *it;
107  if (cy!=NULL){
108  delete cy ;
109  }
110  cy = NULL ;
111  }
112 
113  lines[i].clear();
114  cylinders[i].clear();
115  }
116  }
117  for (unsigned int i = 0; i < circles.size(); i += 1){
118  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
119  ci = *it;
120  if (ci!=NULL){
121  delete ci ;
122  }
123  ci = NULL ;
124  }
125  circles[i].clear();
126  }
128 }
129 
135 void
137 {
138  this->me = p_me;
139 
140  for (unsigned int i = 0; i < scales.size(); i += 1){
141  if(scales[i]){
142  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
143  (*it)->setMovingEdge(&(this->me)) ;
144  }
145 
147  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
148  cy = *it;
149  cy->setMovingEdge(&(this->me)) ;
150  }
151 
153  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
154  ci = *it;
155  ci->setMovingEdge(&(this->me)) ;
156  }
157  }
158  }
159 }
160 
169 void
171 {
172  double residu_1 =1e3;
173  double r =1e3-1;
174  vpMatrix LTL;
175  vpColVector LTR;
176 
177  // compute the interaction matrix and its pseudo inverse
178  vpMbtDistanceLine *l ;
180  vpMbtDistanceCircle *ci ;
181 
182  //vpColVector w;
183  vpColVector factor;
184  //vpColVector error; // s-s*
185  vpColVector weighted_error; // Weighted error vector wi(s-s)*
187 
188  unsigned int iter = 0;
189 
190  //Nombre de moving edges
191  unsigned int nbrow = 0;
192  unsigned int nberrors_lines = 0;
193  unsigned int nberrors_cylinders = 0;
194  unsigned int nberrors_circles = 0;
195 
196  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
197 
198  l = *it;
199 
200  if(l->isVisible() && l->isTracked())
201  {
202  nbrow += l->nbFeatureTotal;
203  nberrors_lines+=l->nbFeatureTotal;
205  }
206  }
207 
208  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
209  cy = *it;
210 
211  if(cy->isVisible() && cy->isTracked())
212  {
213  nbrow += cy->nbFeature ;
214  nberrors_cylinders += cy->nbFeature ;
216  }
217  }
218 
219  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
220  ci = *it;
221 
222  if(ci->isVisible() && ci->isTracked())
223  {
224  nbrow += ci->nbFeature ;
225  nberrors_circles += ci->nbFeature ;
227  }
228  }
229 
230  if (nbrow==0){
231  throw vpTrackingException(vpTrackingException::notEnoughPointError, "No data found to compute the interaction matrix...");
232  }
233 
234  vpMatrix L(nbrow,6), Lp;
235 
236  // compute the error vector
237  m_error.resize(nbrow);
238  unsigned int nerror = m_error.getRows();
239  vpColVector v ;
240 
241  double limite = 3; //Une limite de 3 pixels
242  limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
243 
244  //Parametre pour la premiere phase d'asservissement
245  double e_prev = 0, e_cur, e_next;
246  bool reloop = true;
247  double count = 0;
248 
249  bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
250  if (isoJoIdentity_)
251  oJo.eye();
252 
253  /*** First phase ***/
254 
255  while ( reloop == true && iter<10)
256  {
257  if(iter==0)
258  {
259  weighted_error.resize(nerror) ;
260  m_w.resize(nerror);
261  m_w = 0;
262  factor.resize(nerror);
263  factor = 1;
264  }
265 
266  count = 0;
267 
268  unsigned int n = 0;
269  reloop = false;
270  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
271  if((*it)->isTracked())
272  {
273  l = *it;
275 
276  double fac = 1;
277  if (iter == 0)
278  {
279  for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
280  int index = *itindex;
281  if (l->hiddenface->isAppearing((unsigned int)index))
282  {
283  fac = 0.2;
284  break;
285  }
286  if(l->closeToImageBorder(_I, 10))
287  {
288  fac = 0.1;
289  break;
290  }
291  }
292  }
293 
294  std::list<vpMeSite>::const_iterator itListLine;
295 
296  unsigned int indexFeature = 0;
297 
298  for(unsigned int a = 0 ; a < l->meline.size() ; a++)
299  {
300  if (iter == 0 && l->meline[a] != NULL)
301  itListLine = l->meline[a]->getMeList().begin();
302 
303  for (unsigned int i=0 ; i < l->nbFeature[a] ; i++)
304  {
305  for (unsigned int j=0; j < 6 ; j++)
306  {
307  L[n+i][j] = l->L[indexFeature][j]; //On remplit la matrice d'interaction globale
308  }
309  m_error[n+i] = l->error[indexFeature]; //On remplit la matrice d'erreur
310 
311  if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
312 
313  m_w[n+i] = 0;
314 
315  if (iter == 0)
316  {
317  factor[n+i] = fac;
318  vpMeSite site = *itListLine;
319  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
320  ++itListLine;
321  }
322 
323  //If pour la premiere extremite des moving edges
324  if (indexFeature == 0)
325  {
326  e_cur = l->error[0];
327  if (l->nbFeature[a] > 1)
328  {
329  e_next = l->error[1];
330  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
331  {
332  m_w[n+i] = 1/*0.5*/;
333  }
334  e_prev = e_cur;
335  }
336  else m_w[n+i] = 1;
337  }
338 
339  //If pour la derniere extremite des moving edges
340  else if(indexFeature == l->nbFeatureTotal-1)
341  {
342  e_cur = l->error[indexFeature];
343  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
344  {
345  m_w[n+i] += 1/*0.5*/;
346  }
347  }
348 
349  else
350  {
351  e_cur = l->error[indexFeature];
352  e_next = l->error[indexFeature+1];
353  if ( fabs(e_cur - e_prev) < limite )
354  {
355  m_w[n+i] += 0.5;
356  }
357  if ( fabs(e_cur - e_next) < limite )
358  {
359  m_w[n+i] += 0.5;
360  }
361  e_prev = e_cur;
362  }
363  indexFeature++;
364  }
365  n += l->nbFeature[a] ;
366  }
367  }
368  }
369  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
370  if((*it)->isTracked())
371  {
372  cy = *it;
374  double fac = 1.0;
375 
376  std::list<vpMeSite>::const_iterator itCyl1;
377  std::list<vpMeSite>::const_iterator itCyl2;
378  if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)){
379  itCyl1 = cy->meline1->getMeList().begin();
380  itCyl2 = cy->meline2->getMeList().begin();
381  }
382 
383  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
384  for(unsigned int j=0; j < 6 ; j++){
385  L[n+i][j] = cy->L[i][j]; //On remplit la matrice d'interaction globale
386  }
387  m_error[n+i] = cy->error[i]; //On remplit la matrice d'erreur
388 
389  if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
390 
391  m_w[n+i] = 0;
392 
393  if (iter == 0)
394  {
395  factor[n+i] = fac;
396  vpMeSite site;
397  if(i<cy->nbFeaturel1) {
398  site= *itCyl1;
399  ++itCyl1;
400  }
401  else{
402  site= *itCyl2;
403  ++itCyl2;
404  }
405  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
406  }
407 
408  //If pour la premiere extremite des moving edges
409  if (i == 0)
410  {
411  e_cur = cy->error[0];
412  if (cy->nbFeature > 1)
413  {
414  e_next = cy->error[1];
415  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
416  {
417  m_w[n+i] = 1/*0.5*/;
418  }
419  e_prev = e_cur;
420  }
421  else m_w[n+i] = 1;
422  }
423  if (i == cy->nbFeaturel1)
424  {
425  e_cur = cy->error[i];
426  if (cy->nbFeaturel2 > 1)
427  {
428  e_next = cy->error[i+1];
429  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
430  {
431  m_w[n+i] = 1/*0.5*/;
432  }
433  e_prev = e_cur;
434  }
435  else m_w[n+i] = 1;
436  }
437 
438  //If pour la derniere extremite des moving edges
439  else if(i == cy->nbFeaturel1-1)
440  {
441  e_cur = cy->error[i];
442  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
443  {
444  m_w[n+i] += 1/*0.5*/;
445  }
446  }
447  //If pour la derniere extremite des moving edges
448  else if(i == cy->nbFeature-1)
449  {
450  e_cur = cy->error[i];
451  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
452  {
453  m_w[n+i] += 1/*0.5*/;
454  }
455  }
456 
457  else
458  {
459  e_cur = cy->error[i];
460  e_next = cy->error[i+1];
461  if ( fabs(e_cur - e_prev) < limite ){
462  m_w[n+i] += 0.5;
463  }
464  if ( fabs(e_cur - e_next) < limite ){
465  m_w[n+i] += 0.5;
466  }
467  e_prev = e_cur;
468  }
469  }
470 
471  n+= cy->nbFeature ;
472  }
473  }
474 
475  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
476  if((*it)->isTracked())
477  {
478  ci = *it;
480  double fac = 1.0;
481 
482  std::list<vpMeSite>::const_iterator itCir;
483  if (iter == 0 && (ci->meEllipse != NULL)) {
484  itCir = ci->meEllipse->getMeList().begin();
485  }
486 
487  for(unsigned int i=0 ; i < ci->nbFeature ; i++){
488  for(unsigned int j=0; j < 6 ; j++){
489  L[n+i][j] = ci->L[i][j]; //On remplit la matrice d'interaction globale
490  }
491  m_error[n+i] = ci->error[i]; //On remplit la matrice d'erreur
492 
493  if (m_error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
494 
495  m_w[n+i] = 0;
496 
497  if (iter == 0)
498  {
499  factor[n+i] = fac;
500  vpMeSite site = *itCir;
501  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
502  ++itCir;
503  }
504 
505  //If pour la premiere extremite des moving edges
506  if (i == 0)
507  {
508  e_cur = ci->error[0];
509  if (ci->nbFeature > 1)
510  {
511  e_next = ci->error[1];
512  if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
513  {
514  m_w[n+i] = 1/*0.5*/;
515  }
516  e_prev = e_cur;
517  }
518  else m_w[n+i] = 1;
519  }
520 
521  //If pour la derniere extremite des moving edges
522  else if(i == ci->nbFeature-1)
523  {
524  e_cur = ci->error[i];
525  if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
526  {
527  m_w[n+i] += 1/*0.5*/;
528  }
529  }
530 
531  else
532  {
533  e_cur = ci->error[i];
534  e_next = ci->error[i+1];
535  if ( fabs(e_cur - e_prev) < limite ){
536  m_w[n+i] += 0.5;
537  }
538  if ( fabs(e_cur - e_next) < limite ){
539  m_w[n+i] += 0.5;
540  }
541  e_prev = e_cur;
542  }
543  }
544 
545  n+= ci->nbFeature ;
546  }
547  }
548 
549  count = count / (double)nbrow;
550  if (count < 0.85){
551  reloop = true;
552  }
553 
554  double num=0;
555  double den=0;
556 
557  double wi ; double eri ;
558  for(unsigned int i = 0; i < nerror; i++){
559  wi = m_w[i]*factor[i];
560  eri = m_error[i];
561  num += wi*vpMath::sqr(eri);
562  den += wi ;
563 
564  weighted_error[i] = wi*eri ;
565  }
566 
567  if((iter==0) || compute_interaction){
568  for (unsigned int i=0 ; i < nerror ; i++){
569  for (unsigned int j=0 ; j < 6 ; j++){
570  L[i][j] = m_w[i]*factor[i]*L[i][j] ;
571  }
572  }
573  }
574 
575  // If all the 6 dof should be estimated, we check if the interaction matrix is full rank.
576  // If not we remove automatically the dof that cannot be estimated
577  // This is particularly useful when consering circles (rank 5) and cylinders (rank 4)
578  if (isoJoIdentity_) {
579  cVo.buildFrom(cMo);
580 
581  vpMatrix K; // kernel
582  unsigned int rank = (L*cVo).kernel(K);
583  if(rank == 0) {
584  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
585  }
586  if (rank != 6) {
587  vpMatrix I; // Identity
588  I.eye(6);
589  oJo = I-K.AtA();
590 
591  isoJoIdentity_ = false;
592  }
593  }
594 
595  if(isoJoIdentity_){
596  LTL = L.AtA();
597  computeJTR(L, weighted_error, LTR);
598  v = -0.7*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
599  }
600  else{
601  cVo.buildFrom(cMo);
602  vpMatrix LVJ = (L*cVo*oJo);
603  vpMatrix LVJTLVJ = (LVJ).AtA();
604  vpColVector LVJTR;
605  computeJTR(LVJ, weighted_error, LVJTR);
606  v = -0.7*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
607  v = cVo * v;
608  }
609 
611 
612  iter++;
613  }
614 
615 // std::cout << "\t First minimization in " << iter << " iteration give as initial cMo: \n" << cMo << std::endl ;
616 
617 
618 /*** Second phase ***/
619 
620  vpRobust robust_lines(nberrors_lines);
621  vpRobust robust_cylinders(nberrors_cylinders);
622  vpRobust robust_circles(nberrors_circles);
623  robust_lines.setIteration(0) ;
624  robust_cylinders.setIteration(0) ;
625  robust_circles.setIteration(0) ;
626  iter = 0;
627  vpColVector w_lines(nberrors_lines);
628  vpColVector w_cylinders(nberrors_cylinders);
629  vpColVector w_circles(nberrors_circles);
630  vpColVector error_lines(nberrors_lines);
631  vpColVector error_cylinders(nberrors_cylinders);
632  vpColVector error_circles(nberrors_circles);
633 
634  vpHomogeneousMatrix cMoPrev;
635  vpColVector W_true;
636  vpMatrix L_true;
637  vpMatrix LVJ_true;
638 
639  double mu = 0.01;
640  vpColVector m_error_prev(nbrow);
641  vpColVector m_w_prev(nbrow);
642 
643  while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
644  {
645  unsigned int n = 0 ;
646  unsigned int nlines = 0;
647  unsigned int ncylinders = 0;
648  unsigned int ncircles = 0;
649  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
650  if((*it)->isTracked()){
651  l = *it;
653  for (unsigned int i=0 ; i < l->nbFeatureTotal ; i++){
654  for (unsigned int j=0; j < 6 ; j++){
655  L[n+i][j] = l->L[i][j];
656  m_error[n+i] = l->error[i];
657  error_lines[nlines+i] = m_error[n+i];
658  }
659  }
660  n+= l->nbFeatureTotal;
661  nlines+= l->nbFeatureTotal;
662  }
663  }
664 
665  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
666  if((*it)->isTracked()){
667  cy = *it;
669  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
670  for(unsigned int j=0; j < 6 ; j++){
671  L[n+i][j] = cy->L[i][j];
672  m_error[n+i] = cy->error[i];
673  error_cylinders[ncylinders+i] = m_error[n+i];
674  }
675  }
676 
677  n+= cy->nbFeature ;
678  ncylinders+= cy->nbFeature ;
679  }
680  }
681 
682  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
683  if((*it)->isTracked()){
684  ci = *it;
686  for(unsigned int i=0 ; i < ci->nbFeature ; i++){
687  for(unsigned int j=0; j < 6 ; j++){
688  L[n+i][j] = ci->L[i][j];
689  m_error[n+i] = ci->error[i];
690  error_circles[ncircles+i] = m_error[n+i];
691  }
692  }
693 
694  n+= ci->nbFeature ;
695  ncircles+= ci->nbFeature ;
696  }
697  }
698 
699  bool reStartFromLastIncrement = false;
701  if(m_error.sumSquare()/(double)nbrow > m_error_prev.sumSquare()/(double)nbrow){
702  mu *= 10.0;
703 
704  if(mu > 1.0)
705  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
706 
707  cMo = cMoPrev;
708  m_error = m_error_prev;
709  m_w = m_w_prev;
710  reStartFromLastIncrement = true;
711  }
712  }
713 
714  if(!reStartFromLastIncrement){
715  if(iter==0)
716  {
717  weighted_error.resize(nerror);
718  m_w.resize(nerror);
719  m_w = 1;
720  w_lines.resize(nberrors_lines);
721  w_lines = 1;
722  w_cylinders.resize(nberrors_cylinders);
723  w_cylinders = 1;
724  w_circles.resize(nberrors_circles);
725  w_circles = 1;
726 
727  robust_lines.setThreshold(2/cam.get_px());
728  robust_cylinders.setThreshold(2/cam.get_px());
729  robust_circles.setThreshold(vpMath::sqr(2/cam.get_px()));
730  if(nberrors_lines > 0)
731  robust_lines.MEstimator(vpRobust::TUKEY, error_lines,w_lines);
732  if(nberrors_cylinders > 0)
733  robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
734  if(nberrors_circles > 0)
735  robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
736  }
737  else
738  {
739  robust_lines.setIteration(iter);
740  robust_cylinders.setIteration(iter);
741  robust_circles.setIteration(iter);
742  if(nberrors_lines > 0)
743  robust_lines.MEstimator(vpRobust::TUKEY, error_lines, w_lines);
744  if(nberrors_cylinders > 0)
745  robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
746  if(nberrors_circles > 0)
747  robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
748  }
749 
750  unsigned int cpt = 0;
751  while(cpt<nbrow){
752  if(cpt<nberrors_lines){
753  m_w[cpt] = w_lines[cpt];
754  }
755  else if (cpt<nberrors_lines+nberrors_cylinders){
756  m_w[cpt] = w_cylinders[cpt-nberrors_lines];
757  }
758  else {
759  m_w[cpt] = w_circles[cpt-nberrors_lines-nberrors_cylinders];
760  }
761  cpt++;
762  }
763 
764  double num=0;
765  double den=0;
766  double wi;
767  double eri;
768 
769  L_true = L;
770  W_true = vpColVector(nerror);
771 
772  if(computeCovariance){
773  L_true = L;
774  if(!isoJoIdentity_){
775  cVo.buildFrom(cMo);
776  LVJ_true = (L*cVo*oJo);
777  }
778  }
779 
780  for(unsigned int i=0; i<nerror; i++){
781  wi = m_w[i]*factor[i];
782  W_true[i] = wi;
783  eri = m_error[i];
784  num += wi*vpMath::sqr(eri);
785  den += wi;
786 
787  weighted_error[i] = wi*eri ;
788  }
789 
790  if((iter==0)|| compute_interaction){
791  for (unsigned int i=0 ; i < nerror ; i++){
792  for (unsigned int j=0 ; j < 6 ; j++){
793  L[i][j] = m_w[i]*factor[i]*L[i][j];
794  }
795  }
796  }
797 
798  if(isoJoIdentity_){
799  LTL = L.AtA();
800  computeJTR(L, weighted_error, LTR);
801 
802  switch(m_optimizationMethod){
804  {
805  vpMatrix LMA(LTL.getRows(), LTL.getCols());
806  LMA.eye();
807  vpMatrix LTLmuI = LTL + (LMA*mu);
808  v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
809 
810  if(iter != 0)
811  mu /= 10.0;
812 
813  m_error_prev = m_error;
814  m_w_prev = m_w;
815  break;
816  }
818  default:
819  v = -lambda*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
820  }
821  }
822  else{
823  cVo.buildFrom(cMo);
824  vpMatrix LVJ = (L*cVo*oJo);
825  vpMatrix LVJTLVJ = (LVJ).AtA();
826  vpColVector LVJTR;
827  computeJTR(LVJ, weighted_error, LVJTR);
828 
829  switch(m_optimizationMethod){
831  {
832  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
833  LMA.eye();
834  vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
835  v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
836  v = cVo * v;
837 
838  if(iter != 0)
839  mu /= 10.0;
840 
841  m_error_prev = m_error;
842  m_w_prev = m_w;
843  break;
844  }
846  default:
847  {
848  v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
849  v = cVo * v;
850  break;
851  }
852  }
853  }
854 
855  residu_1 = r;
856  r = sqrt(num/den); //Le critere d'arret prend en compte le poids
857 
858  cMoPrev = cMo;
860 
861  } // endif(!restartFromLast)
862 
863  iter++;
864  }
865 
866  // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
867  if(computeCovariance){
868  vpMatrix D;
869  D.diag(W_true);
870 
871  // Note that here the covariance is computed on cMoPrev for time computation efficiency
872  if(isoJoIdentity_){
874  }
875  else{
877  }
878  }
879 
880  unsigned int n =0 ;
881  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
882  if((*it)->isTracked()){
883  l = *it;
884  unsigned int indexLine = 0;
885  double wmean = 0 ;
886  for(unsigned int a = 0 ; a < l->meline.size() ; a++)
887  {
888  std::list<vpMeSite>::iterator itListLine;
889  if (l->nbFeature[a] > 0) itListLine = l->meline[a]->getMeList().begin();
890 
891  for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
892  wmean += m_w[n+indexLine] ;
893  vpMeSite p = *itListLine;
894  if (m_w[n+indexLine] < 0.5){
896 
897  *itListLine = p;
898  }
899 
900  ++itListLine;
901  indexLine++;
902  }
903  }
904  n+= l->nbFeatureTotal ;
905 
906  if (l->nbFeatureTotal!=0)
907  wmean /= l->nbFeatureTotal ;
908  else
909  wmean = 1;
910 
911  l->setMeanWeight(wmean);
912 
913  if (wmean < 0.8)
914  l->Reinit = true;
915  }
916  }
917 
918  // Same thing with cylinders as with lines
919  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
920  if((*it)->isTracked()){
921  cy = *it;
922  double wmean = 0 ;
923  std::list<vpMeSite>::iterator itListCyl1;
924  std::list<vpMeSite>::iterator itListCyl2;
925 
926  if (cy->nbFeature > 0){
927  itListCyl1 = cy->meline1->getMeList().begin();
928  itListCyl2 = cy->meline2->getMeList().begin();
929  }
930 
931  wmean = 0;
932  for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
933  wmean += m_w[n+i] ;
934  vpMeSite p = *itListCyl1;
935  if (m_w[n+i] < 0.5){
937 
938  *itListCyl1 = p;
939  }
940 
941  ++itListCyl1;
942  }
943 
944  if (cy->nbFeaturel1!=0)
945  wmean /= cy->nbFeaturel1 ;
946  else
947  wmean = 1;
948 
949  cy->setMeanWeight1(wmean);
950 
951  if (wmean < 0.8){
952  cy->Reinit = true;
953  }
954 
955  wmean = 0;
956  for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
957  wmean += m_w[n+i] ;
958  vpMeSite p = *itListCyl2;
959  if (m_w[n+i] < 0.5){
961 
962  *itListCyl2 = p;
963  }
964 
965  ++itListCyl2;
966  }
967 
968  if (cy->nbFeaturel2!=0)
969  wmean /= cy->nbFeaturel2 ;
970  else
971  wmean = 1;
972 
973  cy->setMeanWeight2(wmean);
974 
975  if (wmean < 0.8){
976  cy->Reinit = true;
977  }
978 
979  n+= cy->nbFeature ;
980  }
981  }
982 
983  // Same thing with circles as with lines
984  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
985  if((*it)->isTracked()){
986  ci = *it;
987  double wmean = 0 ;
988  std::list<vpMeSite>::iterator itListCir;
989 
990  if (ci->nbFeature > 0){
991  itListCir = ci->meEllipse->getMeList().begin();
992  }
993 
994  wmean = 0;
995  for(unsigned int i=0 ; i < ci->nbFeature ; i++){
996  wmean += m_w[n+i] ;
997  vpMeSite p = *itListCir;
998  if (m_w[n+i] < 0.5){
1000 
1001  *itListCir = p;
1002  }
1003 
1004  ++itListCir;
1005  }
1006 
1007  if (ci->nbFeature!=0)
1008  wmean /= ci->nbFeature ;
1009  else
1010  wmean = 1;
1011 
1012  ci->setMeanWeight(wmean);
1013 
1014  if (wmean < 0.8){
1015  ci->Reinit = true;
1016  }
1017 
1018  n+= ci->nbFeature ;
1019  }
1020  }
1021 }
1022 
1030 void
1032 {
1033  vpMbtDistanceLine *l ;
1034  projectionError = 0.0;
1035  unsigned int nbFeatures = 0;
1036  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1037  l = *it;
1038  if (l->isVisible() && l->isTracked())
1039  {
1040  for(unsigned int a = 0 ; a < l->meline.size() ; a++){
1041  if(l->meline[a] != NULL){
1042  double lineNormGradient;
1043  unsigned int lineNbFeatures;
1044  l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures);
1045  projectionError += lineNormGradient;
1046  nbFeatures += lineNbFeatures;
1047  }
1048  }
1049  }
1050  }
1051 
1053  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1054  cy = *it;
1055  if (cy->isVisible() && cy->isTracked())
1056  {
1057  if(cy->meline1 != NULL)
1058  {
1059  double cylinderNormGradient = 0;
1060  unsigned int cylinderNbFeatures = 0;
1061  cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures);
1062  projectionError += cylinderNormGradient;
1063  nbFeatures += cylinderNbFeatures;
1064  }
1065 
1066  if(cy->meline2 != NULL)
1067  {
1068  double cylinderNormGradient = 0;
1069  unsigned int cylinderNbFeatures = 0;
1070  cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures);
1071  projectionError += cylinderNormGradient;
1072  nbFeatures += cylinderNbFeatures;
1073  }
1074  }
1075  }
1076 
1078  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1079  c = *it;
1080  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL)
1081  {
1082  double circleNormGradient = 0;
1083  unsigned int circleNbFeatures = 0;
1084  c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures);
1085  projectionError += circleNormGradient;
1086  nbFeatures += circleNbFeatures;
1087  }
1088  }
1089 
1090  if(nbFeatures > 0)
1091  projectionError = vpMath::deg(projectionError/(double)nbFeatures);
1092  else
1093  projectionError = 90.0;
1094 // std::cout << "Norm Gradient = " << errorGradient << std::endl;
1095 }
1096 
1102 void
1104 {
1105  int nbExpectedPoint = 0;
1106  int nbGoodPoint = 0;
1107  int nbBadPoint = 0;
1108 
1109  vpMbtDistanceLine *l ;
1110  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1111  l = *it;
1112  if (l->isVisible() && l->isTracked())
1113  {
1114  for(unsigned int a = 0 ; a < l->meline.size() ; a++){
1115  if(l->meline[a] != NULL){
1116  nbExpectedPoint += (int)l->meline[a]->expecteddensity;
1117  for(std::list<vpMeSite>::const_iterator itme=l->meline[a]->getMeList().begin(); itme!=l->meline[a]->getMeList().end(); ++itme){
1118  vpMeSite pix = *itme;
1119  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
1120  else nbBadPoint++;
1121  }
1122  }
1123  }
1124  }
1125  }
1126 
1127  vpMbtDistanceCylinder *cy ;
1128  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1129  cy = *it;
1130  if ((cy->meline1 !=NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked())
1131  {
1132  nbExpectedPoint += (int)cy->meline1->expecteddensity;
1133  for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
1134  vpMeSite pix = *itme1;
1135  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
1136  else nbBadPoint++;
1137  }
1138  nbExpectedPoint += (int)cy->meline2->expecteddensity;
1139  for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
1140  vpMeSite pix = *itme2;
1141  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
1142  else nbBadPoint++;
1143  }
1144  }
1145  }
1146 
1147  vpMbtDistanceCircle *ci ;
1148  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1149  ci = *it;
1150  if (ci->isVisible() && ci->isTracked() && ci->meEllipse !=NULL)
1151  {
1152  nbExpectedPoint += ci->meEllipse->getExpectedDensity();
1153  for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
1154  vpMeSite pix = *itme;
1155  if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
1156  else nbBadPoint++;
1157  }
1158  }
1159  }
1160 
1161  // Compare the number of good points with the min between the number of expected points and
1162  // number of points that are tracked
1163  int nb_min = (int)vpMath::minimum(percentageGdPt *nbExpectedPoint, percentageGdPt *(nbGoodPoint + nbBadPoint) );
1164  //int nb_min = std::min(val1, val2);
1165  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1166  std::ostringstream oss;
1167  oss << "Not enough moving edges ("
1168  << nbGoodPoint
1169  << ") to track the object: expected "
1170  << nb_min
1171  << ". Try to reduce the threshold="
1172  << percentageGdPt
1173  << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1175  }
1176 }
1177 
1185 void
1187 {
1188  initPyramid(I, Ipyramid);
1189 
1190 // for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
1191  unsigned int lvl = (unsigned int)scales.size();
1192  do{
1193  lvl--;
1194 
1195  projectionError = 90.0;
1196 
1197  if(scales[lvl]){
1198  vpHomogeneousMatrix cMo_1 = cMo;
1199  try
1200  {
1201  downScale(lvl);
1202 
1203  try
1204  {
1205  trackMovingEdge(*Ipyramid[lvl]);
1206  }
1207  catch(...)
1208  {
1209  vpTRACE("Error in moving edge tracking") ;
1210  throw ;
1211  }
1212 
1213  vpMbtDistanceLine *l ;
1214  vpMbtDistanceCylinder *cy ;
1215  vpMbtDistanceCircle *ci ;
1216  // initialize the vector that contains the error and the matrix that contains
1217  // the interaction matrix
1218  // AY: Useless as it is done in coputeVVS()
1219  /*
1220  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
1221  l = *it;
1222  if (l->isVisible()){
1223  l->initInteractionMatrixError() ;
1224  }
1225  }
1226 
1227  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
1228  cy = *it;
1229  if(cy->isVisible()) {
1230  cy->initInteractionMatrixError();
1231  }
1232  }
1233 
1234  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
1235  ci = *it;
1236  if (ci->isVisible()){
1237  ci->initInteractionMatrixError();
1238  }
1239  }
1240  */
1241 
1242  try
1243  {
1244  computeVVS(*Ipyramid[lvl]);
1245  }
1246  catch(vpException &e)
1247  {
1248  covarianceMatrix = -1;
1249  throw e;
1250  }
1251 
1252  try
1253  {
1254  testTracking();
1255  }
1256  catch(vpException &e)
1257  {
1258  throw e;
1259  }
1260 
1261  if (displayFeatures)
1262  {
1263  if(lvl == 0){
1264  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
1265  l = *it;
1266  if (l->isVisible() && l->isTracked()){
1267  l->displayMovingEdges(I);
1268  }
1269  }
1270 
1271  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
1272  cy = *it;
1273  if(cy->isVisible() && cy->isTracked()) {
1274  cy->displayMovingEdges(I);
1275  }
1276  }
1277 
1278  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
1279  ci = *it;
1280  if (ci->isVisible() && ci->isTracked()){
1281  ci->displayMovingEdges(I);
1282  }
1283  }
1284  }
1285  }
1286 
1287  // Looking for new visible face
1288  bool newvisibleface = false ;
1289  visibleFace(I, cMo, newvisibleface) ;
1290 
1291  //cam.computeFov(I.getWidth(), I.getHeight());
1292  if(useScanLine){
1295  }
1296 
1297  try
1298  {
1299  updateMovingEdge(I);
1300  }
1301  catch(vpException &e)
1302  {
1303  throw e;
1304  }
1305 
1306  initMovingEdge(I,cMo) ;
1307  // Reinit the moving edge for the lines which need it.
1308  reinitMovingEdge(I,cMo);
1309 
1310  if(computeProjError)
1312 
1313  upScale(lvl);
1314  }
1315  catch(vpException &e)
1316  {
1317  if(lvl != 0){
1318  cMo = cMo_1;
1319  reInitLevel(lvl);
1320  upScale(lvl);
1321  }
1322  else{
1323  upScale(lvl);
1324  throw(e) ;
1325  }
1326  }
1327  }
1328  } while(lvl != 0);
1329 
1331 }
1332 
1339 {
1340  if(!modelInitialised){
1341  throw vpException(vpException::fatalError, "model not initialized");
1342  }
1343 
1344  bool a = false;
1345 
1346 #ifdef VISP_HAVE_OGRE
1347  if(useOgre){
1348  if(!faces.isOgreInitialised()){
1351  faces.initOgre(cam);
1352  // Turn off Ogre config dialog display for the next call to this function
1353  // since settings are saved in the ogre.cfg file and used during the next
1354  // call
1355  ogreShowConfigDialog = false;
1356  }
1357  }
1358 #endif
1359 
1360 
1361  initPyramid(I, Ipyramid);
1362  visibleFace(I, cMo, a);
1363  unsigned int i = (unsigned int)scales.size();
1364 
1365  resetMovingEdge();
1366 
1367  if(useScanLine){
1368  cam.computeFov(I.getWidth(), I.getHeight());
1371  }
1372 
1373  do {
1374  i--;
1375  if(scales[i]){
1376  downScale(i);
1377  initMovingEdge(*Ipyramid[i], cMo);
1378  upScale(i);
1379  }
1380  } while(i != 0);
1381 
1383 }
1384 
1392 void
1394 {
1395  cMo = cdMo;
1396 
1397  init(I);
1398 }
1399 
1411 void
1412 vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
1413 {
1414  vpMbEdgeTracker::loadConfigFile(configFile.c_str());
1415 }
1416 
1466 void
1467 vpMbEdgeTracker::loadConfigFile(const char* configFile)
1468 {
1469 #ifdef VISP_HAVE_XML2
1470  vpMbtXmlParser xmlp;
1471 
1472  xmlp.setCameraParameters(cam);
1475  xmlp.setMovingEdge(me);
1476 
1477  try{
1478  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1479  xmlp.parse(configFile);
1480  }
1481  catch(...){
1482  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile);
1483  }
1484 
1485  vpCameraParameters camera;
1486  vpMe meParser;
1487  xmlp.getCameraParameters(camera);
1488  xmlp.getMe(meParser);
1489 
1490  setCameraParameters(camera);
1491  setMovingEdge(meParser);
1494 
1495  if(xmlp.hasNearClippingDistance())
1497 
1498  if(xmlp.hasFarClippingDistance())
1500 
1501  if(xmlp.getFovClipping())
1503 
1504  useLodGeneral = xmlp.getLodState();
1507 
1508  applyLodSettingInConfig = false;
1509  if(this->getNbPolygon() > 0) {
1510  applyLodSettingInConfig = true;
1514  }
1515 
1516 #else
1517  vpTRACE("You need the libXML2 to read the config file %s", configFile);
1518 #endif
1519 }
1520 
1521 
1532 void
1534  const vpCameraParameters &camera, const vpColor& col,
1535  const unsigned int thickness, const bool displayFullModel)
1536 {
1537  for (unsigned int i = 0; i < scales.size(); i += 1){
1538  if(scales[i]){
1539  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1540  (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
1541  }
1542 
1543  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1544  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1545  }
1546 
1547  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1548  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1549  }
1550 
1551  break ; //displaying model on one scale only
1552  }
1553  }
1554 
1555 #ifdef VISP_HAVE_OGRE
1556  if(useOgre)
1557  faces.displayOgre(cMo_);
1558 #endif
1559 }
1560 
1571 void
1573  const vpCameraParameters &camera, const vpColor& col,
1574  const unsigned int thickness, const bool displayFullModel)
1575 {
1576  for (unsigned int i = 0; i < scales.size(); i += 1){
1577  if(scales[i]){
1578  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1579  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
1580  }
1581 
1582  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1583  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
1584  }
1585 
1586  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1587  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1588  }
1589  break ; //displaying model on one scale only
1590  }
1591  }
1592 
1593 #ifdef VISP_HAVE_OGRE
1594  if(useOgre)
1595  faces.displayOgre(cMo_);
1596 #endif
1597 }
1598 
1599 
1607 void
1609 {
1610  vpMbtDistanceLine *l ;
1611 
1612  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1613  l = *it;
1614  bool isvisible = false ;
1615 
1616  for(std::list<int>::const_iterator itindex=l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
1617  int index = *itindex;
1618  if (index ==-1) isvisible =true ;
1619  else
1620  {
1621  if (l->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
1622  }
1623  }
1624 
1625  //Si la ligne n'appartient a aucune face elle est tout le temps visible
1626  if (l->Lindex_polygon.empty()) isvisible = true; // Not sure that this can occur
1627 
1628  if (isvisible)
1629  {
1630  l->setVisible(true) ;
1631  l->updateTracked();
1632  if (l->meline.size() == 0 && l->isTracked())
1633  l->initMovingEdge(I, _cMo) ;
1634  }
1635  else
1636  {
1637  l->setVisible(false) ;
1638  for(unsigned int a = 0 ; a < l->meline.size() ; a++){
1639  if (l->meline[a] != NULL) delete l->meline[a] ;
1640  l->nbFeature[a] = 0;
1641  }
1642  l->nbFeatureTotal = 0;
1643  l->meline.clear();
1644  }
1645  }
1646 
1647  vpMbtDistanceCylinder *cy ;
1648  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1649  cy = *it;
1650 
1651  bool isvisible = false ;
1652 
1653  int index = cy->index_polygon;
1654  if (index ==-1) isvisible =true ;
1655  else
1656  {
1657  if (cy->hiddenface->isVisible((unsigned int)index +1 ) ||
1658  cy->hiddenface->isVisible((unsigned int)index +2 ) ||
1659  cy->hiddenface->isVisible((unsigned int)index +3 ) ||
1660  cy->hiddenface->isVisible((unsigned int)index +4 ))
1661  isvisible = true ;
1662  }
1663 // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1664 
1665  if (isvisible)
1666  {
1667  cy->setVisible(true) ;
1668  if (cy->meline1==NULL || cy->meline2==NULL){
1669  if(cy->isTracked())
1670  cy->initMovingEdge(I, _cMo) ;
1671  }
1672  }
1673  else
1674  {
1675  cy->setVisible(false) ;
1676  if (cy->meline1!=NULL) delete cy->meline1;
1677  if (cy->meline2!=NULL) delete cy->meline2;
1678  cy->meline1=NULL;
1679  cy->meline2=NULL;
1680  cy->nbFeature = 0;
1681  cy->nbFeaturel1 = 0;
1682  cy->nbFeaturel2= 0;
1683  }
1684  }
1685 
1686  vpMbtDistanceCircle *ci ;
1687  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1688  ci = *it;
1689  bool isvisible = false ;
1690 
1691  int index = ci->index_polygon;
1692  if (index ==-1) isvisible =true ;
1693  else
1694  {
1695  if (ci->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
1696  }
1697 
1698  if (isvisible)
1699  {
1700  ci->setVisible(true) ;
1701  if (ci->meEllipse==NULL)
1702  {
1703  if(ci->isTracked())
1704  ci->initMovingEdge(I, _cMo) ;
1705  }
1706  }
1707  else
1708  {
1709  ci->setVisible(false) ;
1710  if (ci->meEllipse!=NULL) delete ci->meEllipse;
1711  ci->meEllipse=NULL;
1712  ci->nbFeature = 0;
1713  }
1714  }
1715 }
1716 
1717 
1723 void
1725 {
1726  vpMbtDistanceLine *l ;
1727  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1728  l = *it;
1729  if(l->isVisible() && l->isTracked()){
1730  if(l->meline.size() == 0){
1731  l->initMovingEdge(I, cMo);
1732  }
1733  l->trackMovingEdge(I, cMo) ;
1734  }
1735  }
1736 
1738  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1739  cy = *it;
1740  if(cy->isVisible() && cy->isTracked()) {
1741  if(cy->meline1 == NULL || cy->meline2 == NULL){
1742  cy->initMovingEdge(I, cMo);
1743  }
1744  cy->trackMovingEdge(I, cMo) ;
1745  }
1746  }
1747 
1748  vpMbtDistanceCircle *ci;
1749  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1750  ci = *it;
1751  if(ci->isVisible() && ci->isTracked()){
1752  if(ci->meEllipse == NULL){
1753  ci->initMovingEdge(I, cMo);
1754  }
1755  ci->trackMovingEdge(I, cMo) ;
1756  }
1757  }
1758 }
1759 
1760 
1766 void
1768 {
1769  vpMbtDistanceLine *l ;
1770  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1771  if((*it)->isTracked()){
1772  l = *it;
1773  l->updateMovingEdge(I, cMo) ;
1774  if (l->nbFeatureTotal == 0 && l->isVisible()){
1775  l->Reinit = true;
1776  }
1777  }
1778  }
1779 
1780  vpMbtDistanceCylinder *cy ;
1781  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1782  if((*it)->isTracked()){
1783  cy = *it;
1784  cy->updateMovingEdge(I, cMo) ;
1785  if((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()){
1786  cy->Reinit = true;
1787  }
1788  }
1789  }
1790 
1791  vpMbtDistanceCircle *ci ;
1792  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1793  if((*it)->isTracked()){
1794  ci = *it;
1795  ci->updateMovingEdge(I, cMo) ;
1796  if(ci->nbFeature == 0 && ci->isVisible()){
1797  ci->Reinit = true;
1798  }
1799  }
1800  }
1801 }
1802 
1803 
1812 void
1814 {
1815  vpMbtDistanceLine *l ;
1816  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1817  if((*it)->isTracked()){
1818  l = *it;
1819  if (l->Reinit && l->isVisible())
1820  l->reinitMovingEdge(I, _cMo);
1821  }
1822  }
1823 
1825  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1826  if((*it)->isTracked()){
1827  cy = *it;
1828  if (cy->Reinit && cy->isVisible())
1829  cy->reinitMovingEdge(I, _cMo);
1830  }
1831  }
1832 
1834  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1835  if((*it)->isTracked()){
1836  ci = *it;
1837  if (ci->Reinit && ci->isVisible())
1838  ci->reinitMovingEdge(I, _cMo);
1839  }
1840  }
1841 }
1842 
1843 void
1845  for (unsigned int i = 0; i < scales.size(); i += 1){
1846  if (scales[i]) {
1847  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1848  for(unsigned int a = 0 ; a < (*it)->meline.size() ; a++){
1849  if((*it)->meline[a] != NULL){
1850  delete (*it)->meline[a];
1851  (*it)->meline[a] = NULL;
1852  }
1853  (*it)->meline.clear();
1854  (*it)->nbFeature.clear();
1855  (*it)->nbFeatureTotal = 0;
1856  }
1857  }
1858 
1859  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1860  if((*it)->meline1 != NULL){
1861  delete (*it)->meline1;
1862  (*it)->meline1 = NULL;
1863  }
1864  if((*it)->meline2 != NULL){
1865  delete (*it)->meline2;
1866  (*it)->meline2 = NULL;
1867  }
1868 
1869  (*it)->nbFeature = 0;
1870  (*it)->nbFeaturel1 = 0;
1871  (*it)->nbFeaturel2 = 0;
1872  }
1873 
1874  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
1875  if((*it)->meEllipse != NULL){
1876  delete (*it)->meEllipse;
1877  (*it)->meEllipse = NULL;
1878  }
1879  (*it)->nbFeature = 0;
1880  }
1881  }
1882  }
1883 }
1884 
1893 bool
1895 {
1896  double dx = fabs(P1.get_oX() - P2.get_oX());
1897  double dy = fabs(P1.get_oY() - P2.get_oY());
1898  double dz = fabs(P1.get_oZ() - P2.get_oZ());
1899 
1900  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() && dz <= std::numeric_limits<double>::epsilon())
1901  return true ;
1902  else
1903  return false ;
1904 }
1905 
1906 
1917 void
1918 vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1919 {
1920  //suppress line already in the model
1921  bool already_here = false ;
1922  vpMbtDistanceLine *l ;
1923 
1924  for (unsigned int i = 0; i < scales.size(); i += 1){
1925  if(scales[i]){
1926  downScale(i);
1927  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1928  l = *it;
1929  if((samePoint(*(l->p1),P1) && samePoint(*(l->p2),P2)) ||
1930  (samePoint(*(l->p1),P2) && samePoint(*(l->p2),P1)) ){
1931  already_here = true ;
1932  l->addPolygon(polygon);
1933  l->hiddenface = &faces ;
1934  }
1935  }
1936 
1937  if (!already_here){
1938  l = new vpMbtDistanceLine ;
1939 
1940  l->setCameraParameters(cam) ;
1941  l->buildFrom(P1,P2) ;
1942  l->addPolygon(polygon);
1943  l->setMovingEdge(&me) ;
1944  l->hiddenface = &faces ;
1945  l->useScanLine = useScanLine;
1946 
1947  l->setIndex(nline) ;
1948  l->setName(name);
1949 
1952 
1953  if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1955 
1956  if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1958 
1959  nline +=1 ;
1960  lines[i].push_back(l);
1961  }
1962  upScale(i);
1963  }
1964  }
1965 }
1966 
1972 void
1973 vpMbEdgeTracker::removeLine(const std::string& name)
1974 {
1975  vpMbtDistanceLine *l;
1976 
1977  for(unsigned int i=0; i<scales.size(); i++){
1978  if(scales[i]){
1979  for(std::list<vpMbtDistanceLine*>::iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1980  l = *it;
1981  if (name.compare(l->getName()) == 0){
1982  lines[i].erase(it);
1983  break;
1984  }
1985  }
1986  }
1987  }
1988 }
1989 
2000 void
2001 vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace, const std::string& name)
2002 {
2003  bool already_here = false ;
2004  vpMbtDistanceCircle *ci ;
2005 
2006  for (unsigned int i = 0; i < scales.size(); i += 1){
2007  if(scales[i]){
2008  downScale(i);
2009  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
2010  ci = *it;
2011  if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
2012  (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
2013  already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
2014  }
2015  }
2016 
2017  if (!already_here){
2018  ci = new vpMbtDistanceCircle ;
2019 
2020  ci->setCameraParameters(cam);
2021  ci->buildFrom(P1, P2, P3, r);
2022  ci->setMovingEdge(&me);
2023  ci->setIndex(ncircle);
2024  ci->setName(name);
2025  ci->index_polygon = idFace;
2026  ci->hiddenface = &faces ;
2027 
2028 // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
2029 // ci->getPolygon().setClipping(clippingFlag);
2030 
2031 // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2032 // ci->getPolygon().setNearClippingDistance(distNearClip);
2033 
2034 // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2035 // ci->getPolygon().setFarClippingDistance(distFarClip);
2036 
2037  ncircle +=1;
2038  circles[i].push_back(ci);
2039  }
2040  upScale(i);
2041  }
2042  }
2043 }
2044 
2054 void
2055 vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace, const std::string& name)
2056 {
2057  bool already_here = false ;
2058  vpMbtDistanceCylinder *cy ;
2059 
2060  for (unsigned int i = 0; i < scales.size(); i += 1){
2061  if(scales[i]){
2062  downScale(i);
2063  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
2064  cy = *it;
2065  if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
2066  (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
2067  already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
2068  }
2069  }
2070 
2071  if (!already_here){
2072  cy = new vpMbtDistanceCylinder ;
2073 
2074  cy->setCameraParameters(cam);
2075  cy->buildFrom(P1, P2, r);
2076  cy->setMovingEdge(&me);
2077  cy->setIndex(ncylinder);
2078  cy->setName(name);
2079  cy->index_polygon = idFace;
2080  cy->hiddenface = &faces ;
2081  ncylinder +=1;
2082  cylinders[i].push_back(cy);
2083  }
2084  upScale(i);
2085  }
2086  }
2087 }
2088 
2094 void
2095 vpMbEdgeTracker::removeCylinder(const std::string& name)
2096 {
2098 
2099  for(unsigned int i=0; i<scales.size(); i++){
2100  if(scales[i]){
2101  for(std::list<vpMbtDistanceCylinder*>::iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
2102  cy = *it;
2103  if (name.compare(cy->getName()) == 0){
2104  cylinders[i].erase(it);
2105  break;
2106  }
2107  }
2108  }
2109  }
2110 }
2111 
2117 void
2118 vpMbEdgeTracker::removeCircle(const std::string& name)
2119 {
2120  vpMbtDistanceCircle *ci;
2121 
2122  for(unsigned int i=0; i<scales.size(); i++){
2123  if(scales[i]){
2124  for(std::list<vpMbtDistanceCircle*>::iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
2125  ci = *it;
2126  if (name.compare(ci->getName()) == 0){
2127  circles[i].erase(it);
2128  break;
2129  }
2130  }
2131  }
2132  }
2133 }
2134 
2140 void
2142 {
2143  unsigned int nbpt = p.getNbPoint() ;
2144  if(nbpt > 0){
2145  for (unsigned int i=0 ; i < nbpt-1 ; i++)
2146  addLine(p.p[i], p.p[i+1], p.getIndex()) ;
2147  addLine(p.p[nbpt-1], p.p[0], p.getIndex()) ;
2148  }
2149 }
2150 
2161 void
2163  const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
2164 {
2165  unsigned int n ;
2166  bool changed = false;
2167 
2168  if(!useOgre) {
2169  //n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), changed) ;
2170  n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
2171  }
2172  else{
2173 #ifdef VISP_HAVE_OGRE
2174  n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
2175 #else
2176  n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
2177 #endif
2178  }
2179 
2180  if (n > nbvisiblepolygone)
2181  {
2182  //cout << "une nouvelle face est visible " << endl ;
2183  newvisibleline = true ;
2184  }
2185  else
2186  newvisibleline = false ;
2187 
2188  nbvisiblepolygone= n ;
2189 }
2190 
2203 void
2205 {
2206  unsigned int nbpt = polygon.getNbPoint() ;
2207  if(nbpt > 0){
2208  for (unsigned int i=0 ; i < nbpt-1 ; i++)
2209  addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
2210  addLine(polygon.p[nbpt-1], polygon.p[0], polygon.getIndex(), polygon.getName());
2211  }
2212 }
2225 void
2227 {
2228  unsigned int nbpt = polygon.getNbPoint() ;
2229  if(nbpt > 0){
2230  for (unsigned int i=0 ; i < nbpt-1 ; i++)
2231  addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
2232  }
2233 }
2234 
2246 void
2247 vpMbEdgeTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
2248  const int idFace, const std::string &name)
2249 {
2250  addCircle(p1, p2, p3, radius, (int)idFace, name);
2251 }
2252 
2263 void
2264 vpMbEdgeTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
2265  const std::string &name)
2266 {
2267  addCylinder(p1, p2, radius, (int)idFace, name);
2268 }
2269 
2275 void
2277 {
2278  this->cMo.eye();
2279  vpMbtDistanceLine *l;
2281  vpMbtDistanceCircle *ci;
2282 
2283  for (unsigned int i = 0; i < scales.size(); i += 1){
2284  if(scales[i]){
2285  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
2286  l = *it;
2287  if (l!=NULL) delete l ;
2288  l = NULL ;
2289  }
2290 
2291  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
2292  cy = *it;
2293  if (cy!=NULL) delete cy;
2294  cy = NULL;
2295  }
2296 
2297  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
2298  ci = *it;
2299  if (ci!=NULL) delete ci;
2300  ci = NULL;
2301  }
2302  lines[i].clear();
2303  cylinders[i].clear();
2304  circles[i].clear();
2305  }
2306  }
2307 
2308  faces.reset();
2309 
2310  useScanLine = false;
2311 
2312 #ifdef VISP_HAVE_OGRE
2313  useOgre = false;
2314 #endif
2315 
2317  nline = 0;
2318  ncylinder = 0;
2319  lambda = 1;
2320  nbvisiblepolygone = 0;
2321  percentageGdPt = 0.4;
2322 
2323  angleAppears = vpMath::rad(89);
2326 
2328 
2329  // reinitialization of the scales.
2330  this->setScales(scales);
2331 }
2332 
2342 void
2343 vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
2344  const vpHomogeneousMatrix& cMo_, const bool verbose)
2345 {
2346  reInitModel(I, cad_name.c_str(), cMo_, verbose);
2347 }
2348 
2358 void
2360  const vpHomogeneousMatrix& cMo_, const bool verbose)
2361 {
2362  this->cMo.eye();
2363  vpMbtDistanceLine *l;
2365  vpMbtDistanceCircle *ci;
2366 
2367  for (unsigned int i = 0; i < scales.size(); i += 1){
2368  if(scales[i]){
2369  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
2370  l = *it;
2371  if (l!=NULL) delete l ;
2372  l = NULL ;
2373  }
2374 
2375  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
2376  cy = *it;
2377  if (cy!=NULL) delete cy;
2378  cy = NULL;
2379  }
2380 
2381  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
2382  ci = *it;
2383  if (ci!=NULL) delete ci;
2384  ci = NULL;
2385  }
2386 
2387  lines[i].clear();
2388  cylinders[i].clear();
2389  circles[i].clear();
2390  }
2391  }
2392 
2393  faces.reset();
2394 
2395  //compute_interaction=1;
2396  nline = 0;
2397  ncylinder = 0;
2398  ncircle = 0;
2399  //lambda = 1;
2400  nbvisiblepolygone = 0;
2401 
2402  loadModel(cad_name, verbose);
2403  initFromPose(I, cMo_);
2404 }
2405 
2416 unsigned int
2417 vpMbEdgeTracker::getNbPoints(const unsigned int level) const
2418 {
2419  if((level > scales.size()) || !scales[level]){
2420  throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used", level);
2421  }
2422 
2423  unsigned int nbGoodPoints = 0;
2424  vpMbtDistanceLine *l ;
2425  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[level].begin(); it!=lines[level].end(); ++it){
2426  l = *it;
2427  if (l->isVisible() && l->isTracked())
2428  {
2429  for(unsigned int a = 0 ; a < l->meline.size() ; a++){
2430  if(l->nbFeature[a] != 0)
2431  for(std::list<vpMeSite>::const_iterator itme=l->meline[a]->getMeList().begin(); itme!=l->meline[a]->getMeList().end(); ++itme){
2432  if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
2433  }
2434  }
2435  }
2436  }
2437 
2438  vpMbtDistanceCylinder *cy ;
2439  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[level].begin(); it!=cylinders[level].end(); ++it){
2440  cy = *it;
2441  if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL))
2442  {
2443  for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
2444  if (itme1->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
2445  }
2446  for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
2447  if (itme2->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
2448  }
2449  }
2450  }
2451 
2452  vpMbtDistanceCircle *ci ;
2453  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[level].begin(); it!=circles[level].end(); ++it){
2454  ci = *it;
2455  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL)
2456  {
2457  for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
2458  if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
2459  }
2460  }
2461  }
2462 
2463  return nbGoodPoints;
2464 }
2465 
2486 void
2487 vpMbEdgeTracker::setScales(const std::vector<bool>& scale)
2488 {
2489  unsigned int nbActivatedLevels = 0;
2490  for (unsigned int i = 0; i < scale.size(); i += 1){
2491  if(scale[i]){
2492  nbActivatedLevels++;
2493  }
2494  }
2495  if((scale.size() < 1) || (nbActivatedLevels == 0)){
2496  vpERROR_TRACE(" !! WARNING : must use at least one level for the tracking. Use the global one");
2497  this->scales.resize(0);
2498  this->scales.push_back(true);
2499  lines.resize(1);
2500  lines[0].clear();
2501  cylinders.resize(1);
2502  cylinders[0].clear();
2503  }
2504  else{
2505  this->scales = scale;
2506  lines.resize(scale.size());
2507  cylinders.resize(scale.size());
2508  for (unsigned int i = 0; i < lines.size(); i += 1){
2509  lines[i].clear();
2510  cylinders[i].clear();
2511  }
2512  }
2513 }
2514 
2520 void
2522 {
2523  if( (clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
2524  vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
2525  else if ( dist < 0 )
2526  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
2527  else{
2529  vpMbtDistanceLine *l;
2530 
2531  for (unsigned int i = 0; i < scales.size(); i += 1){
2532  if(scales[i]){
2533  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
2534  l = *it;
2536  }
2537  }
2538  }
2539  }
2540 }
2541 
2547 void
2549 {
2550  if( (clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
2551  vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
2552  else if ( dist < 0 )
2553  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
2554  else{
2556  vpMbtDistanceLine *l;
2557 
2558  for (unsigned int i = 0; i < scales.size(); i += 1){
2559  if(scales[i]){
2560  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
2561  l = *it;
2563  }
2564  }
2565  }
2566  }
2567 }
2568 
2576 void
2577 vpMbEdgeTracker::setClipping(const unsigned int &flags)
2578 {
2579  vpMbTracker::setClipping(flags);
2580 
2581  vpMbtDistanceLine *l;
2582 
2583  for (unsigned int i = 0; i < scales.size(); i += 1){
2584  if(scales[i]){
2585  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
2586  l = *it;
2588  }
2589  }
2590  }
2591 }
2592 
2607 void
2609 {
2610  _pyramid.resize(scales.size());
2611 
2612  if(scales[0]){
2613  _pyramid[0] = &_I;
2614  }
2615  else{
2616  _pyramid[0] = NULL;
2617  }
2618 
2619  for(unsigned int i=1; i<_pyramid.size(); i += 1){
2620  if(scales[i]){
2621  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2622  vpImage<unsigned char>* I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2623 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2624  IplImage* vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
2625  vpI0->imageData = (char*)(_I.bitmap);
2626  IplImage* vpI = cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
2627  cvResize(vpI0, vpI, CV_INTER_NN);
2628  vpImageConvert::convert(vpI, *I);
2629  cvReleaseImage(&vpI);
2630  vpI0->imageData = NULL;
2631  cvReleaseImageHeader(&vpI0);
2632 #else
2633  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale){
2634  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale){
2635  (*I)[k][l] = _I[ii][jj];
2636  }
2637  }
2638 #endif
2639  _pyramid[i] = I;
2640  }
2641  else{
2642  _pyramid[i] = NULL;
2643  }
2644  }
2645 }
2646 
2653 void
2655 {
2656  if(_pyramid.size() > 0){
2657  _pyramid[0] = NULL;
2658  for (unsigned int i = 1; i < _pyramid.size(); i += 1){
2659  if(_pyramid[i] != NULL){
2660  delete _pyramid[i];
2661  _pyramid[i] = NULL;
2662  }
2663  }
2664  _pyramid.resize(0);
2665  }
2666 }
2667 
2678 void
2679 vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level)
2680 {
2681  if(level > scales.size() || !scales[level]){
2682  std::ostringstream oss;
2683  oss << level;
2684  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2685  throw vpException(vpException::dimensionError, errorMsg);
2686  }
2687 
2688  linesList = lines[level];
2689 }
2690 
2691 
2702 void
2703 vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level)
2704 {
2705  if(level > scales.size() || !scales[level]){
2706  std::ostringstream oss;
2707  oss << level;
2708  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2709  throw vpException(vpException::dimensionError, errorMsg);
2710  }
2711 
2712  cylindersList = cylinders[level];
2713 }
2714 
2715 
2726 void
2727 vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level)
2728 {
2729  if(level > scales.size() || !scales[level]){
2730  std::ostringstream oss;
2731  oss << level;
2732  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2733  throw vpException(vpException::dimensionError, errorMsg);
2734  }
2735 
2736  circlesList = circles[level];
2737 }
2738 
2745 void
2746 vpMbEdgeTracker::downScale(const unsigned int _scale)
2747 {
2748  const double ratio = pow(2., (int)_scale);
2749  scaleLevel = _scale;
2750 
2751  vpMatrix K = cam.get_K();
2752 
2753  K[0][0] /= ratio;
2754  K[1][1] /= ratio;
2755  K[0][2] /= ratio;
2756  K[1][2] /= ratio;
2757 
2759 }
2760 
2767 void
2768 vpMbEdgeTracker::upScale(const unsigned int _scale)
2769 {
2770  const double ratio = pow(2., (int)_scale);
2771  scaleLevel = 0;
2772 
2773  vpMatrix K = cam.get_K();
2774 
2775  K[0][0] *= ratio;
2776  K[1][1] *= ratio;
2777  K[0][2] *= ratio;
2778  K[1][2] *= ratio;
2779 
2780 
2782 }
2783 
2791 void
2792 vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2793 {
2794  unsigned int scaleLevel_1 = scaleLevel;
2795  scaleLevel = _lvl;
2796 
2797  vpMbtDistanceLine *l;
2798  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
2799  if((*it)->isTracked()){
2800  l = *it;
2801  l->reinitMovingEdge(*Ipyramid[_lvl], cMo);
2802  }
2803  }
2804 
2806  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
2807  if((*it)->isTracked()){
2808  cy = *it;
2809  cy->reinitMovingEdge(*Ipyramid[_lvl], cMo);
2810  }
2811  }
2812 
2813  vpMbtDistanceCircle *ci;
2814  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
2815  if((*it)->isTracked()){
2816  ci = *it;
2817  ci->reinitMovingEdge(*Ipyramid[_lvl], cMo);
2818  }
2819  }
2820 
2821  trackMovingEdge(*Ipyramid[_lvl]);
2822  updateMovingEdge(*Ipyramid[_lvl]);
2823  scaleLevel = scaleLevel_1;
2824 }
2825 
2832 void
2833 vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2834 {
2835  for (unsigned int i = 0; i < scales.size(); i += 1){
2836  if(scales[i]){
2837  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
2838  /*(*it)->setTracked(useEdgeTracking);
2839  for(std::list<int>::const_iterator itpoly=(*it)->Lindex_polygon.begin(); itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2840  if(faces[(*itpoly)]->getName() != name){
2841  (*it)->setTracked(true);
2842  break;
2843  }
2844  }*/
2845 
2846  (*it)->setTracked(name,useEdgeTracking);
2847  }
2848 
2849  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
2850  if(faces[(unsigned)(*it)->index_polygon]->getName() == name){
2851  (*it)->setTracked(useEdgeTracking);
2852  }
2853  }
2854 
2855  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
2856  if(faces[(unsigned)(*it)->index_polygon]->getName() == name){
2857  (*it)->setTracked(useEdgeTracking);
2858  }
2859  }
2860  }
2861  }
2862 }
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:263
bool computeProjError
Flag used to specify if the gradient error criteria has to be computed or not.
Definition: vpMbTracker.h:131
unsigned int ncylinder
Index of the cylinder to add, and total number of cylinders extracted so far.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
void displayMovingEdges(const vpImage< unsigned char > &I)
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:129
void initFromCalibrationMatrix(const vpMatrix &_K)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setMovingEdge(const vpMe &me)
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setCameraParameters(const vpCameraParameters &camera)
void upScale(const unsigned int _scale)
std::string getName() const
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
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:132
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
bool isAppearing(const unsigned int i)
unsigned int getWidth() const
Definition: vpImage.h:161
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void getCameraParameters(vpCameraParameters &_cam) const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:144
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(const double wmean)
void computeVVS(const vpImage< unsigned char > &_I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void init(const vpImage< unsigned char > &I)
void track(const vpImage< unsigned char > &I)
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:116
vpMatrix L
The interaction matrix.
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:196
void setCameraParameters(const vpCameraParameters &camera)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:72
#define vpERROR_TRACE
Definition: vpDebug.h:391
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
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:115
vpPoint * p1
The first extremity.
void displayMovingEdges(const vpImage< unsigned char > &I)
double getNearClippingDistance() const
void setOgreShowConfigDialog(const bool showConfigDialog)
virtual ~vpMbEdgeTracker()
unsigned int ncircle
Index of the circle to add, and total number of circles extracted so far.
vpColVector error
The error vector.
bool modelInitialised
Flag used to ensure that the CAD model is loaded before the initialisation.
Definition: vpMbTracker.h:123
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:449
void getMe(vpMe &_ecm) const
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void reInitLevel(const unsigned int _lvl)
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
Definition: vpMe.h:59
Manage the line of a polygon used in the model-based tracker.
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 removeCircle(const std::string &name)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:156
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
bool getLodState() const
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0)
void setIndex(const unsigned int i)
virtual void setCameraParameters(const vpCameraParameters &camera)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
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:127
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
vpPoint * p1
The first extremity on the axe.
void downScale(const unsigned int _scale)
bool hasNearClippingDistance() const
Class that defines what is a point.
Definition: vpPoint.h:59
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
Definition: vpMeSite.h:198
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:113
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:141
virtual void setNearClippingDistance(const double &dist)
double getFarClippingDistance() const
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:152
double projectionError
Error angle between the gradient direction of the model features projected at the resulting pose and ...
Definition: vpMbTracker.h:133
bool hasFarClippingDistance() const
vpAROgre * getOgreContext()
std::vector< vpMbtMeLine * > meline
The moving edge container.
int index_polygon
Index of the face which contains the cylinder.
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:117
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
bool Reinit
Indicates if the circle has to be reinitialized.
void getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0)
Error that can be emited by the vpTracker class and its derivates.
double getAngleDisappear() const
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:64
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setName(const std::string &circle_name)
std::string getName() const
Definition: vpMbtPolygon.h:101
vpMatrix AtA() const
Definition: vpMatrix.cpp:391
vpPoint * p2
The second extremity.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:539
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:159
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:414
static double sqr(double x)
Definition: vpMath.h:110
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
void setAngleDisappear(const double &adisappear)
double getMinPolygonAreaThreshold() const
vpColVector error
The error vector.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpColVector m_error
Error s-s*.
Definition: vpMbTracker.h:139
int getIndex() const
Definition: vpMbtPolygon.h:94
bool isVisible() const
Generic class defining intrinsic camera parameters.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:451
bool isTracked() const
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)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual void setFarClippingDistance(const double &dist)
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:152
Implementation of a velocity twist matrix and operations on such kind of matrices.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:141
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
void setCameraParameters(const vpCameraParameters &_cam)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:146
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:185
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void displayMovingEdges(const vpImage< unsigned char > &I)
double get_px() const
std::string getName() const
static double rad(double deg)
Definition: vpMath.h:104
void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace=-1, const std::string &name="")
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void parse(const char *filename)
void setMeanWeight1(const double wmean)
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:189
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeProjectionError(const vpImage< unsigned char > &_I)
void setVisible(bool _isvisible)
bool samePoint(const vpPoint &P1, const vpPoint &P2)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
double getMinLineLengthThreshold() const
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR)
double sumSquare() const
void setScales(const std::vector< bool > &_scales)
vpPoint * p2
The second extremity on the axe.
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, const unsigned int level=0)
void setCameraParameters(const vpCameraParameters &camera)
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:447
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void setIndex(const unsigned int i)
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
vpMatrix get_K() const
static double deg(double rad)
Definition: vpMath.h:97
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:135
virtual void loadModel(const char *modelFile, const bool verbose=false)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:157
bool applyLodSettingInConfig
True if the CAO model is loaded before the call to loadConfigFile, (deduced by the number of polygons...
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
void setAngleAppear(const double &aappear)
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
vpHomogeneousMatrix inverse() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static vpHomogeneousMatrix direct(const vpColVector &v)
void setMeanWeight(const double _wmean)
bool isVisible(const unsigned int i)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:59
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:148
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)
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:309
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:205
unsigned int getHeight() const
Definition: vpImage.h:152
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:135
void setMeanWeight(const double w_mean)
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:1756
virtual void setClipping(const unsigned int &flags)
bool Reinit
Indicates if the line has to be reinitialized.
double radius
The radius of the cylinder.
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:154
unsigned int nline
Index of the polygon to add, and total number of polygon extracted so far.
void addPolygon(vpMbtPolygon &p)
void setName(const std::string line_name)
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
double getAngleAppear() const
virtual void setClipping(const unsigned int &flags)
unsigned int nbFeaturel2
The number of moving edges on line 2.
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:124
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void setName(const std::string &cyl_name)
static int sign(double x)
Definition: vpMath.h:266
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void removeLine(const std::string &name)
bool getFovClipping() const
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:118
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:150
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:173
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpMatrix L
The interaction matrix.
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace=-1, const std::string &name="")
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void eye()
Definition: vpMatrix.cpp:194
void setMovingEdge(const vpMe &_ecm)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:119
std::string getName() const
vpPoint * p1
The center of the circle.
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217
vpColVector m_w
Weights used in the robust scheme.
Definition: vpMbTracker.h:137
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)