Visual Servoing Platform  version 3.0.1
vpMbKltTracker.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  * Model based tracker using only KLT
32  *
33  * Authors:
34  * Romain Tallonneau
35  * Aurelien Yol
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpImageConvert.h>
40 #include <visp3/mbt/vpMbKltTracker.h>
41 #include <visp3/core/vpVelocityTwistMatrix.h>
42 #include <visp3/core/vpTrackingException.h>
43 
44 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
45 
46 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
47 # include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
48 #endif
49 
51  :
52 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
53  cur(),
54 #else
55  cur(NULL),
56 #endif
57  c0Mo(), compute_interaction(true),
58  firstInitialisation(true), maskBorder(5), lambda(0.8), maxIter(200), threshold_outlier(0.5),
59  percentGood(0.6), ctTc0(), tracker(), kltPolygons(), kltCylinders(), circles_disp()
60 {
63  tracker.setMaxFeatures(10000);
65  tracker.setQuality(0.01);
70 
73 
74 #ifdef VISP_HAVE_OGRE
75  faces.getOgreContext()->setWindowName("MBT Klt");
76 #endif
77 }
78 
84 {
85 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
86  if(cur != NULL){
87  cvReleaseImage(&cur);
88  cur = NULL;
89  }
90 #endif
91 
92  // delete the Klt Polygon features
93  vpMbtDistanceKltPoints *kltpoly;
94  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
95  kltpoly = *it;
96  if (kltpoly!=NULL){
97  delete kltpoly ;
98  }
99  kltpoly = NULL ;
100  }
101  kltPolygons.clear();
102 
103  vpMbtDistanceKltCylinder *kltPolyCylinder;
104  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
105  kltPolyCylinder = *it;
106  if (kltPolyCylinder!=NULL){
107  delete kltPolyCylinder ;
108  }
109  kltPolyCylinder = NULL ;
110  }
111  kltCylinders.clear();
112 
113  // delete the structures used to display circles
115  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
116  ci = *it;
117  if (ci!=NULL){
118  delete ci ;
119  }
120  ci = NULL ;
121  }
122 
123  circles_disp.clear();
124 }
125 
126 void
128 {
129  if(!modelInitialised){
130  throw vpException(vpException::fatalError, "model not initialized");
131  }
132 
133  bool reInitialisation = false;
134  if(!useOgre)
135  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
136  else{
137 #ifdef VISP_HAVE_OGRE
138  if(!faces.isOgreInitialised()){
141  faces.initOgre(cam);
142  // Turn off Ogre config dialog display for the next call to this function
143  // since settings are saved in the ogre.cfg file and used during the next
144  // call
145  ogreShowConfigDialog = false;
146  }
147 
148  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
149 
150 #else
151  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
152 #endif
153  }
154  reinit(I);
155 }
156 
157 void
159 {
160  c0Mo = cMo;
161  ctTc0.eye();
162 
164 
165  cam.computeFov(I.getWidth(), I.getHeight());
166 
167  if(useScanLine){
170  }
171 
172  // mask
173 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
174  cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
175 #else
176  IplImage* mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
177  cvZero(mask);
178 #endif
179 
180  vpMbtDistanceKltPoints *kltpoly;
181  vpMbtDistanceKltCylinder *kltPolyCylinder;
182  if(useScanLine){
184  }
185  else{
186  unsigned char val = 255/* - i*15*/;
187  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
188  kltpoly = *it;
189  if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
190  //need to changeFrame when reinit() is called by postTracking
191  //other solution is
192  kltpoly->polygon->changeFrame(cMo);
193  kltpoly->polygon->computePolygonClipped(cam); // Might not be necessary when scanline is activated
194  kltpoly->updateMask(mask, val, maskBorder);
195  }
196  }
197 
198  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
199  kltPolyCylinder = *it;
200 
201  if(kltPolyCylinder->isTracked())
202  {
203  for(unsigned int k = 0 ; k < kltPolyCylinder->listIndicesCylinderBBox.size() ; k++)
204  {
205  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
206  if(faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u){
207  faces[indCylBBox]->computePolygonClipped(cam); // Might not be necessary when scanline is activated
208  }
209  }
210 
211  kltPolyCylinder->updateMask(mask, val, maskBorder);
212  }
213  }
214  }
215 
216  tracker.initTracking(cur, mask);
217 // tracker.track(cur); // AY: Not sure to be usefull but makes sure that the points are valid for tracking and avoid too fast reinitialisations.
218 // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << " points" << std::endl;
219 
220  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
221  kltpoly = *it;
222  if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
223  kltpoly->init(tracker);
224  }
225  }
226 
227  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
228  kltPolyCylinder = *it;
229 
230  if(kltPolyCylinder->isTracked())
231  kltPolyCylinder->init(tracker, cMo);
232  }
233 
234 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
235  cvReleaseImage(&mask);
236 #endif
237 }
238 
243 void
245 {
246  cMo.eye();
247 
248 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
249  if(cur != NULL){
250  cvReleaseImage(&cur);
251  cur = NULL;
252  }
253 #endif
254 
255  // delete the Klt Polygon features
256  vpMbtDistanceKltPoints *kltpoly;
257  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
258  kltpoly = *it;
259  if (kltpoly!=NULL){
260  delete kltpoly ;
261  }
262  kltpoly = NULL ;
263  }
264  kltPolygons.clear();
265 
266  vpMbtDistanceKltCylinder *kltPolyCylinder;
267  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
268  kltPolyCylinder = *it;
269  if (kltPolyCylinder!=NULL){
270  delete kltPolyCylinder ;
271  }
272  kltPolyCylinder = NULL ;
273  }
274  kltCylinders.clear();
275 
276  // delete the structures used to display circles
278  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
279  ci = *it;
280  if (ci!=NULL){
281  delete ci ;
282  }
283  ci = NULL ;
284  }
285 
286  circles_disp.clear();
287 
288  compute_interaction = true;
289  firstInitialisation = true;
290  computeCovariance = false;
291 
294 
295  tracker.setMaxFeatures(10000);
297  tracker.setQuality(0.01);
302 
305 
307 
308  maskBorder = 5;
309  threshold_outlier = 0.5;
310  percentGood = 0.7;
311 
312  lambda = 0.8;
313  maxIter = 200;
314 
315  faces.reset();
316 
318 
319  useScanLine = false;
320 
321 #ifdef VISP_HAVE_OGRE
322  useOgre = false;
323 #endif
324 }
325 
333 std::vector<vpImagePoint>
335 {
336  std::vector<vpImagePoint> kltPoints;
337  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
338  long id;
339  float x_tmp, y_tmp;
340  tracker.getFeature((int)i, id, x_tmp, y_tmp);
341  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
342  }
343 
344  return kltPoints;
345 }
346 
354 std::map<int, vpImagePoint>
356 {
357  std::map<int, vpImagePoint> kltPoints;
358  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
359  long id;
360  float x_tmp, y_tmp;
361  tracker.getFeature((int)i, id, x_tmp, y_tmp);
362 #if TARGET_OS_IPHONE
363  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
364 #else
365  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
366 #endif
367  }
368 
369  return kltPoints;
370 }
371 
377 void
386 }
387 
393 void
395 {
396 // for (unsigned int i = 0; i < faces.size(); i += 1){
397 // faces[i]->setCameraParameters(camera);
398 // }
399 
400  vpMbtDistanceKltPoints *kltpoly;
401  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
402  kltpoly = *it;
403  kltpoly->setCameraParameters(camera);
404  }
405 
406  vpMbtDistanceKltCylinder *kltPolyCylinder;
407  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
408  kltPolyCylinder = *it;
409  kltPolyCylinder->setCameraParameters(camera);
410  }
411 
412  this->cam = camera;
413 }
414 
424 void
426 {
427  if((int)(kltCylinders.size()) != 0)
428  {
429  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). This feature is not implemented yet." << std::endl;
430  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
431  cMo = cdMo;
432  init(I);
433  }
434  else
435  {
436  vpMbtDistanceKltPoints *kltpoly;
437 
438 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
439  std::vector<cv::Point2f> init_pts;
440  std::vector<long> init_ids;
441  std::vector<cv::Point2f> guess_pts;
442 #else
443  unsigned int nbp = 0;
444  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
445  kltpoly = *it;
446  if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
447  nbp += (*it)->getCurrentNumberPoints();
448  }
449 
450  CvPoint2D32f* init_pts = NULL;
451  init_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(init_pts[0]));
452  long *init_ids = (long*)cvAlloc((unsigned int)tracker.getMaxFeatures()*sizeof(long));
453  unsigned int iter_pts = 0;
454 
455  CvPoint2D32f* guess_pts = NULL;
456  guess_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(guess_pts[0]));
457 #endif
458 
459  vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
460  vpHomogeneousMatrix cMcd = cdMc.inverse();
461 
462  vpRotationMatrix cdRc;
463  vpTranslationVector cdtc;
464 
465  cdMc.extract(cdRc);
466  cdMc.extract(cdtc);
467 
468  unsigned int nbCur = 0;
469 
470  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
471  kltpoly = *it;
472 
473  if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
474  kltpoly->polygon->changeFrame(cdMo);
475 
476  //Get the normal to the face at the current state cMo
477  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
478  plan.changeFrame(cMcd);
479 
480  vpColVector Nc = plan.getNormal();
481  Nc.normalize();
482 
483  double invDc = 1.0 / plan.getD();
484 
485  //Create the homography
486  vpMatrix cdHc;
487  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
488  cdHc /= cdHc[2][2];
489 
490  //Create the 2D homography
491  vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
492 
493  //Points displacement
494  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
495  nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
496  for( ; iter != kltpoly->getCurrentPoints().end(); iter++){
497  vpColVector cdp(3);
498  cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
499 
500 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
501  cv::Point2f p((float)cdp[0], (float)cdp[1]);
502  init_pts.push_back(p);
503 # if TARGET_OS_IPHONE
504  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
505 # else
506  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
507 # endif
508 #else
509  init_pts[iter_pts].x = (float)cdp[0];
510  init_pts[iter_pts].y = (float)cdp[1];
511  init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
512 #endif
513 
514  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
515 
516  if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
517  cdp[0] = 0.0;
518  cdp[1] = 0.0;
519  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
520  }
521 
522  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
523  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
524 
525  //Set value to the KLT tracker
526 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
527  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
528  guess_pts.push_back(p_guess);
529 #else
530  guess_pts[iter_pts].x = (float)cdp[0];
531  guess_pts[iter_pts++].y = (float)cdp[1];
532 #endif
533  }
534  }
535  }
536 
538 
539 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
540  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
541 #else
542  tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
543 
544  if(init_pts) cvFree(&init_pts);
545  init_pts = NULL;
546 
547  if(guess_pts) cvFree(&guess_pts);
548  guess_pts = NULL;
549 
550  if(init_ids)cvFree(&init_ids);
551  init_ids = NULL;
552 #endif
553 
554  bool reInitialisation = false;
555  if(!useOgre)
556  faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
557  else{
558 #ifdef VISP_HAVE_OGRE
559  faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
560 #else
561  faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
562 #endif
563  }
564 
565  cam.computeFov(I.getWidth(), I.getHeight());
566 
567  if(useScanLine){
570  }
571 
572  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
573  kltpoly = *it;
574  if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
575  kltpoly->polygon->computePolygonClipped(cam);
576  kltpoly->init(tracker);
577  }
578  }
579 
580  cMo = cdMo;
581  c0Mo = cMo;
582  ctTc0.eye();
583  }
584 }
585 
591 void
593 {
595  kltPoly->setCameraParameters(cam) ;
596  kltPoly->polygon = &polygon;
597  kltPoly->hiddenface = &faces ;
598  kltPoly->useScanLine = useScanLine;
599  kltPolygons.push_back(kltPoly);
600 }
606 void
608 {
610  kltPoly->setCameraParameters(cam) ;
611  kltPoly->polygon = &polygon;
612  kltPoly->hiddenface = &faces ;
613  kltPoly->useScanLine = useScanLine;
614  kltPolygons.push_back(kltPoly);
615 }
616 
624 void
625 vpMbKltTracker::preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
626 {
628  tracker.track(cur);
629 
630  nbInfos = 0;
631  nbFaceUsed = 0;
632  vpMbtDistanceKltPoints *kltpoly;
633 // for (unsigned int i = 0; i < faces.size(); i += 1){
634  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
635  kltpoly = *it;
636  if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
638 // faces[i]->ransac();
639  if(kltpoly->hasEnoughPoints()){
640  nbInfos += kltpoly->getCurrentNumberPoints();
641  nbFaceUsed++;
642  }
643  }
644  }
645 
646  vpMbtDistanceKltCylinder *kltPolyCylinder;
647  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
648  kltPolyCylinder = *it;
649 
650  if(kltPolyCylinder->isTracked())
651  {
652  kltPolyCylinder->computeNbDetectedCurrent(tracker);
653  if(kltPolyCylinder->hasEnoughPoints()){
654  nbInfos += kltPolyCylinder->getCurrentNumberPoints();
655  nbFaceUsed++;
656  }
657  }
658  }
659 }
660 
664 bool
666 {
667  // # For a better Post Tracking, tracker should reinitialize if so faces don't have enough points but are visible.
668  // # Here we are not doing it for more speed performance.
669  bool reInitialisation = false;
670 
671  unsigned int initialNumber = 0;
672  unsigned int currentNumber = 0;
673  unsigned int shift = 0;
674  vpMbtDistanceKltPoints *kltpoly;
675 // for (unsigned int i = 0; i < faces.size(); i += 1){
676  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
677  kltpoly = *it;
678  if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
679  initialNumber += kltpoly->getInitialNumberPoint();
680  if(kltpoly->hasEnoughPoints()){
681  vpSubColVector sub_w(w, shift, 2*kltpoly->getCurrentNumberPoints());
682  shift += 2*kltpoly->getCurrentNumberPoints();
683  kltpoly->removeOutliers(sub_w, threshold_outlier);
684 
685  currentNumber += kltpoly->getCurrentNumberPoints();
686  }
687 // else{
688 // reInitialisation = true;
689 // break;
690 // }
691  }
692  }
693 
694  vpMbtDistanceKltCylinder *kltPolyCylinder;
695  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
696  kltPolyCylinder = *it;
697 
698  if(kltPolyCylinder->isTracked())
699  {
700  initialNumber += kltPolyCylinder->getInitialNumberPoint();
701  if(kltPolyCylinder->hasEnoughPoints()){
702  vpSubColVector sub_w(w, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
703  shift += 2*kltPolyCylinder->getCurrentNumberPoints();
704  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
705 
706  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
707  }
708  }
709  }
710 
711 // if(!reInitialisation){
712  double value = percentGood * (double)initialNumber;
713  if((double)currentNumber < value){
714 // std::cout << "Too many point disappear : " << initialNumber << "/" << currentNumber << std::endl;
715  reInitialisation = true;
716  }
717  else{
718  if(!useOgre)
719  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
720  else{
721 #ifdef VISP_HAVE_OGRE
722  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
723 #else
724  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
725 #endif
726  }
727  }
728 // }
729 
730  if(reInitialisation)
731  return true;
732 
733  return false;
734 }
735 
742 void
743 vpMbKltTracker::computeVVS(const unsigned int &nbInfos, vpColVector &w)
744 {
745  vpMatrix L; // interaction matrix
746  vpColVector R; // residu
747  vpMatrix L_true; // interaction matrix
748  vpMatrix LVJ_true;
749  //vpColVector R_true; // residu
750  vpColVector v; // "speed" for VVS
751  vpHomography H;
752  vpColVector w_true;
753  vpRobust robust(2*nbInfos);
754 
755  vpMatrix LTL;
756  vpColVector LTR;
757  vpHomogeneousMatrix cMoPrev;
758  vpHomogeneousMatrix ctTc0_Prev;
759  vpColVector error_prev(2*nbInfos);
760  double mu = 0.01;
761 
762  double normRes = 0;
763  double normRes_1 = -1;
764  unsigned int iter = 0;
765 
766  R.resize(2*nbInfos);
767  L.resize(2*nbInfos, 6, 0);
768 
769  while( ((int)((normRes - normRes_1)*1e8) != 0 ) && (iter<maxIter) ){
770 
771  unsigned int shift = 0;
772 
774 
775  bool reStartFromLastIncrement = false;
776 
777  computeVVSCheckLevenbergMarquardtKlt(iter, nbInfos, cMoPrev, error_prev, ctTc0_Prev, mu, reStartFromLastIncrement);
778 
779  if(!reStartFromLastIncrement){
780  computeVVSWeights(iter, nbInfos, R, w_true, w, robust);
781 
782  computeVVSPoseEstimation(iter, L, w, L_true, LVJ_true, normRes, normRes_1, w_true, R, LTL, LTR,
783  error_prev, v, mu, cMoPrev, ctTc0_Prev);
784  } // endif(!reStartFromLastIncrement)
785 
786  iter++;
787  }
788 
789  if(computeCovariance){
790  computeVVSCovariance(w_true, cMoPrev, L_true, LVJ_true);
791  }
792 }
793 
794 void
795 vpMbKltTracker::computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos,
796  const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev,
797  double &mu, bool &reStartFromLastIncrement) {
799  if(m_error.sumSquare()/(double)(2*nbInfos) > error_prev.sumSquare()/(double)(2*nbInfos)){
800  mu *= 10.0;
801 
802  if(mu > 1.0)
803  throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
804 
805  cMo = cMoPrev;
806  m_error = error_prev;
807  ctTc0 = ctTc0_Prev;
808  reStartFromLastIncrement = true;
809  }
810  }
811 }
812 
813 void
815  const vpMatrix &L_true, const vpMatrix &LVJ_true) {
816  if(computeCovariance){
817  vpMatrix D;
818  D.diag(w_true);
819 
820  // Note that here the covariance is computed on cMoPrev for time computation efficiency
821  if(isoJoIdentity){
823  }
824  else{
826  }
827  }
828 }
829 
830 void
832  std::list<vpMbtDistanceKltPoints*> &kltPolygons_, std::list<vpMbtDistanceKltCylinder*> &kltCylinders_,
833  const vpHomogeneousMatrix &ctTc0_) {
834  vpMbtDistanceKltPoints *kltpoly;
835 // for (unsigned int i = 0; i < faces.size(); i += 1){
836  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it = kltPolygons_.begin(); it != kltPolygons_.end(); ++it){
837  kltpoly = *it;
838  if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
839  kltpoly->hasEnoughPoints()){
840  vpSubColVector subR(R, shift, 2*kltpoly->getCurrentNumberPoints());
841  vpSubMatrix subL(L, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
842  try{
843  kltpoly->computeHomography(ctTc0_, H);
844  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
845  }catch(...){
846  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
847  }
848 
849  shift += 2*kltpoly->getCurrentNumberPoints();
850  }
851  }
852 
853  vpMbtDistanceKltCylinder *kltPolyCylinder;
854  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it = kltCylinders_.begin(); it != kltCylinders_.end(); ++it){
855  kltPolyCylinder = *it;
856 
857  if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
858  {
859  vpSubColVector subR(R, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
860  vpSubMatrix subL(L, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
861  try{
862  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0_,subR, subL);
863  }catch(...){
864  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
865  }
866 
867  shift += 2*kltPolyCylinder->getCurrentNumberPoints();
868  }
869  }
870 }
871 
872 void
874  const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true,
875  vpColVector &R, vpMatrix &LTL, vpColVector &LTR, vpColVector &error_prev, vpColVector &v, double &mu,
876  vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev) {
877  m_error = R;
878  if(computeCovariance){
879  L_true = L;
880  if(!isoJoIdentity){
882  cVo.buildFrom(cMo);
883  LVJ_true = (L*cVo*oJo);
884  }
885  }
886 
887  normRes_1 = normRes;
888  normRes = 0;
889  for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
890  w_true[i] = w[i];
891  R[i] = R[i] * w[i];
892  normRes += R[i];
893  }
894 
895  if((iter == 0) || compute_interaction){
896  for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
897  for(unsigned int j=0; j<6; j++){
898  L[i][j] *= w[i];
899  }
900  }
901  }
902 
903  if(isoJoIdentity){
904  LTL = L.AtA();
905  computeJTR(L, R, LTR);
906 
907  switch(m_optimizationMethod){
909  {
910  vpMatrix LMA(LTL.getRows(), LTL.getCols());
911  LMA.eye();
912  vpMatrix LTLmuI = LTL + (LMA*mu);
913  v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
914 
915  if(iter != 0)
916  mu /= 10.0;
917 
918  error_prev = m_error;
919  break;
920  }
922  default:
923  v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
924  }
925  }
926  else{
928  cVo.buildFrom(cMo);
929  vpMatrix LVJ = (L*cVo*oJo);
930  vpMatrix LVJTLVJ = (LVJ).AtA();
931  vpColVector LVJTR;
932  computeJTR(LVJ, R, LVJTR);
933 
934  switch(m_optimizationMethod){
936  {
937  vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
938  LMA.eye();
939  vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
940  v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
941  v = cVo * v;
942 
943  if(iter != 0)
944  mu /= 10.0;
945 
946  error_prev = m_error;
947  break;
948  }
950  default:
951  {
952  v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
953  v = cVo * v;
954  break;
955  }
956  }
957  }
958 
959  cMoPrev = cMo;
960  ctTc0_Prev = ctTc0;
962  cMo = ctTc0 * c0Mo;
963 }
964 
965 void
966 vpMbKltTracker::computeVVSWeights(const unsigned int iter, const unsigned int nbInfos, const vpColVector &R,
967  vpColVector &w_true, vpColVector &w, vpRobust &robust) {
968  if(iter == 0){
969  w_true.resize(2*nbInfos);
970  w.resize(2*nbInfos);
971  w = 1;
972  w_true = 1;
973  }
974  robust.setIteration(iter);
975  robust.setThreshold(2/cam.get_px());
976  robust.MEstimator( vpRobust::TUKEY, R, w);
977 }
978 
986 void
988 {
989  unsigned int nbInfos = 0;
990  unsigned int nbFaceUsed = 0;
991 
992  try{
993  preTracking(I, nbInfos, nbFaceUsed);
994  }
995  catch(vpException &e){
996  throw e;
997  }
998 
999  if(nbInfos < 4 || nbFaceUsed == 0){
1000  vpERROR_TRACE("\n\t\t Error-> not enough data") ;
1001  throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
1002  }
1003 
1004  //vpColVector w;
1005  computeVVS(nbInfos, m_w);
1006 
1007  if(postTracking(I, m_w))
1008  reinit(I);
1009 }
1010 
1022 void
1023 vpMbKltTracker::loadConfigFile(const std::string& configFile)
1024 {
1025  vpMbKltTracker::loadConfigFile(configFile.c_str());
1026 }
1027 
1074 void
1075 vpMbKltTracker::loadConfigFile(const char* configFile)
1076 {
1077 #ifdef VISP_HAVE_XML2
1078  vpMbtKltXmlParser xmlp;
1079 
1080  xmlp.setMaxFeatures(10000);
1081  xmlp.setWindowSize(5);
1082  xmlp.setQuality(0.01);
1083  xmlp.setMinDistance(5);
1084  xmlp.setHarrisParam(0.01);
1085  xmlp.setBlockSize(3);
1086  xmlp.setPyramidLevels(3);
1087  xmlp.setMaskBorder(maskBorder);
1090 
1091  try{
1092  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1093  xmlp.parse(configFile);
1094  }
1095  catch(...){
1096  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
1097  throw vpException(vpException::ioError, "problem to parse configuration file.");
1098  }
1099 
1100  vpCameraParameters camera;
1101  xmlp.getCameraParameters(camera);
1102  setCameraParameters(camera);
1103 
1104  tracker.setMaxFeatures((int)xmlp.getMaxFeatures());
1105  tracker.setWindowSize((int)xmlp.getWindowSize());
1106  tracker.setQuality(xmlp.getQuality());
1109  tracker.setBlockSize((int)xmlp.getBlockSize());
1111  maskBorder = xmlp.getMaskBorder();
1114 
1115  //if(useScanLine)
1116  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1117 
1118  if(xmlp.hasNearClippingDistance())
1120 
1121  if(xmlp.hasFarClippingDistance())
1123 
1124  if(xmlp.getFovClipping())
1126 
1127  useLodGeneral = xmlp.getLodState();
1130 
1131  applyLodSettingInConfig = false;
1132  if(this->getNbPolygon() > 0) {
1133  applyLodSettingInConfig = true;
1137  }
1138 
1139 #else
1140  vpTRACE("You need the libXML2 to read the config file %s", configFile);
1141 #endif
1142 }
1143 
1154 void
1156  const vpColor& col, const unsigned int thickness, const bool displayFullModel)
1157 {
1158  vpCameraParameters c = camera;
1159 
1160  if(clippingFlag > 3) // Contains at least one FOV constraint
1161  c.computeFov(I.getWidth(), I.getHeight());
1162 
1163  vpMbtDistanceKltPoints *kltpoly;
1164  vpMbtDistanceKltCylinder *kltPolyCylinder;
1165 
1166  // Previous version 12/08/2015
1167 // for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1168 // kltpoly = *it;
1169 // kltpoly->polygon->changeFrame(cMo_);
1170 // kltpoly->polygon->computePolygonClipped(c);
1171 // }
1172  faces.computeClippedPolygons(cMo_,c);
1173 
1174  if(useScanLine && !displayFullModel)
1176 
1177  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1178  kltpoly = *it;
1179 
1180  kltpoly->display(I,cMo_,camera,col,thickness,displayFullModel);
1181 
1182  if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1183  kltpoly->displayPrimitive(I);
1184 // faces[i]->displayNormal(I);
1185  }
1186  }
1187 
1188  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
1189  kltPolyCylinder = *it;
1190 
1191  kltPolyCylinder->display(I,cMo_,camera,col,thickness,displayFullModel);
1192 
1193  if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1194  kltPolyCylinder->displayPrimitive(I);
1195  }
1196 
1197  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
1198  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1199  }
1200 
1201 #ifdef VISP_HAVE_OGRE
1202  if(useOgre)
1203  faces.displayOgre(cMo_);
1204 #endif
1205 }
1206 
1217 void
1219  const vpColor& col , const unsigned int thickness, const bool displayFullModel)
1220 {
1221  vpCameraParameters c = camera;
1222 
1223  if(clippingFlag > 3) // Contains at least one FOV constraint
1224  c.computeFov(I.getWidth(), I.getHeight());
1225 
1226  vpMbtDistanceKltPoints *kltpoly;
1227  vpMbtDistanceKltCylinder *kltPolyCylinder;
1228 
1229  // Previous version 12/08/2015
1230 // for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1231 // kltpoly = *it;
1232 // kltpoly->polygon->changeFrame(cMo_);
1233 // kltpoly->polygon->computePolygonClipped(c);
1234 // }
1235  faces.computeClippedPolygons(cMo_,c);
1236 
1237  if(useScanLine && !displayFullModel)
1239 
1240  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1241  kltpoly = *it;
1242 
1243  kltpoly->display(I,cMo_,camera,col,thickness,displayFullModel);
1244 
1245  if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1246  kltpoly->displayPrimitive(I);
1247 // faces[i]->displayNormal(I);
1248  }
1249  }
1250 
1251  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
1252  kltPolyCylinder = *it;
1253 
1254  kltPolyCylinder->display(I,cMo_,camera,col,thickness,displayFullModel);
1255 
1256  if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1257  kltPolyCylinder->displayPrimitive(I);
1258  }
1259 
1260  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
1261  (*it)->display(I, cMo_, camera, col, thickness);
1262  }
1263 
1264 #ifdef VISP_HAVE_OGRE
1265  if(useOgre)
1266  faces.displayOgre(cMo_);
1267 #endif
1268 }
1269 
1278 void
1280 {
1281  unsigned int nbTotalPoints = 0;
1282  vpMbtDistanceKltPoints *kltpoly;
1283 // for (unsigned int i = 0; i < faces.size(); i += 1){
1284  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1285  kltpoly = *it;
1286  if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints()){
1287  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1288  }
1289  }
1290 
1291  vpMbtDistanceKltCylinder *kltPolyCylinder;
1292  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
1293  kltPolyCylinder = *it;
1294  if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1295  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1296  }
1297 
1298  if(nbTotalPoints < 10){
1299  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1301  "test tracking failed (too few points to realize a good tracking).");
1302  }
1303 }
1304 
1315 void
1316 vpMbKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
1317  const std::string &/*name*/)
1318 {
1320  kltPoly->setCameraParameters(cam) ;
1321 
1322  kltPoly->buildFrom(p1,p2,radius);
1323 
1324  // Add the Cylinder BBox to the list of polygons
1325  kltPoly->listIndicesCylinderBBox.push_back(idFace+1);
1326  kltPoly->listIndicesCylinderBBox.push_back(idFace+2);
1327  kltPoly->listIndicesCylinderBBox.push_back(idFace+3);
1328  kltPoly->listIndicesCylinderBBox.push_back(idFace+4);
1329 
1330  kltPoly->hiddenface = &faces ;
1331  kltPoly->useScanLine = useScanLine;
1332  kltCylinders.push_back(kltPoly);
1333 }
1334 
1345 void
1346 vpMbKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
1347  const int /*idFace*/, const std::string &name)
1348 {
1349  addCircle(p1, p2, p3, radius, name);
1350 }
1351 
1361 void
1362 vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name)
1363 {
1364  bool already_here = false ;
1365  vpMbtDistanceCircle *ci ;
1366 
1367 // for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1368 // ci = *it;
1369 // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
1370 // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
1371 // already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
1372 // }
1373 // }
1374 
1375  if (!already_here){
1376  ci = new vpMbtDistanceCircle ;
1377 
1378  ci->setCameraParameters(cam);
1379  ci->setName(name);
1380  ci->buildFrom(P1, P2, P3, r);
1381  circles_disp.push_back(ci);
1382  }
1383 }
1384 
1394 void
1395 vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
1396  const vpHomogeneousMatrix& cMo_, const bool verbose)
1397 {
1398  reInitModel(I, cad_name.c_str(), cMo_, verbose);
1399 }
1400 
1410 void
1412  const vpHomogeneousMatrix& cMo_, const bool verbose)
1413 {
1414  this->cMo.eye();
1415 
1416 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1417  if(cur != NULL){
1418  cvReleaseImage(&cur);
1419  cur = NULL;
1420  }
1421 #endif
1422 
1423  firstInitialisation = true;
1424 
1425 
1426  // delete the Klt Polygon features
1427  vpMbtDistanceKltPoints *kltpoly;
1428  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1429  kltpoly = *it;
1430  if (kltpoly!=NULL){
1431  delete kltpoly ;
1432  }
1433  kltpoly = NULL ;
1434  }
1435  kltPolygons.clear();
1436 
1437  vpMbtDistanceKltCylinder *kltPolyCylinder;
1438  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
1439  kltPolyCylinder = *it;
1440  if (kltPolyCylinder!=NULL){
1441  delete kltPolyCylinder ;
1442  }
1443  kltPolyCylinder = NULL ;
1444  }
1445  kltCylinders.clear();
1446 
1447  // delete the structures used to display circles
1448  vpMbtDistanceCircle *ci;
1449  for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
1450  ci = *it;
1451  if (ci!=NULL){
1452  delete ci ;
1453  }
1454  ci = NULL ;
1455  }
1456 
1457 
1458  faces.reset();
1459 
1460  loadModel(cad_name, verbose);
1461  initFromPose(I, cMo_);
1462 }
1463 
1470 void
1471 vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1472 {
1473  vpMbtDistanceKltPoints *kltpoly;
1474  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1475  kltpoly = *it;
1476  if(kltpoly->polygon->getName() == name){
1477  kltpoly->setTracked(useKltTracking);
1478  }
1479  }
1480 }
1481 
1482 #elif !defined(VISP_BUILD_SHARED_LIBS)
1483 // Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no symbols
1484 void dummy_vpMbKltTracker() {};
1485 #endif //VISP_HAVE_OPENCV
virtual 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.
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="")
virtual void track(const vpImage< unsigned char > &I)
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:266
std::list< vpMbtDistanceKltPoints * > kltPolygons
void setQuality(const double &q)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1741
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:129
void displayPrimitive(const vpImage< unsigned char > &_I)
void setTracked(const bool &track)
bool getFovClipping() const
virtual ~vpMbKltTracker()
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
void computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L, const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true, vpColVector &R, vpMatrix &LTL, vpColVector &LTR, vpColVector &error_prev, vpColVector &v, double &mu, vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
void setCameraParameters(const vpCameraParameters &camera)
void init(const vpKltOpencv &_tracker)
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 removeOutliers(const vpColVector &weight, const double &threshold_outlier)
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
void setHarrisFreeParameter(double harris_k)
void parse(const char *filename)
double getAngleAppear() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
std::vector< vpImagePoint > getKltImagePoints() const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:144
unsigned int maxIter
The maximum iteration of the virtual visual servoing stage.
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getRows() const
Definition: vpImage.h:204
std::string getName() const
Definition: vpMbtPolygon.h:101
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
unsigned int getWindowSize() const
vpMatrix AtA() const
Definition: vpMatrix.cpp:376
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
#define vpERROR_TRACE
Definition: vpDebug.h:391
unsigned int getBlockSize() const
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void setMaxFeatures(const int maxCount)
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)
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:115
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
unsigned int getMaxFeatures() const
void setOgreShowConfigDialog(const bool showConfigDialog)
bool modelInitialised
Flag used to ensure that the CAD model is loaded before the initialisation.
Definition: vpMbTracker.h:123
void setMinDistance(double minDistance)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
error that can be emited by ViSP classes.
Definition: vpException.h:73
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:118
vpMbScanLine & getMbScanLineRenderer()
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
vpHomogeneousMatrix inverse() const
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
void computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos, const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev, double &mu, bool &reStartFromLastIncrement)
void getCameraParameters(vpCameraParameters &_cam) const
void computeVVSWeights(const unsigned int iter, const unsigned int nbInfos, const vpColVector &R, vpColVector &w_true, vpColVector &w, vpRobust &robust)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:156
bool hasNearClippingDistance() const
vpMatrix get_K() const
bool getLodState() const
void extract(vpRotationMatrix &R) const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:62
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:127
bool firstInitialisation
Flag to specify whether the init method is called the first or not (specific calls to realize in this...
virtual void reinit(const vpImage< unsigned char > &I)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void testTracking()
Class that defines what is a point.
Definition: vpPoint.h:59
virtual void loadConfigFile(const std::string &configFile)
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:113
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
Implementation of a rotation matrix and operations on such kind of matrices.
void setQuality(double qualityLevel)
vpHomogeneousMatrix ctTc0
The estimated displacement of the pose between the current instant and the initial position...
unsigned int getMaskBorder() const
std::map< int, int > & getCurrentPointsInd()
double getAngleDisappear() const
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:132
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
vpAROgre * getOgreContext()
void changeFrame(const vpHomogeneousMatrix &cMo)
void setPyramidLevels(const unsigned int &pL)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
vpColVector & normalize()
Parse an Xml file to extract configuration parameters of a Mbt Klt object.Data parser for the KLT mod...
Manage a circle used in the model-based tracker.
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:114
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:117
Error that can be emited by the vpTracker class and its derivates.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:64
double getNearClippingDistance() const
double getD() const
Definition: vpPlane.h:112
virtual void setCameraParameters(const vpCameraParameters &_cam)
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
void setName(const std::string &circle_name)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:372
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
double getFarClippingDistance() const
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
void setAngleDisappear(const double &adisappear)
vpColVector m_error
Error s-s*.
Definition: vpMbTracker.h:139
void setTrackerId(int tid)
Does nothing. Just here for compat with previous releases that use OpenCV C api to do the tracking...
Definition: vpKltOpencv.h:150
unsigned int getCols() const
Definition: vpImage.h:166
unsigned int maskBorder
Erosion of the mask.
Generic class defining intrinsic camera parameters.
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:128
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:135
double getMinDistance() const
Get the minimal Euclidean distance between detected corners during initialization.
Definition: vpKltOpencv.h:116
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
void setMinDistance(const double &mD)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:146
vpKltOpencv tracker
Points tracker.
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
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)
std::list< vpMbtDistanceKltCylinder * > kltCylinders
bool useScanLine
Use scanline rendering.
unsigned int getCurrentNumberPoints() const
std::map< int, vpImagePoint > & getCurrentPoints()
static double rad(double deg)
Definition: vpMath.h:104
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
double threshold_outlier
Threshold below which the weight associated to a point to consider this one as an outlier...
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void preTracking(const vpImage< unsigned char > &I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
cv::Mat cur
Temporary OpenCV image for fast conversion.
double lambda
The gain of the virtual visual servoing stage.
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
Definition: vpKltOpencv.h:110
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)
unsigned int getCurrentNumberPoints() const
void setMaxFeatures(const unsigned int &mF)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
void getFeature(const int &index, long &id, float &x, float &y) const
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
vpColVector getNormal() const
Definition: vpPlane.cpp:246
void setWindowSize(const int winSize)
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
virtual void setCameraParameters(const vpCameraParameters &_cam)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:135
virtual void loadModel(const char *modelFile, const bool verbose=false)
unsigned int getHeight() const
Definition: vpImage.h:175
double getMinDistance() const
void setCameraParameters(const vpCameraParameters &cam)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:157
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:286
double getMinPolygonAreaThreshold() const
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
unsigned int getInitialNumberPoint() const
void setAngleAppear(const double &aappear)
double getHarrisParam() const
bool hasFarClippingDistance() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:76
std::map< int, vpImagePoint > getKltImagePointsWithId() const
void setBlockSize(const int blockSize)
double sumSquare() const
static vpHomogeneousMatrix direct(const vpColVector &v)
bool isVisible(const unsigned int i)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:60
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:148
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
unsigned int getInitialNumberPoint() const
unsigned int getPyramidLevels() const
void computeVVSInteractionMatrixAndResidu(unsigned int shift, vpColVector &R, vpMatrix &L, vpHomography &H, std::list< vpMbtDistanceKltPoints *> &kltPolygons_, std::list< vpMbtDistanceKltCylinder *> &kltCylinders_, const vpHomogeneousMatrix &ctTc0_)
virtual void setClipping(const unsigned int &flags)
double getQuality() const
Get the parameter characterizing the minimal accepted quality of image corners.
Definition: vpKltOpencv.h:130
void computeVVSCovariance(const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true)
double getQuality() const
virtual void init(const vpImage< unsigned char > &I)
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:101
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void setUseHarris(const int useHarrisDetector)
double getMinLineLengthThreshold() const
void setBlockSize(const unsigned int &bs)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:154
void displayOgre(const vpHomogeneousMatrix &cMo)
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:129
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
virtual void setFarClippingDistance(const double &dist)
unsigned int getWidth() const
Definition: vpImage.h:226
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) 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
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void computeVVS(const unsigned int &nbInfos, vpColVector &w)
Class that consider the case of a translation vector.
void eye()
Definition: vpMatrix.cpp:194
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.
void track(const cv::Mat &I)
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
vpMatrix get_K_inverse() const
bool useScanLine
Use scanline rendering.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
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="")
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)