Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpMbKltTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Model based tracker using only KLT
33  *
34  * Authors:
35  * Romain Tallonneau
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpImageConvert.h>
41 #include <visp3/core/vpTrackingException.h>
42 #include <visp3/core/vpVelocityTwistMatrix.h>
43 #include <visp3/mbt/vpMbKltTracker.h>
44 
45 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
46 
47 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
48 #include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
49 #endif
50 
52  :
53 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
54  cur(),
55 #else
56  cur(NULL),
57 #endif
58  c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
59  kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
60  m_weightedError_klt(), m_robust_klt()
61 {
64  tracker.setMaxFeatures(10000);
66  tracker.setQuality(0.01);
71 
74 
75 #ifdef VISP_HAVE_OGRE
76  faces.getOgreContext()->setWindowName("MBT Klt");
77 #endif
78 
79  m_lambda = 0.8;
80  m_maxIter = 200;
81 }
82 
88 {
89 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
90  if (cur != NULL) {
91  cvReleaseImage(&cur);
92  cur = NULL;
93  }
94 #endif
95 
96  // delete the Klt Polygon features
97  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
98  vpMbtDistanceKltPoints *kltpoly = *it;
99  if (kltpoly != NULL) {
100  delete kltpoly;
101  }
102  kltpoly = NULL;
103  }
104  kltPolygons.clear();
105 
106  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
107  ++it) {
108  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
109  if (kltPolyCylinder != NULL) {
110  delete kltPolyCylinder;
111  }
112  kltPolyCylinder = NULL;
113  }
114  kltCylinders.clear();
115 
116  // delete the structures used to display circles
117  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
118  vpMbtDistanceCircle *ci = *it;
119  if (ci != NULL) {
120  delete ci;
121  }
122  ci = NULL;
123  }
124 
125  circles_disp.clear();
126 }
127 
129 {
130  if (!modelInitialised) {
131  throw vpException(vpException::fatalError, "model not initialized");
132  }
133 
134  bool reInitialisation = false;
135  if (!useOgre)
136  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
137  else {
138 #ifdef VISP_HAVE_OGRE
139  if (!faces.isOgreInitialised()) {
142  faces.initOgre(cam);
143  // Turn off Ogre config dialog display for the next call to this
144  // function since settings are saved in the ogre.cfg file and used
145  // during the next call
146  ogreShowConfigDialog = false;
147  }
148 
149  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
150 
151 #else
152  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
153 #endif
154  }
155  reinit(I);
156 }
157 
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  } else {
185  unsigned char val = 255 /* - i*15*/;
186  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
187  kltpoly = *it;
188  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
189  // need to changeFrame when reinit() is called by postTracking
190  // other solution is
191  kltpoly->polygon->changeFrame(cMo);
192  kltpoly->polygon->computePolygonClipped(cam); // Might not be necessary when scanline is activated
193  kltpoly->updateMask(mask, val, maskBorder);
194  }
195  }
196 
197  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
198  ++it) {
199  kltPolyCylinder = *it;
200 
201  if (kltPolyCylinder->isTracked()) {
202  for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
203  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
204  if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
205  faces[indCylBBox]->computePolygonClipped(cam); // Might not be necessary when scanline is activated
206  }
207  }
208 
209  kltPolyCylinder->updateMask(mask, val, maskBorder);
210  }
211  }
212  }
213 
214  tracker.initTracking(cur, mask);
215  // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
216  // the points are valid for tracking and avoid too fast reinitialisations.
217  // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
218  // 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, m_mask);
224  }
225  }
226 
227  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
228  ++it) {
229  kltPolyCylinder = *it;
230 
231  if (kltPolyCylinder->isTracked())
232  kltPolyCylinder->init(tracker, cMo);
233  }
234 
235 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
236  cvReleaseImage(&mask);
237 #endif
238 }
239 
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  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
257  vpMbtDistanceKltPoints *kltpoly = *it;
258  if (kltpoly != NULL) {
259  delete kltpoly;
260  }
261  kltpoly = NULL;
262  }
263  kltPolygons.clear();
264 
265  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
266  ++it) {
267  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
268  if (kltPolyCylinder != NULL) {
269  delete kltPolyCylinder;
270  }
271  kltPolyCylinder = NULL;
272  }
273  kltCylinders.clear();
274 
275  // delete the structures used to display circles
276  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
277  vpMbtDistanceCircle *ci = *it;
278  if (ci != NULL) {
279  delete ci;
280  }
281  ci = NULL;
282  }
283 
284  circles_disp.clear();
285 
286  m_computeInteraction = true;
287  firstInitialisation = true;
288  computeCovariance = false;
289 
292 
293  tracker.setMaxFeatures(10000);
295  tracker.setQuality(0.01);
300 
303 
305 
306  maskBorder = 5;
307  threshold_outlier = 0.5;
308  percentGood = 0.6;
309 
310  m_lambda = 0.8;
311  m_maxIter = 200;
312 
313  faces.reset();
314 
316 
317  useScanLine = false;
318 
319 #ifdef VISP_HAVE_OGRE
320  useOgre = false;
321 #endif
322 }
323 
332 std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
333 {
334  std::vector<vpImagePoint> kltPoints;
335  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
336  long id;
337  float x_tmp, y_tmp;
338  tracker.getFeature((int)i, id, x_tmp, y_tmp);
339  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
340  }
341 
342  return kltPoints;
343 }
344 
353 std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
354 {
355  std::map<int, vpImagePoint> kltPoints;
356  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
357  long id;
358  float x_tmp, y_tmp;
359  tracker.getFeature((int)i, id, x_tmp, y_tmp);
360 #if TARGET_OS_IPHONE
361  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
362 #else
363  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
364 #endif
365  }
366 
367  return kltPoints;
368 }
369 
376 {
384 }
385 
392 {
393  // for (unsigned int i = 0; i < faces.size(); i += 1){
394  // faces[i]->setCameraParameters(camera);
395  // }
396 
397  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
398  vpMbtDistanceKltPoints *kltpoly = *it;
399  kltpoly->setCameraParameters(camera);
400  }
401 
402  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
403  ++it) {
404  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
405  kltPolyCylinder->setCameraParameters(camera);
406  }
407 
408  this->cam = camera;
409 }
410 
421 {
422  if (!kltCylinders.empty()) {
423  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
424  "This feature is not implemented yet."
425  << std::endl;
426  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
427  cMo = cdMo;
428  init(I);
429  } else {
430  vpMbtDistanceKltPoints *kltpoly;
431 
432 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
433  std::vector<cv::Point2f> init_pts;
434  std::vector<long> init_ids;
435  std::vector<cv::Point2f> guess_pts;
436 #else
437  unsigned int nbp = 0;
438  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
439  kltpoly = *it;
440  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
441  nbp += (*it)->getCurrentNumberPoints();
442  }
443 
444  CvPoint2D32f *init_pts = NULL;
445  init_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(init_pts[0]));
446  long *init_ids = (long *)cvAlloc((unsigned int)tracker.getMaxFeatures() * sizeof(long));
447  unsigned int iter_pts = 0;
448 
449  CvPoint2D32f *guess_pts = NULL;
450  guess_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(guess_pts[0]));
451 #endif
452 
453  vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
454  vpHomogeneousMatrix cMcd = cdMc.inverse();
455 
456  vpRotationMatrix cdRc;
457  vpTranslationVector cdtc;
458 
459  cdMc.extract(cdRc);
460  cdMc.extract(cdtc);
461 
462  // unsigned int nbCur = 0;
463 
464  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
465  kltpoly = *it;
466 
467  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
468  kltpoly->polygon->changeFrame(cdMo);
469 
470  // Get the normal to the face at the current state cMo
471  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
472  plan.changeFrame(cMcd);
473 
474  vpColVector Nc = plan.getNormal();
475  Nc.normalize();
476 
477  double invDc = 1.0 / plan.getD();
478 
479  // Create the homography
480  vpMatrix cdHc;
481  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
482  cdHc /= cdHc[2][2];
483 
484  // Create the 2D homography
485  vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
486 
487  // Points displacement
488  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
489  // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
490  for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
491 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
492 #if TARGET_OS_IPHONE
493  if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
494  init_ids.end())
495 #else
496  if (std::find(init_ids.begin(), init_ids.end(),
497  (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
498 #endif
499  {
500  // KLT point already processed (a KLT point can exist in another
501  // vpMbtDistanceKltPoints due to possible overlapping faces)
502  continue;
503  }
504 #endif
505 
506  vpColVector cdp(3);
507  cdp[0] = iter->second.get_j();
508  cdp[1] = iter->second.get_i();
509  cdp[2] = 1.0;
510 
511 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
512  cv::Point2f p((float)cdp[0], (float)cdp[1]);
513  init_pts.push_back(p);
514 #if TARGET_OS_IPHONE
515  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
516 #else
517  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
518 #endif
519 #else
520  init_pts[iter_pts].x = (float)cdp[0];
521  init_pts[iter_pts].y = (float)cdp[1];
522  init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
523 #endif
524 
525  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
526 
527  if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
528  cdp[0] = 0.0;
529  cdp[1] = 0.0;
530  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
531  }
532 
533  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
534  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
535 
536 // Set value to the KLT tracker
537 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
538  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
539  guess_pts.push_back(p_guess);
540 #else
541  guess_pts[iter_pts].x = (float)cdp[0];
542  guess_pts[iter_pts++].y = (float)cdp[1];
543 #endif
544  }
545  }
546  }
547 
549 
550 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
551  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
552 #else
553  tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
554 
555  if (init_pts)
556  cvFree(&init_pts);
557  init_pts = NULL;
558 
559  if (guess_pts)
560  cvFree(&guess_pts);
561  guess_pts = NULL;
562 
563  if (init_ids)
564  cvFree(&init_ids);
565  init_ids = NULL;
566 #endif
567 
568  bool reInitialisation = false;
569  if (!useOgre)
570  faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
571  else {
572 #ifdef VISP_HAVE_OGRE
573  faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
574 #else
575  faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
576 #endif
577  }
578 
579  cam.computeFov(I.getWidth(), I.getHeight());
580 
581  if (useScanLine) {
584  }
585 
586  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
587  kltpoly = *it;
588  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
589  kltpoly->polygon->computePolygonClipped(cam);
590  kltpoly->init(tracker, m_mask);
591  }
592  }
593 
594  cMo = cdMo;
595  c0Mo = cMo;
596  ctTc0.eye();
597  }
598 }
599 
607 {
609  kltPoly->setCameraParameters(cam);
610  kltPoly->polygon = &polygon;
611  kltPoly->hiddenface = &faces;
612  kltPoly->useScanLine = useScanLine;
613  kltPolygons.push_back(kltPoly);
614 }
622 {
624  kltPoly->setCameraParameters(cam);
625  kltPoly->polygon = &polygon;
626  kltPoly->hiddenface = &faces;
627  kltPoly->useScanLine = useScanLine;
628  kltPolygons.push_back(kltPoly);
629 }
630 
638 {
640  tracker.track(cur);
641 
642  m_nbInfos = 0;
643  m_nbFaceUsed = 0;
644  // for (unsigned int i = 0; i < faces.size(); i += 1){
645  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
646  vpMbtDistanceKltPoints *kltpoly = *it;
647  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
649  // faces[i]->ransac();
650  if (kltpoly->hasEnoughPoints()) {
651  m_nbInfos += kltpoly->getCurrentNumberPoints();
652  m_nbFaceUsed++;
653  }
654  }
655  }
656 
657  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
658  ++it) {
659  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
660 
661  if (kltPolyCylinder->isTracked()) {
662  kltPolyCylinder->computeNbDetectedCurrent(tracker);
663  if (kltPolyCylinder->hasEnoughPoints()) {
664  m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
665  m_nbFaceUsed++;
666  }
667  }
668  }
669 }
670 
675 {
676  // # For a better Post Tracking, tracker should reinitialize if so faces
677  // don't have enough points but are visible. # Here we are not doing it for
678  // more speed performance.
679  bool reInitialisation = false;
680 
681  unsigned int initialNumber = 0;
682  unsigned int currentNumber = 0;
683  unsigned int shift = 0;
684  // for (unsigned int i = 0; i < faces.size(); i += 1){
685  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
686  vpMbtDistanceKltPoints *kltpoly = *it;
687  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
688  initialNumber += kltpoly->getInitialNumberPoint();
689  if (kltpoly->hasEnoughPoints()) {
690  vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
691  shift += 2 * kltpoly->getCurrentNumberPoints();
692  kltpoly->removeOutliers(sub_w, threshold_outlier);
693 
694  currentNumber += kltpoly->getCurrentNumberPoints();
695  }
696  // else{
697  // reInitialisation = true;
698  // break;
699  // }
700  }
701  }
702 
703  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
704  ++it) {
705  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
706 
707  if (kltPolyCylinder->isTracked()) {
708  initialNumber += kltPolyCylinder->getInitialNumberPoint();
709  if (kltPolyCylinder->hasEnoughPoints()) {
710  vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
711  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
712  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
713 
714  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
715  }
716  }
717  }
718 
719  // if(!reInitialisation){
720  double value = percentGood * (double)initialNumber;
721  if ((double)currentNumber < value) {
722  // std::cout << "Too many point disappear : " << initialNumber << "/"
723  // << currentNumber << std::endl;
724  reInitialisation = true;
725  } else {
726  if (!useOgre)
727  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
728  else {
729 #ifdef VISP_HAVE_OGRE
730  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
731 #else
732  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
733 #endif
734  }
735  }
736  // }
737 
738  if (reInitialisation)
739  return true;
740 
741  return false;
742 }
743 
748 {
749  vpMatrix L_true; // interaction matrix without weighting
750  vpMatrix LVJ_true;
751  vpColVector v; // "speed" for VVS
752 
753  vpMatrix LTL;
754  vpColVector LTR;
755  vpHomogeneousMatrix cMoPrev;
756  vpHomogeneousMatrix ctTc0_Prev;
757  vpColVector error_prev;
758  double mu = m_initialMu;
759 
760  double normRes = 0;
761  double normRes_1 = -1;
762  unsigned int iter = 0;
763 
765 
766  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
768 
769  bool reStartFromLastIncrement = false;
770  computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
771  if (reStartFromLastIncrement) {
772  ctTc0 = ctTc0_Prev;
773  }
774 
775  if (!reStartFromLastIncrement) {
777 
778  if (computeCovariance) {
779  L_true = m_L_klt;
780  if (!isoJoIdentity) {
782  cVo.buildFrom(cMo);
783  LVJ_true = (m_L_klt * cVo * oJo);
784  }
785  }
786 
787  normRes_1 = normRes;
788  normRes = 0.0;
789 
790  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
792  normRes += m_weightedError_klt[i];
793  }
794 
795  if ((iter == 0) || m_computeInteraction) {
796  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
797  for (unsigned int j = 0; j < 6; j++) {
798  m_L_klt[i][j] *= m_w_klt[i];
799  }
800  }
801  }
802 
804  v);
805 
806  cMoPrev = cMo;
807  ctTc0_Prev = ctTc0;
809  cMo = ctTc0 * c0Mo;
810  } // endif(!reStartFromLastIncrement)
811 
812  iter++;
813  }
814 
815  computeCovarianceMatrixVVS(isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
816 }
817 
819 {
820  unsigned int nbFeatures = 2 * m_nbInfos;
821 
822  m_L_klt.resize(nbFeatures, 6, false, false);
823  m_error_klt.resize(nbFeatures, false);
824 
825  m_weightedError_klt.resize(nbFeatures, false);
826  m_w_klt.resize(nbFeatures, false);
827  m_w_klt = 1;
828 
829  m_robust_klt.resize(nbFeatures);
831 }
832 
834 {
835  unsigned int shift = 0;
836  vpHomography H;
837 
838  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
839  vpMbtDistanceKltPoints *kltpoly = *it;
840  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
841  kltpoly->hasEnoughPoints()) {
842  vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
843  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
844 
845  try {
846  kltpoly->computeHomography(ctTc0, H);
847  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
848  } catch (...) {
849  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
850  }
851 
852  shift += 2 * kltpoly->getCurrentNumberPoints();
853  }
854  }
855 
856  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
857  ++it) {
858  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
859 
860  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
861  vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
862  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
863 
864  try {
865  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
866  } catch (...) {
867  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
868  }
869 
870  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
871  }
872  }
873 }
874 
883 {
884  preTracking(I);
885 
886  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
887  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
888  }
889 
890  computeVVS();
891 
892  if (postTracking(I, m_w_klt))
893  reinit(I);
894 }
895 
943 void vpMbKltTracker::loadConfigFile(const std::string &configFile)
944 {
945  // Load projection error config
946  vpMbTracker::loadConfigFile(configFile);
947 
948 #ifdef VISP_HAVE_XML2
949  vpMbtKltXmlParser xmlp;
950 
951  xmlp.setMaxFeatures(10000);
952  xmlp.setWindowSize(5);
953  xmlp.setQuality(0.01);
954  xmlp.setMinDistance(5);
955  xmlp.setHarrisParam(0.01);
956  xmlp.setBlockSize(3);
957  xmlp.setPyramidLevels(3);
961 
962  try {
963  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
964  xmlp.parse(configFile.c_str());
965  } catch (...) {
966  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
967  throw vpException(vpException::ioError, "problem to parse configuration file.");
968  }
969 
970  vpCameraParameters camera;
971  xmlp.getCameraParameters(camera);
972  setCameraParameters(camera);
973 
975  tracker.setWindowSize((int)xmlp.getWindowSize());
976  tracker.setQuality(xmlp.getQuality());
979  tracker.setBlockSize((int)xmlp.getBlockSize());
981  maskBorder = xmlp.getMaskBorder();
984 
985  // if(useScanLine)
986  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
987 
988  if (xmlp.hasNearClippingDistance())
990 
991  if (xmlp.hasFarClippingDistance())
993 
994  if (xmlp.getFovClipping())
996 
997  useLodGeneral = xmlp.getLodState();
1000 
1001  applyLodSettingInConfig = false;
1002  if (this->getNbPolygon() > 0) {
1003  applyLodSettingInConfig = true;
1007  }
1008 
1009 #else
1010  vpTRACE("You need the libXML2 to read the config file %s", configFile.c_str());
1011 #endif
1012 }
1013 
1026  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1027  const bool displayFullModel)
1028 {
1029  vpCameraParameters c = camera;
1030 
1031  if (clippingFlag > 3) // Contains at least one FOV constraint
1032  c.computeFov(I.getWidth(), I.getHeight());
1033 
1034  // vpMbtDistanceKltPoints *kltpoly;
1035  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1036 
1037  // Previous version 12/08/2015
1038  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1039  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1040  // kltpoly = *it;
1041  // kltpoly->polygon->changeFrame(cMo_);
1042  // kltpoly->polygon->computePolygonClipped(c);
1043  // }
1044  faces.computeClippedPolygons(cMo_, c);
1045 
1046  if (useScanLine && !displayFullModel)
1048 
1049  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1050  vpMbtDistanceKltPoints *kltpoly = *it;
1051 
1052  kltpoly->display(I, cMo_, camera, col, thickness, displayFullModel);
1053 
1054  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1055  kltpoly->displayPrimitive(I);
1056  // faces[i]->displayNormal(I);
1057  }
1058  }
1059 
1060  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1061  ++it) {
1062  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1063 
1064  kltPolyCylinder->display(I, cMo_, camera, col, thickness, displayFullModel);
1065 
1066  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1067  kltPolyCylinder->displayPrimitive(I);
1068  }
1069 
1070  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1071  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1072  }
1073 
1074 #ifdef VISP_HAVE_OGRE
1075  if (useOgre)
1076  faces.displayOgre(cMo_);
1077 #endif
1078 }
1079 
1092  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1093  const bool displayFullModel)
1094 {
1095  vpCameraParameters c = camera;
1096 
1097  if (clippingFlag > 3) // Contains at least one FOV constraint
1098  c.computeFov(I.getWidth(), I.getHeight());
1099 
1100  // vpMbtDistanceKltPoints *kltpoly;
1101  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1102 
1103  // Previous version 12/08/2015
1104  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1105  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1106  // kltpoly = *it;
1107  // kltpoly->polygon->changeFrame(cMo_);
1108  // kltpoly->polygon->computePolygonClipped(c);
1109  // }
1110  faces.computeClippedPolygons(cMo_, c);
1111 
1112  if (useScanLine && !displayFullModel)
1114 
1115  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1116  vpMbtDistanceKltPoints *kltpoly = *it;
1117 
1118  kltpoly->display(I, cMo_, camera, col, thickness, displayFullModel);
1119 
1120  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1121  kltpoly->displayPrimitive(I);
1122  // faces[i]->displayNormal(I);
1123  }
1124  }
1125 
1126  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1127  ++it) {
1128  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1129 
1130  kltPolyCylinder->display(I, cMo_, camera, col, thickness, displayFullModel);
1131 
1132  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1133  kltPolyCylinder->displayPrimitive(I);
1134  }
1135 
1136  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1137  (*it)->display(I, cMo_, camera, col, thickness);
1138  }
1139 
1140 #ifdef VISP_HAVE_OGRE
1141  if (useOgre)
1142  faces.displayOgre(cMo_);
1143 #endif
1144 }
1145 
1155 {
1156  unsigned int nbTotalPoints = 0;
1157  // for (unsigned int i = 0; i < faces.size(); i += 1){
1158  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1159  vpMbtDistanceKltPoints *kltpoly = *it;
1160  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1161  kltpoly->hasEnoughPoints()) {
1162  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1163  }
1164  }
1165 
1166  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1167  ++it) {
1168  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1169  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1170  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1171  }
1172 
1173  if (nbTotalPoints < 10) {
1174  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1176  "test tracking failed (too few points to realize a good tracking).");
1177  }
1178 }
1179 
1190 void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
1191  const std::string & /*name*/)
1192 {
1194  kltPoly->setCameraParameters(cam);
1195 
1196  kltPoly->buildFrom(p1, p2, radius);
1197 
1198  // Add the Cylinder BBox to the list of polygons
1199  kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1200  kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1201  kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1202  kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1203 
1204  kltPoly->hiddenface = &faces;
1205  kltPoly->useScanLine = useScanLine;
1206  kltCylinders.push_back(kltPoly);
1207 }
1208 
1220 void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
1221  const int /*idFace*/, const std::string &name)
1222 {
1223  addCircle(p1, p2, p3, radius, name);
1224 }
1225 
1234 void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r,
1235  const std::string &name)
1236 {
1237  bool already_here = false;
1238 
1239  // for(std::list<vpMbtDistanceCircle*>::const_iterator
1240  // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1241  // ci = *it;
1242  // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1243  // samePoint(*(ci->p3),P3)) ||
1244  // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1245  // samePoint(*(ci->p3),P2)) ){
1246  // already_here = (std::fabs(ci->radius - r) <
1247  // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1248  // r));
1249  // }
1250  // }
1251 
1252  if (!already_here) {
1254 
1255  ci->setCameraParameters(cam);
1256  ci->setName(name);
1257  ci->buildFrom(P1, P2, P3, r);
1258  circles_disp.push_back(ci);
1259  }
1260 }
1261 
1274 void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1275  const vpHomogeneousMatrix &cMo_, const bool verbose,
1276  const vpHomogeneousMatrix &T)
1277 {
1278  this->cMo.eye();
1279 
1280 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1281  if (cur != NULL) {
1282  cvReleaseImage(&cur);
1283  cur = NULL;
1284  }
1285 #endif
1286 
1287  firstInitialisation = true;
1288 
1289  // delete the Klt Polygon features
1290  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1291  vpMbtDistanceKltPoints *kltpoly = *it;
1292  if (kltpoly != NULL) {
1293  delete kltpoly;
1294  }
1295  kltpoly = NULL;
1296  }
1297  kltPolygons.clear();
1298 
1299  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1300  ++it) {
1301  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1302  if (kltPolyCylinder != NULL) {
1303  delete kltPolyCylinder;
1304  }
1305  kltPolyCylinder = NULL;
1306  }
1307  kltCylinders.clear();
1308 
1309  // delete the structures used to display circles
1310  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1311  vpMbtDistanceCircle *ci = *it;
1312  if (ci != NULL) {
1313  delete ci;
1314  }
1315  ci = NULL;
1316  }
1317 
1318  faces.reset();
1319 
1320  loadModel(cad_name, verbose, T);
1321  initFromPose(I, cMo_);
1322 }
1323 
1331 void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1332 {
1333  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1334  vpMbtDistanceKltPoints *kltpoly = *it;
1335  if (kltpoly->polygon->getName() == name) {
1336  kltpoly->setTracked(useKltTracking);
1337  }
1338  }
1339 }
1340 
1341 #elif !defined(VISP_BUILD_SHARED_LIBS)
1342 // Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
1343 // symbols
1344 void dummy_vpMbKltTracker(){};
1345 #endif // VISP_HAVE_OPENCV
virtual void setKltOpencv(const vpKltOpencv &t)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="")
bool m_computeInteraction
Definition: vpMbTracker.h:191
virtual void track(const vpImage< unsigned char > &I)
unsigned int getCols() const
Definition: vpImage.h:169
vpMatrix get_K_inverse() const
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:130
unsigned int getMaskBorder() const
void setQuality(const double &q)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void displayPrimitive(const vpImage< unsigned char > &_I)
void setTracked(const bool &track)
virtual ~vpMbKltTracker()
double getHarrisParam() const
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
void setCameraParameters(const vpCameraParameters &camera)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
void setHarrisFreeParameter(double harris_k)
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:115
unsigned int getWidth() const
Definition: vpImage.h:239
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void getCameraParameters(vpCameraParameters &_cam) const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:149
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
Implementation of an homogeneous matrix and operations on such kind of matrices.
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
Definition: vpKltOpencv.h:111
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
#define vpERROR_TRACE
Definition: vpDebug.h:393
virtual void loadConfigFile(const std::string &configFile)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setMaxFeatures(const int maxCount)
std::list< vpMbtDistanceKltCylinder * > kltCylinders
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:119
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
unsigned int getBlockSize() const
double getNearClippingDistance() const
void setOgreShowConfigDialog(const bool showConfigDialog)
bool modelInitialised
Definition: vpMbTracker.h:129
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:71
vpMbScanLine & getMbScanLineRenderer()
vpColVector m_weightedError_klt
Weighted error.
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getMaxFeatures() const
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:161
bool getLodState() 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:134
virtual void reinit(const vpImage< unsigned char > &I)
virtual void computeVVSInteractionMatrixAndResidu()
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void testTracking()
bool hasNearClippingDistance() const
Class that defines what is a point.
Definition: vpPoint.h:58
virtual void loadConfigFile(const std::string &configFile)
unsigned int getInitialNumberPoint() const
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:117
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
std::map< int, int > & getCurrentPointsInd()
double getFarClippingDistance() const
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
bool hasFarClippingDistance() const
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpAROgre * getOgreContext()
void changeFrame(const vpHomogeneousMatrix &cMo)
unsigned int getRows() const
Definition: vpImage.h:211
std::vector< vpImagePoint > getKltImagePoints() const
void setPyramidLevels(const unsigned int &pL)
vpRobust m_robust_klt
Robust.
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.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:121
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:66
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)
std::string getName() const
Definition: vpMbtPolygon.h:108
double getQuality() const
Definition: vpKltOpencv.h:133
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
void displayPrimitive(const vpImage< unsigned char > &_I)
void setWindowSize(const unsigned int &w)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:164
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:183
void setAngleDisappear(const double &adisappear)
double getMinPolygonAreaThreshold() const
void setTrackerId(int tid)
Definition: vpKltOpencv.h:155
unsigned int maskBorder
Erosion of the mask.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Generic class defining intrinsic camera parameters.
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:102
unsigned int m_nbFaceUsed
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
std::map< int, vpImagePoint > getKltImagePointsWithId() const
unsigned int getInitialNumberPoint() const
void extract(vpRotationMatrix &R) const
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:199
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:193
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
void setHarrisParam(const double &hp)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:146
unsigned int getRows() const
Definition: vpArray2D.h:156
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:151
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 >())
vpColVector getNormal() const
Definition: vpPlane.cpp:238
void setPyramidLevels(const int pyrMaxLevel)
bool useScanLine
Use scanline rendering.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:195
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
double get_px() const
std::map< int, vpImagePoint > & getCurrentPoints()
static double rad(double deg)
Definition: vpMath.h:102
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
double threshold_outlier
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
cv::Mat cur
Temporary OpenCV image for fast conversion.
double getMinLineLengthThreshold() const
void setMaxFeatures(const unsigned int &mF)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
vpColVector m_w_klt
Robust weights.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:227
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
void setWindowSize(const int winSize)
double getQuality() const
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:185
vpMatrix get_K() const
static double deg(double rad)
Definition: vpMath.h:95
virtual void setCameraParameters(const vpCameraParameters &_cam)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:144
void setCameraParameters(const vpCameraParameters &cam)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:162
void preTracking(const vpImage< unsigned char > &I)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:181
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setAngleAppear(const double &aappear)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
vpHomogeneousMatrix inverse() const
void setBlockSize(const int blockSize)
static vpHomogeneousMatrix direct(const vpColVector &v)
void getFeature(const int &index, long &id, float &x, float &y) const
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:153
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
virtual void computeVVSInit()
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 getHeight() const
Definition: vpImage.h:178
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
virtual void setClipping(const unsigned int &flags)
unsigned int m_nbInfos
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:88
void setUseHarris(const int useHarrisDetector)
unsigned int getCurrentNumberPoints() 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:159
double getMinDistance() const
Definition: vpKltOpencv.h:118
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
void displayOgre(const vpHomogeneousMatrix &cMo)
double getAngleAppear() const
unsigned int getCurrentNumberPoints() const
vpColVector m_error_klt
(s - s*)
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
virtual void setFarClippingDistance(const double &dist)
double getMinDistance() const
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
bool getFovClipping() const
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:178
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
Class that consider the case of a translation vector.
virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
std::list< vpMbtDistanceKltPoints * > kltPolygons
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:123
void setMaskBorder(const unsigned int &mb)
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
vpHomogeneousMatrix c0Mo
Initial pose.
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:135
double getD() const
Definition: vpPlane.h:108
void track(const cv::Mat &I)
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)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
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:244
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
vpMatrix m_L_klt
Interaction matrix.
void computeFov(const unsigned int &w, const unsigned int &h)