Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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 #include <visp3/mbt/vpMbtXmlGenericParser.h>
45 
46 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
47 
48 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
49 #include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
50 #endif
51 
53  :
54 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
55  cur(),
56 #else
57  cur(NULL),
58 #endif
59  c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
60  kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
61  m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt()
62 {
65  tracker.setMaxFeatures(10000);
67  tracker.setQuality(0.01);
72 
73 #ifdef VISP_HAVE_OGRE
74  faces.getOgreContext()->setWindowName("MBT Klt");
75 #endif
76 
77  m_lambda = 0.8;
78  m_maxIter = 200;
79 }
80 
86 {
87 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
88  if (cur != NULL) {
89  cvReleaseImage(&cur);
90  cur = NULL;
91  }
92 #endif
93 
94  // delete the Klt Polygon features
95  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
96  vpMbtDistanceKltPoints *kltpoly = *it;
97  if (kltpoly != NULL) {
98  delete kltpoly;
99  }
100  kltpoly = NULL;
101  }
102  kltPolygons.clear();
103 
104  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
105  ++it) {
106  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
107  if (kltPolyCylinder != NULL) {
108  delete kltPolyCylinder;
109  }
110  kltPolyCylinder = NULL;
111  }
112  kltCylinders.clear();
113 
114  // delete the structures used to display circles
115  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
116  vpMbtDistanceCircle *ci = *it;
117  if (ci != NULL) {
118  delete ci;
119  }
120  ci = NULL;
121  }
122 
123  circles_disp.clear();
124 }
125 
127 {
128  if (!modelInitialised) {
129  throw vpException(vpException::fatalError, "model not initialized");
130  }
131 
132  bool reInitialisation = false;
133  if (!useOgre)
134  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
135  else {
136 #ifdef VISP_HAVE_OGRE
137  if (!faces.isOgreInitialised()) {
140  faces.initOgre(cam);
141  // Turn off Ogre config dialog display for the next call to this
142  // function since settings are saved in the ogre.cfg file and used
143  // during the next call
144  ogreShowConfigDialog = false;
145  }
146 
147  faces.setVisibleOgre(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
148 
149 #else
150  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
151 #endif
152  }
153  reinit(I);
154 }
155 
157 {
158  c0Mo = cMo;
159  ctTc0.eye();
160 
162 
163  cam.computeFov(I.getWidth(), I.getHeight());
164 
165  if (useScanLine) {
168  }
169 
170 // mask
171 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
172  cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
173 #else
174  IplImage *mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
175  cvZero(mask);
176 #endif
177 
178  vpMbtDistanceKltPoints *kltpoly;
179  vpMbtDistanceKltCylinder *kltPolyCylinder;
180  if (useScanLine) {
182  } else {
183  unsigned char val = 255 /* - i*15*/;
184  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
185  kltpoly = *it;
186  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
187  // need to changeFrame when reinit() is called by postTracking
188  // other solution is
189  kltpoly->polygon->changeFrame(cMo);
190  kltpoly->polygon->computePolygonClipped(cam); // Might not be necessary when scanline is activated
191  kltpoly->updateMask(mask, val, maskBorder);
192  }
193  }
194 
195  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
196  ++it) {
197  kltPolyCylinder = *it;
198 
199  if (kltPolyCylinder->isTracked()) {
200  for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
201  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
202  if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
203  faces[indCylBBox]->computePolygonClipped(cam); // Might not be necessary when scanline is activated
204  }
205  }
206 
207  kltPolyCylinder->updateMask(mask, val, maskBorder);
208  }
209  }
210  }
211 
212  tracker.initTracking(cur, mask);
213  // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
214  // the points are valid for tracking and avoid too fast reinitialisations.
215  // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
216  // points" << std::endl;
217 
218  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
219  kltpoly = *it;
220  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
221  kltpoly->init(tracker, m_mask);
222  }
223  }
224 
225  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
226  ++it) {
227  kltPolyCylinder = *it;
228 
229  if (kltPolyCylinder->isTracked())
230  kltPolyCylinder->init(tracker, cMo);
231  }
232 
233 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
234  cvReleaseImage(&mask);
235 #endif
236 }
237 
243 {
244  cMo.eye();
245 
246 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
247  if (cur != NULL) {
248  cvReleaseImage(&cur);
249  cur = NULL;
250  }
251 #endif
252 
253  // delete the Klt Polygon features
254  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
255  vpMbtDistanceKltPoints *kltpoly = *it;
256  if (kltpoly != NULL) {
257  delete kltpoly;
258  }
259  kltpoly = NULL;
260  }
261  kltPolygons.clear();
262 
263  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
264  ++it) {
265  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
266  if (kltPolyCylinder != NULL) {
267  delete kltPolyCylinder;
268  }
269  kltPolyCylinder = NULL;
270  }
271  kltCylinders.clear();
272 
273  // delete the structures used to display circles
274  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
275  vpMbtDistanceCircle *ci = *it;
276  if (ci != NULL) {
277  delete ci;
278  }
279  ci = NULL;
280  }
281 
282  circles_disp.clear();
283 
284  m_computeInteraction = true;
285  firstInitialisation = true;
286  computeCovariance = false;
287 
290 
291  tracker.setMaxFeatures(10000);
293  tracker.setQuality(0.01);
298 
301 
303 
304  maskBorder = 5;
305  threshold_outlier = 0.5;
306  percentGood = 0.6;
307 
308  m_lambda = 0.8;
309  m_maxIter = 200;
310 
311  faces.reset();
312 
314 
315  useScanLine = false;
316 
317 #ifdef VISP_HAVE_OGRE
318  useOgre = false;
319 #endif
320 }
321 
330 std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
331 {
332  std::vector<vpImagePoint> kltPoints;
333  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
334  long id;
335  float x_tmp, y_tmp;
336  tracker.getFeature((int)i, id, x_tmp, y_tmp);
337  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
338  }
339 
340  return kltPoints;
341 }
342 
351 std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
352 {
353  std::map<int, vpImagePoint> kltPoints;
354  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
355  long id;
356  float x_tmp, y_tmp;
357  tracker.getFeature((int)i, id, x_tmp, y_tmp);
358 #if TARGET_OS_IPHONE
359  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
360 #else
361  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
362 #endif
363  }
364 
365  return kltPoints;
366 }
367 
374 {
382 }
383 
390 {
391  // for (unsigned int i = 0; i < faces.size(); i += 1){
392  // faces[i]->setCameraParameters(camera);
393  // }
394 
395  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
396  vpMbtDistanceKltPoints *kltpoly = *it;
397  kltpoly->setCameraParameters(camera);
398  }
399 
400  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
401  ++it) {
402  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
403  kltPolyCylinder->setCameraParameters(camera);
404  }
405 
406  this->cam = camera;
407 }
408 
409 void vpMbKltTracker::setPose(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
410  const vpHomogeneousMatrix &cdMo)
411 {
412  if (I_color) {
413  vpImageConvert::convert(*I_color, m_I);
414  }
415 
416  if (!kltCylinders.empty()) {
417  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
418  "This feature is not implemented yet."
419  << std::endl;
420  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
421  cMo = cdMo;
422  if (I) {
423  init(*I);
424  } else {
425  init(m_I);
426  }
427  } else {
428  vpMbtDistanceKltPoints *kltpoly;
429 
430 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
431  std::vector<cv::Point2f> init_pts;
432  std::vector<long> init_ids;
433  std::vector<cv::Point2f> guess_pts;
434 #else
435  unsigned int nbp = 0;
436  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
437  kltpoly = *it;
438  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
439  nbp += (*it)->getCurrentNumberPoints();
440  }
441 
442  CvPoint2D32f *init_pts = NULL;
443  init_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(init_pts[0]));
444  long *init_ids = (long *)cvAlloc((unsigned int)tracker.getMaxFeatures() * sizeof(long));
445  unsigned int iter_pts = 0;
446 
447  CvPoint2D32f *guess_pts = NULL;
448  guess_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(guess_pts[0]));
449 #endif
450 
451  vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
452  vpHomogeneousMatrix cMcd = cdMc.inverse();
453 
454  vpRotationMatrix cdRc;
455  vpTranslationVector cdtc;
456 
457  cdMc.extract(cdRc);
458  cdMc.extract(cdtc);
459 
460  // unsigned int nbCur = 0;
461 
462  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
463  kltpoly = *it;
464 
465  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
466  kltpoly->polygon->changeFrame(cdMo);
467 
468  // Get the normal to the face at the current state cMo
469  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
470  plan.changeFrame(cMcd);
471 
472  vpColVector Nc = plan.getNormal();
473  Nc.normalize();
474 
475  double invDc = 1.0 / plan.getD();
476 
477  // Create the homography
478  vpMatrix cdHc;
479  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
480  cdHc /= cdHc[2][2];
481 
482  // Create the 2D homography
483  vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
484 
485  // Points displacement
486  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
487  // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
488  for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
489 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
490 #if TARGET_OS_IPHONE
491  if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
492  init_ids.end())
493 #else
494  if (std::find(init_ids.begin(), init_ids.end(),
495  (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
496 #endif
497  {
498  // KLT point already processed (a KLT point can exist in another
499  // vpMbtDistanceKltPoints due to possible overlapping faces)
500  continue;
501  }
502 #endif
503 
504  vpColVector cdp(3);
505  cdp[0] = iter->second.get_j();
506  cdp[1] = iter->second.get_i();
507  cdp[2] = 1.0;
508 
509 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
510  cv::Point2f p((float)cdp[0], (float)cdp[1]);
511  init_pts.push_back(p);
512 #if TARGET_OS_IPHONE
513  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
514 #else
515  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
516 #endif
517 #else
518  init_pts[iter_pts].x = (float)cdp[0];
519  init_pts[iter_pts].y = (float)cdp[1];
520  init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
521 #endif
522 
523  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
524 
525  if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
526  cdp[0] = 0.0;
527  cdp[1] = 0.0;
528  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
529  }
530 
531  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
532  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
533 
534 // Set value to the KLT tracker
535 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
536  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
537  guess_pts.push_back(p_guess);
538 #else
539  guess_pts[iter_pts].x = (float)cdp[0];
540  guess_pts[iter_pts++].y = (float)cdp[1];
541 #endif
542  }
543  }
544  }
545 
546  if (I) {
548  } else {
550  }
551 
552 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
553  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
554 #else
555  tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
556 
557  if (init_pts)
558  cvFree(&init_pts);
559  init_pts = NULL;
560 
561  if (guess_pts)
562  cvFree(&guess_pts);
563  guess_pts = NULL;
564 
565  if (init_ids)
566  cvFree(&init_ids);
567  init_ids = NULL;
568 #endif
569 
570  bool reInitialisation = false;
571  if (!useOgre) {
572  if (I) {
573  faces.setVisible(I->getWidth(), I->getHeight(), cam, cdMo, angleAppears, angleDisappears, reInitialisation);
574  } else {
575  faces.setVisible(m_I.getWidth(), m_I.getHeight(), cam, cdMo, angleAppears, angleDisappears, reInitialisation);
576  }
577  } else {
578 #ifdef VISP_HAVE_OGRE
579  if (I) {
580  faces.setVisibleOgre(I->getWidth(), I->getHeight(), cam, cdMo, angleAppears, angleDisappears, reInitialisation);
581  } else {
582  faces.setVisibleOgre(m_I.getWidth(), m_I.getHeight(), cam, cdMo, angleAppears, angleDisappears, reInitialisation);
583  }
584 #else
585  if (I) {
586  faces.setVisible(I->getWidth(), I->getHeight(), cam, cdMo, angleAppears, angleDisappears, reInitialisation);
587  } else {
588  faces.setVisible(m_I.getWidth(), m_I.getHeight(), cam, cdMo, angleAppears, angleDisappears, reInitialisation);
589  }
590 #endif
591  }
592 
593  cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
594 
595  if (useScanLine) {
598  }
599 
600  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
601  kltpoly = *it;
602  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
603  kltpoly->polygon->computePolygonClipped(cam);
604  kltpoly->init(tracker, m_mask);
605  }
606  }
607 
608  cMo = cdMo;
609  c0Mo = cMo;
610  ctTc0.eye();
611  }
612 }
613 
624 {
625  vpMbKltTracker::setPose(&I, NULL, cdMo);
626 }
627 
638 {
639  vpMbKltTracker::setPose(NULL, &I_color, cdMo);
640 }
641 
649 {
651  kltPoly->setCameraParameters(cam);
652  kltPoly->polygon = &polygon;
653  kltPoly->hiddenface = &faces;
654  kltPoly->useScanLine = useScanLine;
655  kltPolygons.push_back(kltPoly);
656 }
664 {
666  kltPoly->setCameraParameters(cam);
667  kltPoly->polygon = &polygon;
668  kltPoly->hiddenface = &faces;
669  kltPoly->useScanLine = useScanLine;
670  kltPolygons.push_back(kltPoly);
671 }
672 
680 {
682  tracker.track(cur);
683 
684  m_nbInfos = 0;
685  m_nbFaceUsed = 0;
686  // for (unsigned int i = 0; i < faces.size(); i += 1){
687  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
688  vpMbtDistanceKltPoints *kltpoly = *it;
689  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
691  // faces[i]->ransac();
692  if (kltpoly->hasEnoughPoints()) {
693  m_nbInfos += kltpoly->getCurrentNumberPoints();
694  m_nbFaceUsed++;
695  }
696  }
697  }
698 
699  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
700  ++it) {
701  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
702 
703  if (kltPolyCylinder->isTracked()) {
704  kltPolyCylinder->computeNbDetectedCurrent(tracker);
705  if (kltPolyCylinder->hasEnoughPoints()) {
706  m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
707  m_nbFaceUsed++;
708  }
709  }
710  }
711 }
712 
717 {
718  // # For a better Post Tracking, tracker should reinitialize if so faces
719  // don't have enough points but are visible. # Here we are not doing it for
720  // more speed performance.
721  bool reInitialisation = false;
722 
723  unsigned int initialNumber = 0;
724  unsigned int currentNumber = 0;
725  unsigned int shift = 0;
726  // for (unsigned int i = 0; i < faces.size(); i += 1){
727  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
728  vpMbtDistanceKltPoints *kltpoly = *it;
729  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
730  initialNumber += kltpoly->getInitialNumberPoint();
731  if (kltpoly->hasEnoughPoints()) {
732  vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
733  shift += 2 * kltpoly->getCurrentNumberPoints();
734  kltpoly->removeOutliers(sub_w, threshold_outlier);
735 
736  currentNumber += kltpoly->getCurrentNumberPoints();
737  }
738  // else{
739  // reInitialisation = true;
740  // break;
741  // }
742  }
743  }
744 
745  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
746  ++it) {
747  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
748 
749  if (kltPolyCylinder->isTracked()) {
750  initialNumber += kltPolyCylinder->getInitialNumberPoint();
751  if (kltPolyCylinder->hasEnoughPoints()) {
752  vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
753  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
754  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
755 
756  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
757  }
758  }
759  }
760 
761  // if(!reInitialisation){
762  double value = percentGood * (double)initialNumber;
763  if ((double)currentNumber < value) {
764  // std::cout << "Too many point disappear : " << initialNumber << "/"
765  // << currentNumber << std::endl;
766  reInitialisation = true;
767  } else {
768  if (!useOgre)
769  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
770  else {
771 #ifdef VISP_HAVE_OGRE
772  faces.setVisibleOgre(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
773 #else
774  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
775 #endif
776  }
777  }
778  // }
779 
780  if (reInitialisation)
781  return true;
782 
783  return false;
784 }
785 
790 {
791  vpMatrix L_true; // interaction matrix without weighting
792  vpMatrix LVJ_true;
793  vpColVector v; // "speed" for VVS
794 
795  vpMatrix LTL;
796  vpColVector LTR;
797  vpHomogeneousMatrix cMoPrev;
798  vpHomogeneousMatrix ctTc0_Prev;
799  vpColVector error_prev;
800  double mu = m_initialMu;
801 
802  double normRes = 0;
803  double normRes_1 = -1;
804  unsigned int iter = 0;
805 
807 
808  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
810 
811  bool reStartFromLastIncrement = false;
812  computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
813  if (reStartFromLastIncrement) {
814  ctTc0 = ctTc0_Prev;
815  }
816 
817  if (!reStartFromLastIncrement) {
819 
820  if (computeCovariance) {
821  L_true = m_L_klt;
822  if (!isoJoIdentity) {
824  cVo.buildFrom(cMo);
825  LVJ_true = (m_L_klt * cVo * oJo);
826  }
827  }
828 
829  normRes_1 = normRes;
830  normRes = 0.0;
831 
832  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
834  normRes += m_weightedError_klt[i];
835  }
836 
837  if ((iter == 0) || m_computeInteraction) {
838  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
839  for (unsigned int j = 0; j < 6; j++) {
840  m_L_klt[i][j] *= m_w_klt[i];
841  }
842  }
843  }
844 
846  v);
847 
848  cMoPrev = cMo;
849  ctTc0_Prev = ctTc0;
851  cMo = ctTc0 * c0Mo;
852  } // endif(!reStartFromLastIncrement)
853 
854  iter++;
855  }
856 
857  computeCovarianceMatrixVVS(isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
858 }
859 
861 {
862  unsigned int nbFeatures = 2 * m_nbInfos;
863 
864  m_L_klt.resize(nbFeatures, 6, false, false);
865  m_error_klt.resize(nbFeatures, false);
866 
867  m_weightedError_klt.resize(nbFeatures, false);
868  m_w_klt.resize(nbFeatures, false);
869  m_w_klt = 1;
870 
871  m_robust_klt.resize(nbFeatures);
873 }
874 
876 {
877  unsigned int shift = 0;
878  vpHomography H;
879 
880  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
881  vpMbtDistanceKltPoints *kltpoly = *it;
882  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
883  kltpoly->hasEnoughPoints()) {
884  vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
885  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
886 
887  try {
888  kltpoly->computeHomography(ctTc0, H);
889  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
890  } catch (...) {
891  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
892  }
893 
894  shift += 2 * kltpoly->getCurrentNumberPoints();
895  }
896  }
897 
898  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
899  ++it) {
900  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
901 
902  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
903  vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
904  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
905 
906  try {
907  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
908  } catch (...) {
909  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
910  }
911 
912  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
913  }
914  }
915 }
916 
925 {
926  preTracking(I);
927 
928  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
929  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
930  }
931 
932  computeVVS();
933 
934  if (postTracking(I, m_w_klt))
935  reinit(I);
936 
937  if (displayFeatures) {
939  }
940 }
941 
950 {
951  vpImageConvert::convert(I_color, m_I);
952  preTracking(m_I);
953 
954  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
955  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
956  }
957 
958  computeVVS();
959 
960  if (postTracking(m_I, m_w_klt))
961  reinit(m_I);
962 }
963 
1006 void vpMbKltTracker::loadConfigFile(const std::string &configFile)
1007 {
1008  // Load projection error config
1009  vpMbTracker::loadConfigFile(configFile);
1010 
1011 #ifdef VISP_HAVE_PUGIXML
1013 
1014  xmlp.setKltMaxFeatures(10000);
1015  xmlp.setKltWindowSize(5);
1016  xmlp.setKltQuality(0.01);
1017  xmlp.setKltMinDistance(5);
1018  xmlp.setKltHarrisParam(0.01);
1019  xmlp.setKltBlockSize(3);
1020  xmlp.setKltPyramidLevels(3);
1024 
1025  try {
1026  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1027  xmlp.parse(configFile.c_str());
1028  } catch (...) {
1029  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1030  throw vpException(vpException::ioError, "problem to parse configuration file.");
1031  }
1032 
1033  vpCameraParameters camera;
1034  xmlp.getCameraParameters(camera);
1035  setCameraParameters(camera);
1036 
1038  tracker.setWindowSize((int)xmlp.getKltWindowSize());
1042  tracker.setBlockSize((int)xmlp.getKltBlockSize());
1044  maskBorder = xmlp.getKltMaskBorder();
1047 
1048  // if(useScanLine)
1049  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1050 
1051  if (xmlp.hasNearClippingDistance())
1053 
1054  if (xmlp.hasFarClippingDistance())
1056 
1057  if (xmlp.getFovClipping())
1059 
1060  useLodGeneral = xmlp.getLodState();
1063 
1064  applyLodSettingInConfig = false;
1065  if (this->getNbPolygon() > 0) {
1066  applyLodSettingInConfig = true;
1070  }
1071 
1072 #else
1073  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
1074 #endif
1075 }
1076 
1089  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1090  const bool displayFullModel)
1091 {
1092  std::vector<std::vector<double> > models = vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, camera, displayFullModel);
1093 
1094  for (size_t i = 0; i < models.size(); i++) {
1095  if (vpMath::equal(models[i][0], 0)) {
1096  vpImagePoint ip1(models[i][1], models[i][2]);
1097  vpImagePoint ip2(models[i][3], models[i][4]);
1098  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1099  } else if (vpMath::equal(models[i][0], 1)) {
1100  vpImagePoint center(models[i][1], models[i][2]);
1101  double mu20 = models[i][3];
1102  double mu11 = models[i][4];
1103  double mu02 = models[i][5];
1104  vpDisplay::displayEllipse(I, center, mu20, mu11, mu02, true, col, thickness);
1105  }
1106  }
1107 
1108  if (displayFeatures) {
1109  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1110  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1113 
1115  double id = m_featuresToBeDisplayedKlt[i][5];
1116  std::stringstream ss;
1117  ss << id;
1118  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1119  }
1120  }
1121  }
1122 
1123 #ifdef VISP_HAVE_OGRE
1124  if (useOgre)
1125  faces.displayOgre(cMo_);
1126 #endif
1127 }
1128 
1141  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1142  const bool displayFullModel)
1143 {
1144  std::vector<std::vector<double> > models = vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, camera, displayFullModel);
1145 
1146  for (size_t i = 0; i < models.size(); i++) {
1147  if (vpMath::equal(models[i][0], 0)) {
1148  vpImagePoint ip1(models[i][1], models[i][2]);
1149  vpImagePoint ip2(models[i][3], models[i][4]);
1150  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1151  } else if (vpMath::equal(models[i][0], 1)) {
1152  vpImagePoint center(models[i][1], models[i][2]);
1153  double mu20 = models[i][3];
1154  double mu11 = models[i][4];
1155  double mu02 = models[i][5];
1156  vpDisplay::displayEllipse(I, center, mu20, mu11, mu02, true, col, thickness);
1157  }
1158  }
1159 
1160  if (displayFeatures) {
1161  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1162  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1165 
1167  double id = m_featuresToBeDisplayedKlt[i][5];
1168  std::stringstream ss;
1169  ss << id;
1170  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1171  }
1172  }
1173  }
1174 
1175 #ifdef VISP_HAVE_OGRE
1176  if (useOgre)
1177  faces.displayOgre(cMo_);
1178 #endif
1179 }
1180 
1181 std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1182 {
1183  std::vector<std::vector<double> > features;
1184 
1185  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1186  vpMbtDistanceKltPoints *kltpoly = *it;
1187 
1188  if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1189  std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1190  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1191  }
1192  }
1193 
1194  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1195  ++it) {
1196  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1197 
1198  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1199  std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1200  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1201  }
1202  }
1203 
1204  return features;
1205 }
1206 
1220 std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1221  const vpHomogeneousMatrix &cMo_,
1222  const vpCameraParameters &camera,
1223  const bool displayFullModel)
1224 {
1225  std::vector<std::vector<double> > models;
1226 
1227  vpCameraParameters c = camera;
1228 
1229  if (clippingFlag > 3) // Contains at least one FOV constraint
1230  c.computeFov(width, height);
1231 
1232  // vpMbtDistanceKltPoints *kltpoly;
1233  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1234 
1235  // Previous version 12/08/2015
1236  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1237  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1238  // kltpoly = *it;
1239  // kltpoly->polygon->changeFrame(cMo_);
1240  // kltpoly->polygon->computePolygonClipped(c);
1241  // }
1242  faces.computeClippedPolygons(cMo_, c);
1243 
1244  if (useScanLine && !displayFullModel)
1245  faces.computeScanLineRender(cam, width, height);
1246 
1247  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1248  vpMbtDistanceKltPoints *kltpoly = *it;
1249  std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(camera, displayFullModel);
1250  models.insert(models.end(), modelLines.begin(), modelLines.end());
1251  }
1252 
1253  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1254  ++it) {
1255  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1256  std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo_, camera);
1257  models.insert(models.end(), modelLines.begin(), modelLines.end());
1258  }
1259 
1260  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1261  vpMbtDistanceCircle *displayCircle = *it;
1262  std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo_, camera, displayFullModel);
1263  models.push_back(paramsCircle);
1264  }
1265 
1266  return models;
1267 }
1268 
1278 {
1279  unsigned int nbTotalPoints = 0;
1280  // for (unsigned int i = 0; i < faces.size(); i += 1){
1281  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1282  vpMbtDistanceKltPoints *kltpoly = *it;
1283  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1284  kltpoly->hasEnoughPoints()) {
1285  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1286  }
1287  }
1288 
1289  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1290  ++it) {
1291  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1292  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1293  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1294  }
1295 
1296  if (nbTotalPoints < 10) {
1297  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1299  "test tracking failed (too few points to realize a good tracking).");
1300  }
1301 }
1302 
1313 void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
1314  const std::string & /*name*/)
1315 {
1317  kltPoly->setCameraParameters(cam);
1318 
1319  kltPoly->buildFrom(p1, p2, radius);
1320 
1321  // Add the Cylinder BBox to the list of polygons
1322  kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1323  kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1324  kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1325  kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1326 
1327  kltPoly->hiddenface = &faces;
1328  kltPoly->useScanLine = useScanLine;
1329  kltCylinders.push_back(kltPoly);
1330 }
1331 
1343 void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
1344  const int /*idFace*/, const std::string &name)
1345 {
1346  addCircle(p1, p2, p3, radius, name);
1347 }
1348 
1357 void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r,
1358  const std::string &name)
1359 {
1360  bool already_here = false;
1361 
1362  // for(std::list<vpMbtDistanceCircle*>::const_iterator
1363  // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1364  // ci = *it;
1365  // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1366  // samePoint(*(ci->p3),P3)) ||
1367  // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1368  // samePoint(*(ci->p3),P2)) ){
1369  // already_here = (std::fabs(ci->radius - r) <
1370  // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1371  // r));
1372  // }
1373  // }
1374 
1375  if (!already_here) {
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 
1397 void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1398  const vpHomogeneousMatrix &cMo_, const bool verbose,
1399  const vpHomogeneousMatrix &T)
1400 {
1401  this->cMo.eye();
1402 
1403 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1404  if (cur != NULL) {
1405  cvReleaseImage(&cur);
1406  cur = NULL;
1407  }
1408 #endif
1409 
1410  firstInitialisation = true;
1411 
1412  // delete the Klt Polygon features
1413  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1414  vpMbtDistanceKltPoints *kltpoly = *it;
1415  if (kltpoly != NULL) {
1416  delete kltpoly;
1417  }
1418  kltpoly = NULL;
1419  }
1420  kltPolygons.clear();
1421 
1422  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1423  ++it) {
1424  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1425  if (kltPolyCylinder != NULL) {
1426  delete kltPolyCylinder;
1427  }
1428  kltPolyCylinder = NULL;
1429  }
1430  kltCylinders.clear();
1431 
1432  // delete the structures used to display circles
1433  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1434  vpMbtDistanceCircle *ci = *it;
1435  if (ci != NULL) {
1436  delete ci;
1437  }
1438  ci = NULL;
1439  }
1440 
1441  faces.reset();
1442 
1443  loadModel(cad_name, verbose, T);
1444  initFromPose(I, cMo_);
1445 }
1446 
1454 void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1455 {
1456  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1457  vpMbtDistanceKltPoints *kltpoly = *it;
1458  if (kltpoly->polygon->getName() == name) {
1459  kltpoly->setTracked(useKltTracking);
1460  }
1461  }
1462 }
1463 
1464 #elif !defined(VISP_BUILD_SHARED_LIBS)
1465 // Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
1466 // symbols
1467 void dummy_vpMbKltTracker(){};
1468 #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:185
virtual void track(const vpImage< unsigned char > &I)
unsigned int getCols() const
Definition: vpImage.h:170
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
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void setTracked(const bool &track)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
virtual ~vpMbKltTracker()
double getFarClippingDistance() const
std::vector< std::vector< double > > getModelForDisplay(const vpCameraParameters &cam, const bool displayFullModel=false)
std::vector< std::vector< double > > getFeaturesForDisplay()
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:237
void setKltQuality(const double &q)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
double getNearClippingDistance() const
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setKltPyramidLevels(const unsigned int &pL)
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)
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int getKltBlockSize() const
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setMaxFeatures(const int maxCount)
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
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:113
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void setOgreShowConfigDialog(const bool showConfigDialog)
bool modelInitialised
Definition: vpMbTracker.h:123
void setMinDistance(double minDistance)
void getCameraParameters(vpCameraParameters &cam) const
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
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
double getLodMinLineLengthThreshold() const
void setKltMaskBorder(const unsigned int &mb)
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
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
std::vector< double > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
virtual void reinit(const vpImage< unsigned char > &I)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void computeVVSInteractionMatrixAndResidu()
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void testTracking()
static const vpColor red
Definition: vpColor.h:180
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:111
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Implementation of a rotation matrix and operations on such kind of matrices.
void setQuality(double qualityLevel)
vpHomogeneousMatrix ctTc0
std::map< int, int > & getCurrentPointsInd()
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
void changeFrame(const vpHomogeneousMatrix &cMo)
unsigned int getRows() const
Definition: vpImage.h:209
std::vector< vpImagePoint > getKltImagePoints() const
vpRobust m_robust_klt
Robust.
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
vpColVector & normalize()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
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:66
void setAngleDisappear(const double &adisappear)
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
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setKltMinDistance(const double &mD)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
void setTrackerId(int tid)
Definition: vpKltOpencv.h:155
unsigned int maskBorder
Erosion of the mask.
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:193
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
unsigned int getRows() const
Definition: vpArray2D.h:289
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
vpKltOpencv tracker
Points tracker.
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
vpColVector getNormal() const
Definition: vpPlane.cpp:238
void setPyramidLevels(const int pyrMaxLevel)
void setKltWindowSize(const unsigned int &w)
bool useScanLine
Use scanline rendering.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
double get_px() const
std::map< int, vpImagePoint > & getCurrentPoints()
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:108
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.
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:221
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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="")
unsigned int getKltPyramidLevels() const
void setWindowSize(const int winSize)
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
vpMatrix get_K() const
static double deg(double rad)
Definition: vpMath.h:101
virtual void setCameraParameters(const vpCameraParameters &_cam)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
void setCameraParameters(const vpCameraParameters &cam)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
void preTracking(const vpImage< unsigned char > &I)
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
unsigned int getKltMaskBorder() const
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:147
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:364
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
virtual void computeVVSInit()
std::vector< std::vector< double > > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int getHeight() const
Definition: vpImage.h:179
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
unsigned int getKltMaxFeatures() const
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
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void setUseHarris(const int useHarrisDetector)
unsigned int getCurrentNumberPoints() const
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:153
double getMinDistance() const
Definition: vpKltOpencv.h:118
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
void displayOgre(const vpHomogeneousMatrix &cMo)
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)
void setKltBlockSize(const unsigned int &bs)
void setKltMaxFeatures(const unsigned int &mF)
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
double getLodMinPolygonAreaThreshold() const
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
std::vector< std::vector< double > > getFeaturesForDisplay()
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:117
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
unsigned int getKltWindowSize() const
double getD() const
Definition: vpPlane.h:108
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void track(const cv::Mat &I)
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:310
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
vpMatrix m_L_klt
Interaction matrix.
void computeFov(const unsigned int &w, const unsigned int &h)