ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpMbEdgeKltTracker.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbEdgeKltTracker.cpp 4122 2013-02-08 10:45:54Z ayol $
4  *
5  * Copyright (C) 2005 - 2013 Inria. All rights reserved.
6  *
7  * This software was developed at:
8  * IRISA/INRIA Rennes
9  * Projet Lagadic
10  * Campus Universitaire de Beaulieu
11  * 35042 Rennes Cedex
12  * http://www.irisa.fr/lagadic
13  *
14  * This file is part of the ViSP toolkit
15  *
16  * This file may be distributed under the terms of the Q Public License
17  * as defined by Trolltech AS of Norway and appearing in the file
18  * LICENSE included in the packaging of this file.
19  *
20  * Licensees holding valid ViSP Professional Edition licenses may
21  * use this file in accordance with the ViSP Commercial License
22  * Agreement provided with the Software.
23  *
24  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
25  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
26  *
27  * Contact visp@irisa.fr if any conditions of this licensing are
28  * not clear to you.
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 #include <visp/vpMbEdgeKltTracker.h>
40 
41 #ifdef VISP_HAVE_OPENCV
42 
44 {
45  compute_interaction = true;
46  computeCovariance = false;
47 
48  lambda = 0.8;
49  thresholdKLT = 2.0;
50  thresholdMBT = 2.0;
51  maxIter = 200;
53 
54 #ifdef VISP_HAVE_OGRE
56 #endif
57 }
58 
64 {
65 }
66 
73 void
75 {
77 
79 
80  unsigned int n = 0;
81  for(unsigned int i = 0; i < vpMbKltTracker::faces.size() ; i++){
82  if(vpMbKltTracker::faces[i]->isVisible()){
83  vpMbEdgeTracker::faces[i]->isvisible = true;
84  n++;
85  }
86  else
87  vpMbEdgeTracker::faces[i]->isvisible = false;
88  }
90 
91  unsigned int i=scales.size();
92  do {
93  i--;
94  if(scales[i]){
95  downScale(i);
97  upScale(i);
98  }
99  } while(i != 0);
100 
102 }
103 
113 void
115 {
116  if(firstTrack){
117  vpMbKltTracker::setPose(I, cdMo);
118 
120  lines[scaleLevel].front() ;
121  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
122  l = *it;
123  if(l->meline != NULL){
124  delete l->meline;
125  l->meline = NULL;
126  }
127  }
128 
129  initPyramid(I, Ipyramid);
130 
131  unsigned int n = 0;
132  for(unsigned int i = 0; i < vpMbKltTracker::faces.size() ; i++){
133  if(vpMbKltTracker::faces[i]->isVisible()){
134  vpMbEdgeTracker::faces[i]->isvisible = true;
135  n++;
136  }
137  else
138  vpMbEdgeTracker::faces[i]->isvisible = false;
139  }
141 
142  unsigned int i=scales.size();
143  do {
144  i--;
145  if(scales[i]){
146  downScale(i);
148  upScale(i);
149  }
150  } while(i != 0);
151 
153  }
154 }
155 
160 void
162 {
165 }
166 
167 unsigned int
168 vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
169 {
170  vpMbtDistanceLine *l ;
172 
173  if(lvl >= scales.size() || !scales[lvl]){
174  throw vpException(vpException::dimensionError, "lvl not used.");
175  }
176 
177  unsigned int nbrow = 0;
178  for(std::list<vpMbtDistanceLine*>::iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
179  l = *it;
180  nbrow += l->nbFeature ;
182  }
183 
184  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
185  cy = *it;
186  nbrow += cy->nbFeature ;
188  }
189 
190  return nbrow;
191 }
192 
204 void
205 vpMbEdgeKltTracker::loadConfigFile(const std::string& configFile)
206 {
207  vpMbEdgeKltTracker::loadConfigFile(configFile.c_str());
208 }
209 
271 void
272 vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
273 {
274 #ifdef VISP_HAVE_XML2
276  vpMbKltTracker::loadConfigFile(configFile);
277 #else
278  vpTRACE("You need the libXML2 to read the config file %s", configFile);
279 #endif
280 }
281 
292 void
293 vpMbEdgeKltTracker::loadModel(const std::string& modelFile)
294 {
295  vpMbTracker::loadModel(modelFile);
296 }
297 
301 bool
303  const unsigned int lvl)
304 {
305  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
306 
307  postTrackingMbt(w_mbt,lvl);
308 
309  if (displayFeatures)
310  {
311  if(lvl == 0){
313  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
314  l = *it;
315  if (l->isVisible()){
316  l->displayMovingEdges(I);
317  }
318  }
319 
321  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
322  cy = *it;
323  cy->displayMovingEdges(I);
324  }
325  }
326  }
327 
329 
330 // bool useless = false ;
331 // vpMbEdgeTracker::visibleFace(_I, cMo, useless) ;
332  unsigned int n = 0;
333  for(unsigned int i = 0; i < vpMbKltTracker::faces.size() ; i++){
334  if(vpMbKltTracker::faces[i]->isVisible()){
335  vpMbEdgeTracker::faces[i]->isvisible = true;
336  n++;
337  }
338  else
339  vpMbEdgeTracker::faces[i]->isvisible = false;
340  }
342 
345 
346  return reInit;
347 }
348 
359 void
361 {
362 
363  if(lvl >= scales.size() || !scales[lvl]){
364  throw vpException(vpException::dimensionError, "_lvl not used.");
365  }
366  unsigned int n =0 ;
368  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
369  l = *it;
370  {
371  double wmean = 0 ;
372  std::list<vpMeSite>::iterator itListLine;
373  if (l->nbFeature > 0) itListLine = l->meline->getMeList().begin();
374 
375  for (unsigned int i=0 ; i < l->nbFeature ; i++){
376  wmean += w[n+i] ;
377  vpMeSite p = *itListLine;
378  if (w[n+i] < 0.5){
380 
381  *itListLine = p;
382  }
383 
384  ++itListLine;
385  }
386  n+= l->nbFeature ;
387 
388  if (l->nbFeature!=0)
389  wmean /= l->nbFeature ;
390  else
391  wmean = 1;
392 
393  l->setMeanWeight(wmean);
394 
395  if (wmean < 0.8)
396  l->Reinit = true;
397  }
398  }
399 
400  // Same thing with cylinders as with lines
402  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
403  cy = *it;
404  double wmean = 0 ;
405  std::list<vpMeSite>::iterator itListCyl1;
406  std::list<vpMeSite>::iterator itListCyl2;
407  if (cy->nbFeature > 0){
408  itListCyl1 = cy->meline1->getMeList().begin();
409  itListCyl2 = cy->meline2->getMeList().begin();
410  }
411 
412  wmean = 0;
413  for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
414  wmean += w[n+i] ;
415  vpMeSite p = *itListCyl1;
416  if (w[n+i] < 0.5){
418 
419  *itListCyl1 = p;
420  }
421 
422  ++itListCyl1;
423  }
424 
425  if (cy->nbFeaturel1!=0)
426  wmean /= cy->nbFeaturel1 ;
427  else
428  wmean = 1;
429 
430  cy->setMeanWeight1(wmean);
431 
432  if (wmean < 0.8){
433  cy->Reinit = true;
434  }
435 
436  wmean = 0;
437  for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
438  wmean += w[n+i] ;
439  vpMeSite p = *itListCyl2;
440  if (w[n+i] < 0.5){
442 
443  *itListCyl2 = p;
444  }
445 
446  ++itListCyl2;
447  }
448 
449  if (cy->nbFeaturel2!=0)
450  wmean /= cy->nbFeaturel2 ;
451  else
452  wmean = 1;
453 
454  cy->setMeanWeight2(wmean);
455 
456  if (wmean < 0.8){
457  cy->Reinit = true;
458  }
459 
460  n+= cy->nbFeature ;
461  }
462 }
463 
473 void
474 vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl)
475 {
476  vpColVector factor;
477  unsigned int nbrow = trackFirstLoop(I, factor, lvl);
478 
479  if(nbrow < 4 && nbInfos < 4){
480  vpERROR_TRACE("\n\t\t Error-> not enough data") ;
481  throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
482  }
483  else if(nbrow < 4)
484  nbrow = 0;
485 
486  double residu = 0;
487  double residu_1 = -1;
488  unsigned int iter = 0;
489 
490  vpMatrix *J;
491  vpMatrix J_mbt, J_klt; // interaction matrix
492  vpColVector *R;
493  vpColVector R_mbt, R_klt; // residu
494  vpMatrix J_true;
495  vpColVector R_true;
496  vpColVector w_true;
497 
498  if(nbrow != 0){
499  J_mbt.resize(nbrow,6);
500  R_mbt.resize(nbrow);
501  }
502 
503  if(nbInfos != 0){
504  J_klt.resize(2*nbInfos,6);
505  R_klt.resize(2*nbInfos);
506  }
507 
508  vpColVector w; // weight from MEstimator
509  vpColVector v; // "speed" for VVS
510  vpRobust robust_mbt(0), robust_klt(0);
511  vpHomography H;
512 
513  vpMatrix JTJ, JTR;
514 
515  double factorMBT = 1.0;
516  double factorKLT = 1.0;
517 
518  //More efficient weight repartition for hybrid tracker should come soon...
519 // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
520 // factorKLT = 1.0 - factorMBT;
521  factorMBT = 0.35;
522  factorKLT = 0.65;
523 
524  double residuMBT = 0;
525  double residuKLT = 0;
526 
527  while( ((int)((residu - residu_1)*1e8) !=0 ) && (iter<maxIter) ){
528  J = new vpMatrix();
529  R = new vpColVector();
530 
531  if(nbrow >= 4)
532  trackSecondLoop(I,J_mbt,R_mbt,cMo,lvl);
533 
534  if(nbInfos >= 4){
535  unsigned int shift = 0;
536  for (unsigned int i = 0; i < vpMbKltTracker::faces.size(); i += 1){
537  if(vpMbKltTracker::faces[i]->isVisible() && vpMbKltTracker::faces[i]->hasEnoughPoints()){
538  vpSubColVector subR(R_klt, shift, 2*vpMbKltTracker::faces[i]->getNbPointsCur());
539  vpSubMatrix subJ(J_klt, shift, 0, 2*vpMbKltTracker::faces[i]->getNbPointsCur(), 6);
540  vpMbKltTracker::faces[i]->computeHomography(ctTc0, H);
541  vpMbKltTracker::faces[i]->computeInteractionMatrixAndResidu(subR, subJ);
542  shift += 2*vpMbKltTracker::faces[i]->getNbPointsCur();
543  }
544  }
545  }
546 
547  if(iter == 0){
548  w.resize(nbrow + 2*nbInfos);
549  w=1;
550 
551  w_mbt.resize(nbrow);
552  w_mbt = 1;
553  robust_mbt.resize(nbrow);
554 
555  w_klt.resize(2*nbInfos);
556  w_klt = 1;
557  robust_klt.resize(2*nbInfos);
558 
559  w_true.resize(nbrow + 2*nbInfos);
560  }
561 
562  /* robust */
563  if(nbrow > 3){
564  residuMBT = 0;
565  for(unsigned int i = 0; i < R_mbt.getRows(); i++)
566  residuMBT += fabs(R_mbt[i]);
567  residuMBT /= R_mbt.getRows();
568 
569  robust_mbt.setIteration(iter);
570  robust_mbt.setThreshold(thresholdMBT/cam.get_px());
571  robust_mbt.MEstimator( vpRobust::TUKEY, R_mbt, w_mbt);
572  J->stackMatrices(J_mbt);
573  R->stackMatrices(R_mbt);
574  }
575 
576  if(nbInfos > 3){
577  residuKLT = 0;
578  for(unsigned int i = 0; i < R_klt.getRows(); i++)
579  residuKLT += fabs(R_klt[i]);
580  residuKLT /= R_klt.getRows();
581 
582  robust_klt.setIteration(iter);
583  robust_klt.setThreshold(thresholdKLT/cam.get_px());
584  robust_klt.MEstimator( vpRobust::TUKEY, R_klt, w_klt);
585 
586  J->stackMatrices(J_klt);
587  R->stackMatrices(R_klt);
588  }
589 
590  unsigned int cpt = 0;
591  while(cpt< (nbrow+2*nbInfos)){
592  if(cpt<(unsigned)nbrow){
593  w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT) ;
594  }
595  else
596  w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
597  cpt++;
598  }
599 
600  if(computeCovariance){
601  R_true = (*R);
602  J_true = (*J);
603  }
604 
605  residu_1 = residu;
606  residu = 0;
607  double num = 0;
608  double den = 0;
609  for (unsigned int i = 0; i < static_cast<unsigned int>(R->getRows()); i++){
610  num += w[i]*vpMath::sqr((*R)[i]);
611  den += w[i];
612 
613  w_true[i] = w[i]*w[i];
614  (*R)[i] *= w[i];
616  for (unsigned int j = 0; j < 6; j += 1){
617  (*J)[i][j] *= w[i];
618  }
619  }
620  }
621 
622  residu = sqrt(num/den);
623 
624  JTJ = J->AtA();
625  computeJTR(*J, *R, JTR);
626  v = -lambda * JTJ.pseudoInverse() * JTR;
629 
630  iter++;
631 
632  delete J;
633  delete R;
634  }
635 
636  if(computeCovariance){
637  vpMatrix D;
638  D.diag(w_true);
640  }
641 }
642 
650 void
652 {
653  unsigned int nbInfos;
654  unsigned int nbFaceUsed;
655  vpColVector w_klt;
656 
657  vpMbKltTracker::preTracking(I, nbInfos, nbFaceUsed);
658 
659  if(nbInfos >= 4)
660  vpMbKltTracker::computeVVS(nbInfos, w_klt);
661  else{
662  nbInfos = 0;
663  std::cout << "[ERROR] Unable to init with KLT" << std::endl;
664  }
665 
667 
668  vpColVector w_mbt;
669  computeVVS(I, nbInfos, w_mbt, w_klt);
670 
671  if(postTracking(I, w_mbt, w_klt)){
674  }
675 }
676 
677 unsigned int
679 {
680  vpMbtDistanceLine *l ;
682 
683  if(lvl >= scales.size() || !scales[lvl]){
684  throw vpException(vpException::dimensionError, "_lvl not used.");
685  }
686 
687  unsigned int nbrow = initMbtTracking(lvl);
688 
689  if (nbrow==0){
690 // vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
691 // throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
692  return nbrow;
693  }
694 
695  factor.resize(nbrow);
696  factor = 1;
697 
698  unsigned int n = 0;
699  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
700  l = *it;
702 
703  double fac = 1;
704  for(std::list<int>::const_iterator it = l->Lindex_polygon.begin(); it!=l->Lindex_polygon.end(); ++it){
705  int index = *it;
706  if (l->hiddenface->isAppearing((unsigned int)index)) {
707  fac = 0.2;
708  break;
709  }
710  if(l->closeToImageBorder(I, 10)){
711  fac = 0.1;
712  break;
713  }
714  }
715 
716  std::list<vpMeSite>::const_iterator itListLine;
717  if (l->meline != NULL)
718  itListLine = l->meline->getMeList().begin();
719 
720  for (unsigned int i=0 ; i < l->nbFeature ; i++){
721  factor[n+i] = fac;
722  vpMeSite site = *itListLine;
723  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
724  ++itListLine;
725  }
726  n+= l->nbFeature ;
727  }
728 
729  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
730  cy = *it;
732  double fac = 1.0;
733 
734  std::list<vpMeSite>::const_iterator itCyl1;
735  std::list<vpMeSite>::const_iterator itCyl2;
736  if ((cy->meline1 != NULL || cy->meline2 != NULL)){
737  itCyl1 = cy->meline1->getMeList().begin();
738  itCyl2 = cy->meline2->getMeList().begin();
739  }
740 
741  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
742  factor[n+i] = fac;
743  vpMeSite site;
744  if(i<cy->nbFeaturel1) {
745  site= *itCyl1;
746  ++itCyl1;
747  }
748  else{
749  site= *itCyl2;
750  ++itCyl2;
751  }
752  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
753  }
754 
755  n+= cy->nbFeature ;
756  }
757 
758  return nbrow;
759 }
760 
761 void
763  vpHomogeneousMatrix& cMo, const unsigned int lvl)
764 {
767 
768  unsigned int n = 0 ;
769  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
770  l = *it;
772  for (unsigned int i=0 ; i < l->nbFeature ; i++){
773  for (unsigned int j=0; j < 6 ; j++){
774  L[n+i][j] = l->L[i][j];
775  error[n+i] = l->error[i];
776  }
777  }
778  n+= l->nbFeature;
779  }
780 
781  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
782  cy = *it;
783  cy->computeInteractionMatrixError(cMo, I) ;
784  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
785  for(unsigned int j=0; j < 6 ; j++){
786  L[n+i][j] = cy->L[i][j];
787  error[n+i] = cy->error[i];
788  }
789  }
790 
791  n+= cy->nbFeature ;
792  }
793 }
794 
800 void
802 {
803  this->cam = cam;
804 
807 }
808 
815 void
816 vpMbEdgeKltTracker::initFaceFromCorners(const std::vector<vpPoint>& corners, const unsigned int indexFace)
817 {
818  vpMbEdgeTracker::initFaceFromCorners(corners, indexFace);
819  vpMbKltTracker::initFaceFromCorners(corners, indexFace);
820 }
821 
831 void
832 vpMbEdgeKltTracker::initCylinder(const vpPoint& _p1, const vpPoint _p2, const double _radius, const unsigned int _indexCylinder)
833 {
834  vpMbEdgeTracker::initCylinder(_p1, _p2, _radius, _indexCylinder);
835 }
836 
847 void
849  const vpColor& col , const unsigned int thickness, const bool displayFullModel)
850 {
851  vpMbKltTracker::display(I, cMo, cam, col, thickness, displayFullModel);
852 
853  for (unsigned int i = 0; i < scales.size(); i += 1){
854  if(scales[i]){
855  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
856  (*it)->display(I, cMo, cam, col, thickness);
857  }
858  break ; //displaying model on one scale only
859  }
860  }
861 }
862 
873 void
875  const vpColor& col , const unsigned int thickness, const bool displayFullModel)
876 {
877  vpMbKltTracker::display(I, cMo, cam, col, thickness, displayFullModel);
878 
879  for (unsigned int i = 0; i < scales.size(); i += 1){
880  if(scales[i]){
881  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
882  (*it)->display(I, cMo, cam, col, thickness);
883  }
884  break ; //displaying model on one scale only
885  }
886  }
887 }
888 
889 #endif //VISP_HAVE_OPENCV