Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
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  }
222  else {
223  unsigned char val = 255 /* - i*15*/;
224  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
225  kltpoly = *it;
226  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
227  // need to changeFrame when reinit() is called by postTracking
228  // other solution is
229  kltpoly->polygon->changeFrame(m_cMo);
230  kltpoly->polygon->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
231  kltpoly->updateMask(mask, val, maskBorder);
232  }
233  }
234 
235  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
236  ++it) {
237  kltPolyCylinder = *it;
238 
239  if (kltPolyCylinder->isTracked()) {
240  for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
241  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
242  if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
243  faces[indCylBBox]->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
244  }
245  }
246 
247  kltPolyCylinder->updateMask(mask, val, maskBorder);
248  }
249  }
250  }
251 
252  tracker.initTracking(cur, mask);
253  // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
254  // the points are valid for tracking and avoid too fast reinitialisations.
255  // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
256  // points" << std::endl;
257 
258  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
259  kltpoly = *it;
260  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
261  kltpoly->init(tracker, m_mask);
262  }
263  }
264 
265  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
266  ++it) {
267  kltPolyCylinder = *it;
268 
269  if (kltPolyCylinder->isTracked())
270  kltPolyCylinder->init(tracker, m_cMo);
271  }
272 }
273 
279 {
280  m_cMo.eye();
281 
282  // delete the Klt Polygon features
283  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
284  vpMbtDistanceKltPoints *kltpoly = *it;
285  if (kltpoly != nullptr) {
286  delete kltpoly;
287  }
288  kltpoly = nullptr;
289  }
290  kltPolygons.clear();
291 
292  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
293  ++it) {
294  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
295  if (kltPolyCylinder != nullptr) {
296  delete kltPolyCylinder;
297  }
298  kltPolyCylinder = nullptr;
299  }
300  kltCylinders.clear();
301 
302  // delete the structures used to display circles
303  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
304  vpMbtDistanceCircle *ci = *it;
305  if (ci != nullptr) {
306  delete ci;
307  }
308  ci = nullptr;
309  }
310 
311  circles_disp.clear();
312 
313  m_computeInteraction = true;
314  firstInitialisation = true;
315  computeCovariance = false;
316 
319 
320  tracker.setMaxFeatures(10000);
322  tracker.setQuality(0.01);
327 
330 
332 
333  maskBorder = 5;
334  threshold_outlier = 0.5;
335  percentGood = 0.6;
336 
337  m_lambda = 0.8;
338  m_maxIter = 200;
339 
340  faces.reset();
341 
343 
344  useScanLine = false;
345 
346 #ifdef VISP_HAVE_OGRE
347  useOgre = false;
348 #endif
349 }
350 
359 std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
360 {
361  std::vector<vpImagePoint> kltPoints;
362  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
363  long id;
364  float x_tmp, y_tmp;
365  tracker.getFeature((int)i, id, x_tmp, y_tmp);
366  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
367  }
368 
369  return kltPoints;
370 }
371 
380 std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
381 {
382  std::map<int, vpImagePoint> kltPoints;
383  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
384  long id;
385  float x_tmp, y_tmp;
386  tracker.getFeature((int)i, id, x_tmp, y_tmp);
387 #ifdef TARGET_OS_IPHONE
388  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
389 #else
390  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
391 #endif
392  }
393 
394  return kltPoints;
395 }
396 
403 {
411 }
412 
419 {
420  // for (unsigned int i = 0; i < faces.size(); i += 1){
421  // faces[i]->setCameraParameters(camera);
422  // }
423 
424  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
425  vpMbtDistanceKltPoints *kltpoly = *it;
426  kltpoly->setCameraParameters(cam);
427  }
428 
429  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
430  ++it) {
431  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
432  kltPolyCylinder->setCameraParameters(cam);
433  }
434 
435  m_cam = cam;
436 }
437 
438 void vpMbKltTracker::setPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
439  const vpHomogeneousMatrix &cdMo)
440 {
441  if (I_color) {
442  vpImageConvert::convert(*I_color, m_I);
443  }
444 
445  if (!kltCylinders.empty()) {
446  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
447  "This feature is not implemented yet."
448  << std::endl;
449  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
450  m_cMo = cdMo;
451  if (I) {
452  init(*I);
453  }
454  else {
455  init(m_I);
456  }
457  }
458  else {
459  vpMbtDistanceKltPoints *kltpoly;
460 
461  std::vector<cv::Point2f> init_pts;
462  std::vector<long> init_ids;
463  std::vector<cv::Point2f> guess_pts;
464 
465  vpHomogeneousMatrix cdMc = cdMo * m_cMo.inverse();
466  vpHomogeneousMatrix cMcd = cdMc.inverse();
467 
468  vpRotationMatrix cdRc;
469  vpTranslationVector cdtc;
470 
471  cdMc.extract(cdRc);
472  cdMc.extract(cdtc);
473 
474  // unsigned int nbCur = 0;
475 
476  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
477  kltpoly = *it;
478 
479  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
480  kltpoly->polygon->changeFrame(cdMo);
481 
482  // Get the normal to the face at the current state cMo
483  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
484  plan.changeFrame(cMcd);
485 
486  vpColVector Nc = plan.getNormal();
487  Nc.normalize();
488 
489  double invDc = 1.0 / plan.getD();
490 
491  // Create the homography
492  vpMatrix cdHc;
493  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
494  cdHc /= cdHc[2][2];
495 
496  // Compute homography in the pixel space cdGc = K * cdHc * K^{-1}
497  vpMatrix cdGc = homography2collineation(cdHc, m_cam);
498 
499  // Points displacement
500  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
501  // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
502  for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
503 #ifdef TARGET_OS_IPHONE
504  if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
505  init_ids.end())
506 #else
507  if (std::find(init_ids.begin(), init_ids.end(),
508  (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
509 #endif
510  {
511  // KLT point already processed (a KLT point can exist in another
512  // vpMbtDistanceKltPoints due to possible overlapping faces)
513  continue;
514  }
515 
516  vpColVector cdp(3);
517  cdp[0] = iter->second.get_j();
518  cdp[1] = iter->second.get_i();
519  cdp[2] = 1.0;
520 
521  cv::Point2f p((float)cdp[0], (float)cdp[1]);
522  init_pts.push_back(p);
523 #ifdef TARGET_OS_IPHONE
524  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
525 #else
526  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
527 #endif
528 
529  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
530 
531  if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
532  cdp[0] = 0.0;
533  cdp[1] = 0.0;
534  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
535  }
536 
537  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
538  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
539 
540  // Set value to the KLT tracker
541  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
542  guess_pts.push_back(p_guess);
543  }
544  }
545  }
546 
547  if (I) {
549  }
550  else {
552  }
553 
554  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
555 
556  bool reInitialisation = false;
557  if (!useOgre) {
558  if (I) {
559  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
560  }
561  else {
562  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
563  }
564  }
565  else {
566 #ifdef VISP_HAVE_OGRE
567  if (I) {
569  reInitialisation);
570  }
571  else {
573  reInitialisation);
574  }
575 #else
576  if (I) {
577  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
578  }
579  else {
580  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
581  }
582 #endif
583  }
584 
585  m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
586 
587  if (useScanLine) {
590  }
591 
592  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
593  kltpoly = *it;
594  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
596  kltpoly->init(tracker, m_mask);
597  }
598  }
599 
600  m_cMo = cdMo;
601  c0Mo = m_cMo;
602  ctTc0.eye();
603  }
604 }
605 
616 {
617  vpMbKltTracker::setPose(&I, nullptr, cdMo);
618 }
619 
630 {
631  vpMbKltTracker::setPose(nullptr, &I_color, cdMo);
632 }
633 
641 {
643  kltPoly->setCameraParameters(m_cam);
644  kltPoly->polygon = &polygon;
645  kltPoly->hiddenface = &faces;
646  kltPoly->useScanLine = useScanLine;
647  kltPolygons.push_back(kltPoly);
648 }
656 {
658  kltPoly->setCameraParameters(m_cam);
659  kltPoly->polygon = &polygon;
660  kltPoly->hiddenface = &faces;
661  kltPoly->useScanLine = useScanLine;
662  kltPolygons.push_back(kltPoly);
663 }
664 
672 {
674  tracker.track(cur);
675 
676  m_nbInfos = 0;
677  m_nbFaceUsed = 0;
678  // for (unsigned int i = 0; i < faces.size(); i += 1){
679  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
680  vpMbtDistanceKltPoints *kltpoly = *it;
681  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
683  // faces[i]->ransac();
684  if (kltpoly->hasEnoughPoints()) {
685  m_nbInfos += kltpoly->getCurrentNumberPoints();
686  m_nbFaceUsed++;
687  }
688  }
689  }
690 
691  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
692  ++it) {
693  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
694 
695  if (kltPolyCylinder->isTracked()) {
696  kltPolyCylinder->computeNbDetectedCurrent(tracker);
697  if (kltPolyCylinder->hasEnoughPoints()) {
698  m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
699  m_nbFaceUsed++;
700  }
701  }
702  }
703 }
704 
709 {
710  // # For a better Post Tracking, tracker should reinitialize if so faces
711  // don't have enough points but are visible. # Here we are not doing it for
712  // more speed performance.
713  bool reInitialisation = false;
714 
715  unsigned int initialNumber = 0;
716  unsigned int currentNumber = 0;
717  unsigned int shift = 0;
718  // for (unsigned int i = 0; i < faces.size(); i += 1){
719  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
720  vpMbtDistanceKltPoints *kltpoly = *it;
721  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
722  initialNumber += kltpoly->getInitialNumberPoint();
723  if (kltpoly->hasEnoughPoints()) {
724  vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
725  shift += 2 * kltpoly->getCurrentNumberPoints();
726  kltpoly->removeOutliers(sub_w, threshold_outlier);
727 
728  currentNumber += kltpoly->getCurrentNumberPoints();
729  }
730  // else{
731  // reInitialisation = true;
732  // break;
733  // }
734  }
735  }
736 
737  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
738  ++it) {
739  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
740 
741  if (kltPolyCylinder->isTracked()) {
742  initialNumber += kltPolyCylinder->getInitialNumberPoint();
743  if (kltPolyCylinder->hasEnoughPoints()) {
744  vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
745  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
746  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
747 
748  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
749  }
750  }
751  }
752 
753  // if(!reInitialisation){
754  double value = percentGood * (double)initialNumber;
755  if ((double)currentNumber < value) {
756  // std::cout << "Too many point disappear : " << initialNumber << "/"
757  // << currentNumber << std::endl;
758  reInitialisation = true;
759  }
760  else {
761  if (!useOgre)
762  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
763  else {
764 #ifdef VISP_HAVE_OGRE
766 #else
767  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
768 #endif
769  }
770  }
771  // }
772 
773  if (reInitialisation)
774  return true;
775 
776  return false;
777 }
778 
783 {
784  vpMatrix L_true; // interaction matrix without weighting
785  vpMatrix LVJ_true;
786  vpColVector v; // "speed" for VVS
787 
788  vpMatrix LTL;
789  vpColVector LTR;
790  vpHomogeneousMatrix cMoPrev;
791  vpHomogeneousMatrix ctTc0_Prev;
792  vpColVector error_prev;
793  double mu = m_initialMu;
794 
795  double normRes = 0;
796  double normRes_1 = -1;
797  unsigned int iter = 0;
798 
799  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
800  if (isoJoIdentity)
801  oJo.eye();
802 
804 
805  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
807 
808  bool reStartFromLastIncrement = false;
809  computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
810  if (reStartFromLastIncrement) {
811  ctTc0 = ctTc0_Prev;
812  }
813 
814  if (!reStartFromLastIncrement) {
816 
817  if (computeCovariance) {
818  L_true = m_L_klt;
819  if (!isoJoIdentity) {
821  cVo.build(m_cMo);
822  LVJ_true = (m_L_klt * cVo * oJo);
823  }
824  }
825 
826  normRes_1 = normRes;
827  normRes = 0.0;
828 
829  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
831  normRes += m_weightedError_klt[i];
832  }
833 
834  if ((iter == 0) || m_computeInteraction) {
835  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
836  for (unsigned int j = 0; j < 6; j++) {
837  m_L_klt[i][j] *= m_w_klt[i];
838  }
839  }
840  }
841 
842  computeVVSPoseEstimation(m_isoJoIdentity, iter, m_L_klt, LTL, m_weightedError_klt, m_error_klt, error_prev, LTR, mu, v);
843 
844  cMoPrev = m_cMo;
845  ctTc0_Prev = ctTc0;
847  m_cMo = ctTc0 * c0Mo;
848  } // endif(!reStartFromLastIncrement)
849 
850  iter++;
851  }
852 
853  computeCovarianceMatrixVVS(m_isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
854 }
855 
857 {
858  unsigned int nbFeatures = 2 * m_nbInfos;
859 
860  m_L_klt.resize(nbFeatures, 6, false, false);
861  m_error_klt.resize(nbFeatures, false);
862 
863  m_weightedError_klt.resize(nbFeatures, false);
864  m_w_klt.resize(nbFeatures, false);
865  m_w_klt = 1;
866 
868 }
869 
871 {
872  unsigned int shift = 0;
873  vpHomography H;
874 
875  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
876  vpMbtDistanceKltPoints *kltpoly = *it;
877  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
878  kltpoly->hasEnoughPoints()) {
879  vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
880  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
881 
882  try {
883  kltpoly->computeHomography(ctTc0, H);
884  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
885  }
886  catch (...) {
887  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
888  }
889 
890  shift += 2 * kltpoly->getCurrentNumberPoints();
891  }
892  }
893 
894  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
895  ++it) {
896  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
897 
898  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
899  vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
900  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
901 
902  try {
903  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
904  }
905  catch (...) {
906  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
907  }
908 
909  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
910  }
911  }
912 }
913 
922 {
923  preTracking(I);
924 
925  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
926  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
927  }
928 
929  computeVVS();
930 
931  if (postTracking(I, m_w_klt))
932  reinit(I);
933 
934  if (displayFeatures) {
936  }
937 }
938 
947 {
948  vpImageConvert::convert(I_color, m_I);
949  preTracking(m_I);
950 
951  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
952  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
953  }
954 
955  computeVVS();
956 
957  if (postTracking(m_I, m_w_klt))
958  reinit(m_I);
959 }
960 
1004 void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
1005 {
1006 #if defined(VISP_HAVE_PUGIXML)
1007 // Load projection error config
1008  vpMbTracker::loadConfigFile(configFile, verbose);
1009 
1011  xmlp.setVerbose(verbose);
1012  xmlp.setKltMaxFeatures(10000);
1013  xmlp.setKltWindowSize(5);
1014  xmlp.setKltQuality(0.01);
1015  xmlp.setKltMinDistance(5);
1016  xmlp.setKltHarrisParam(0.01);
1017  xmlp.setKltBlockSize(3);
1018  xmlp.setKltPyramidLevels(3);
1022 
1023  try {
1024  if (verbose) {
1025  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1026  }
1027  xmlp.parse(configFile.c_str());
1028  }
1029  catch (...) {
1030  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1031  throw vpException(vpException::ioError, "problem to parse configuration file.");
1032  }
1033 
1034  vpCameraParameters camera;
1035  xmlp.getCameraParameters(camera);
1036  setCameraParameters(camera);
1037 
1039  tracker.setWindowSize((int)xmlp.getKltWindowSize());
1043  tracker.setBlockSize((int)xmlp.getKltBlockSize());
1045  maskBorder = xmlp.getKltMaskBorder();
1048 
1049  // if(useScanLine)
1050  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1051 
1052  if (xmlp.hasNearClippingDistance())
1054 
1055  if (xmlp.hasFarClippingDistance())
1057 
1058  if (xmlp.getFovClipping())
1060 
1061  useLodGeneral = xmlp.getLodState();
1064 
1065  applyLodSettingInConfig = false;
1066  if (this->getNbPolygon() > 0) {
1067  applyLodSettingInConfig = true;
1071  }
1072 #else
1073  (void)configFile;
1074  (void)verbose;
1075  throw(vpException(vpException::ioError, "vpMbKltTracker::loadConfigFile() needs pugixml built-in 3rdparty"));
1076 #endif
1077 }
1078 
1091  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1092  bool displayFullModel)
1093 {
1094  std::vector<std::vector<double> > models =
1095  vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1096 
1097  for (size_t i = 0; i < models.size(); i++) {
1098  if (vpMath::equal(models[i][0], 0)) {
1099  vpImagePoint ip1(models[i][1], models[i][2]);
1100  vpImagePoint ip2(models[i][3], models[i][4]);
1101  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1102  }
1103  else if (vpMath::equal(models[i][0], 1)) {
1104  vpImagePoint center(models[i][1], models[i][2]);
1105  double n20 = models[i][3];
1106  double n11 = models[i][4];
1107  double n02 = models[i][5];
1108  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1109  }
1110  }
1111 
1112  if (displayFeatures) {
1113  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1114  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1117 
1119  double id = m_featuresToBeDisplayedKlt[i][5];
1120  std::stringstream ss;
1121  ss << id;
1122  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1123  }
1124  }
1125  }
1126 
1127 #ifdef VISP_HAVE_OGRE
1128  if (useOgre)
1129  faces.displayOgre(cMo);
1130 #endif
1131 }
1132 
1145  const vpColor &col, unsigned int thickness, bool displayFullModel)
1146 {
1147  std::vector<std::vector<double> > models =
1148  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  }
1156  else if (vpMath::equal(models[i][0], 1)) {
1157  vpImagePoint center(models[i][1], models[i][2]);
1158  double n20 = models[i][3];
1159  double n11 = models[i][4];
1160  double n02 = models[i][5];
1161  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1162  }
1163  }
1164 
1165  if (displayFeatures) {
1166  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1167  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1170 
1172  double id = m_featuresToBeDisplayedKlt[i][5];
1173  std::stringstream ss;
1174  ss << id;
1175  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1176  }
1177  }
1178  }
1179 
1180 #ifdef VISP_HAVE_OGRE
1181  if (useOgre)
1182  faces.displayOgre(cMo);
1183 #endif
1184 }
1185 
1186 std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1187 {
1188  std::vector<std::vector<double> > features;
1189 
1190  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1191  vpMbtDistanceKltPoints *kltpoly = *it;
1192 
1193  if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1194  std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1195  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1196  }
1197  }
1198 
1199  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1200  ++it) {
1201  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1202 
1203  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1204  std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1205  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1206  }
1207  }
1208 
1209  return features;
1210 }
1211 
1227 std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1228  const vpHomogeneousMatrix &cMo,
1229  const vpCameraParameters &cam,
1230  bool displayFullModel)
1231 {
1232  std::vector<std::vector<double> > models;
1233 
1234  vpCameraParameters c = cam;
1235 
1236  if (clippingFlag > 3) // Contains at least one FOV constraint
1237  c.computeFov(width, height);
1238 
1239  // vpMbtDistanceKltPoints *kltpoly;
1240  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1241 
1242  // Previous version 12/08/2015
1243  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1244  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1245  // kltpoly = *it;
1246  // kltpoly->polygon->changeFrame(cMo_);
1247  // kltpoly->polygon->computePolygonClipped(c);
1248  // }
1249  faces.computeClippedPolygons(cMo, c);
1250 
1251  if (useScanLine && !displayFullModel)
1252  faces.computeScanLineRender(m_cam, width, height);
1253 
1254  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1255  vpMbtDistanceKltPoints *kltpoly = *it;
1256  std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(cam, displayFullModel);
1257  models.insert(models.end(), modelLines.begin(), modelLines.end());
1258  }
1259 
1260  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1261  ++it) {
1262  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1263  std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo, cam);
1264  models.insert(models.end(), modelLines.begin(), modelLines.end());
1265  }
1266 
1267  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1268  vpMbtDistanceCircle *displayCircle = *it;
1269  std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo, cam, displayFullModel);
1270  if (!paramsCircle.empty()) {
1271  models.push_back(paramsCircle);
1272  }
1273  }
1274 
1275  return models;
1276 }
1277 
1285 {
1286  unsigned int nbTotalPoints = 0;
1287  // for (unsigned int i = 0; i < faces.size(); i += 1){
1288  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1289  vpMbtDistanceKltPoints *kltpoly = *it;
1290  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1291  kltpoly->hasEnoughPoints()) {
1292  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1293  }
1294  }
1295 
1296  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1297  ++it) {
1298  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1299  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1300  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1301  }
1302 
1303  if (nbTotalPoints < 10) {
1304  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1306  "test tracking failed (too few points to realize a good tracking).");
1307  }
1308 }
1309 
1320 void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1321  const std::string & /*name*/)
1322 {
1324  kltPoly->setCameraParameters(m_cam);
1325 
1326  kltPoly->buildFrom(p1, p2, radius);
1327 
1328  // Add the Cylinder BBox to the list of polygons
1329  kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1330  kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1331  kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1332  kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1333 
1334  kltPoly->hiddenface = &faces;
1335  kltPoly->useScanLine = useScanLine;
1336  kltCylinders.push_back(kltPoly);
1337 }
1338 
1350 void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int /*idFace*/,
1351  const std::string &name)
1352 {
1353  addCircle(p1, p2, p3, radius, name);
1354 }
1355 
1364 void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
1365  const std::string &name)
1366 {
1367  bool already_here = false;
1368 
1369  // for(std::list<vpMbtDistanceCircle*>::const_iterator
1370  // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1371  // ci = *it;
1372  // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1373  // samePoint(*(ci->p3),P3)) ||
1374  // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1375  // samePoint(*(ci->p3),P2)) ){
1376  // already_here = (std::fabs(ci->radius - r) <
1377  // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1378  // r));
1379  // }
1380  // }
1381 
1382  if (!already_here) {
1384 
1386  ci->setName(name);
1387  ci->buildFrom(P1, P2, P3, r);
1388  circles_disp.push_back(ci);
1389  }
1390 }
1391 
1404 void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1405  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1406 {
1407  m_cMo.eye();
1408 
1409  firstInitialisation = true;
1410 
1411  // delete the Klt Polygon features
1412  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1413  vpMbtDistanceKltPoints *kltpoly = *it;
1414  if (kltpoly != nullptr) {
1415  delete kltpoly;
1416  }
1417  kltpoly = nullptr;
1418  }
1419  kltPolygons.clear();
1420 
1421  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1422  ++it) {
1423  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1424  if (kltPolyCylinder != nullptr) {
1425  delete kltPolyCylinder;
1426  }
1427  kltPolyCylinder = nullptr;
1428  }
1429  kltCylinders.clear();
1430 
1431  // delete the structures used to display circles
1432  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1433  vpMbtDistanceCircle *ci = *it;
1434  if (ci != nullptr) {
1435  delete ci;
1436  }
1437  ci = nullptr;
1438  }
1439 
1440  faces.reset();
1441 
1442  loadModel(cad_name, verbose, T);
1443  initFromPose(I, cMo);
1444 }
1445 
1453 void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1454 {
1455  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1456  vpMbtDistanceKltPoints *kltpoly = *it;
1457  if (kltpoly->polygon->getName() == name) {
1458  kltpoly->setTracked(useKltTracking);
1459  }
1460  }
1461 }
1462 
1463 #elif !defined(VISP_BUILD_SHARED_LIBS)
1464 // Work around to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no symbols
1465 void dummy_vpMbKltTracker() { };
1466 #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:354
unsigned int getRows() const
Definition: vpArray2D.h:339
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:1058
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:66
@ fatalError
Fatal error.
Definition: vpException.h:71
@ divideByZeroError
Division by zero.
Definition: vpException.h:69
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:245
unsigned int getCols() const
Definition: vpImage.h:175
unsigned int getHeight() const
Definition: vpImage.h:184
unsigned int getRows() const
Definition: vpImage.h:215
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:454
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:448
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)
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.
vpHomogeneousMatrix ctTc0
std::list< vpMbtDistanceKltPoints * > kltPolygons
virtual void computeVVSInit() vp_override
virtual void init(const vpImage< unsigned char > &I) vp_override
virtual void setKltOpencv(const vpKltOpencv &t)
cv::Mat cur
Temporary OpenCV image for fast conversion.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) vp_override
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) vp_override
std::map< int, vpImagePoint > getKltImagePointsWithId() const
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
vpColVector m_weightedError_klt
Weighted error.
virtual void computeVVSInteractionMatrixAndResidu() vp_override
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="")
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="") vp_override
unsigned int m_nbInfos
virtual void track(const vpImage< unsigned char > &I) vp_override
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
vpRobust m_robust_klt
Robust.
virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override
virtual void reinit(const vpImage< unsigned char > &I)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
std::vector< vpImagePoint > getKltImagePoints() const
unsigned int maskBorder
Erosion of the mask.
unsigned int m_nbFaceUsed
vpColVector m_w_klt
Robust weights.
void setCameraParameters(const vpCameraParameters &cam) vp_override
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
virtual void testTracking() vp_override
vpMatrix m_L_klt
Interaction matrix.
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) vp_override
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="") vp_override
virtual void initFaceFromCorners(vpMbtPolygon &polygon) vp_override
void resetTracker() vp_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:372
double getD() const
Definition: vpPlane.h:106
vpColVector getNormal() const
Definition: vpPlane.cpp:249
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:135
Implementation of a rotation matrix and operations on such kind of matrices.
Definition of the vpSubMatrix class that provides a mask on a vpMatrix. All properties of vpMatrix ar...
Definition: vpSubMatrix.h:55
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 & build(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:384