Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpMbEdgeKltTracker.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
32  *
33  * Authors:
34  * Romain Tallonneau
35  * Aurelien Yol
36  *
37  *****************************************************************************/
38 
39 //#define VP_DEBUG_MODE 1 // Activate debug level 1
40 
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/mbt/vpMbEdgeKltTracker.h>
43 #include <visp3/core/vpTrackingException.h>
44 #include <visp3/core/vpVelocityTwistMatrix.h>
45 
46 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
47 
49  : compute_interaction(true), lambda(0.8), thresholdKLT(2.), thresholdMBT(2.), maxIter(200)
50 {
51  computeCovariance = false;
52 
54 
57 
58 #ifdef VISP_HAVE_OGRE
59  faces.getOgreContext()->setWindowName("MBT Hybrid");
60 #endif
61 }
62 
68 {
69 }
70 
77 void
79 {
81 
83 
85 
86  unsigned int i = (unsigned int)scales.size();
87  do {
88  i--;
89  if(scales[i]){
90  downScale(i);
92  upScale(i);
93  }
94  } while(i != 0);
95 
97 }
98 
108 void
110 {
111  vpMbKltTracker::setPose(I, cdMo);
112 
113  resetMovingEdge();
114 
115  if(useScanLine){
116  cam.computeFov(I.getWidth(), I.getHeight());
119  }
120 
121  initPyramid(I, Ipyramid);
122 
123  unsigned int i = (unsigned int)scales.size();
124  do {
125  i--;
126  if(scales[i]){
127  downScale(i);
129  upScale(i);
130  }
131  } while(i != 0);
132 
134 }
135 
140 void
142 {
145 }
146 
147 unsigned int
148 vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
149 {
150  vpMbtDistanceLine *l ;
152  vpMbtDistanceCircle *ci ;
153 
154  if(lvl >= scales.size() || !scales[lvl]){
155  throw vpException(vpException::dimensionError, "lvl not used.");
156  }
157 
158  unsigned int nbrow = 0;
159  for(std::list<vpMbtDistanceLine*>::iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
160  l = *it;
161 
162  if(l->isVisible() && l->isTracked()){
163  nbrow += l->nbFeatureTotal ;
165  }
166  }
167 
168  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
169  cy = *it;
170 
171  if(cy->isVisible() && cy->isTracked()){
172  nbrow += cy->nbFeature ;
174  }
175  }
176 
177  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
178  ci = *it;
179 
180  if(ci->isVisible() && ci ->isTracked()){
181  nbrow += ci->nbFeature ;
183  }
184  }
185 
186  return nbrow;
187 }
188 
200 void
201 vpMbEdgeKltTracker::loadConfigFile(const std::string& configFile)
202 {
203  vpMbEdgeKltTracker::loadConfigFile(configFile.c_str());
204 }
205 
269 void
270 vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
271 {
272 #ifdef VISP_HAVE_XML2
274 
275  xmlp.setCameraParameters(cam);
278 
279  xmlp.setMovingEdge(me);
280 
281  xmlp.setMaxFeatures(10000);
282  xmlp.setWindowSize(5);
283  xmlp.setQuality(0.01);
284  xmlp.setMinDistance(5);
285  xmlp.setHarrisParam(0.01);
286  xmlp.setBlockSize(3);
287  xmlp.setPyramidLevels(3);
289 
290  try{
291  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
292  xmlp.parse(configFile);
293  }
294  catch(...){
295  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
296  throw vpException(vpException::ioError, "problem to parse configuration file.");
297  }
298 
299  vpCameraParameters camera;
300  xmlp.getCameraParameters(camera);
301  setCameraParameters(camera);
302 
305 
306  if(xmlp.hasNearClippingDistance())
308 
309  if(xmlp.hasFarClippingDistance())
311 
312  if(xmlp.getFovClipping()){
314  }
315 
316  useLodGeneral = xmlp.getLodState();
319 
320  applyLodSettingInConfig = false;
321  if(this->getNbPolygon() > 0) {
326  }
327 
328  vpMe meParser;
329  xmlp.getMe(meParser);
331 
333  tracker.setWindowSize((int)xmlp.getWindowSize());
334  tracker.setQuality(xmlp.getQuality());
337  tracker.setBlockSize((int)xmlp.getBlockSize());
339  maskBorder = xmlp.getMaskBorder();
340 
341  //if(useScanLine)
342  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
343 
344 #else
345  vpTRACE("You need the libXML2 to read the config file %s", configFile);
346 #endif
347 }
348 
352 bool
354  const unsigned int lvl)
355 {
356 
357  postTrackingMbt(w_mbt,lvl);
358 
359  if (displayFeatures)
360  {
361  if(lvl == 0){
363  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
364  l = *it;
365  if (l->isVisible() && l->isTracked()){
366  l->displayMovingEdges(I);
367  }
368  }
369 
371  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
372  cy = *it;
373  // A cylinder is always visible: #FIXME AY: Still valid?
374  if(cy->isTracked())
375  cy->displayMovingEdges(I);
376  }
377 
378  vpMbtDistanceCircle *ci ;
379  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
380  ci = *it;
381  if (ci->isVisible() && ci->isTracked()){
382  ci->displayMovingEdges(I);
383  }
384  }
385  }
386  }
387 
388  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
389 
390  if(useScanLine){
391  cam.computeFov(I.getWidth(), I.getHeight());
394  }
395 
397 
400 
401  if(computeProjError)
403 
404  if(reInit)
405  return true;
406 
407  return false;
408 }
409 
420 void
422 {
423  if(lvl >= scales.size() || !scales[lvl]){
424  throw vpException(vpException::dimensionError, "_lvl not used.");
425  }
426 
427  unsigned int n =0 ;
429  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
430  if((*it)->isTracked()){
431  l = *it;
432  unsigned int indexLine = 0;
433  double wmean = 0 ;
434 
435  for(unsigned int a = 0 ; a < l->meline.size() ; a++)
436  {
437  std::list<vpMeSite>::iterator itListLine;
438  if (l->nbFeature[a] > 0) itListLine = l->meline[a]->getMeList().begin();
439 
440  for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
441  wmean += w[n+indexLine] ;
442  vpMeSite p = *itListLine;
443  if (w[n+indexLine] < 0.5){
445  *itListLine = p;
446  }
447 
448  ++itListLine;
449  indexLine++;
450  }
451  }
452 
453  n+= l->nbFeatureTotal ;
454 
455  if (l->nbFeatureTotal!=0)
456  wmean /= l->nbFeatureTotal ;
457  else
458  wmean = 1;
459 
460  l->setMeanWeight(wmean);
461 
462  if (wmean < 0.8)
463  l->Reinit = true;
464  }
465  }
466 
467  // Same thing with cylinders as with lines
469  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
470  if((*it)->isTracked()){
471  cy = *it;
472  double wmean = 0 ;
473  std::list<vpMeSite>::iterator itListCyl1;
474  std::list<vpMeSite>::iterator itListCyl2;
475  if (cy->nbFeature > 0){
476  itListCyl1 = cy->meline1->getMeList().begin();
477  itListCyl2 = cy->meline2->getMeList().begin();
478  }
479 
480  wmean = 0;
481  for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
482  wmean += w[n+i] ;
483  vpMeSite p = *itListCyl1;
484  if (w[n+i] < 0.5){
486 
487  *itListCyl1 = p;
488  }
489 
490  ++itListCyl1;
491  }
492 
493  if (cy->nbFeaturel1!=0)
494  wmean /= cy->nbFeaturel1 ;
495  else
496  wmean = 1;
497 
498  cy->setMeanWeight1(wmean);
499 
500  if (wmean < 0.8){
501  cy->Reinit = true;
502  }
503 
504  wmean = 0;
505  for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
506  wmean += w[n+i] ;
507  vpMeSite p = *itListCyl2;
508  if (w[n+i] < 0.5){
510 
511  *itListCyl2 = p;
512  }
513 
514  ++itListCyl2;
515  }
516 
517  if (cy->nbFeaturel2!=0)
518  wmean /= cy->nbFeaturel2 ;
519  else
520  wmean = 1;
521 
522  cy->setMeanWeight2(wmean);
523 
524  if (wmean < 0.8){
525  cy->Reinit = true;
526  }
527 
528  n+= cy->nbFeature ;
529  }
530  }
531 
532  // Same thing with circles as with lines
534  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
535  if((*it)->isTracked()){
536  ci = *it;
537  double wmean = 0 ;
538  std::list<vpMeSite>::iterator itListCir;
539 
540  if (ci->nbFeature > 0){
541  itListCir = ci->meEllipse->getMeList().begin();
542  }
543 
544  wmean = 0;
545  for(unsigned int i=0 ; i < ci->nbFeature ; i++){
546  wmean += w[n+i] ;
547  vpMeSite p = *itListCir;
548  if (w[n+i] < 0.5){
550 
551  *itListCir = p;
552  }
553 
554  ++itListCir;
555  }
556 
557  if (ci->nbFeature!=0)
558  wmean /= ci->nbFeature ;
559  else
560  wmean = 1;
561 
562  ci->setMeanWeight(wmean);
563 
564  if (wmean < 0.8){
565  ci->Reinit = true;
566  }
567 
568  n+= ci->nbFeature ;
569  }
570  }
571 }
572 
582 void
583 vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl)
584 {
585  vpColVector factor;
586  unsigned int nbrow = trackFirstLoop(I, factor, lvl);
587 
588  if(nbrow < 4 && nbInfos < 4){
589  vpERROR_TRACE("\n\t\t Error-> not enough data") ;
590  throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
591  }
592  else if(nbrow < 4)
593  nbrow = 0;
594 
595  double residu = 0;
596  double residu_1 = -1;
597  unsigned int iter = 0;
598 
599  vpMatrix *L;
600  vpMatrix L_mbt, L_klt; // interaction matrix
601  vpColVector *R;
602  vpColVector R_mbt, R_klt; // residu
603  vpMatrix L_true;
604  vpMatrix LVJ_true;
605  //vpColVector R_true;
606  vpColVector w_true;
607 
608  if(nbrow != 0){
609  L_mbt.resize(nbrow,6);
610  R_mbt.resize(nbrow);
611  }
612 
613  if(nbInfos != 0){
614  L_klt.resize(2*nbInfos,6);
615  R_klt.resize(2*nbInfos);
616  }
617 
618  //vpColVector w; // weight from MEstimator
619  vpColVector v; // "speed" for VVS
620  vpRobust robust_mbt(0), robust_klt(0);
621  vpHomography H;
622 
623  vpMatrix LTL;
624  vpColVector LTR;
625 
626  double factorMBT = 1.0;
627  double factorKLT = 1.0;
628 
629  //More efficient weight repartition for hybrid tracker should come soon...
630 // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
631 // factorKLT = 1.0 - factorMBT;
632  factorMBT = 0.35;
633  factorKLT = 0.65;
634 
635  if (nbrow < 4)
636  factorKLT = 1.;
637  if (nbInfos < 4)
638  factorMBT = 1.;
639 
640  double residuMBT = 0;
641  double residuKLT = 0;
642 
643  vpHomogeneousMatrix cMoPrev;
644  vpHomogeneousMatrix ctTc0_Prev;
645  vpColVector m_error_prev(2*nbInfos + nbrow);
646  vpColVector m_w_prev(2*nbInfos + nbrow);
647  double mu = 0.01;
648 
649  while( ((int)((residu - residu_1)*1e8) !=0 ) && (iter<maxIter) ){
650  L = new vpMatrix();
651  R = new vpColVector();
652 
653  if(nbrow >= 4)
654  trackSecondLoop(I,L_mbt,R_mbt,cMo,lvl);
655 
656  if(nbInfos >= 4){
657  unsigned int shift = 0;
658  vpMbtDistanceKltPoints *kltpoly;
659  // for (unsigned int i = 0; i < faces.size(); i += 1){
660  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=vpMbKltTracker::kltPolygons.begin(); it!=vpMbKltTracker::kltPolygons.end(); ++it){
661  kltpoly = *it;
662  if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()){
663  vpSubColVector subR(R_klt, shift, 2*kltpoly->getCurrentNumberPoints());
664  vpSubMatrix subL(L_klt, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
665  kltpoly->computeHomography(ctTc0, H);
666  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
667  shift += 2*kltpoly->getCurrentNumberPoints();
668  }
669  }
670 
671  vpMbtDistanceKltCylinder *kltPolyCylinder;
672  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
673  kltPolyCylinder = *it;
674 
675  if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
676  {
677  vpSubColVector subR(R_klt, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
678  vpSubMatrix subL(L_klt, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
679  try{
680  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0,subR, subL);
681  }catch(...){
682  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
683  }
684 
685  shift += 2*kltPolyCylinder->getCurrentNumberPoints();
686  }
687  }
688  }
689 
690  bool reStartFromLastIncrement = false;
692  if(m_error.sumSquare()/(double)(2*nbInfos + nbrow) > m_error_prev.sumSquare()/(double)(2*nbInfos + nbrow)){
693  mu *= 10.0;
694 
695  if(mu > 1.0)
696  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
697 
698  cMo = cMoPrev;
699  m_error = m_error_prev;
700  m_w = m_w_prev;
701  ctTc0 = ctTc0_Prev;
702  reStartFromLastIncrement = true;
703  }
704  }
705 
706  if(!reStartFromLastIncrement){
707  if(iter == 0){
708  m_w.resize(nbrow + 2*nbInfos);
709  m_w=1;
710 
711  if(nbrow != 0){
712  w_mbt.resize(nbrow);
713  w_mbt = 1;
714  robust_mbt.resize(nbrow);
715  }
716 
717  if(nbInfos != 0){
718  w_klt.resize(2*nbInfos);
719  w_klt = 1;
720  robust_klt.resize(2*nbInfos);
721  }
722 
723  w_true.resize(nbrow + 2*nbInfos);
724  }
725 
726  /* robust */
727  if(nbrow > 3){
728  residuMBT = 0;
729  for(unsigned int i = 0; i < R_mbt.getRows(); i++)
730  residuMBT += fabs(R_mbt[i]);
731  residuMBT /= R_mbt.getRows();
732 
733  robust_mbt.setIteration(iter);
734  robust_mbt.setThreshold(thresholdMBT/cam.get_px());
735  robust_mbt.MEstimator( vpRobust::TUKEY, R_mbt, w_mbt);
736  L->stack(L_mbt);
737  R->stack(R_mbt);
738  }
739 
740  if(nbInfos > 3){
741  residuKLT = 0;
742  for(unsigned int i = 0; i < R_klt.getRows(); i++)
743  residuKLT += fabs(R_klt[i]);
744  residuKLT /= R_klt.getRows();
745 
746  robust_klt.setIteration(iter);
747  robust_klt.setThreshold(thresholdKLT/cam.get_px());
748  robust_klt.MEstimator( vpRobust::TUKEY, R_klt, w_klt);
749 
750  L->stack(L_klt);
751  R->stack(R_klt);
752  }
753 
754  unsigned int cpt = 0;
755  while(cpt< (nbrow+2*nbInfos)){
756  if(cpt<(unsigned)nbrow){
757  m_w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT) ;
758  }
759  else
760  m_w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
761  cpt++;
762  }
763 
764  m_error = (*R);
765  if(computeCovariance){
766  L_true = (*L);
767  if(!isoJoIdentity){
769  cVo.buildFrom(cMo);
770  LVJ_true = ((*L)*cVo*oJo);
771  }
772  }
773 
774  residu_1 = residu;
775  residu = 0;
776  double num = 0;
777  double den = 0;
778  for (unsigned int i = 0; i < static_cast<unsigned int>(R->getRows()); i++){
779  num += m_w[i]*vpMath::sqr((*R)[i]);
780  den += m_w[i];
781 
782  w_true[i] = m_w[i];
783  (*R)[i] *= m_w[i];
785  for (unsigned int j = 0; j < 6; j += 1){
786  (*L)[i][j] *= m_w[i];
787  }
788  }
789  }
790 
791  residu = sqrt(num/den);
792 
793  if(isoJoIdentity){
794  LTL = L->AtA();
795  computeJTR(*L, *R, LTR);
796 
797  switch(m_optimizationMethod){
799  {
800  vpMatrix LMA(LTL.getRows(), LTL.getCols());
801  LMA.eye();
802  vpMatrix LTLmuI = LTL + (LMA*mu);
803  v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
804 
805  if(iter != 0)
806  mu /= 10.0;
807 
808  m_error_prev = m_error;
809  m_w_prev = m_w;
810  break;
811  }
813  default:
814  v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
815  }
816  }
817  else{
819  cVo.buildFrom(cMo);
820  vpMatrix LVJ = ((*L)*cVo*oJo);
821  vpMatrix LVJTLVJ = (LVJ).AtA();
822  vpColVector LVJTR;
823  computeJTR(LVJ, *R, LVJTR);
824 
825  switch(m_optimizationMethod){
827  {
828  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
829  LMA.eye();
830  vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
831  v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
832  v = cVo * v;
833 
834  if(iter != 0)
835  mu /= 10.0;
836 
837  m_error_prev = m_error;
838  m_w_prev = m_w;
839  break;
840  }
842  default:
843  {
844  v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
845  v = cVo * v;
846  break;
847  }
848  }
849  }
850 
851  cMoPrev = cMo;
852  ctTc0_Prev = ctTc0;
854  cMo = ctTc0 * c0Mo;
855  }
856 
857  iter++;
858 
859  delete L;
860  delete R;
861  }
862 
863  if(computeCovariance){
864  vpMatrix D;
865  D.diag(w_true);
866 
867  // Note that here the covariance is computed on cMoPrev for time computation efficiency
868  if(isoJoIdentity){
870  }
871  else{
873  }
874  }
875 }
876 
884 void
886 {
887  unsigned int nbInfos = 0;
888  unsigned int nbFaceUsed = 0;
889  vpColVector w_klt;
890 
891  try{
892  vpMbKltTracker::preTracking(I, nbInfos, nbFaceUsed);
893  }
894  catch(...){}
895 
896  if(nbInfos >= 4)
897  vpMbKltTracker::computeVVS(nbInfos, w_klt);
898  else{
899  nbInfos = 0;
900  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
901  }
902 
904 
905  vpColVector w_mbt;
906  computeVVS(I, nbInfos, w_mbt, w_klt);
907 
908  if(postTracking(I, w_mbt, w_klt)){
910 
911  // AY : Removed as edge tracked, if necessary, is reinitialized in postTracking()
912 
913 // initPyramid(I, Ipyramid);
914 
915 // unsigned int i = (unsigned int)scales.size();
916 // do {
917 // i--;
918 // if(scales[i]){
919 // downScale(i);
920 // initMovingEdge(*Ipyramid[i], cMo);
921 // upScale(i);
922 // }
923 // } while(i != 0);
924 
925 // cleanPyramid(Ipyramid);
926  }
927 }
928 
929 unsigned int
931 {
932  vpMbtDistanceLine *l ;
934  vpMbtDistanceCircle *ci ;
935 
936  if(lvl >= scales.size() || !scales[lvl]){
937  throw vpException(vpException::dimensionError, "_lvl not used.");
938  }
939 
940  unsigned int nbrow = initMbtTracking(lvl);
941 
942  if (nbrow==0){
943 // vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
944 // throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
945  return nbrow;
946  }
947 
948  factor.resize(nbrow);
949  factor = 1;
950 
951  unsigned int n = 0;
952  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
953  if((*it)->isTracked()){
954  l = *it;
956 
957  double fac = 1;
958  for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
959  int index = *itindex;
960  if (l->hiddenface->isAppearing((unsigned int)index)) {
961  fac = 0.2;
962  break;
963  }
964  if(l->closeToImageBorder(I, 10)){
965  fac = 0.1;
966  break;
967  }
968  }
969 
970  unsigned int indexFeature = 0;
971  for(unsigned int a = 0 ; a < l->meline.size(); a++){
972  std::list<vpMeSite>::const_iterator itListLine;
973  if (l->meline[a] != NULL)
974  {
975  itListLine = l->meline[a]->getMeList().begin();
976 
977  for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
978  factor[n+i] = fac;
979  vpMeSite site = *itListLine;
980  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
981  ++itListLine;
982  indexFeature++;
983  }
984  n+= l->nbFeature[a] ;
985  }
986  }
987  }
988  }
989 
990  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
991  if((*it)->isTracked()){
992  cy = *it;
994  double fac = 1.0;
995 
996  std::list<vpMeSite>::const_iterator itCyl1;
997  std::list<vpMeSite>::const_iterator itCyl2;
998  if ((cy->meline1 != NULL || cy->meline2 != NULL)){
999  itCyl1 = cy->meline1->getMeList().begin();
1000  itCyl2 = cy->meline2->getMeList().begin();
1001  }
1002 
1003  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
1004  factor[n+i] = fac;
1005  vpMeSite site;
1006  if(i<cy->nbFeaturel1) {
1007  site= *itCyl1;
1008  ++itCyl1;
1009  }
1010  else{
1011  site= *itCyl2;
1012  ++itCyl2;
1013  }
1014  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
1015  }
1016 
1017  n+= cy->nbFeature ;
1018  }
1019  }
1020 
1021  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
1022  if((*it)->isTracked()){
1023  ci = *it;
1025  double fac = 1.0;
1026 
1027  std::list<vpMeSite>::const_iterator itCir;
1028  if (ci->meEllipse != NULL) {
1029  itCir = ci->meEllipse->getMeList().begin();
1030  }
1031 
1032  for(unsigned int i=0 ; i < ci->nbFeature ; i++){
1033  factor[n+i] = fac;
1034  vpMeSite site = *itCir;
1035  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
1036  ++itCir;
1037  }
1038 
1039  n+= ci->nbFeature ;
1040  }
1041  }
1042 
1043  return nbrow;
1044 }
1045 
1046 void
1048  vpHomogeneousMatrix& cMo_, const unsigned int lvl)
1049 {
1050  vpMbtDistanceLine* l;
1051  vpMbtDistanceCylinder *cy ;
1052  vpMbtDistanceCircle *ci ;
1053 
1054  unsigned int n = 0 ;
1055  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
1056  if((*it)->isTracked()){
1057  l = *it;
1059  for (unsigned int i=0 ; i < l->nbFeatureTotal ; i++){
1060  for (unsigned int j=0; j < 6 ; j++){
1061  L[n+i][j] = l->L[i][j];
1062  error[n+i] = l->error[i];
1063  }
1064  }
1065  n+= l->nbFeatureTotal;
1066  }
1067  }
1068 
1069  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
1070  if((*it)->isTracked()){
1071  cy = *it;
1072  cy->computeInteractionMatrixError(cMo_, I) ;
1073  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
1074  for(unsigned int j=0; j < 6 ; j++){
1075  L[n+i][j] = cy->L[i][j];
1076  error[n+i] = cy->error[i];
1077  }
1078  }
1079  n+= cy->nbFeature ;
1080  }
1081  }
1082  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
1083  if((*it)->isTracked()){
1084  ci = *it;
1085  ci->computeInteractionMatrixError(cMo_) ;
1086  for(unsigned int i=0 ; i < ci->nbFeature ; i++){
1087  for(unsigned int j=0; j < 6 ; j++){
1088  L[n+i][j] = ci->L[i][j];
1089  error[n+i] = ci->error[i];
1090  }
1091  }
1092 
1093  n+= ci->nbFeature ;
1094  }
1095  }
1096 }
1097 
1103 void
1105 {
1106  this->cam = camera;
1107 
1110 }
1111 
1117 void
1119 {
1122 }
1128 void
1130 {
1133 }
1134 
1146 void
1147 vpMbEdgeKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
1148  const int idFace, const std::string &name)
1149 {
1150  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1151 }
1152 
1163 void
1164 vpMbEdgeKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
1165  const std::string &name)
1166 {
1167  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1168  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1169 }
1170 
1181 void
1183  const vpColor& col, const unsigned int thickness, const bool displayFullModel)
1184 {
1185  for (unsigned int i = 0; i < scales.size(); i += 1){
1186  if(scales[i]){
1187  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1188  (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
1189  }
1190 
1191  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1192  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1193  }
1194 
1195  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1196  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1197  }
1198 
1199  break ; //displaying model on one scale only
1200  }
1201  }
1202 
1203  vpMbtDistanceKltPoints *kltpoly;
1204  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1205  kltpoly = *it;
1206  if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
1207  kltpoly->displayPrimitive(I);
1208  }
1209  }
1210 
1211  vpMbtDistanceKltCylinder *kltPolyCylinder;
1212  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
1213  kltPolyCylinder = *it;
1214  if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1215  kltPolyCylinder->displayPrimitive(I);
1216  }
1217 
1218 #ifdef VISP_HAVE_OGRE
1219  if(useOgre)
1220  faces.displayOgre(cMo_);
1221 #endif
1222 }
1223 
1234 void
1236  const vpColor& col , const unsigned int thickness, const bool displayFullModel)
1237 {
1238  for (unsigned int i = 0; i < scales.size(); i += 1){
1239  if(scales[i]){
1240  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
1241  (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
1242  }
1243 
1244  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
1245  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1246  }
1247 
1248  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
1249  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1250  }
1251 
1252  break ; //displaying model on one scale only
1253  }
1254  }
1255 
1256  vpMbtDistanceKltPoints *kltpoly;
1257  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1258  kltpoly = *it;
1259  if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
1260  kltpoly->displayPrimitive(I);
1261  }
1262  }
1263 
1264  vpMbtDistanceKltCylinder *kltPolyCylinder;
1265  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
1266  kltPolyCylinder = *it;
1267  if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1268  kltPolyCylinder->displayPrimitive(I);
1269  }
1270 
1271 #ifdef VISP_HAVE_OGRE
1272  if(useOgre)
1273  faces.displayOgre(cMo_);
1274 #endif
1275 }
1276 
1286 void
1287 vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
1288  const vpHomogeneousMatrix& cMo_, const bool verbose)
1289 {
1290  reInitModel(I, cad_name.c_str(), cMo_, verbose);
1291 }
1292 
1302 void
1304  const vpHomogeneousMatrix& cMo_, const bool verbose)
1305 {
1306  // Reinit klt
1307 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1308  if(cur != NULL){
1309  cvReleaseImage(&cur);
1310  cur = NULL;
1311  }
1312 #endif
1313 
1314  // delete the Klt Polygon features
1315  vpMbtDistanceKltPoints *kltpoly;
1316  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1317  kltpoly = *it;
1318  if (kltpoly!=NULL){
1319  delete kltpoly ;
1320  }
1321  kltpoly = NULL ;
1322  }
1323  kltPolygons.clear();
1324 
1325  vpMbtDistanceKltCylinder *kltPolyCylinder;
1326  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
1327  kltPolyCylinder = *it;
1328  if (kltPolyCylinder!=NULL){
1329  delete kltPolyCylinder ;
1330  }
1331  kltPolyCylinder = NULL ;
1332  }
1333  kltCylinders.clear();
1334 
1335  // delete the structures used to display circles
1336  vpMbtDistanceCircle *ci;
1337  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
1338  ci = *it;
1339  if (ci!=NULL){
1340  delete ci ;
1341  }
1342  ci = NULL ;
1343  }
1344 
1345  circles_disp.clear();
1346 
1347  firstInitialisation = true;
1348 
1349  // Reinit edge
1350  vpMbtDistanceLine *l;
1352 
1353  for (unsigned int i = 0; i < scales.size(); i += 1){
1354  if(scales[i]){
1355  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
1356  l = *it;
1357  if (l!=NULL) delete l ;
1358  l = NULL ;
1359  }
1360 
1361  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
1362  cy = *it;
1363  if (cy!=NULL) delete cy;
1364  cy = NULL;
1365  }
1366 
1367  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
1368  ci = *it;
1369  if (ci!=NULL) delete ci;
1370  ci = NULL;
1371  }
1372 
1373  lines[i].clear();
1374  cylinders[i].clear();
1375  circles[i].clear();
1376  }
1377  }
1378 
1379  //compute_interaction=1;
1380  nline = 0;
1381  ncylinder = 0;
1382  ncircle = 0;
1383  //lambda = 1;
1384  nbvisiblepolygone = 0;
1385 
1386  // Reinit common parts
1387  faces.reset();
1388 
1389  loadModel(cad_name, verbose);
1390 
1391  this->cMo = cMo_;
1392  init(I);
1393 }
1394 
1395 #elif !defined(VISP_BUILD_SHARED_LIBS)
1396 // Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has no symbols
1397 void dummy_vpMbEdgeKltTracker() {};
1398 #endif //VISP_HAVE_OPENCV
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:266
void trackSecondLoop(const vpImage< unsigned char > &I, vpMatrix &L, vpColVector &_error, vpHomogeneousMatrix &cMo, const unsigned int lvl=0)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double r, const int idFace=0, const std::string &name="")
std::list< vpMbtDistanceKltPoints * > kltPolygons
bool computeProjError
Flag used to specify if the gradient error criteria has to be computed or not.
Definition: vpMbTracker.h:131
double lambda
The gain of the virtual visual servoing stage.
unsigned int ncylinder
Index of the cylinder to add, and total number of cylinders extracted so far.
unsigned int getMaskBorder() const
void setQuality(const double &q)
void postTrackingMbt(vpColVector &w, const unsigned int level=0)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void displayMovingEdges(const vpImage< unsigned char > &I)
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:129
void displayPrimitive(const vpImage< unsigned char > &_I)
void setMovingEdge(const vpMe &me)
double getHarrisParam() const
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void upScale(const unsigned int _scale)
void parse(const char *filename)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:182
void setHarrisFreeParameter(double harris_k)
bool isAppearing(const unsigned int i)
unsigned int getWidth() const
Definition: vpImage.h:226
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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.
vpMatrix L
The interaction matrix.
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:72
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
#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
void setMaxFeatures(const int maxCount)
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:115
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
unsigned int getBlockSize() const
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
void displayMovingEdges(const vpImage< unsigned char > &I)
double getNearClippingDistance() const
unsigned int ncircle
Index of the circle to add, and total number of circles extracted so far.
double thresholdKLT
The threshold used in the robust estimation of KLT.
vpColVector error
The error vector.
void getMe(vpMe &_ecm) const
void setMinDistance(double minDistance)
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.
vpMbScanLine & getMbScanLineRenderer()
Definition: vpMe.h:59
unsigned int getMaxFeatures() const
Manage the line of a polygon used in the model-based tracker.
unsigned int nbFeature
The number of moving edges.
virtual void setClipping(const unsigned int &flags)
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...
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
virtual void setCameraParameters(const vpCameraParameters &camera)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:62
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:127
bool firstInitialisation
Flag to specify whether the init method is called the first or not (specific calls to realize in this...
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).
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setFarClippingDistance(const double &dist)
void downScale(const unsigned int _scale)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
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)
void setQuality(double qualityLevel)
virtual void init(const vpImage< unsigned char > &I)
vpHomogeneousMatrix ctTc0
The estimated displacement of the pose between the current instant and the initial position...
double getFarClippingDistance() const
unsigned int nbFeaturel1
The number of moving edges on line 1.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
bool hasFarClippingDistance() const
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
vpAROgre * getOgreContext()
std::vector< vpMbtMeLine * > meline
The moving edge container.
void setPyramidLevels(const unsigned int &pL)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
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.
unsigned int getWindowSize() const
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 computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
vpMatrix AtA() const
Definition: vpMatrix.cpp:376
void displayPrimitive(const vpImage< unsigned char > &_I)
void setWindowSize(const unsigned int &w)
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:524
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.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
vpColVector m_error
Error s-s*.
Definition: vpMbTracker.h:139
bool isVisible() const
virtual void track(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
virtual 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)
Generic class defining intrinsic camera parameters.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
bool isTracked() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
bool compute_interaction
If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
void setHarrisParam(const double &hp)
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 setMinDistance(const double &mD)
void loadConfigFile(const char *configFile)
void setCameraParameters(const vpCameraParameters &_cam)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:146
vpKltOpencv tracker
Points tracker.
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), const vpImage< unsigned char > &I=vpImage< unsigned char >())
void setPyramidLevels(const int pyrMaxLevel)
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:185
std::list< vpMbtDistanceKltCylinder * > kltCylinders
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void displayMovingEdges(const vpImage< unsigned char > &I)
double get_px() const
static double rad(double deg)
Definition: vpMath.h:104
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void setMeanWeight1(const double wmean)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setMaxIter(const unsigned int max)
void computeProjectionError(const vpImage< unsigned char > &_I)
void preTracking(const vpImage< unsigned char > &I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
cv::Mat cur
Temporary OpenCV image for fast conversion.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
unsigned int maxIter
The maximum iteration of the virtual visual servoing stage.
double getMinLineLengthThreshold() const
void setMaxFeatures(const unsigned int &mF)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
double sumSquare() const
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 setWindowSize(const int winSize)
double getQuality() const
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
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)
virtual void initCylinder(const vpPoint &, const vpPoint &, const double r, const int idFace, const std::string &name="")
void setCameraParameters(const vpCameraParameters &cam)
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 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="")
unsigned int initMbtTracking(const unsigned int level=0)
vpHomogeneousMatrix inverse() const
unsigned int trackFirstLoop(const vpImage< unsigned char > &I, vpColVector &factor, const unsigned int lvl=0)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setBlockSize(const int blockSize)
static vpHomogeneousMatrix direct(const vpColVector &v)
void setMeanWeight(const double _wmean)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:60
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:148
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:286
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl=0)
unsigned int getHeight() const
Definition: vpImage.h:175
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:1741
bool Reinit
Indicates if the line has to be reinitialized.
virtual void init(const vpImage< unsigned char > &I)
unsigned int getCurrentNumberPoints() const
void setBlockSize(const unsigned int &bs)
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 trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
double getAngleAppear() const
unsigned int nbFeaturel2
The number of moving edges on line 2.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
unsigned int getCurrentNumberPoints() const
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:129
double getMinDistance() const
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:134
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
bool getFovClipping() const
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:123
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:173
vpMatrix L
The interaction matrix.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void computeVVS(const unsigned int &nbInfos, vpColVector &w)
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
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
void setMaskBorder(const unsigned int &mb)
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
vpHomogeneousMatrix c0Mo
Initial pose.
unsigned int getPyramidLevels() const
double thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setNearClippingDistance(const double &dist)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225
vpColVector m_w
Weights used in the robust scheme.
Definition: vpMbTracker.h:137
virtual void setLod(const bool useLod, const std::string &name="")
void computeVVS(const vpImage< unsigned char > &I, const unsigned int &nbInfos, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl=0)
void computeFov(const unsigned int &w, const unsigned int &h)