ViSP  2.9.0
vpMbKltTracker.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbKltTracker.cpp 4661 2014-02-10 19:34:58Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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  * Model based tracker using only KLT
35  *
36  * Authors:
37  * Romain Tallonneau
38  * Aurelien Yol
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpMbKltTracker.h>
43 
44 #ifdef VISP_HAVE_OPENCV
45 
47  : cur(NULL), c0Mo(), angleAppears(0), angleDisappears(0), compute_interaction(true),
48  firstInitialisation(true), maskBorder(5), lambda(0.8), maxIter(200), threshold_outlier(0.5),
49  percentGood(0.6), useOgre(false), ctTc0(), tracker(), faces(), firstTrack(false),
50  distNearClip(0.01), distFarClip(100), clippingFlag(vpMbtPolygon::NO_CLIPPING)
51 {
54 
55  tracker.setMaxFeatures(10000);
57  tracker.setQuality(0.01);
62 
65 
66 #ifdef VISP_HAVE_OGRE
67  faces.getOgreContext()->setWindowName("MBT KLT");
68 #endif
69 }
70 
76 {
77  if(cur != NULL){
78  cvReleaseImage(&cur);
79  cur = NULL;
80  }
81 }
82 
83 void
85 {
86  if(!modelInitialised){
87  throw vpException(vpException::fatalError, "model not initialized");
88  }
89 
90  bool reInitialisation = false;
91  if(!useOgre)
92  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
93  else{
94 #ifdef VISP_HAVE_OGRE
95  if(!faces.isOgreInitialised()){
98  }
99 
100  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
101 
102 #else
103  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
104 #endif
105  }
106 
107  reinit(I);
108 }
109 
110 void
112 {
113  c0Mo = cMo;
114  ctTc0.setIdentity();
115  firstTrack = false;
116 
118 
119  // mask
120  IplImage* mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
121  cvZero(mask);
122 
123  unsigned char val = 255/* - i*15*/;
124  for (unsigned int i = 0; i < faces.size(); i += 1){
125  if(faces[i]->isVisible())
126  faces[i]->updateMask(mask, val, maskBorder);
127  }
128 
129  tracker.initTracking(cur, mask);
130 
131  for (unsigned int i = 0; i < faces.size(); i += 1){
132  if(faces[i]->isVisible()){
133  faces[i]->init(tracker);
134  }
135  }
136 
137  cvReleaseImage(&mask);
138 }
139 
144 void
146 {
147  cMo.setIdentity();
148 
149  if(cur != NULL){
150  cvReleaseImage(&cur);
151  cur = NULL;
152  }
153 
154  compute_interaction = true;
155  firstInitialisation = true;
156  computeCovariance = false;
157  firstTrack = false;
158 
161 
162  tracker.setMaxFeatures(10000);
164  tracker.setQuality(0.01);
169 
172 
174 
175  maskBorder = 5;
176  threshold_outlier = 0.5;
177  percentGood = 0.7;
178 
179  lambda = 0.8;
180  maxIter = 200;
181 
182  faces.reset();
183 
184 #ifdef VISP_HAVE_OGRE
185  useOgre = false;
186 #endif
187 }
188 
196 std::vector<vpImagePoint>
198 {
199  std::vector<vpImagePoint> kltPoints;
200  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
201  int id;
202  float x_tmp, y_tmp;
203  tracker.getFeature((int)i, id, x_tmp, y_tmp);
204  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
205  }
206 
207  return kltPoints;
208 }
209 
217 std::map<int, vpImagePoint>
219 {
220  std::map<int, vpImagePoint> kltPoints;
221  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
222  int id;
223  float x_tmp, y_tmp;
224  tracker.getFeature((int)i, id, x_tmp, y_tmp);
225  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
226  }
227 
228  return kltPoints;
229 }
230 
236 void
245 }
246 
252 void
254 {
255  for (unsigned int i = 0; i < faces.size(); i += 1){
256  faces[i]->setCameraParameters(camera);
257  }
258  this->cam = camera;
259 }
260 
268 void
270 {
271  useOgre = v;
272  if(useOgre){
273 #ifndef VISP_HAVE_OGRE
274  useOgre = false;
275  std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
276 #endif
277  }
278 }
279 
289 void
291 {
292  if(firstTrack){
293  bool reInitialisation = false;
294  if(!useOgre)
295  faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
296  else{
297  #ifdef VISP_HAVE_OGRE
298  faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
299  #else
300  faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
301  #endif
302  }
303 
304  if(reInitialisation){
305  std::cout << "WARNING: Visibility changed, must reinitialize to update pose" << std::endl;
306  cMo = cdMo;
307  reinit(I);
308  }
309  else{
310  vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
311  vpHomogeneousMatrix cMcd = cdMc.inverse();
312 
313  vpRotationMatrix cdRc;
314  vpTranslationVector cdtc;
315 
316  cdMc.extract(cdRc);
317  cdMc.extract(cdtc);
318 
319  CvPoint2D32f* initial_guess = NULL;
320  initial_guess = (CvPoint2D32f*)cvAlloc((unsigned int)tracker.getMaxFeatures()*sizeof(initial_guess[0]));
321 
322  for (unsigned int i = 0; i < faces.size(); i += 1){
323  if(faces[i]->isVisible() && faces[i]->hasEnoughPoints()){
324  //Get the normal to the face at the current state cMo
325  vpPlane plan(faces[i]->p[0], faces[i]->p[1], faces[i]->p[2]);
326  plan.changeFrame(cMcd);
327 
328  vpColVector Nc = plan.getNormal();
329  Nc.normalize();
330 
331  double invDc = 1.0 / plan.getD();
332 
333  //Create the homography
334  vpMatrix cdHc;
335  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
336  cdHc /= cdHc[2][2];
337 
338  //Create the 2D homography
339  vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
340 
341  //Points displacement
342  std::map<int, vpImagePoint>::const_iterator iter = faces[i]->getCurrentPoints().begin();
343  for( ; iter != faces[i]->getCurrentPoints().end(); iter++){
344  vpColVector cdp(3);
345  cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
346 
347  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
348 
349  if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
350  cdp[0] = 0.0;
351  cdp[1] = 0.0;
352  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
353  }
354 
355  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
356  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
357 
358  //Set value to the KLT tracker
359  initial_guess[(faces[i]->getCurrentPointsInd())[iter->first]].x = (float)cdp[0];
360  initial_guess[(faces[i]->getCurrentPointsInd())[iter->first]].y = (float)cdp[1];
361  }
362  }
363  }
364 
365  tracker.setInitialGuess(&initial_guess);
366 
367  if(initial_guess) cvFree(&initial_guess);
368  initial_guess = NULL;
369 
370  cMo = cdMo;
371  }
372  }
373 }
374 
380 void
382 {
383  if( (clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING && dist <= distNearClip)
384  vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
385  else if ( dist < 0 )
386  vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
387  else{
389  distFarClip = dist;
390  for (unsigned int i = 0; i < faces.size(); i ++){
391  faces[i]->setFarClippingDistance(distFarClip);
392  }
393  }
394 }
395 
401 void
403 {
404  if( (clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING && dist >= distFarClip)
405  vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
406  else if ( dist < 0 )
407  vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
408  else{
410  distNearClip = dist;
411  for (unsigned int i = 0; i < faces.size(); i ++){
412  faces[i]->setNearClippingDistance(distNearClip);
413  }
414  }
415 }
416 
424 void
425 vpMbKltTracker::setClipping(const unsigned int &flags)
426 {
427  clippingFlag = flags;
428  for (unsigned int i = 0; i < faces.size(); i ++)
430 }
431 
438 void
439 vpMbKltTracker::initFaceFromCorners(const std::vector<vpPoint>& corners, const unsigned int indexFace)
440 {
441  if( corners.size() > 2){ // This tracker can't handle lignes
442  vpMbtKltPolygon *polygon = new vpMbtKltPolygon;
443  // polygon->setCameraParameters(cam);
444  polygon->setNbPoint((unsigned int)corners.size());
445  polygon->setIndex((int)indexFace);
446  for(unsigned int j = 0; j < corners.size(); j++) {
447  polygon->addPoint(j, corners[j]);
448  }
449  faces.addPolygon(polygon);
450  faces.getPolygon().back()->setCameraParameters(cam);
451 
453  faces.getPolygon().back()->setClipping(clippingFlag);
454 
455  if((clippingFlag & vpMbtPolygon::NEAR_CLIPPING) == vpMbtPolygon::NEAR_CLIPPING)
456  faces.getPolygon().back()->setNearClippingDistance(distNearClip);
457 
458  if((clippingFlag & vpMbtPolygon::FAR_CLIPPING) == vpMbtPolygon::FAR_CLIPPING)
459  faces.getPolygon().back()->setFarClippingDistance(distFarClip);
460 
461  delete polygon;
462  polygon = NULL;
463  }
464 }
465 
473 void
474 vpMbKltTracker::preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
475 {
477  tracker.track(cur);
478 
479  if(!firstTrack)
480  firstTrack = true;
481 
482  nbInfos = 0;
483  nbFaceUsed = 0;
484  for (unsigned int i = 0; i < faces.size(); i += 1){
485  if(faces[i]->isVisible()){
486  faces[i]->computeNbDetectedCurrent(tracker);
487 // faces[i]->ransac();
488  if(faces[i]->hasEnoughPoints()){
489  nbInfos += faces[i]->getNbPointsCur();
490  nbFaceUsed++;
491  }
492  }
493  }
494 }
495 
499 bool
501 {
502  // # For a better Post Tracking, tracker should reinitialize if so faces don't have enough points but are visible.
503  // # Here we are not doing it for more speed performance.
504  bool reInitialisation = false;
505 
506  unsigned int initialNumber = 0;
507  unsigned int currentNumber = 0;
508  unsigned int shift = 0;
509  for (unsigned int i = 0; i < faces.size(); i += 1){
510  if(faces[i]->isVisible()){
511  initialNumber += faces[i]->getInitialNumberPoint();
512  if(faces[i]->hasEnoughPoints()){
513  vpSubColVector sub_w(w, shift, 2*faces[i]->getNbPointsCur());
514  faces[i]->removeOutliers(sub_w, threshold_outlier);
515  shift += 2*faces[i]->getNbPointsCur();
516 
517  currentNumber += faces[i]->getNbPointsCur();
518  }
519 // else{
520 // reInitialisation = true;
521 // break;
522 // }
523  }
524  }
525 
526 // if(!reInitialisation){
527  double value = percentGood * (double)initialNumber;
528  if((double)currentNumber < value){
529 // std::cout << "Too many point disappear : " << initialNumber << "/" << currentNumber << std::endl;
530  reInitialisation = true;
531  }
532  else{
533  if(!useOgre)
534  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
535  else{
536 #ifdef VISP_HAVE_OGRE
537  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
538 #else
539  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
540 #endif
541  }
542  }
543 // }
544 
545  if(reInitialisation)
546  return true;
547 
548  return false;
549 }
550 
557 void
558 vpMbKltTracker::computeVVS(const unsigned int &nbInfos, vpColVector &w)
559 {
560  vpMatrix J; // interaction matrix
561  vpColVector R; // residu
562  vpMatrix J_true; // interaction matrix
563  vpColVector R_true; // residu
564  vpColVector v; // "speed" for VVS
565  vpHomography H;
566  vpColVector w_true;
567  vpRobust robust(2*nbInfos);
568 
569  vpMatrix JTJ, JTR;
570 
571  double normRes = 0;
572  double normRes_1 = -1;
573  unsigned int iter = 0;
574 
575  R.resize(2*nbInfos);
576  J.resize(2*nbInfos, 6, 0);
577 
578  while( ((int)((normRes - normRes_1)*1e8) != 0 ) && (iter<maxIter) ){
579 
580  unsigned int shift = 0;
581  for (unsigned int i = 0; i < faces.size(); i += 1){
582  if(faces[i]->isVisible() && faces[i]->hasEnoughPoints()){
583  vpSubColVector subR(R, shift, 2*faces[i]->getNbPointsCur());
584  vpSubMatrix subJ(J, shift, 0, 2*faces[i]->getNbPointsCur(), 6);
585  try{
586  faces[i]->computeHomography(ctTc0, H);
587  faces[i]->computeInteractionMatrixAndResidu(subR, subJ);
588  }catch(...){
589  std::cerr << "exception while tracking face " << i << std::endl;
590  throw ;
591  }
592 
593  shift += 2*faces[i]->getNbPointsCur();
594  }
595  }
596 
597  /* robust */
598  if(iter == 0){
599  w_true.resize(2*nbInfos);
600  w.resize(2*nbInfos);
601  w = 1;
602  }
603  robust.setIteration(iter);
604  robust.setThreshold(2/cam.get_px());
605  robust.MEstimator( vpRobust::TUKEY, R, w);
606 
607  if(computeCovariance){
608  R_true = R;
609  J_true = J;
610  }
611 
612  normRes_1 = normRes;
613  normRes = 0;
614  for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
615  w_true = w[i] * w[i];
616  R[i] = R[i] * w[i];
617  normRes += R[i];
618  }
619 
620  if((iter == 0) || compute_interaction){
621  for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
622  for(unsigned int j=0; j<6; j++){
623  J[i][j] *= w[i];
624  }
625  }
626  }
627 
628  JTJ = J.AtA();
629  computeJTR(J, R, JTR);
630  v = -lambda * JTJ.pseudoInverse(1e-16) * JTR;
631 
633 
634  iter++;
635  }
636 
637  if(computeCovariance){
638  vpMatrix D;
639  D.diag(w_true);
641  }
642 
643  cMo = ctTc0 * c0Mo;
644 }
645 
653 void
655 {
656  unsigned int nbInfos;
657  unsigned int nbFaceUsed;
658  preTracking(I, nbInfos, nbFaceUsed);
659 
660  if(nbInfos < 4 || nbFaceUsed == 0){
661  vpERROR_TRACE("\n\t\t Error-> not enough data") ;
662  throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
663  }
664 
665  vpColVector w;
666  computeVVS(nbInfos, w);
667 
668  if(postTracking(I, w))
669  reinit(I);
670 }
671 
683 void
684 vpMbKltTracker::loadConfigFile(const std::string& configFile)
685 {
686  vpMbKltTracker::loadConfigFile(configFile.c_str());
687 }
688 
735 void
736 vpMbKltTracker::loadConfigFile(const char* configFile)
737 {
738 #ifdef VISP_HAVE_XML2
739  vpMbtKltXmlParser xmlp;
740 
741  xmlp.setMaxFeatures(10000);
742  xmlp.setWindowSize(5);
743  xmlp.setQuality(0.01);
744  xmlp.setMinDistance(5);
745  xmlp.setHarrisParam(0.01);
746  xmlp.setBlockSize(3);
747  xmlp.setPyramidLevels(3);
751 
752  try{
753  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
754  xmlp.parse(configFile);
755  }
756  catch(...){
757  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
758  throw vpException(vpException::ioError, "problem to parse configuration file.");
759  }
760 
761  vpCameraParameters camera;
762  xmlp.getCameraParameters(camera);
763  setCameraParameters(camera);
764 
766  tracker.setWindowSize((int)xmlp.getWindowSize());
767  tracker.setQuality(xmlp.getQuality());
770  tracker.setBlockSize((int)xmlp.getBlockSize());
772  maskBorder = xmlp.getMaskBorder();
775 
776  if(xmlp.hasNearClippingDistance())
778 
779  if(xmlp.hasFarClippingDistance())
781 
782  if(xmlp.getFovClipping())
784 
785 #else
786  vpTRACE("You need the libXML2 to read the config file %s", configFile);
787 #endif
788 }
789 
800 void
802  const vpColor& col, const unsigned int thickness, const bool displayFullModel)
803 {
804  vpCameraParameters c = camera;
805 
806  if(clippingFlag > 3) // Contains at least one FOV constraint
807  c.computeFov(I.getWidth(), I.getHeight());
808 
809  for (unsigned int i = 0; i < faces.size(); i += 1){
810  if(displayFullModel || faces[i]->isVisible())
811  {
812  faces[i]->changeFrame(cMo_);
813  faces[i]->computeRoiClipped(c);
814  std::vector<std::pair<vpImagePoint,unsigned int> > roi;
815  faces[i]->getRoiClipped(c, roi);
816 
817  for (unsigned int j = 0; j < roi.size(); j += 1){
818  if(((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
819  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
820  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
821  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
822  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
823  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
824  vpImagePoint ip1, ip2;
825  ip1 = roi[j].first;
826  ip2 = roi[(j+1)%roi.size()].first;
827 
828  vpDisplay::displayLine (I, ip1, ip2, col, thickness);
829  }
830  }
831  }
832  if(displayFeatures && faces[i]->hasEnoughPoints() && faces[i]->isVisible()) {
833  faces[i]->displayPrimitive(I);
834 // faces[i]->displayNormal(I);
835  }
836  }
837 
838 #ifdef VISP_HAVE_OGRE
839  if(useOgre)
840  faces.displayOgre(cMo_);
841 #endif
842 }
843 
854 void
856  const vpColor& col , const unsigned int thickness, const bool displayFullModel)
857 {
858  vpCameraParameters c = camera;
859 
860  if(clippingFlag > 3) // Contains at least one FOV constraint
861  c.computeFov(I.getWidth(), I.getHeight());
862 
863  for (unsigned int i = 0; i < faces.size(); i += 1){
864  if(displayFullModel || faces[i]->isVisible())
865  {
866  faces[i]->changeFrame(cMo_);
867  faces[i]->computeRoiClipped(c);
868  std::vector<std::pair<vpImagePoint,unsigned int> > roi;
869  faces[i]->getRoiClipped(c, roi);
870 
871  for (unsigned int j = 0; j < roi.size(); j += 1){
872  if(((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
873  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
874  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
875  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
876  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
877  ((roi[(j+1)%roi.size()].second & roi[j].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
878  vpImagePoint ip1, ip2;
879  ip1 = roi[j].first;
880  ip2 = roi[(j+1)%roi.size()].first;
881 
882  vpDisplay::displayLine (I, ip1, ip2, col, thickness);
883  }
884  }
885  }
886  if(displayFeatures && faces[i]->hasEnoughPoints() && faces[i]->isVisible()) {
887  faces[i]->displayPrimitive(I);
888 // faces[i]->displayNormal(I);
889  }
890  }
891 
892 #ifdef VISP_HAVE_OGRE
893  if(useOgre)
894  faces.displayOgre(cMo_);
895 #endif
896 }
897 
906 void
908 {
909  unsigned int nbTotalPoints = 0;
910  for (unsigned int i = 0; i < faces.size(); i += 1){
911  if(faces[i]->isVisible()){
912  nbTotalPoints += faces[i]->getNbPointsCur();
913  }
914  }
915 
916  if(nbTotalPoints < 10){
917  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
919  "test tracking failed (too few points to realize a good tracking).");
920  }
921 }
922 
923 #endif //VISP_HAVE_OPENCV
void setKltOpencv(const vpKltOpencv &t)
bool compute_interaction
If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
virtual void track(const vpImage< unsigned char > &I)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
vpMatrix get_K_inverse() const
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:234
int getPyramidLevels() const
Get the number of pyramid levels.
Definition: vpKltOpencv.h:196
unsigned int getMaskBorder() const
void setQuality(const double &q)
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:118
double distFarClip
Distance for near clipping.
virtual ~vpMbKltTracker()
void setQuality(double input)
Definition: vpKltOpencv.h:264
double getHarrisParam() const
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:183
void track(const IplImage *I)
double distNearClip
Distance for near clipping.
unsigned int clippingFlag
Flags specifying which clipping to used.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:136
int getMaxFeatures() const
Get Max number of features.
Definition: vpKltOpencv.h:183
void parse(const char *filename)
unsigned int getWidth() const
Definition: vpImage.h:159
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void getCameraParameters(vpCameraParameters &_cam) const
unsigned int maxIter
The maximum iteration of the virtual visual servoing stage.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpTRACE
Definition: vpDebug.h:418
IplImage * cur
Temporary OpenCV image for fast conversion.
double getHarrisFreeParameter() const
Get Harris free parameter.
Definition: vpKltOpencv.h:179
void setIdentity()
Basic initialisation (identity)
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
double percentGood
Percentage of good points, according to the initial number, that must have the tracker.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:108
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
unsigned int getBlockSize() const
void setUseHarris(const int input)
Definition: vpKltOpencv.h:273
double getNearClippingDistance() const
bool modelInitialised
Flag used to ensure that the CAD model is loaded before the initialisation.
Definition: vpMbTracker.h:112
error that can be emited by ViSP classes.
Definition: vpException.h:76
void computeJTR(const vpMatrix &J, const vpColVector &R, vpMatrix &JTR)
unsigned int getMaxFeatures() const
void setInitialGuess(CvPoint2D32f **guess_pts)
void setPyramidLevels(const int input)
Definition: vpKltOpencv.h:263
void setBlockSize(const int input)
Definition: vpKltOpencv.h:219
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:66
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:116
bool firstInitialisation
Flag to specify whether the init method is called the first or not (specific calls to realize in this...
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
virtual void reinit(const vpImage< unsigned char > &I)
virtual void testTracking()
bool hasNearClippingDistance() const
virtual void loadConfigFile(const std::string &configFile)
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:106
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
The vpRotationMatrix considers the particular case of a rotation matrix.
vpHomogeneousMatrix ctTc0
The estimated displacement of the pose between the current instant and the initial position...
double getFarClippingDistance() const
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
bool hasFarClippingDistance() const
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:178
void addPoint(const unsigned int n, const vpPoint &P)
vpAROgre * getOgreContext()
std::vector< vpImagePoint > getKltImagePoints() const
void setPyramidLevels(const unsigned int &pL)
Parse an Xml file to extract configuration parameters of a Mbt Klt object.Data parser for the KLT mod...
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:67
void getFeature(int index, int &id, float &x, float &y) const
void setWindowSize(const int input)
Definition: vpKltOpencv.h:274
vpMatrix AtA() const
Definition: vpMatrix.cpp:1408
double getQuality() const
Get the quality of the tracker.
Definition: vpKltOpencv.h:198
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:344
void setWindowSize(const unsigned int &w)
void setAngleDisappear(const double &adisappear)
void setTrackerId(int tid)
Definition: vpKltOpencv.h:265
Definition of the vpSubColVector vpSubColVector class provides a mask on a vpColVector all properties...
unsigned int maskBorder
Erosion of the mask.
Generic class defining intrinsic camera parameters.
int getBlockSize() const
Get the block size.
Definition: vpKltOpencv.h:172
std::map< int, vpImagePoint > getKltImagePointsWithId() const
bool useOgre
Use Ogre3d for visibility tests.
vpMbHiddenFaces< vpMbtKltPolygon > faces
Set of faces describing the object.
void extract(vpRotationMatrix &R) const
void setHarrisParam(const double &hp)
bool firstTrack
First track() called.
void setMinDistance(const double &mD)
void addPolygon(PolygonType *p)
vpKltOpencv tracker
Points tracker.
vpColVector getNormal() const
Definition: vpPlane.cpp:218
double get_px() const
static double rad(double deg)
Definition: vpMath.h:100
double threshold_outlier
Threshold below which the weight associated to a point to consider this one as an outlier...
void diag(const vpColVector &A)
Definition: vpMatrix.cpp:2572
void preTracking(const vpImage< unsigned char > &I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
unsigned int size() const
double lambda
The gain of the virtual visual servoing stage.
void setMaxFeatures(const unsigned int &mF)
void initTracking(const IplImage *I, const IplImage *mask=NULL)
double getQuality() const
vpMatrix get_K() const
static double deg(double rad)
Definition: vpMath.h:93
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:120
void setCameraParameters(const vpCameraParameters &cam)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
virtual void setOgreVisibilityTest(const bool &v)
void setAngleAppear(const double &aappear)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV.
Definition: vpKltOpencv.h:103
vpHomogeneousMatrix inverse() const
virtual void setNearClippingDistance(const double &dist)
static vpHomogeneousMatrix direct(const vpColVector &v)
int getNbFeatures() const
Get the current number of features.
Definition: vpKltOpencv.h:187
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:63
virtual void initFaceFromCorners(const std::vector< vpPoint > &corners, const unsigned int indexFace=-1)
double angleAppears
Angle used to detect a face appearance.
unsigned int getHeight() const
Definition: vpImage.h:150
virtual void setClipping(const unsigned int &flags)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1861
virtual void init(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void setBlockSize(const unsigned int &bs)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
double getMinDistance() const
Get Min Distance.
Definition: vpKltOpencv.h:185
void displayOgre(const vpHomogeneousMatrix &cMo)
double getAngleAppear() const
void setMaxFeatures(const int input)
double angleDisappears
Angle used to detect a face disappearance.
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:128
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
double getMinDistance() const
virtual void setNbPoint(const unsigned int nb)
vpColVector & normalize()
normalise the vector
bool getFovClipping() const
void setMinDistance(double input)
Definition: vpKltOpencv.h:243
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:122
void computeVVS(const unsigned int &nbInfos, vpColVector &w)
Class that consider the case of a translation vector.
void setMaskBorder(const unsigned int &mb)
vpHomogeneousMatrix c0Mo
Initial pose.
int getWindowSize() const
Get Max number of features.
Definition: vpKltOpencv.h:200
virtual void setFarClippingDistance(const double &dist)
double getD() const
Definition: vpPlane.h:117
unsigned int getPyramidLevels() const
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)
void setHarrisFreeParameter(double input)
Definition: vpKltOpencv.h:227
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94
std::vector< PolygonType * > & getPolygon()
virtual void setIndex(const int i)
Definition: vpMbtPolygon.h:215
void computeFov(const unsigned int &w, const unsigned int &h)