ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpMbEdgeKltTracker.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbEdgeKltTracker.cpp 4327 2013-07-19 14:08:01Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
35  *
36  * Authors:
37  * Romain Tallonneau
38  * Aurelien Yol
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpMbEdgeKltTracker.h>
43 
44 #ifdef VISP_HAVE_OPENCV
45 
47 {
48  compute_interaction = true;
49  computeCovariance = false;
50 
51  lambda = 0.8;
52  thresholdKLT = 2.0;
53  thresholdMBT = 2.0;
54  maxIter = 200;
56 
57 #ifdef VISP_HAVE_OGRE
59 #endif
60 }
61 
67 {
68 }
69 
76 void
78 {
80 
82 
83  unsigned int n = 0;
84  for(unsigned int i = 0; i < vpMbKltTracker::faces.size() ; i++){
85  if(vpMbKltTracker::faces[i]->isVisible()){
86  vpMbEdgeTracker::faces[i]->isvisible = true;
87  n++;
88  }
89  else
90  vpMbEdgeTracker::faces[i]->isvisible = false;
91  }
93 
94  unsigned int i = (unsigned int)scales.size();
95  do {
96  i--;
97  if(scales[i]){
98  downScale(i);
100  upScale(i);
101  }
102  } while(i != 0);
103 
105 }
106 
116 void
118 {
119  if(firstTrack){
120  vpMbKltTracker::setPose(I, cdMo);
121 
123  lines[scaleLevel].front() ;
124  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
125  l = *it;
126  if(l->meline != NULL){
127  delete l->meline;
128  l->meline = NULL;
129  }
130  }
131 
132  initPyramid(I, Ipyramid);
133 
134  unsigned int n = 0;
135  for(unsigned int i = 0; i < vpMbKltTracker::faces.size() ; i++){
136  if(vpMbKltTracker::faces[i]->isVisible()){
137  vpMbEdgeTracker::faces[i]->isvisible = true;
138  n++;
139  }
140  else
141  vpMbEdgeTracker::faces[i]->isvisible = false;
142  }
144 
145  unsigned int i = (unsigned int)scales.size();
146  do {
147  i--;
148  if(scales[i]){
149  downScale(i);
151  upScale(i);
152  }
153  } while(i != 0);
154 
156  }
157 }
158 
163 void
165 {
168 }
169 
170 unsigned int
171 vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
172 {
173  vpMbtDistanceLine *l ;
175 
176  if(lvl >= scales.size() || !scales[lvl]){
177  throw vpException(vpException::dimensionError, "lvl not used.");
178  }
179 
180  unsigned int nbrow = 0;
181  for(std::list<vpMbtDistanceLine*>::iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
182  l = *it;
183  nbrow += l->nbFeature ;
185  }
186 
187  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
188  cy = *it;
189  nbrow += cy->nbFeature ;
191  }
192 
193  return nbrow;
194 }
195 
207 void
208 vpMbEdgeKltTracker::loadConfigFile(const std::string& configFile)
209 {
210  vpMbEdgeKltTracker::loadConfigFile(configFile.c_str());
211 }
212 
277 void
278 vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
279 {
280 #ifdef VISP_HAVE_XML2
282  vpMbKltTracker::loadConfigFile(configFile);
283 #else
284  vpTRACE("You need the libXML2 to read the config file %s", configFile);
285 #endif
286 }
287 
298 void
299 vpMbEdgeKltTracker::loadModel(const std::string& modelFile)
300 {
301  vpMbTracker::loadModel(modelFile);
302 }
303 
307 bool
309  const unsigned int lvl)
310 {
311  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
312 
313  postTrackingMbt(w_mbt,lvl);
314 
315  if (displayFeatures)
316  {
317  if(lvl == 0){
319  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
320  l = *it;
321  if (l->isVisible()){
322  l->displayMovingEdges(I);
323  }
324  }
325 
327  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
328  cy = *it;
329  cy->displayMovingEdges(I);
330  }
331  }
332  }
333 
334  if(reInit)
335  return true;
336 
338  unsigned int n = 0;
339  for(unsigned int i = 0; i < vpMbKltTracker::faces.size() ; i++){
340  if(vpMbKltTracker::faces[i]->isVisible()){
341  vpMbEdgeTracker::faces[i]->isvisible = true;
342  n++;
343  }
344  else
345  vpMbEdgeTracker::faces[i]->isvisible = false;
346  }
348 
351 
352  return false;
353 }
354 
365 void
367 {
368 
369  if(lvl >= scales.size() || !scales[lvl]){
370  throw vpException(vpException::dimensionError, "_lvl not used.");
371  }
372  unsigned int n =0 ;
374  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
375  l = *it;
376  {
377  double wmean = 0 ;
378  std::list<vpMeSite>::iterator itListLine;
379  if (l->nbFeature > 0) itListLine = l->meline->getMeList().begin();
380 
381  for (unsigned int i=0 ; i < l->nbFeature ; i++){
382  wmean += w[n+i] ;
383  vpMeSite p = *itListLine;
384  if (w[n+i] < 0.5){
386 
387  *itListLine = p;
388  }
389 
390  ++itListLine;
391  }
392  n+= l->nbFeature ;
393 
394  if (l->nbFeature!=0)
395  wmean /= l->nbFeature ;
396  else
397  wmean = 1;
398 
399  l->setMeanWeight(wmean);
400 
401  if (wmean < 0.8)
402  l->Reinit = true;
403  }
404  }
405 
406  // Same thing with cylinders as with lines
408  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
409  cy = *it;
410  double wmean = 0 ;
411  std::list<vpMeSite>::iterator itListCyl1;
412  std::list<vpMeSite>::iterator itListCyl2;
413  if (cy->nbFeature > 0){
414  itListCyl1 = cy->meline1->getMeList().begin();
415  itListCyl2 = cy->meline2->getMeList().begin();
416  }
417 
418  wmean = 0;
419  for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
420  wmean += w[n+i] ;
421  vpMeSite p = *itListCyl1;
422  if (w[n+i] < 0.5){
424 
425  *itListCyl1 = p;
426  }
427 
428  ++itListCyl1;
429  }
430 
431  if (cy->nbFeaturel1!=0)
432  wmean /= cy->nbFeaturel1 ;
433  else
434  wmean = 1;
435 
436  cy->setMeanWeight1(wmean);
437 
438  if (wmean < 0.8){
439  cy->Reinit = true;
440  }
441 
442  wmean = 0;
443  for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
444  wmean += w[n+i] ;
445  vpMeSite p = *itListCyl2;
446  if (w[n+i] < 0.5){
448 
449  *itListCyl2 = p;
450  }
451 
452  ++itListCyl2;
453  }
454 
455  if (cy->nbFeaturel2!=0)
456  wmean /= cy->nbFeaturel2 ;
457  else
458  wmean = 1;
459 
460  cy->setMeanWeight2(wmean);
461 
462  if (wmean < 0.8){
463  cy->Reinit = true;
464  }
465 
466  n+= cy->nbFeature ;
467  }
468 }
469 
479 void
480 vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl)
481 {
482  vpColVector factor;
483  unsigned int nbrow = trackFirstLoop(I, factor, lvl);
484 
485  if(nbrow < 4 && nbInfos < 4){
486  vpERROR_TRACE("\n\t\t Error-> not enough data") ;
487  throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
488  }
489  else if(nbrow < 4)
490  nbrow = 0;
491 
492  double residu = 0;
493  double residu_1 = -1;
494  unsigned int iter = 0;
495 
496  vpMatrix *J;
497  vpMatrix J_mbt, J_klt; // interaction matrix
498  vpColVector *R;
499  vpColVector R_mbt, R_klt; // residu
500  vpMatrix J_true;
501  vpColVector R_true;
502  vpColVector w_true;
503 
504  if(nbrow != 0){
505  J_mbt.resize(nbrow,6);
506  R_mbt.resize(nbrow);
507  }
508 
509  if(nbInfos != 0){
510  J_klt.resize(2*nbInfos,6);
511  R_klt.resize(2*nbInfos);
512  }
513 
514  vpColVector w; // weight from MEstimator
515  vpColVector v; // "speed" for VVS
516  vpRobust robust_mbt(0), robust_klt(0);
517  vpHomography H;
518 
519  vpMatrix JTJ, JTR;
520 
521  double factorMBT = 1.0;
522  double factorKLT = 1.0;
523 
524  //More efficient weight repartition for hybrid tracker should come soon...
525 // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
526 // factorKLT = 1.0 - factorMBT;
527  factorMBT = 0.35;
528  factorKLT = 0.65;
529 
530  double residuMBT = 0;
531  double residuKLT = 0;
532 
533  while( ((int)((residu - residu_1)*1e8) !=0 ) && (iter<maxIter) ){
534  J = new vpMatrix();
535  R = new vpColVector();
536 
537  if(nbrow >= 4)
538  trackSecondLoop(I,J_mbt,R_mbt,cMo,lvl);
539 
540  if(nbInfos >= 4){
541  unsigned int shift = 0;
542  for (unsigned int i = 0; i < vpMbKltTracker::faces.size(); i += 1){
543  if(vpMbKltTracker::faces[i]->isVisible() && vpMbKltTracker::faces[i]->hasEnoughPoints()){
544  vpSubColVector subR(R_klt, shift, 2*vpMbKltTracker::faces[i]->getNbPointsCur());
545  vpSubMatrix subJ(J_klt, shift, 0, 2*vpMbKltTracker::faces[i]->getNbPointsCur(), 6);
546  vpMbKltTracker::faces[i]->computeHomography(ctTc0, H);
547  vpMbKltTracker::faces[i]->computeInteractionMatrixAndResidu(subR, subJ);
548  shift += 2*vpMbKltTracker::faces[i]->getNbPointsCur();
549  }
550  }
551  }
552 
553  if(iter == 0){
554  w.resize(nbrow + 2*nbInfos);
555  w=1;
556 
557  w_mbt.resize(nbrow);
558  w_mbt = 1;
559  robust_mbt.resize(nbrow);
560 
561  w_klt.resize(2*nbInfos);
562  w_klt = 1;
563  robust_klt.resize(2*nbInfos);
564 
565  w_true.resize(nbrow + 2*nbInfos);
566  }
567 
568  /* robust */
569  if(nbrow > 3){
570  residuMBT = 0;
571  for(unsigned int i = 0; i < R_mbt.getRows(); i++)
572  residuMBT += fabs(R_mbt[i]);
573  residuMBT /= R_mbt.getRows();
574 
575  robust_mbt.setIteration(iter);
576  robust_mbt.setThreshold(thresholdMBT/cam.get_px());
577  robust_mbt.MEstimator( vpRobust::TUKEY, R_mbt, w_mbt);
578  J->stackMatrices(J_mbt);
579  R->stackMatrices(R_mbt);
580  }
581 
582  if(nbInfos > 3){
583  residuKLT = 0;
584  for(unsigned int i = 0; i < R_klt.getRows(); i++)
585  residuKLT += fabs(R_klt[i]);
586  residuKLT /= R_klt.getRows();
587 
588  robust_klt.setIteration(iter);
589  robust_klt.setThreshold(thresholdKLT/cam.get_px());
590  robust_klt.MEstimator( vpRobust::TUKEY, R_klt, w_klt);
591 
592  J->stackMatrices(J_klt);
593  R->stackMatrices(R_klt);
594  }
595 
596  unsigned int cpt = 0;
597  while(cpt< (nbrow+2*nbInfos)){
598  if(cpt<(unsigned)nbrow){
599  w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT) ;
600  }
601  else
602  w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
603  cpt++;
604  }
605 
606  if(computeCovariance){
607  R_true = (*R);
608  J_true = (*J);
609  }
610 
611  residu_1 = residu;
612  residu = 0;
613  double num = 0;
614  double den = 0;
615  for (unsigned int i = 0; i < static_cast<unsigned int>(R->getRows()); i++){
616  num += w[i]*vpMath::sqr((*R)[i]);
617  den += w[i];
618 
619  w_true[i] = w[i]*w[i];
620  (*R)[i] *= w[i];
622  for (unsigned int j = 0; j < 6; j += 1){
623  (*J)[i][j] *= w[i];
624  }
625  }
626  }
627 
628  residu = sqrt(num/den);
629 
630  JTJ = J->AtA();
631  computeJTR(*J, *R, JTR);
632  v = -lambda * JTJ.pseudoInverse() * JTR;
635 
636  iter++;
637 
638  delete J;
639  delete R;
640  }
641 
642  if(computeCovariance){
643  vpMatrix D;
644  D.diag(w_true);
646  }
647 }
648 
656 void
658 {
659  unsigned int nbInfos;
660  unsigned int nbFaceUsed;
661  vpColVector w_klt;
662 
663  vpMbKltTracker::preTracking(I, nbInfos, nbFaceUsed);
664 
665  if(nbInfos >= 4)
666  vpMbKltTracker::computeVVS(nbInfos, w_klt);
667  else{
668  nbInfos = 0;
669  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
670  }
671 
673 
674  vpColVector w_mbt;
675  computeVVS(I, nbInfos, w_mbt, w_klt);
676 
677  if(postTracking(I, w_mbt, w_klt)){
679 
680  initPyramid(I, Ipyramid);
681 
682  unsigned int n = 0;
683  for(unsigned int i = 0; i < vpMbKltTracker::faces.size() ; i++){
684  if(vpMbKltTracker::faces[i]->isVisible()){
685  vpMbEdgeTracker::faces[i]->isvisible = true;
686  n++;
687  }
688  else
689  vpMbEdgeTracker::faces[i]->isvisible = false;
690  }
692 
693  unsigned int i = (unsigned int)scales.size();
694  do {
695  i--;
696  if(scales[i]){
697  downScale(i);
699  upScale(i);
700  }
701  } while(i != 0);
702 
704  }
705 }
706 
707 unsigned int
709 {
710  vpMbtDistanceLine *l ;
712 
713  if(lvl >= scales.size() || !scales[lvl]){
714  throw vpException(vpException::dimensionError, "_lvl not used.");
715  }
716 
717  unsigned int nbrow = initMbtTracking(lvl);
718 
719  if (nbrow==0){
720 // vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
721 // throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
722  return nbrow;
723  }
724 
725  factor.resize(nbrow);
726  factor = 1;
727 
728  unsigned int n = 0;
729  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
730  l = *it;
732 
733  double fac = 1;
734  for(std::list<int>::const_iterator it = l->Lindex_polygon.begin(); it!=l->Lindex_polygon.end(); ++it){
735  int index = *it;
736  if (l->hiddenface->isAppearing((unsigned int)index)) {
737  fac = 0.2;
738  break;
739  }
740  if(l->closeToImageBorder(I, 10)){
741  fac = 0.1;
742  break;
743  }
744  }
745 
746  std::list<vpMeSite>::const_iterator itListLine;
747  if (l->meline != NULL)
748  itListLine = l->meline->getMeList().begin();
749 
750  for (unsigned int i=0 ; i < l->nbFeature ; i++){
751  factor[n+i] = fac;
752  vpMeSite site = *itListLine;
753  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
754  ++itListLine;
755  }
756  n+= l->nbFeature ;
757  }
758 
759  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
760  cy = *it;
762  double fac = 1.0;
763 
764  std::list<vpMeSite>::const_iterator itCyl1;
765  std::list<vpMeSite>::const_iterator itCyl2;
766  if ((cy->meline1 != NULL || cy->meline2 != NULL)){
767  itCyl1 = cy->meline1->getMeList().begin();
768  itCyl2 = cy->meline2->getMeList().begin();
769  }
770 
771  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
772  factor[n+i] = fac;
773  vpMeSite site;
774  if(i<cy->nbFeaturel1) {
775  site= *itCyl1;
776  ++itCyl1;
777  }
778  else{
779  site= *itCyl2;
780  ++itCyl2;
781  }
782  if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
783  }
784 
785  n+= cy->nbFeature ;
786  }
787 
788  return nbrow;
789 }
790 
791 void
793  vpHomogeneousMatrix& cMo, const unsigned int lvl)
794 {
797 
798  unsigned int n = 0 ;
799  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
800  l = *it;
802  for (unsigned int i=0 ; i < l->nbFeature ; i++){
803  for (unsigned int j=0; j < 6 ; j++){
804  L[n+i][j] = l->L[i][j];
805  error[n+i] = l->error[i];
806  }
807  }
808  n+= l->nbFeature;
809  }
810 
811  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
812  cy = *it;
813  cy->computeInteractionMatrixError(cMo, I) ;
814  for(unsigned int i=0 ; i < cy->nbFeature ; i++){
815  for(unsigned int j=0; j < 6 ; j++){
816  L[n+i][j] = cy->L[i][j];
817  error[n+i] = cy->error[i];
818  }
819  }
820 
821  n+= cy->nbFeature ;
822  }
823 }
824 
830 void
832 {
833  this->cam = cam;
834 
837 }
838 
845 void
846 vpMbEdgeKltTracker::initFaceFromCorners(const std::vector<vpPoint>& corners, const unsigned int indexFace)
847 {
848  vpMbEdgeTracker::initFaceFromCorners(corners, indexFace);
849  vpMbKltTracker::initFaceFromCorners(corners, indexFace);
850 }
851 
861 void
862 vpMbEdgeKltTracker::initCylinder(const vpPoint& _p1, const vpPoint _p2, const double _radius, const unsigned int _indexCylinder)
863 {
864  vpMbEdgeTracker::initCylinder(_p1, _p2, _radius, _indexCylinder);
865 }
866 
877 void
879  const vpColor& col , const unsigned int thickness, const bool displayFullModel)
880 {
881  vpMbtDistanceLine *l ;
882 
883  for (unsigned int i = 0; i < scales.size(); i += 1){
884  if(scales[i]){
885  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
886  l = *it;
887  l->display(I,cMo, cam, col, thickness, displayFullModel);
888  }
889 
890  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
891  (*it)->display(I, cMo, cam, col, thickness);
892  }
893 
894  break ; //displaying model on one scale only
895  }
896  }
897 
898  for (unsigned int i = 0; i < vpMbKltTracker::faces.size(); i += 1){
899  if(displayFeatures && vpMbKltTracker::faces[i]->hasEnoughPoints() && vpMbKltTracker::faces[i]->isVisible()) {
900  vpMbKltTracker::faces[i]->displayPrimitive(I);
901  }
902  }
903 
904 #ifdef VISP_HAVE_OGRE
907 #endif
908 }
909 
920 void
922  const vpColor& col , const unsigned int thickness, const bool displayFullModel)
923 {
924  vpMbtDistanceLine *l ;
925 
926  for (unsigned int i = 0; i < scales.size(); i += 1){
927  if(scales[i]){
928  for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
929  l = *it;
930  l->display(I,cMo, cam, col, thickness, displayFullModel);
931  }
932 
933  for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
934  (*it)->display(I, cMo, cam, col, thickness);
935  }
936 
937  break ; //displaying model on one scale only
938  }
939  }
940 
941  for (unsigned int i = 0; i < vpMbKltTracker::faces.size(); i += 1){
942  if(displayFeatures && vpMbKltTracker::faces[i]->hasEnoughPoints() && vpMbKltTracker::faces[i]->isVisible()) {
943  vpMbKltTracker::faces[i]->displayPrimitive(I);
944  }
945  }
946 
947 #ifdef VISP_HAVE_OGRE
950 #endif
951 }
952 
953 #endif //VISP_HAVE_OPENCV