Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 
52 namespace {
72 vpMatrix homography2collineation(const vpMatrix &H, const vpCameraParameters &cam)
73 {
74  vpMatrix G(3, 3);
75  double px = cam.get_px();
76  double py = cam.get_py();
77  double u0 = cam.get_u0();
78  double v0 = cam.get_v0();
79  double one_over_px = cam.get_px_inverse();
80  double one_over_py = cam.get_py_inverse();
81  double h00 = H[0][0], h01 = H[0][1], h02 = H[0][2];
82  double h10 = H[1][0], h11 = H[1][1], h12 = H[1][2];
83  double h20 = H[2][0], h21 = H[2][1], h22 = H[2][2];
84 
85  double A = h00 * px + u0 * h20;
86  double B = h01 * px + u0 * h21;
87  double C = h02 * px + u0 * h22;
88  double D = h10 * py + v0 * h20;
89  double E = h11 * py + v0 * h21;
90  double F = h12 * py + v0 * h22;
91 
92  G[0][0] = A * one_over_px;
93  G[1][0] = D * one_over_px;
94  G[2][0] = h20 * one_over_px;
95 
96  G[0][1] = B * one_over_py;
97  G[1][1] = E * one_over_py;
98  G[2][1] = h21 * one_over_py;
99 
100  double u0_one_over_px = u0 * one_over_px;
101  double v0_one_over_py = v0 * one_over_py;
102 
103  G[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C;
104  G[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F;
105  G[2][2] = - h20 * u0_one_over_px - h21 * v0_one_over_py + h22;
106 
107  return G;
108 }
109 }
110 
112  :
113 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
114  cur(),
115 #else
116  cur(NULL),
117 #endif
118  c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
119  kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
120  m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt()
121 {
124  tracker.setMaxFeatures(10000);
126  tracker.setQuality(0.01);
131 
132 #ifdef VISP_HAVE_OGRE
133  faces.getOgreContext()->setWindowName("MBT Klt");
134 #endif
135 
136  m_lambda = 0.8;
137  m_maxIter = 200;
138 }
139 
145 {
146 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
147  if (cur != NULL) {
148  cvReleaseImage(&cur);
149  cur = NULL;
150  }
151 #endif
152 
153  // delete the Klt Polygon features
154  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
155  vpMbtDistanceKltPoints *kltpoly = *it;
156  if (kltpoly != NULL) {
157  delete kltpoly;
158  }
159  kltpoly = NULL;
160  }
161  kltPolygons.clear();
162 
163  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
164  ++it) {
165  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
166  if (kltPolyCylinder != NULL) {
167  delete kltPolyCylinder;
168  }
169  kltPolyCylinder = NULL;
170  }
171  kltCylinders.clear();
172 
173  // delete the structures used to display circles
174  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
175  vpMbtDistanceCircle *ci = *it;
176  if (ci != NULL) {
177  delete ci;
178  }
179  ci = NULL;
180  }
181 
182  circles_disp.clear();
183 }
184 
186 {
187  if (!modelInitialised) {
188  throw vpException(vpException::fatalError, "model not initialized");
189  }
190 
191  bool reInitialisation = false;
192  if (!useOgre)
193  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
194  else {
195 #ifdef VISP_HAVE_OGRE
196  if (!faces.isOgreInitialised()) {
200  // Turn off Ogre config dialog display for the next call to this
201  // function since settings are saved in the ogre.cfg file and used
202  // during the next call
203  ogreShowConfigDialog = false;
204  }
205 
207 
208 #else
209  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
210 #endif
211  }
212  reinit(I);
213 }
214 
216 {
217  c0Mo = m_cMo;
218  ctTc0.eye();
219 
221 
223 
224  if (useScanLine) {
227  }
228 
229 // mask
230 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
231  cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
232 #else
233  IplImage *mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
234  cvZero(mask);
235 #endif
236 
237  vpMbtDistanceKltPoints *kltpoly;
238  vpMbtDistanceKltCylinder *kltPolyCylinder;
239  if (useScanLine) {
241  } else {
242  unsigned char val = 255 /* - i*15*/;
243  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
244  kltpoly = *it;
245  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
246  // need to changeFrame when reinit() is called by postTracking
247  // other solution is
248  kltpoly->polygon->changeFrame(m_cMo);
249  kltpoly->polygon->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
250  kltpoly->updateMask(mask, val, maskBorder);
251  }
252  }
253 
254  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
255  ++it) {
256  kltPolyCylinder = *it;
257 
258  if (kltPolyCylinder->isTracked()) {
259  for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
260  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
261  if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
262  faces[indCylBBox]->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
263  }
264  }
265 
266  kltPolyCylinder->updateMask(mask, val, maskBorder);
267  }
268  }
269  }
270 
271  tracker.initTracking(cur, mask);
272  // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
273  // the points are valid for tracking and avoid too fast reinitialisations.
274  // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
275  // points" << std::endl;
276 
277  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
278  kltpoly = *it;
279  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
280  kltpoly->init(tracker, m_mask);
281  }
282  }
283 
284  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
285  ++it) {
286  kltPolyCylinder = *it;
287 
288  if (kltPolyCylinder->isTracked())
289  kltPolyCylinder->init(tracker, m_cMo);
290  }
291 
292 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
293  cvReleaseImage(&mask);
294 #endif
295 }
296 
302 {
303  m_cMo.eye();
304 
305 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
306  if (cur != NULL) {
307  cvReleaseImage(&cur);
308  cur = NULL;
309  }
310 #endif
311 
312  // delete the Klt Polygon features
313  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
314  vpMbtDistanceKltPoints *kltpoly = *it;
315  if (kltpoly != NULL) {
316  delete kltpoly;
317  }
318  kltpoly = NULL;
319  }
320  kltPolygons.clear();
321 
322  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
323  ++it) {
324  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
325  if (kltPolyCylinder != NULL) {
326  delete kltPolyCylinder;
327  }
328  kltPolyCylinder = NULL;
329  }
330  kltCylinders.clear();
331 
332  // delete the structures used to display circles
333  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
334  vpMbtDistanceCircle *ci = *it;
335  if (ci != NULL) {
336  delete ci;
337  }
338  ci = NULL;
339  }
340 
341  circles_disp.clear();
342 
343  m_computeInteraction = true;
344  firstInitialisation = true;
345  computeCovariance = false;
346 
349 
350  tracker.setMaxFeatures(10000);
352  tracker.setQuality(0.01);
357 
360 
362 
363  maskBorder = 5;
364  threshold_outlier = 0.5;
365  percentGood = 0.6;
366 
367  m_lambda = 0.8;
368  m_maxIter = 200;
369 
370  faces.reset();
371 
373 
374  useScanLine = false;
375 
376 #ifdef VISP_HAVE_OGRE
377  useOgre = false;
378 #endif
379 }
380 
389 std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
390 {
391  std::vector<vpImagePoint> kltPoints;
392  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
393  long id;
394  float x_tmp, y_tmp;
395  tracker.getFeature((int)i, id, x_tmp, y_tmp);
396  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
397  }
398 
399  return kltPoints;
400 }
401 
410 std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
411 {
412  std::map<int, vpImagePoint> kltPoints;
413  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
414  long id;
415  float x_tmp, y_tmp;
416  tracker.getFeature((int)i, id, x_tmp, y_tmp);
417 #if TARGET_OS_IPHONE
418  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
419 #else
420  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
421 #endif
422  }
423 
424  return kltPoints;
425 }
426 
433 {
441 }
442 
449 {
450  // for (unsigned int i = 0; i < faces.size(); i += 1){
451  // faces[i]->setCameraParameters(camera);
452  // }
453 
454  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
455  vpMbtDistanceKltPoints *kltpoly = *it;
456  kltpoly->setCameraParameters(cam);
457  }
458 
459  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
460  ++it) {
461  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
462  kltPolyCylinder->setCameraParameters(cam);
463  }
464 
465  m_cam = cam;
466 }
467 
468 void vpMbKltTracker::setPose(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
469  const vpHomogeneousMatrix &cdMo)
470 {
471  if (I_color) {
472  vpImageConvert::convert(*I_color, m_I);
473  }
474 
475  if (!kltCylinders.empty()) {
476  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
477  "This feature is not implemented yet."
478  << std::endl;
479  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
480  m_cMo = cdMo;
481  if (I) {
482  init(*I);
483  } else {
484  init(m_I);
485  }
486  } else {
487  vpMbtDistanceKltPoints *kltpoly;
488 
489 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
490  std::vector<cv::Point2f> init_pts;
491  std::vector<long> init_ids;
492  std::vector<cv::Point2f> guess_pts;
493 #else
494  unsigned int nbp = 0;
495  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
496  kltpoly = *it;
497  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
498  nbp += (*it)->getCurrentNumberPoints();
499  }
500 
501  CvPoint2D32f *init_pts = NULL;
502  init_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(init_pts[0]));
503  long *init_ids = (long *)cvAlloc((unsigned int)tracker.getMaxFeatures() * sizeof(long));
504  unsigned int iter_pts = 0;
505 
506  CvPoint2D32f *guess_pts = NULL;
507  guess_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(guess_pts[0]));
508 #endif
509 
510  vpHomogeneousMatrix cdMc = cdMo * m_cMo.inverse();
511  vpHomogeneousMatrix cMcd = cdMc.inverse();
512 
513  vpRotationMatrix cdRc;
514  vpTranslationVector cdtc;
515 
516  cdMc.extract(cdRc);
517  cdMc.extract(cdtc);
518 
519  // unsigned int nbCur = 0;
520 
521  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
522  kltpoly = *it;
523 
524  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
525  kltpoly->polygon->changeFrame(cdMo);
526 
527  // Get the normal to the face at the current state cMo
528  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
529  plan.changeFrame(cMcd);
530 
531  vpColVector Nc = plan.getNormal();
532  Nc.normalize();
533 
534  double invDc = 1.0 / plan.getD();
535 
536  // Create the homography
537  vpMatrix cdHc;
538  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
539  cdHc /= cdHc[2][2];
540 
541  // Compute homography in the pixel space cdGc = K * cdHc * K^{-1}
542  vpMatrix cdGc = homography2collineation(cdHc, m_cam);
543 
544  // Points displacement
545  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
546  // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
547  for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
548 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
549 #if TARGET_OS_IPHONE
550  if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
551  init_ids.end())
552 #else
553  if (std::find(init_ids.begin(), init_ids.end(),
554  (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
555 #endif
556  {
557  // KLT point already processed (a KLT point can exist in another
558  // vpMbtDistanceKltPoints due to possible overlapping faces)
559  continue;
560  }
561 #endif
562 
563  vpColVector cdp(3);
564  cdp[0] = iter->second.get_j();
565  cdp[1] = iter->second.get_i();
566  cdp[2] = 1.0;
567 
568 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
569  cv::Point2f p((float)cdp[0], (float)cdp[1]);
570  init_pts.push_back(p);
571 #if TARGET_OS_IPHONE
572  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
573 #else
574  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
575 #endif
576 #else
577  init_pts[iter_pts].x = (float)cdp[0];
578  init_pts[iter_pts].y = (float)cdp[1];
579  init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
580 #endif
581 
582  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
583 
584  if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
585  cdp[0] = 0.0;
586  cdp[1] = 0.0;
587  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
588  }
589 
590  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
591  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
592 
593 // Set value to the KLT tracker
594 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
595  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
596  guess_pts.push_back(p_guess);
597 #else
598  guess_pts[iter_pts].x = (float)cdp[0];
599  guess_pts[iter_pts++].y = (float)cdp[1];
600 #endif
601  }
602  }
603  }
604 
605  if (I) {
607  } else {
609  }
610 
611 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
612  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
613 #else
614  tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
615 
616  if (init_pts)
617  cvFree(&init_pts);
618  init_pts = NULL;
619 
620  if (guess_pts)
621  cvFree(&guess_pts);
622  guess_pts = NULL;
623 
624  if (init_ids)
625  cvFree(&init_ids);
626  init_ids = NULL;
627 #endif
628 
629  bool reInitialisation = false;
630  if (!useOgre) {
631  if (I) {
632  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
633  } else {
634  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
635  }
636  } else {
637 #ifdef VISP_HAVE_OGRE
638  if (I) {
639  faces.setVisibleOgre(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
640  } else {
642  }
643 #else
644  if (I) {
645  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
646  } else {
647  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
648  }
649 #endif
650  }
651 
652  m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
653 
654  if (useScanLine) {
657  }
658 
659  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
660  kltpoly = *it;
661  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
663  kltpoly->init(tracker, m_mask);
664  }
665  }
666 
667  m_cMo = cdMo;
668  c0Mo = m_cMo;
669  ctTc0.eye();
670  }
671 }
672 
683 {
684  vpMbKltTracker::setPose(&I, NULL, cdMo);
685 }
686 
697 {
698  vpMbKltTracker::setPose(NULL, &I_color, cdMo);
699 }
700 
708 {
710  kltPoly->setCameraParameters(m_cam);
711  kltPoly->polygon = &polygon;
712  kltPoly->hiddenface = &faces;
713  kltPoly->useScanLine = useScanLine;
714  kltPolygons.push_back(kltPoly);
715 }
723 {
725  kltPoly->setCameraParameters(m_cam);
726  kltPoly->polygon = &polygon;
727  kltPoly->hiddenface = &faces;
728  kltPoly->useScanLine = useScanLine;
729  kltPolygons.push_back(kltPoly);
730 }
731 
739 {
741  tracker.track(cur);
742 
743  m_nbInfos = 0;
744  m_nbFaceUsed = 0;
745  // for (unsigned int i = 0; i < faces.size(); i += 1){
746  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
747  vpMbtDistanceKltPoints *kltpoly = *it;
748  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
750  // faces[i]->ransac();
751  if (kltpoly->hasEnoughPoints()) {
752  m_nbInfos += kltpoly->getCurrentNumberPoints();
753  m_nbFaceUsed++;
754  }
755  }
756  }
757 
758  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
759  ++it) {
760  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
761 
762  if (kltPolyCylinder->isTracked()) {
763  kltPolyCylinder->computeNbDetectedCurrent(tracker);
764  if (kltPolyCylinder->hasEnoughPoints()) {
765  m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
766  m_nbFaceUsed++;
767  }
768  }
769  }
770 }
771 
776 {
777  // # For a better Post Tracking, tracker should reinitialize if so faces
778  // don't have enough points but are visible. # Here we are not doing it for
779  // more speed performance.
780  bool reInitialisation = false;
781 
782  unsigned int initialNumber = 0;
783  unsigned int currentNumber = 0;
784  unsigned int shift = 0;
785  // for (unsigned int i = 0; i < faces.size(); i += 1){
786  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
787  vpMbtDistanceKltPoints *kltpoly = *it;
788  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
789  initialNumber += kltpoly->getInitialNumberPoint();
790  if (kltpoly->hasEnoughPoints()) {
791  vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
792  shift += 2 * kltpoly->getCurrentNumberPoints();
793  kltpoly->removeOutliers(sub_w, threshold_outlier);
794 
795  currentNumber += kltpoly->getCurrentNumberPoints();
796  }
797  // else{
798  // reInitialisation = true;
799  // break;
800  // }
801  }
802  }
803 
804  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
805  ++it) {
806  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
807 
808  if (kltPolyCylinder->isTracked()) {
809  initialNumber += kltPolyCylinder->getInitialNumberPoint();
810  if (kltPolyCylinder->hasEnoughPoints()) {
811  vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
812  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
813  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
814 
815  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
816  }
817  }
818  }
819 
820  // if(!reInitialisation){
821  double value = percentGood * (double)initialNumber;
822  if ((double)currentNumber < value) {
823  // std::cout << "Too many point disappear : " << initialNumber << "/"
824  // << currentNumber << std::endl;
825  reInitialisation = true;
826  } else {
827  if (!useOgre)
828  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
829  else {
830 #ifdef VISP_HAVE_OGRE
832 #else
833  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
834 #endif
835  }
836  }
837  // }
838 
839  if (reInitialisation)
840  return true;
841 
842  return false;
843 }
844 
849 {
850  vpMatrix L_true; // interaction matrix without weighting
851  vpMatrix LVJ_true;
852  vpColVector v; // "speed" for VVS
853 
854  vpMatrix LTL;
855  vpColVector LTR;
856  vpHomogeneousMatrix cMoPrev;
857  vpHomogeneousMatrix ctTc0_Prev;
858  vpColVector error_prev;
859  double mu = m_initialMu;
860 
861  double normRes = 0;
862  double normRes_1 = -1;
863  unsigned int iter = 0;
864 
866 
867  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
869 
870  bool reStartFromLastIncrement = false;
871  computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
872  if (reStartFromLastIncrement) {
873  ctTc0 = ctTc0_Prev;
874  }
875 
876  if (!reStartFromLastIncrement) {
878 
879  if (computeCovariance) {
880  L_true = m_L_klt;
881  if (!isoJoIdentity) {
883  cVo.buildFrom(m_cMo);
884  LVJ_true = (m_L_klt * cVo * oJo);
885  }
886  }
887 
888  normRes_1 = normRes;
889  normRes = 0.0;
890 
891  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
893  normRes += m_weightedError_klt[i];
894  }
895 
896  if ((iter == 0) || m_computeInteraction) {
897  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
898  for (unsigned int j = 0; j < 6; j++) {
899  m_L_klt[i][j] *= m_w_klt[i];
900  }
901  }
902  }
903 
905  v);
906 
907  cMoPrev = m_cMo;
908  ctTc0_Prev = ctTc0;
910  m_cMo = ctTc0 * c0Mo;
911  } // endif(!reStartFromLastIncrement)
912 
913  iter++;
914  }
915 
916  computeCovarianceMatrixVVS(isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
917 }
918 
920 {
921  unsigned int nbFeatures = 2 * m_nbInfos;
922 
923  m_L_klt.resize(nbFeatures, 6, false, false);
924  m_error_klt.resize(nbFeatures, false);
925 
926  m_weightedError_klt.resize(nbFeatures, false);
927  m_w_klt.resize(nbFeatures, false);
928  m_w_klt = 1;
929 
931 }
932 
934 {
935  unsigned int shift = 0;
936  vpHomography H;
937 
938  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
939  vpMbtDistanceKltPoints *kltpoly = *it;
940  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
941  kltpoly->hasEnoughPoints()) {
942  vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
943  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
944 
945  try {
946  kltpoly->computeHomography(ctTc0, H);
947  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
948  } catch (...) {
949  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
950  }
951 
952  shift += 2 * kltpoly->getCurrentNumberPoints();
953  }
954  }
955 
956  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
957  ++it) {
958  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
959 
960  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
961  vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
962  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
963 
964  try {
965  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
966  } catch (...) {
967  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
968  }
969 
970  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
971  }
972  }
973 }
974 
983 {
984  preTracking(I);
985 
986  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
987  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
988  }
989 
990  computeVVS();
991 
992  if (postTracking(I, m_w_klt))
993  reinit(I);
994 
995  if (displayFeatures) {
997  }
998 }
999 
1008 {
1009  vpImageConvert::convert(I_color, m_I);
1010  preTracking(m_I);
1011 
1012  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
1013  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
1014  }
1015 
1016  computeVVS();
1017 
1018  if (postTracking(m_I, m_w_klt))
1019  reinit(m_I);
1020 }
1021 
1065 void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
1066 {
1067  // Load projection error config
1068  vpMbTracker::loadConfigFile(configFile, verbose);
1069 
1071  xmlp.setVerbose(verbose);
1072  xmlp.setKltMaxFeatures(10000);
1073  xmlp.setKltWindowSize(5);
1074  xmlp.setKltQuality(0.01);
1075  xmlp.setKltMinDistance(5);
1076  xmlp.setKltHarrisParam(0.01);
1077  xmlp.setKltBlockSize(3);
1078  xmlp.setKltPyramidLevels(3);
1082 
1083  try {
1084  if (verbose) {
1085  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1086  }
1087  xmlp.parse(configFile.c_str());
1088  } catch (...) {
1089  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1090  throw vpException(vpException::ioError, "problem to parse configuration file.");
1091  }
1092 
1093  vpCameraParameters camera;
1094  xmlp.getCameraParameters(camera);
1095  setCameraParameters(camera);
1096 
1098  tracker.setWindowSize((int)xmlp.getKltWindowSize());
1102  tracker.setBlockSize((int)xmlp.getKltBlockSize());
1104  maskBorder = xmlp.getKltMaskBorder();
1107 
1108  // if(useScanLine)
1109  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1110 
1111  if (xmlp.hasNearClippingDistance())
1113 
1114  if (xmlp.hasFarClippingDistance())
1116 
1117  if (xmlp.getFovClipping())
1119 
1120  useLodGeneral = xmlp.getLodState();
1123 
1124  applyLodSettingInConfig = false;
1125  if (this->getNbPolygon() > 0) {
1126  applyLodSettingInConfig = true;
1130  }
1131 }
1132 
1145  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1146  bool displayFullModel)
1147 {
1148  std::vector<std::vector<double> > models = vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1149 
1150  for (size_t i = 0; i < models.size(); i++) {
1151  if (vpMath::equal(models[i][0], 0)) {
1152  vpImagePoint ip1(models[i][1], models[i][2]);
1153  vpImagePoint ip2(models[i][3], models[i][4]);
1154  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1155  } else if (vpMath::equal(models[i][0], 1)) {
1156  vpImagePoint center(models[i][1], models[i][2]);
1157  double n20 = models[i][3];
1158  double n11 = models[i][4];
1159  double n02 = models[i][5];
1160  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1161  }
1162  }
1163 
1164  if (displayFeatures) {
1165  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1166  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1169 
1171  double id = m_featuresToBeDisplayedKlt[i][5];
1172  std::stringstream ss;
1173  ss << id;
1174  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1175  }
1176  }
1177  }
1178 
1179 #ifdef VISP_HAVE_OGRE
1180  if (useOgre)
1181  faces.displayOgre(cMo);
1182 #endif
1183 }
1184 
1197  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1198  bool displayFullModel)
1199 {
1200  std::vector<std::vector<double> > models = vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1201 
1202  for (size_t i = 0; i < models.size(); i++) {
1203  if (vpMath::equal(models[i][0], 0)) {
1204  vpImagePoint ip1(models[i][1], models[i][2]);
1205  vpImagePoint ip2(models[i][3], models[i][4]);
1206  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1207  } else if (vpMath::equal(models[i][0], 1)) {
1208  vpImagePoint center(models[i][1], models[i][2]);
1209  double n20 = models[i][3];
1210  double n11 = models[i][4];
1211  double n02 = models[i][5];
1212  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1213  }
1214  }
1215 
1216  if (displayFeatures) {
1217  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1218  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1221 
1223  double id = m_featuresToBeDisplayedKlt[i][5];
1224  std::stringstream ss;
1225  ss << id;
1226  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1227  }
1228  }
1229  }
1230 
1231 #ifdef VISP_HAVE_OGRE
1232  if (useOgre)
1233  faces.displayOgre(cMo);
1234 #endif
1235 }
1236 
1237 std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1238 {
1239  std::vector<std::vector<double> > features;
1240 
1241  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1242  vpMbtDistanceKltPoints *kltpoly = *it;
1243 
1244  if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1245  std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1246  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1247  }
1248  }
1249 
1250  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1251  ++it) {
1252  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1253 
1254  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1255  std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1256  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1257  }
1258  }
1259 
1260  return features;
1261 }
1262 
1277 std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1278  const vpHomogeneousMatrix &cMo,
1279  const vpCameraParameters &cam,
1280  bool displayFullModel)
1281 {
1282  std::vector<std::vector<double> > models;
1283 
1284  vpCameraParameters c = cam;
1285 
1286  if (clippingFlag > 3) // Contains at least one FOV constraint
1287  c.computeFov(width, height);
1288 
1289  // vpMbtDistanceKltPoints *kltpoly;
1290  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1291 
1292  // Previous version 12/08/2015
1293  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1294  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1295  // kltpoly = *it;
1296  // kltpoly->polygon->changeFrame(cMo_);
1297  // kltpoly->polygon->computePolygonClipped(c);
1298  // }
1299  faces.computeClippedPolygons(cMo, c);
1300 
1301  if (useScanLine && !displayFullModel)
1302  faces.computeScanLineRender(m_cam, width, height);
1303 
1304  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1305  vpMbtDistanceKltPoints *kltpoly = *it;
1306  std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(cam, displayFullModel);
1307  models.insert(models.end(), modelLines.begin(), modelLines.end());
1308  }
1309 
1310  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1311  ++it) {
1312  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1313  std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo, cam);
1314  models.insert(models.end(), modelLines.begin(), modelLines.end());
1315  }
1316 
1317  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1318  vpMbtDistanceCircle *displayCircle = *it;
1319  std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo, cam, displayFullModel);
1320  if (!paramsCircle.empty()) {
1321  models.push_back(paramsCircle);
1322  }
1323  }
1324 
1325  return models;
1326 }
1327 
1335 {
1336  unsigned int nbTotalPoints = 0;
1337  // for (unsigned int i = 0; i < faces.size(); i += 1){
1338  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1339  vpMbtDistanceKltPoints *kltpoly = *it;
1340  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1341  kltpoly->hasEnoughPoints()) {
1342  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1343  }
1344  }
1345 
1346  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1347  ++it) {
1348  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1349  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1350  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1351  }
1352 
1353  if (nbTotalPoints < 10) {
1354  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1356  "test tracking failed (too few points to realize a good tracking).");
1357  }
1358 }
1359 
1370 void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1371  const std::string & /*name*/)
1372 {
1374  kltPoly->setCameraParameters(m_cam);
1375 
1376  kltPoly->buildFrom(p1, p2, radius);
1377 
1378  // Add the Cylinder BBox to the list of polygons
1379  kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1380  kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1381  kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1382  kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1383 
1384  kltPoly->hiddenface = &faces;
1385  kltPoly->useScanLine = useScanLine;
1386  kltCylinders.push_back(kltPoly);
1387 }
1388 
1400 void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1401  int /*idFace*/, const std::string &name)
1402 {
1403  addCircle(p1, p2, p3, radius, name);
1404 }
1405 
1414 void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
1415  const std::string &name)
1416 {
1417  bool already_here = false;
1418 
1419  // for(std::list<vpMbtDistanceCircle*>::const_iterator
1420  // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1421  // ci = *it;
1422  // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1423  // samePoint(*(ci->p3),P3)) ||
1424  // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1425  // samePoint(*(ci->p3),P2)) ){
1426  // already_here = (std::fabs(ci->radius - r) <
1427  // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1428  // r));
1429  // }
1430  // }
1431 
1432  if (!already_here) {
1434 
1436  ci->setName(name);
1437  ci->buildFrom(P1, P2, P3, r);
1438  circles_disp.push_back(ci);
1439  }
1440 }
1441 
1454 void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1455  const vpHomogeneousMatrix &cMo, bool verbose,
1456  const vpHomogeneousMatrix &T)
1457 {
1458  m_cMo.eye();
1459 
1460 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1461  if (cur != NULL) {
1462  cvReleaseImage(&cur);
1463  cur = NULL;
1464  }
1465 #endif
1466 
1467  firstInitialisation = true;
1468 
1469  // delete the Klt Polygon features
1470  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1471  vpMbtDistanceKltPoints *kltpoly = *it;
1472  if (kltpoly != NULL) {
1473  delete kltpoly;
1474  }
1475  kltpoly = NULL;
1476  }
1477  kltPolygons.clear();
1478 
1479  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1480  ++it) {
1481  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1482  if (kltPolyCylinder != NULL) {
1483  delete kltPolyCylinder;
1484  }
1485  kltPolyCylinder = NULL;
1486  }
1487  kltCylinders.clear();
1488 
1489  // delete the structures used to display circles
1490  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1491  vpMbtDistanceCircle *ci = *it;
1492  if (ci != NULL) {
1493  delete ci;
1494  }
1495  ci = NULL;
1496  }
1497 
1498  faces.reset();
1499 
1500  loadModel(cad_name, verbose, T);
1501  initFromPose(I, cMo);
1502 }
1503 
1511 void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1512 {
1513  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1514  vpMbtDistanceKltPoints *kltpoly = *it;
1515  if (kltpoly->polygon->getName() == name) {
1516  kltpoly->setTracked(useKltTracking);
1517  }
1518  }
1519 }
1520 
1521 #elif !defined(VISP_BUILD_SHARED_LIBS)
1522 // Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
1523 // symbols
1524 void dummy_vpMbKltTracker(){};
1525 #endif // VISP_HAVE_OPENCV
virtual void setKltOpencv(const vpKltOpencv &t)
bool m_computeInteraction
Definition: vpMbTracker.h:185
virtual void track(const vpImage< unsigned char > &I)
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
void setTracked(const bool &track)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void setMaxFeatures(int maxCount)
virtual ~vpMbKltTracker()
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
virtual void computeVVSCheckLevenbergMarquardt(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::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)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setKltQuality(const double &q)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
std::vector< vpImagePoint > getKltImagePoints() const
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
void getCameraParameters(vpCameraParameters &cam) const
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
void setKltPyramidLevels(const unsigned int &pL)
unsigned int getRows() const
Definition: vpImage.h:218
std::string getName() const
Definition: vpMbtPolygon.h:108
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
#define vpERROR_TRACE
Definition: vpDebug.h:393
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
std::vector< double > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
std::list< vpMbtDistanceKltCylinder * > kltCylinders
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
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)
bool modelInitialised
Definition: vpMbTracker.h:123
void setMinDistance(double minDistance)
error that can be emited by ViSP classes.
Definition: vpException.h:71
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
vpMbScanLine & getMbScanLineRenderer()
vpColVector m_weightedError_klt
Weighted error.
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getRows() const
Definition: vpArray2D.h:289
vpHomogeneousMatrix inverse() const
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
unsigned int getKltBlockSize() const
void setKltMaskBorder(const unsigned int &mb)
void extract(vpRotationMatrix &R) const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:62
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
std::vector< std::vector< double > > getModelForDisplay(const vpCameraParameters &cam, 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 setLod(bool useLod, const std::string &name="")
virtual void computeVVSInteractionMatrixAndResidu()
double get_py_inverse() const
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:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
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()
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:135
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
void changeFrame(const vpHomogeneousMatrix &cMo)
vpRobust m_robust_klt
Robust.
vpColVector & normalize()
Manage a circle used in the model-based tracker.
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:115
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
Error that can be emited by the vpTracker class and its derivates.
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="")
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
double getD() const
Definition: vpPlane.h:108
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)
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)
void setPyramidLevels(int pyrMaxLevel)
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 getCols() const
Definition: vpImage.h:179
unsigned int maskBorder
Erosion of the mask.
Generic class defining intrinsic camera parameters.
unsigned int getKltWindowSize() const
void setOgreShowConfigDialog(bool showConfigDialog)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
unsigned int m_nbFaceUsed
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:130
unsigned int getKltMaxFeatures() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
double getMinDistance() const
Definition: vpKltOpencv.h:118
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
void setWindowSize(int winSize)
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
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)
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)
unsigned int getCurrentNumberPoints() const
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
std::map< int, vpImagePoint > & getCurrentPoints()
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:110
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 getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
Definition: vpKltOpencv.h:111
unsigned int getCurrentNumberPoints() const
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
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void getFeature(const int &index, long &id, float &x, float &y) const
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, 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)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
vpColVector getNormal() const
Definition: vpPlane.cpp:238
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:103
virtual void setCameraParameters(const vpCameraParameters &_cam)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
unsigned int getHeight() const
Definition: vpImage.h:188
void setCameraParameters(const vpCameraParameters &cam)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
void preTracking(const vpImage< unsigned char > &I)
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
unsigned int getInitialNumberPoint() const
double getLodMinLineLengthThreshold() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
std::map< int, vpImagePoint > getKltImagePointsWithId() const
static vpHomogeneousMatrix direct(const vpColVector &v)
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
void setUseHarris(int useHarrisDetector)
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 getKltMaskBorder() const
unsigned int getInitialNumberPoint() const
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, const std::string &name="")
virtual void setClipping(const unsigned int &flags)
double getQuality() const
Definition: vpKltOpencv.h:133
unsigned int m_nbInfos
virtual void init(const vpImage< unsigned char > &I)
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:102
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
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
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
void displayOgre(const vpHomogeneousMatrix &cMo)
vpColVector m_error_klt
(s - s*)
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)
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:246
void setBlockSize(int blockSize)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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()
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< std::vector< double > > getFeaturesForDisplay()
Class that consider the case of a translation vector.
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.
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)
double getLodMinPolygonAreaThreshold() const
double get_px_inverse() const
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)
unsigned int getKltPyramidLevels() const
virtual void setNearClippingDistance(const double &dist)
vpMatrix m_L_klt
Interaction matrix.
void computeFov(const unsigned int &w, const unsigned int &h)