Visual Servoing Platform  version 3.6.1 under development (2023-12-06)
vpMbKltTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpImageConvert.h>
37 #include <visp3/core/vpTrackingException.h>
38 #include <visp3/core/vpVelocityTwistMatrix.h>
39 #include <visp3/mbt/vpMbKltTracker.h>
40 #include <visp3/mbt/vpMbtXmlGenericParser.h>
41 
42 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
43 
44 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
45 #include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
46 #endif
47 
48 namespace
49 {
69 vpMatrix homography2collineation(const vpMatrix &H, const vpCameraParameters &cam)
70 {
71  vpMatrix G(3, 3);
72  double px = cam.get_px();
73  double py = cam.get_py();
74  double u0 = cam.get_u0();
75  double v0 = cam.get_v0();
76  double one_over_px = cam.get_px_inverse();
77  double one_over_py = cam.get_py_inverse();
78  double h00 = H[0][0], h01 = H[0][1], h02 = H[0][2];
79  double h10 = H[1][0], h11 = H[1][1], h12 = H[1][2];
80  double h20 = H[2][0], h21 = H[2][1], h22 = H[2][2];
81 
82  double A = h00 * px + u0 * h20;
83  double B = h01 * px + u0 * h21;
84  double C = h02 * px + u0 * h22;
85  double D = h10 * py + v0 * h20;
86  double E = h11 * py + v0 * h21;
87  double F = h12 * py + v0 * h22;
88 
89  G[0][0] = A * one_over_px;
90  G[1][0] = D * one_over_px;
91  G[2][0] = h20 * one_over_px;
92 
93  G[0][1] = B * one_over_py;
94  G[1][1] = E * one_over_py;
95  G[2][1] = h21 * one_over_py;
96 
97  double u0_one_over_px = u0 * one_over_px;
98  double v0_one_over_py = v0 * one_over_py;
99 
100  G[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C;
101  G[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F;
102  G[2][2] = -h20 * u0_one_over_px - h21 * v0_one_over_py + h22;
103 
104  return G;
105 }
106 } // namespace
107 
109  :
110  cur(), c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
111  kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
112  m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt()
113 {
116  tracker.setMaxFeatures(10000);
118  tracker.setQuality(0.01);
123 
124 #ifdef VISP_HAVE_OGRE
125  faces.getOgreContext()->setWindowName("MBT Klt");
126 #endif
127 
128  m_lambda = 0.8;
129  m_maxIter = 200;
130 }
131 
137 {
138  // delete the Klt Polygon features
139  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
140  vpMbtDistanceKltPoints *kltpoly = *it;
141  if (kltpoly != nullptr) {
142  delete kltpoly;
143  }
144  kltpoly = nullptr;
145  }
146  kltPolygons.clear();
147 
148  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
149  ++it) {
150  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
151  if (kltPolyCylinder != nullptr) {
152  delete kltPolyCylinder;
153  }
154  kltPolyCylinder = nullptr;
155  }
156  kltCylinders.clear();
157 
158  // delete the structures used to display circles
159  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
160  vpMbtDistanceCircle *ci = *it;
161  if (ci != nullptr) {
162  delete ci;
163  }
164  ci = nullptr;
165  }
166 
167  circles_disp.clear();
168 }
169 
171 {
172  if (!modelInitialised) {
173  throw vpException(vpException::fatalError, "model not initialized");
174  }
175 
176  bool reInitialisation = false;
177  if (!useOgre)
178  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
179  else {
180 #ifdef VISP_HAVE_OGRE
181  if (!faces.isOgreInitialised()) {
185  // Turn off Ogre config dialog display for the next call to this
186  // function since settings are saved in the ogre.cfg file and used
187  // during the next call
188  ogreShowConfigDialog = false;
189  }
190 
192 
193 #else
194  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
195 #endif
196  }
197  reinit(I);
198 }
199 
201 {
202  c0Mo = m_cMo;
203  ctTc0.eye();
204 
206 
208 
209  if (useScanLine) {
212  }
213 
214  // mask
215  cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
216 
217  vpMbtDistanceKltPoints *kltpoly;
218  vpMbtDistanceKltCylinder *kltPolyCylinder;
219  if (useScanLine) {
221  } else {
222  unsigned char val = 255 /* - i*15*/;
223  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
224  kltpoly = *it;
225  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
226  // need to changeFrame when reinit() is called by postTracking
227  // other solution is
228  kltpoly->polygon->changeFrame(m_cMo);
229  kltpoly->polygon->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
230  kltpoly->updateMask(mask, val, maskBorder);
231  }
232  }
233 
234  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
235  ++it) {
236  kltPolyCylinder = *it;
237 
238  if (kltPolyCylinder->isTracked()) {
239  for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
240  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
241  if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
242  faces[indCylBBox]->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
243  }
244  }
245 
246  kltPolyCylinder->updateMask(mask, val, maskBorder);
247  }
248  }
249  }
250 
251  tracker.initTracking(cur, mask);
252  // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
253  // the points are valid for tracking and avoid too fast reinitialisations.
254  // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
255  // points" << std::endl;
256 
257  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
258  kltpoly = *it;
259  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
260  kltpoly->init(tracker, m_mask);
261  }
262  }
263 
264  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
265  ++it) {
266  kltPolyCylinder = *it;
267 
268  if (kltPolyCylinder->isTracked())
269  kltPolyCylinder->init(tracker, m_cMo);
270  }
271 }
272 
278 {
279  m_cMo.eye();
280 
281  // delete the Klt Polygon features
282  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
283  vpMbtDistanceKltPoints *kltpoly = *it;
284  if (kltpoly != nullptr) {
285  delete kltpoly;
286  }
287  kltpoly = nullptr;
288  }
289  kltPolygons.clear();
290 
291  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
292  ++it) {
293  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
294  if (kltPolyCylinder != nullptr) {
295  delete kltPolyCylinder;
296  }
297  kltPolyCylinder = nullptr;
298  }
299  kltCylinders.clear();
300 
301  // delete the structures used to display circles
302  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
303  vpMbtDistanceCircle *ci = *it;
304  if (ci != nullptr) {
305  delete ci;
306  }
307  ci = nullptr;
308  }
309 
310  circles_disp.clear();
311 
312  m_computeInteraction = true;
313  firstInitialisation = true;
314  computeCovariance = false;
315 
318 
319  tracker.setMaxFeatures(10000);
321  tracker.setQuality(0.01);
326 
329 
331 
332  maskBorder = 5;
333  threshold_outlier = 0.5;
334  percentGood = 0.6;
335 
336  m_lambda = 0.8;
337  m_maxIter = 200;
338 
339  faces.reset();
340 
342 
343  useScanLine = false;
344 
345 #ifdef VISP_HAVE_OGRE
346  useOgre = false;
347 #endif
348 }
349 
358 std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
359 {
360  std::vector<vpImagePoint> kltPoints;
361  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
362  long id;
363  float x_tmp, y_tmp;
364  tracker.getFeature((int)i, id, x_tmp, y_tmp);
365  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
366  }
367 
368  return kltPoints;
369 }
370 
379 std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
380 {
381  std::map<int, vpImagePoint> kltPoints;
382  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
383  long id;
384  float x_tmp, y_tmp;
385  tracker.getFeature((int)i, id, x_tmp, y_tmp);
386 #ifdef TARGET_OS_IPHONE
387  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
388 #else
389  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
390 #endif
391  }
392 
393  return kltPoints;
394 }
395 
402 {
410 }
411 
418 {
419  // for (unsigned int i = 0; i < faces.size(); i += 1){
420  // faces[i]->setCameraParameters(camera);
421  // }
422 
423  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
424  vpMbtDistanceKltPoints *kltpoly = *it;
425  kltpoly->setCameraParameters(cam);
426  }
427 
428  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
429  ++it) {
430  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
431  kltPolyCylinder->setCameraParameters(cam);
432  }
433 
434  m_cam = cam;
435 }
436 
437 void vpMbKltTracker::setPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
438  const vpHomogeneousMatrix &cdMo)
439 {
440  if (I_color) {
441  vpImageConvert::convert(*I_color, m_I);
442  }
443 
444  if (!kltCylinders.empty()) {
445  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
446  "This feature is not implemented yet."
447  << std::endl;
448  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
449  m_cMo = cdMo;
450  if (I) {
451  init(*I);
452  } else {
453  init(m_I);
454  }
455  } else {
456  vpMbtDistanceKltPoints *kltpoly;
457 
458  std::vector<cv::Point2f> init_pts;
459  std::vector<long> init_ids;
460  std::vector<cv::Point2f> guess_pts;
461 
462  vpHomogeneousMatrix cdMc = cdMo * m_cMo.inverse();
463  vpHomogeneousMatrix cMcd = cdMc.inverse();
464 
465  vpRotationMatrix cdRc;
466  vpTranslationVector cdtc;
467 
468  cdMc.extract(cdRc);
469  cdMc.extract(cdtc);
470 
471  // unsigned int nbCur = 0;
472 
473  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
474  kltpoly = *it;
475 
476  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
477  kltpoly->polygon->changeFrame(cdMo);
478 
479  // Get the normal to the face at the current state cMo
480  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
481  plan.changeFrame(cMcd);
482 
483  vpColVector Nc = plan.getNormal();
484  Nc.normalize();
485 
486  double invDc = 1.0 / plan.getD();
487 
488  // Create the homography
489  vpMatrix cdHc;
490  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
491  cdHc /= cdHc[2][2];
492 
493  // Compute homography in the pixel space cdGc = K * cdHc * K^{-1}
494  vpMatrix cdGc = homography2collineation(cdHc, m_cam);
495 
496  // Points displacement
497  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
498  // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
499  for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
500 #ifdef TARGET_OS_IPHONE
501  if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
502  init_ids.end())
503 #else
504  if (std::find(init_ids.begin(), init_ids.end(),
505  (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
506 #endif
507  {
508  // KLT point already processed (a KLT point can exist in another
509  // vpMbtDistanceKltPoints due to possible overlapping faces)
510  continue;
511  }
512 
513  vpColVector cdp(3);
514  cdp[0] = iter->second.get_j();
515  cdp[1] = iter->second.get_i();
516  cdp[2] = 1.0;
517 
518  cv::Point2f p((float)cdp[0], (float)cdp[1]);
519  init_pts.push_back(p);
520 #ifdef TARGET_OS_IPHONE
521  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
522 #else
523  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
524 #endif
525 
526  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
527 
528  if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
529  cdp[0] = 0.0;
530  cdp[1] = 0.0;
531  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
532  }
533 
534  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
535  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
536 
537  // Set value to the KLT tracker
538  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
539  guess_pts.push_back(p_guess);
540  }
541  }
542  }
543 
544  if (I) {
546  } else {
548  }
549 
550  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
551 
552  bool reInitialisation = false;
553  if (!useOgre) {
554  if (I) {
555  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
556  } else {
557  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
558  }
559  } else {
560 #ifdef VISP_HAVE_OGRE
561  if (I) {
563  reInitialisation);
564  } else {
566  reInitialisation);
567  }
568 #else
569  if (I) {
570  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
571  } else {
572  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
573  }
574 #endif
575  }
576 
577  m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
578 
579  if (useScanLine) {
582  }
583 
584  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
585  kltpoly = *it;
586  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
588  kltpoly->init(tracker, m_mask);
589  }
590  }
591 
592  m_cMo = cdMo;
593  c0Mo = m_cMo;
594  ctTc0.eye();
595  }
596 }
597 
608 {
609  vpMbKltTracker::setPose(&I, nullptr, cdMo);
610 }
611 
622 {
623  vpMbKltTracker::setPose(nullptr, &I_color, cdMo);
624 }
625 
633 {
635  kltPoly->setCameraParameters(m_cam);
636  kltPoly->polygon = &polygon;
637  kltPoly->hiddenface = &faces;
638  kltPoly->useScanLine = useScanLine;
639  kltPolygons.push_back(kltPoly);
640 }
648 {
650  kltPoly->setCameraParameters(m_cam);
651  kltPoly->polygon = &polygon;
652  kltPoly->hiddenface = &faces;
653  kltPoly->useScanLine = useScanLine;
654  kltPolygons.push_back(kltPoly);
655 }
656 
664 {
666  tracker.track(cur);
667 
668  m_nbInfos = 0;
669  m_nbFaceUsed = 0;
670  // for (unsigned int i = 0; i < faces.size(); i += 1){
671  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
672  vpMbtDistanceKltPoints *kltpoly = *it;
673  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
675  // faces[i]->ransac();
676  if (kltpoly->hasEnoughPoints()) {
677  m_nbInfos += kltpoly->getCurrentNumberPoints();
678  m_nbFaceUsed++;
679  }
680  }
681  }
682 
683  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
684  ++it) {
685  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
686 
687  if (kltPolyCylinder->isTracked()) {
688  kltPolyCylinder->computeNbDetectedCurrent(tracker);
689  if (kltPolyCylinder->hasEnoughPoints()) {
690  m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
691  m_nbFaceUsed++;
692  }
693  }
694  }
695 }
696 
701 {
702  // # For a better Post Tracking, tracker should reinitialize if so faces
703  // don't have enough points but are visible. # Here we are not doing it for
704  // more speed performance.
705  bool reInitialisation = false;
706 
707  unsigned int initialNumber = 0;
708  unsigned int currentNumber = 0;
709  unsigned int shift = 0;
710  // for (unsigned int i = 0; i < faces.size(); i += 1){
711  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
712  vpMbtDistanceKltPoints *kltpoly = *it;
713  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
714  initialNumber += kltpoly->getInitialNumberPoint();
715  if (kltpoly->hasEnoughPoints()) {
716  vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
717  shift += 2 * kltpoly->getCurrentNumberPoints();
718  kltpoly->removeOutliers(sub_w, threshold_outlier);
719 
720  currentNumber += kltpoly->getCurrentNumberPoints();
721  }
722  // else{
723  // reInitialisation = true;
724  // break;
725  // }
726  }
727  }
728 
729  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
730  ++it) {
731  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
732 
733  if (kltPolyCylinder->isTracked()) {
734  initialNumber += kltPolyCylinder->getInitialNumberPoint();
735  if (kltPolyCylinder->hasEnoughPoints()) {
736  vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
737  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
738  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
739 
740  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
741  }
742  }
743  }
744 
745  // if(!reInitialisation){
746  double value = percentGood * (double)initialNumber;
747  if ((double)currentNumber < value) {
748  // std::cout << "Too many point disappear : " << initialNumber << "/"
749  // << currentNumber << std::endl;
750  reInitialisation = true;
751  } else {
752  if (!useOgre)
753  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
754  else {
755 #ifdef VISP_HAVE_OGRE
757 #else
758  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
759 #endif
760  }
761  }
762  // }
763 
764  if (reInitialisation)
765  return true;
766 
767  return false;
768 }
769 
774 {
775  vpMatrix L_true; // interaction matrix without weighting
776  vpMatrix LVJ_true;
777  vpColVector v; // "speed" for VVS
778 
779  vpMatrix LTL;
780  vpColVector LTR;
781  vpHomogeneousMatrix cMoPrev;
782  vpHomogeneousMatrix ctTc0_Prev;
783  vpColVector error_prev;
784  double mu = m_initialMu;
785 
786  double normRes = 0;
787  double normRes_1 = -1;
788  unsigned int iter = 0;
789 
790  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
791  if (isoJoIdentity)
792  oJo.eye();
793 
795 
796  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
798 
799  bool reStartFromLastIncrement = false;
800  computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
801  if (reStartFromLastIncrement) {
802  ctTc0 = ctTc0_Prev;
803  }
804 
805  if (!reStartFromLastIncrement) {
807 
808  if (computeCovariance) {
809  L_true = m_L_klt;
810  if (!isoJoIdentity) {
812  cVo.buildFrom(m_cMo);
813  LVJ_true = (m_L_klt * cVo * oJo);
814  }
815  }
816 
817  normRes_1 = normRes;
818  normRes = 0.0;
819 
820  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
822  normRes += m_weightedError_klt[i];
823  }
824 
825  if ((iter == 0) || m_computeInteraction) {
826  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
827  for (unsigned int j = 0; j < 6; j++) {
828  m_L_klt[i][j] *= m_w_klt[i];
829  }
830  }
831  }
832 
833  computeVVSPoseEstimation(m_isoJoIdentity, iter, m_L_klt, LTL, m_weightedError_klt, m_error_klt, error_prev, LTR, mu, v);
834 
835  cMoPrev = m_cMo;
836  ctTc0_Prev = ctTc0;
838  m_cMo = ctTc0 * c0Mo;
839  } // endif(!reStartFromLastIncrement)
840 
841  iter++;
842  }
843 
844  computeCovarianceMatrixVVS(m_isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
845 }
846 
848 {
849  unsigned int nbFeatures = 2 * m_nbInfos;
850 
851  m_L_klt.resize(nbFeatures, 6, false, false);
852  m_error_klt.resize(nbFeatures, false);
853 
854  m_weightedError_klt.resize(nbFeatures, false);
855  m_w_klt.resize(nbFeatures, false);
856  m_w_klt = 1;
857 
859 }
860 
862 {
863  unsigned int shift = 0;
864  vpHomography H;
865 
866  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
867  vpMbtDistanceKltPoints *kltpoly = *it;
868  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
869  kltpoly->hasEnoughPoints()) {
870  vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
871  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
872 
873  try {
874  kltpoly->computeHomography(ctTc0, H);
875  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
876  } catch (...) {
877  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
878  }
879 
880  shift += 2 * kltpoly->getCurrentNumberPoints();
881  }
882  }
883 
884  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
885  ++it) {
886  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
887 
888  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
889  vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
890  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
891 
892  try {
893  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
894  } catch (...) {
895  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
896  }
897 
898  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
899  }
900  }
901 }
902 
911 {
912  preTracking(I);
913 
914  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
915  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
916  }
917 
918  computeVVS();
919 
920  if (postTracking(I, m_w_klt))
921  reinit(I);
922 
923  if (displayFeatures) {
925  }
926 }
927 
936 {
937  vpImageConvert::convert(I_color, m_I);
938  preTracking(m_I);
939 
940  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
941  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
942  }
943 
944  computeVVS();
945 
946  if (postTracking(m_I, m_w_klt))
947  reinit(m_I);
948 }
949 
993 void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
994 {
995  // Load projection error config
996  vpMbTracker::loadConfigFile(configFile, verbose);
997 
999  xmlp.setVerbose(verbose);
1000  xmlp.setKltMaxFeatures(10000);
1001  xmlp.setKltWindowSize(5);
1002  xmlp.setKltQuality(0.01);
1003  xmlp.setKltMinDistance(5);
1004  xmlp.setKltHarrisParam(0.01);
1005  xmlp.setKltBlockSize(3);
1006  xmlp.setKltPyramidLevels(3);
1010 
1011  try {
1012  if (verbose) {
1013  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1014  }
1015  xmlp.parse(configFile.c_str());
1016  } catch (...) {
1017  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1018  throw vpException(vpException::ioError, "problem to parse configuration file.");
1019  }
1020 
1021  vpCameraParameters camera;
1022  xmlp.getCameraParameters(camera);
1023  setCameraParameters(camera);
1024 
1026  tracker.setWindowSize((int)xmlp.getKltWindowSize());
1030  tracker.setBlockSize((int)xmlp.getKltBlockSize());
1032  maskBorder = xmlp.getKltMaskBorder();
1035 
1036  // if(useScanLine)
1037  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1038 
1039  if (xmlp.hasNearClippingDistance())
1041 
1042  if (xmlp.hasFarClippingDistance())
1044 
1045  if (xmlp.getFovClipping())
1047 
1048  useLodGeneral = xmlp.getLodState();
1051 
1052  applyLodSettingInConfig = false;
1053  if (this->getNbPolygon() > 0) {
1054  applyLodSettingInConfig = true;
1058  }
1059 }
1060 
1073  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1074  bool displayFullModel)
1075 {
1076  std::vector<std::vector<double> > models =
1077  vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1078 
1079  for (size_t i = 0; i < models.size(); i++) {
1080  if (vpMath::equal(models[i][0], 0)) {
1081  vpImagePoint ip1(models[i][1], models[i][2]);
1082  vpImagePoint ip2(models[i][3], models[i][4]);
1083  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1084  } else if (vpMath::equal(models[i][0], 1)) {
1085  vpImagePoint center(models[i][1], models[i][2]);
1086  double n20 = models[i][3];
1087  double n11 = models[i][4];
1088  double n02 = models[i][5];
1089  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1090  }
1091  }
1092 
1093  if (displayFeatures) {
1094  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1095  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1098 
1100  double id = m_featuresToBeDisplayedKlt[i][5];
1101  std::stringstream ss;
1102  ss << id;
1103  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1104  }
1105  }
1106  }
1107 
1108 #ifdef VISP_HAVE_OGRE
1109  if (useOgre)
1110  faces.displayOgre(cMo);
1111 #endif
1112 }
1113 
1126  const vpColor &col, unsigned int thickness, bool displayFullModel)
1127 {
1128  std::vector<std::vector<double> > models =
1129  vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1130 
1131  for (size_t i = 0; i < models.size(); i++) {
1132  if (vpMath::equal(models[i][0], 0)) {
1133  vpImagePoint ip1(models[i][1], models[i][2]);
1134  vpImagePoint ip2(models[i][3], models[i][4]);
1135  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1136  } else if (vpMath::equal(models[i][0], 1)) {
1137  vpImagePoint center(models[i][1], models[i][2]);
1138  double n20 = models[i][3];
1139  double n11 = models[i][4];
1140  double n02 = models[i][5];
1141  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1142  }
1143  }
1144 
1145  if (displayFeatures) {
1146  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1147  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1150 
1152  double id = m_featuresToBeDisplayedKlt[i][5];
1153  std::stringstream ss;
1154  ss << id;
1155  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1156  }
1157  }
1158  }
1159 
1160 #ifdef VISP_HAVE_OGRE
1161  if (useOgre)
1162  faces.displayOgre(cMo);
1163 #endif
1164 }
1165 
1166 std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1167 {
1168  std::vector<std::vector<double> > features;
1169 
1170  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1171  vpMbtDistanceKltPoints *kltpoly = *it;
1172 
1173  if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1174  std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1175  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1176  }
1177  }
1178 
1179  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1180  ++it) {
1181  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1182 
1183  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1184  std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1185  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1186  }
1187  }
1188 
1189  return features;
1190 }
1191 
1207 std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1208  const vpHomogeneousMatrix &cMo,
1209  const vpCameraParameters &cam,
1210  bool displayFullModel)
1211 {
1212  std::vector<std::vector<double> > models;
1213 
1214  vpCameraParameters c = cam;
1215 
1216  if (clippingFlag > 3) // Contains at least one FOV constraint
1217  c.computeFov(width, height);
1218 
1219  // vpMbtDistanceKltPoints *kltpoly;
1220  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1221 
1222  // Previous version 12/08/2015
1223  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1224  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1225  // kltpoly = *it;
1226  // kltpoly->polygon->changeFrame(cMo_);
1227  // kltpoly->polygon->computePolygonClipped(c);
1228  // }
1229  faces.computeClippedPolygons(cMo, c);
1230 
1231  if (useScanLine && !displayFullModel)
1232  faces.computeScanLineRender(m_cam, width, height);
1233 
1234  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1235  vpMbtDistanceKltPoints *kltpoly = *it;
1236  std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(cam, displayFullModel);
1237  models.insert(models.end(), modelLines.begin(), modelLines.end());
1238  }
1239 
1240  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1241  ++it) {
1242  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1243  std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo, cam);
1244  models.insert(models.end(), modelLines.begin(), modelLines.end());
1245  }
1246 
1247  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1248  vpMbtDistanceCircle *displayCircle = *it;
1249  std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo, cam, displayFullModel);
1250  if (!paramsCircle.empty()) {
1251  models.push_back(paramsCircle);
1252  }
1253  }
1254 
1255  return models;
1256 }
1257 
1265 {
1266  unsigned int nbTotalPoints = 0;
1267  // for (unsigned int i = 0; i < faces.size(); i += 1){
1268  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1269  vpMbtDistanceKltPoints *kltpoly = *it;
1270  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1271  kltpoly->hasEnoughPoints()) {
1272  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1273  }
1274  }
1275 
1276  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1277  ++it) {
1278  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1279  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1280  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1281  }
1282 
1283  if (nbTotalPoints < 10) {
1284  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1286  "test tracking failed (too few points to realize a good tracking).");
1287  }
1288 }
1289 
1300 void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1301  const std::string & /*name*/)
1302 {
1304  kltPoly->setCameraParameters(m_cam);
1305 
1306  kltPoly->buildFrom(p1, p2, radius);
1307 
1308  // Add the Cylinder BBox to the list of polygons
1309  kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1310  kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1311  kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1312  kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1313 
1314  kltPoly->hiddenface = &faces;
1315  kltPoly->useScanLine = useScanLine;
1316  kltCylinders.push_back(kltPoly);
1317 }
1318 
1330 void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int /*idFace*/,
1331  const std::string &name)
1332 {
1333  addCircle(p1, p2, p3, radius, name);
1334 }
1335 
1344 void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
1345  const std::string &name)
1346 {
1347  bool already_here = false;
1348 
1349  // for(std::list<vpMbtDistanceCircle*>::const_iterator
1350  // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1351  // ci = *it;
1352  // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1353  // samePoint(*(ci->p3),P3)) ||
1354  // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1355  // samePoint(*(ci->p3),P2)) ){
1356  // already_here = (std::fabs(ci->radius - r) <
1357  // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1358  // r));
1359  // }
1360  // }
1361 
1362  if (!already_here) {
1364 
1366  ci->setName(name);
1367  ci->buildFrom(P1, P2, P3, r);
1368  circles_disp.push_back(ci);
1369  }
1370 }
1371 
1384 void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1385  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1386 {
1387  m_cMo.eye();
1388 
1389  firstInitialisation = true;
1390 
1391  // delete the Klt Polygon features
1392  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1393  vpMbtDistanceKltPoints *kltpoly = *it;
1394  if (kltpoly != nullptr) {
1395  delete kltpoly;
1396  }
1397  kltpoly = nullptr;
1398  }
1399  kltPolygons.clear();
1400 
1401  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1402  ++it) {
1403  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1404  if (kltPolyCylinder != nullptr) {
1405  delete kltPolyCylinder;
1406  }
1407  kltPolyCylinder = nullptr;
1408  }
1409  kltCylinders.clear();
1410 
1411  // delete the structures used to display circles
1412  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1413  vpMbtDistanceCircle *ci = *it;
1414  if (ci != nullptr) {
1415  delete ci;
1416  }
1417  ci = nullptr;
1418  }
1419 
1420  faces.reset();
1421 
1422  loadModel(cad_name, verbose, T);
1423  initFromPose(I, cMo);
1424 }
1425 
1433 void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1434 {
1435  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1436  vpMbtDistanceKltPoints *kltpoly = *it;
1437  if (kltpoly->polygon->getName() == name) {
1438  kltpoly->setTracked(useKltTracking);
1439  }
1440  }
1441 }
1442 
1443 #elif !defined(VISP_BUILD_SHARED_LIBS)
1444 // Work around to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
1445 // symbols
1446 void dummy_vpMbKltTracker(){};
1447 #endif // VISP_HAVE_OPENCV
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:282
unsigned int getRows() const
Definition: vpArray2D.h:267
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
double get_px_inverse() const
double get_py_inverse() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector & normalize()
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1049
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ ioError
I/O error.
Definition: vpException.h:79
@ fatalError
Fatal error.
Definition: vpException.h:84
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:168
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:240
unsigned int getCols() const
Definition: vpImage.h:173
unsigned int getHeight() const
Definition: vpImage.h:182
unsigned int getRows() const
Definition: vpImage.h:212
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:73
double getQuality() const
Definition: vpKltOpencv.h:210
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:192
void setBlockSize(int blockSize)
Definition: vpKltOpencv.h:266
void setQuality(double qualityLevel)
Definition: vpKltOpencv.h:355
void track(const cv::Mat &I)
void setTrackerId(int tid)
Definition: vpKltOpencv.h:359
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:212
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:197
void setHarrisFreeParameter(double harris_k)
Definition: vpKltOpencv.h:274
void getFeature(const int &index, long &id, float &x, float &y) const
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
Definition: vpKltOpencv.h:188
void setMaxFeatures(int maxCount)
Definition: vpKltOpencv.h:314
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
Definition: vpKltOpencv.cpp:93
double getMinDistance() const
Definition: vpKltOpencv.h:195
void setMinDistance(double minDistance)
Definition: vpKltOpencv.h:323
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:168
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:207
void setUseHarris(int useHarrisDetector)
Definition: vpKltOpencv.h:367
void setWindowSize(int winSize)
Definition: vpKltOpencv.h:376
void setPyramidLevels(int pyrMaxLevel)
Definition: vpKltOpencv.h:342
static double rad(double deg)
Definition: vpMath.h:127
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void eye()
Definition: vpMatrix.cpp:437
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
vpMbScanLine & getMbScanLineRenderer()
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void displayOgre(const vpHomogeneousMatrix &cMo)
void setOgreShowConfigDialog(bool showConfigDialog)
virtual void initFaceFromLines(vpMbtPolygon &polygon) override
std::list< vpMbtDistanceKltCylinder * > kltCylinders
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpColVector m_error_klt
(s - s*)
vpHomogeneousMatrix c0Mo
Initial pose.
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="") override
vpHomogeneousMatrix ctTc0
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) override
std::list< vpMbtDistanceKltPoints * > kltPolygons
virtual void setKltOpencv(const vpKltOpencv &t)
cv::Mat cur
Temporary OpenCV image for fast conversion.
std::map< int, vpImagePoint > getKltImagePointsWithId() const
void resetTracker() override
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="") override
vpColVector m_weightedError_klt
Weighted error.
virtual ~vpMbKltTracker()
vpKltOpencv tracker
Points tracker.
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
double threshold_outlier
void preTracking(const vpImage< unsigned char > &I)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, const std::string &name="")
unsigned int m_nbInfos
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) override
virtual void track(const vpImage< unsigned char > &I) override
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
vpRobust m_robust_klt
Robust.
virtual void reinit(const vpImage< unsigned char > &I)
virtual void initFaceFromCorners(vpMbtPolygon &polygon) override
std::vector< vpImagePoint > getKltImagePoints() const
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) override
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) override
unsigned int maskBorder
Erosion of the mask.
unsigned int m_nbFaceUsed
virtual void testTracking() override
vpColVector m_w_klt
Robust weights.
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
virtual void computeVVSInit() override
vpMatrix m_L_klt
Interaction matrix.
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
void setCameraParameters(const vpCameraParameters &cam) override
virtual void init(const vpImage< unsigned char > &I) override
virtual void computeVVSInteractionMatrixAndResidu() override
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
bool modelInitialised
Definition: vpMbTracker.h:123
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
virtual void setLod(bool useLod, const std::string &name="")
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
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=nullptr, vpColVector *const m_w_prev=nullptr)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
virtual void setFarClippingDistance(const double &dist)
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setCameraParameters(const vpCameraParameters &camera)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
std::vector< double > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void setName(const std::string &circle_name)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void setCameraParameters(const vpCameraParameters &_cam)
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
std::vector< std::vector< double > > getFeaturesForDisplay()
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
bool useScanLine
Use scanline rendering.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int getCurrentNumberPoints() const
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
unsigned int getInitialNumberPoint() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
bool useScanLine
Use scanline rendering.
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=nullptr)
std::vector< std::vector< double > > getFeaturesForDisplay()
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=nullptr)
virtual void setCameraParameters(const vpCameraParameters &_cam)
unsigned int getCurrentNumberPoints() const
std::map< int, int > & getCurrentPointsInd()
std::vector< std::vector< double > > getModelForDisplay(const vpCameraParameters &cam, bool displayFullModel=false)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
std::map< int, vpImagePoint > & getCurrentPoints()
unsigned int getInitialNumberPoint() const
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
void setTracked(const bool &track)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:58
std::string getName() const
Definition: vpMbtPolygon.h:98
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
Parse an Xml file to extract configuration parameters of a mbtConfig object.
unsigned int getKltMaxFeatures() const
void setKltMinDistance(const double &mD)
unsigned int getKltBlockSize() const
void getCameraParameters(vpCameraParameters &cam) const
void setKltMaskBorder(const unsigned int &mb)
double getLodMinLineLengthThreshold() const
unsigned int getKltMaskBorder() const
void setAngleDisappear(const double &adisappear)
void setKltPyramidLevels(const unsigned int &pL)
void setKltWindowSize(const unsigned int &w)
void setKltMaxFeatures(const unsigned int &mF)
void setAngleAppear(const double &aappear)
void setKltBlockSize(const unsigned int &bs)
void setKltHarrisParam(const double &hp)
void parse(const std::string &filename)
void setKltQuality(const double &q)
unsigned int getKltPyramidLevels() const
unsigned int getKltWindowSize() const
double getLodMinPolygonAreaThreshold() const
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:54
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:361
double getD() const
Definition: vpPlane.h:106
vpColVector getNormal() const
Definition: vpPlane.cpp:245
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
void changeFrame(const vpHomogeneousMatrix &cMo)
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:127
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:76
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:154
Implementation of a rotation matrix and operations on such kind of matrices.
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:58
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ fatalError
Tracker fatal error.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:388