Visual Servoing Platform  version 3.6.1 under development (2024-10-13)
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/vpDebug.h>
38 #include <visp3/core/vpTrackingException.h>
39 #include <visp3/core/vpVelocityTwistMatrix.h>
40 #include <visp3/mbt/vpMbKltTracker.h>
41 #include <visp3/mbt/vpMbtXmlGenericParser.h>
42 
43 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
44 
45 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
46 #include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
47 #endif
48 
49 BEGIN_VISP_NAMESPACE
50 
51 #ifndef DOXYGEN_SHOULD_SKIP_THIS
52 namespace
53 {
73 vpMatrix homography2collineation(const vpMatrix &H, const vpCameraParameters &cam)
74 {
75  vpMatrix G(3, 3);
76  double px = cam.get_px();
77  double py = cam.get_py();
78  double u0 = cam.get_u0();
79  double v0 = cam.get_v0();
80  double one_over_px = cam.get_px_inverse();
81  double one_over_py = cam.get_py_inverse();
82  double h00 = H[0][0], h01 = H[0][1], h02 = H[0][2];
83  double h10 = H[1][0], h11 = H[1][1], h12 = H[1][2];
84  double h20 = H[2][0], h21 = H[2][1], h22 = H[2][2];
85 
86  double A = h00 * px + u0 * h20;
87  double B = h01 * px + u0 * h21;
88  double C = h02 * px + u0 * h22;
89  double D = h10 * py + v0 * h20;
90  double E = h11 * py + v0 * h21;
91  double F = h12 * py + v0 * h22;
92 
93  G[0][0] = A * one_over_px;
94  G[1][0] = D * one_over_px;
95  G[2][0] = h20 * one_over_px;
96 
97  G[0][1] = B * one_over_py;
98  G[1][1] = E * one_over_py;
99  G[2][1] = h21 * one_over_py;
100 
101  double u0_one_over_px = u0 * one_over_px;
102  double v0_one_over_py = v0 * one_over_py;
103 
104  G[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C;
105  G[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F;
106  G[2][2] = -h20 * u0_one_over_px - h21 * v0_one_over_py + h22;
107 
108  return G;
109 }
110 } // namespace
111 
112 #endif // DOXYGEN_SHOULD_SKIP_THIS
113 
114 vpMbKltTracker::vpMbKltTracker()
115  :
116  cur(), c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
117  kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
118  m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt()
119 {
120  tracker.setTrackerId(1);
121  tracker.setUseHarris(1);
122  tracker.setMaxFeatures(10000);
123  tracker.setWindowSize(5);
124  tracker.setQuality(0.01);
125  tracker.setMinDistance(5);
126  tracker.setHarrisFreeParameter(0.01);
127  tracker.setBlockSize(3);
128  tracker.setPyramidLevels(3);
129 
130 #ifdef VISP_HAVE_OGRE
131  faces.getOgreContext()->setWindowName("MBT Klt");
132 #endif
133 
134  m_lambda = 0.8;
135  m_maxIter = 200;
136 }
137 
142 vpMbKltTracker::~vpMbKltTracker()
143 {
144  // delete the Klt Polygon features
145  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
146  vpMbtDistanceKltPoints *kltpoly = *it;
147  if (kltpoly != nullptr) {
148  delete kltpoly;
149  }
150  kltpoly = nullptr;
151  }
152  kltPolygons.clear();
153 
154  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
155  ++it) {
156  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
157  if (kltPolyCylinder != nullptr) {
158  delete kltPolyCylinder;
159  }
160  kltPolyCylinder = nullptr;
161  }
162  kltCylinders.clear();
163 
164  // delete the structures used to display circles
165  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
166  vpMbtDistanceCircle *ci = *it;
167  if (ci != nullptr) {
168  delete ci;
169  }
170  ci = nullptr;
171  }
172 
173  circles_disp.clear();
174 }
175 
176 void vpMbKltTracker::init(const vpImage<unsigned char> &I)
177 {
178  if (!modelInitialised) {
179  throw vpException(vpException::fatalError, "model not initialized");
180  }
181 
182  bool reInitialisation = false;
183  if (!useOgre)
184  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
185  else {
186 #ifdef VISP_HAVE_OGRE
187  if (!faces.isOgreInitialised()) {
188  faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
189  faces.setOgreShowConfigDialog(ogreShowConfigDialog);
190  faces.initOgre(m_cam);
191  // Turn off Ogre config dialog display for the next call to this
192  // function since settings are saved in the ogre.cfg file and used
193  // during the next call
194  ogreShowConfigDialog = false;
195  }
196 
197  faces.setVisibleOgre(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
198 
199 #else
200  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
201 #endif
202  }
203  reinit(I);
204 }
205 
206 void vpMbKltTracker::reinit(const vpImage<unsigned char> &I)
207 {
208  c0Mo = m_cMo;
209  ctTc0.eye();
210 
211  vpImageConvert::convert(I, cur);
212 
213  m_cam.computeFov(I.getWidth(), I.getHeight());
214 
215  if (useScanLine) {
216  faces.computeClippedPolygons(m_cMo, m_cam);
217  faces.computeScanLineRender(m_cam, I.getWidth(), I.getHeight());
218  }
219 
220  // mask
221  cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
222 
223  vpMbtDistanceKltPoints *kltpoly;
224  vpMbtDistanceKltCylinder *kltPolyCylinder;
225  if (useScanLine) {
226  vpImageConvert::convert(faces.getMbScanLineRenderer().getMask(), mask);
227  }
228  else {
229  unsigned char val = 255 /* - i*15*/;
230  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
231  kltpoly = *it;
232  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
233  // need to changeFrame when reinit() is called by postTracking
234  // other solution is
235  kltpoly->polygon->changeFrame(m_cMo);
236  kltpoly->polygon->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
237  kltpoly->updateMask(mask, val, maskBorder);
238  }
239  }
240 
241  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
242  ++it) {
243  kltPolyCylinder = *it;
244 
245  if (kltPolyCylinder->isTracked()) {
246  for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
247  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
248  if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
249  faces[indCylBBox]->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
250  }
251  }
252 
253  kltPolyCylinder->updateMask(mask, val, maskBorder);
254  }
255  }
256  }
257 
258  tracker.initTracking(cur, mask);
259  // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
260  // the points are valid for tracking and avoid too fast reinitialisations.
261  // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
262  // points" << std::endl;
263 
264  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
265  kltpoly = *it;
266  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
267  kltpoly->init(tracker, m_mask);
268  }
269  }
270 
271  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
272  ++it) {
273  kltPolyCylinder = *it;
274 
275  if (kltPolyCylinder->isTracked())
276  kltPolyCylinder->init(tracker, m_cMo);
277  }
278 }
279 
284 void vpMbKltTracker::resetTracker()
285 {
286  m_cMo.eye();
287 
288  // delete the Klt Polygon features
289  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
290  vpMbtDistanceKltPoints *kltpoly = *it;
291  if (kltpoly != nullptr) {
292  delete kltpoly;
293  }
294  kltpoly = nullptr;
295  }
296  kltPolygons.clear();
297 
298  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
299  ++it) {
300  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
301  if (kltPolyCylinder != nullptr) {
302  delete kltPolyCylinder;
303  }
304  kltPolyCylinder = nullptr;
305  }
306  kltCylinders.clear();
307 
308  // delete the structures used to display circles
309  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
310  vpMbtDistanceCircle *ci = *it;
311  if (ci != nullptr) {
312  delete ci;
313  }
314  ci = nullptr;
315  }
316 
317  circles_disp.clear();
318 
319  m_computeInteraction = true;
320  firstInitialisation = true;
321  computeCovariance = false;
322 
323  tracker.setTrackerId(1);
324  tracker.setUseHarris(1);
325 
326  tracker.setMaxFeatures(10000);
327  tracker.setWindowSize(5);
328  tracker.setQuality(0.01);
329  tracker.setMinDistance(5);
330  tracker.setHarrisFreeParameter(0.01);
331  tracker.setBlockSize(3);
332  tracker.setPyramidLevels(3);
333 
334  angleAppears = vpMath::rad(89);
335  angleDisappears = vpMath::rad(89);
336 
337  clippingFlag = vpPolygon3D::NO_CLIPPING;
338 
339  maskBorder = 5;
340  threshold_outlier = 0.5;
341  percentGood = 0.6;
342 
343  m_lambda = 0.8;
344  m_maxIter = 200;
345 
346  faces.reset();
347 
348  m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
349 
350  useScanLine = false;
351 
352 #ifdef VISP_HAVE_OGRE
353  useOgre = false;
354 #endif
355 }
356 
365 std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
366 {
367  std::vector<vpImagePoint> kltPoints;
368  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
369  long id;
370  float x_tmp, y_tmp;
371  tracker.getFeature((int)i, id, x_tmp, y_tmp);
372  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
373  }
374 
375  return kltPoints;
376 }
377 
386 std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
387 {
388  std::map<int, vpImagePoint> kltPoints;
389  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
390  long id;
391  float x_tmp, y_tmp;
392  tracker.getFeature((int)i, id, x_tmp, y_tmp);
393 #ifdef TARGET_OS_IPHONE
394  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
395 #else
396  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
397 #endif
398  }
399 
400  return kltPoints;
401 }
402 
408 void vpMbKltTracker::setKltOpencv(const vpKltOpencv &t)
409 {
410  tracker.setMaxFeatures(t.getMaxFeatures());
411  tracker.setWindowSize(t.getWindowSize());
412  tracker.setQuality(t.getQuality());
413  tracker.setMinDistance(t.getMinDistance());
414  tracker.setHarrisFreeParameter(t.getHarrisFreeParameter());
415  tracker.setBlockSize(t.getBlockSize());
416  tracker.setPyramidLevels(t.getPyramidLevels());
417 }
418 
424 void vpMbKltTracker::setCameraParameters(const vpCameraParameters &cam)
425 {
426  // for (unsigned int i = 0; i < faces.size(); i += 1){
427  // faces[i]->setCameraParameters(camera);
428  // }
429 
430  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
431  vpMbtDistanceKltPoints *kltpoly = *it;
432  kltpoly->setCameraParameters(cam);
433  }
434 
435  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
436  ++it) {
437  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
438  kltPolyCylinder->setCameraParameters(cam);
439  }
440 
441  m_cam = cam;
442 }
443 
444 void vpMbKltTracker::setPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
445  const vpHomogeneousMatrix &cdMo)
446 {
447  if (I_color) {
448  vpImageConvert::convert(*I_color, m_I);
449  }
450 
451  if (!kltCylinders.empty()) {
452  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
453  "This feature is not implemented yet."
454  << std::endl;
455  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
456  m_cMo = cdMo;
457  if (I) {
458  init(*I);
459  }
460  else {
461  init(m_I);
462  }
463  }
464  else {
465  vpMbtDistanceKltPoints *kltpoly;
466 
467  std::vector<cv::Point2f> init_pts;
468  std::vector<long> init_ids;
469  std::vector<cv::Point2f> guess_pts;
470 
471  vpHomogeneousMatrix cdMc = cdMo * m_cMo.inverse();
472  vpHomogeneousMatrix cMcd = cdMc.inverse();
473 
474  vpRotationMatrix cdRc;
475  vpTranslationVector cdtc;
476 
477  cdMc.extract(cdRc);
478  cdMc.extract(cdtc);
479 
480  // unsigned int nbCur = 0;
481 
482  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
483  kltpoly = *it;
484 
485  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
486  kltpoly->polygon->changeFrame(cdMo);
487 
488  // Get the normal to the face at the current state cMo
489  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
490  plan.changeFrame(cMcd);
491 
492  vpColVector Nc = plan.getNormal();
493  Nc.normalize();
494 
495  double invDc = 1.0 / plan.getD();
496 
497  // Create the homography
498  vpMatrix cdHc;
499  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
500  cdHc /= cdHc[2][2];
501 
502  // Compute homography in the pixel space cdGc = K * cdHc * K^{-1}
503  vpMatrix cdGc = homography2collineation(cdHc, m_cam);
504 
505  // Points displacement
506  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
507  // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
508  for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
509 #ifdef TARGET_OS_IPHONE
510  if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
511  init_ids.end())
512 #else
513  if (std::find(init_ids.begin(), init_ids.end(),
514  (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
515 #endif
516  {
517  // KLT point already processed (a KLT point can exist in another
518  // vpMbtDistanceKltPoints due to possible overlapping faces)
519  continue;
520  }
521 
522  vpColVector cdp(3);
523  cdp[0] = iter->second.get_j();
524  cdp[1] = iter->second.get_i();
525  cdp[2] = 1.0;
526 
527  cv::Point2f p((float)cdp[0], (float)cdp[1]);
528  init_pts.push_back(p);
529 #ifdef TARGET_OS_IPHONE
530  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
531 #else
532  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
533 #endif
534 
535  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
536 
537  if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
538  cdp[0] = 0.0;
539  cdp[1] = 0.0;
540  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
541  }
542 
543  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
544  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
545 
546  // Set value to the KLT tracker
547  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
548  guess_pts.push_back(p_guess);
549  }
550  }
551  }
552 
553  if (I) {
554  vpImageConvert::convert(*I, cur);
555  }
556  else {
557  vpImageConvert::convert(m_I, cur);
558  }
559 
560  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
561 
562  bool reInitialisation = false;
563  if (!useOgre) {
564  if (I) {
565  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
566  }
567  else {
568  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
569  }
570  }
571  else {
572 #ifdef VISP_HAVE_OGRE
573  if (I) {
574  faces.setVisibleOgre(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears,
575  reInitialisation);
576  }
577  else {
578  faces.setVisibleOgre(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears,
579  reInitialisation);
580  }
581 #else
582  if (I) {
583  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
584  }
585  else {
586  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
587  }
588 #endif
589  }
590 
591  m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
592 
593  if (useScanLine) {
594  faces.computeClippedPolygons(cdMo, m_cam);
595  faces.computeScanLineRender(m_cam, I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
596  }
597 
598  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
599  kltpoly = *it;
600  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
601  kltpoly->polygon->computePolygonClipped(m_cam);
602  kltpoly->init(tracker, m_mask);
603  }
604  }
605 
606  m_cMo = cdMo;
607  c0Mo = m_cMo;
608  ctTc0.eye();
609  }
610 }
611 
621 void vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
622 {
623  vpMbKltTracker::setPose(&I, nullptr, cdMo);
624 }
625 
635 void vpMbKltTracker::setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo)
636 {
637  vpMbKltTracker::setPose(nullptr, &I_color, cdMo);
638 }
639 
646 void vpMbKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
647 {
648  vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
649  kltPoly->setCameraParameters(m_cam);
650  kltPoly->polygon = &polygon;
651  kltPoly->hiddenface = &faces;
652  kltPoly->useScanLine = useScanLine;
653  kltPolygons.push_back(kltPoly);
654 }
661 void vpMbKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
662 {
663  vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
664  kltPoly->setCameraParameters(m_cam);
665  kltPoly->polygon = &polygon;
666  kltPoly->hiddenface = &faces;
667  kltPoly->useScanLine = useScanLine;
668  kltPolygons.push_back(kltPoly);
669 }
670 
677 void vpMbKltTracker::preTracking(const vpImage<unsigned char> &I)
678 {
679  vpImageConvert::convert(I, cur);
680  tracker.track(cur);
681 
682  m_nbInfos = 0;
683  m_nbFaceUsed = 0;
684  // for (unsigned int i = 0; i < faces.size(); i += 1){
685  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
686  vpMbtDistanceKltPoints *kltpoly = *it;
687  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
688  kltpoly->computeNbDetectedCurrent(tracker, m_mask);
689  // faces[i]->ransac();
690  if (kltpoly->hasEnoughPoints()) {
691  m_nbInfos += kltpoly->getCurrentNumberPoints();
692  m_nbFaceUsed++;
693  }
694  }
695  }
696 
697  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
698  ++it) {
699  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
700 
701  if (kltPolyCylinder->isTracked()) {
702  kltPolyCylinder->computeNbDetectedCurrent(tracker);
703  if (kltPolyCylinder->hasEnoughPoints()) {
704  m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
705  m_nbFaceUsed++;
706  }
707  }
708  }
709 }
710 
714 bool vpMbKltTracker::postTracking(const vpImage<unsigned char> &I, vpColVector &w)
715 {
716  // # For a better Post Tracking, tracker should reinitialize if so faces
717  // don't have enough points but are visible. # Here we are not doing it for
718  // more speed performance.
719  bool reInitialisation = false;
720 
721  unsigned int initialNumber = 0;
722  unsigned int currentNumber = 0;
723  unsigned int shift = 0;
724  // for (unsigned int i = 0; i < faces.size(); i += 1){
725  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
726  vpMbtDistanceKltPoints *kltpoly = *it;
727  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
728  initialNumber += kltpoly->getInitialNumberPoint();
729  if (kltpoly->hasEnoughPoints()) {
730  vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
731  shift += 2 * kltpoly->getCurrentNumberPoints();
732  kltpoly->removeOutliers(sub_w, threshold_outlier);
733 
734  currentNumber += kltpoly->getCurrentNumberPoints();
735  }
736  // else{
737  // reInitialisation = true;
738  // break;
739  // }
740  }
741  }
742 
743  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
744  ++it) {
745  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
746 
747  if (kltPolyCylinder->isTracked()) {
748  initialNumber += kltPolyCylinder->getInitialNumberPoint();
749  if (kltPolyCylinder->hasEnoughPoints()) {
750  vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
751  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
752  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
753 
754  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
755  }
756  }
757  }
758 
759  // if(!reInitialisation){
760  double value = percentGood * (double)initialNumber;
761  if ((double)currentNumber < value) {
762  // std::cout << "Too many point disappear : " << initialNumber << "/"
763  // << currentNumber << std::endl;
764  reInitialisation = true;
765  }
766  else {
767  if (!useOgre)
768  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
769  else {
770 #ifdef VISP_HAVE_OGRE
771  faces.setVisibleOgre(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
772 #else
773  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
774 #endif
775  }
776  }
777  // }
778 
779  if (reInitialisation)
780  return true;
781 
782  return false;
783 }
784 
788 void vpMbKltTracker::computeVVS()
789 {
790  vpMatrix L_true; // interaction matrix without weighting
791  vpMatrix LVJ_true;
792  vpColVector v; // "speed" for VVS
793 
794  vpMatrix LTL;
795  vpColVector LTR;
796  vpHomogeneousMatrix cMoPrev;
797  vpHomogeneousMatrix ctTc0_Prev;
798  vpColVector error_prev;
799  double mu = m_initialMu;
800 
801  double normRes = 0;
802  double normRes_1 = -1;
803  unsigned int iter = 0;
804 
805  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
806  if (isoJoIdentity)
807  oJo.eye();
808 
809  vpMbKltTracker::computeVVSInit();
810 
811  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
812  vpMbKltTracker::computeVVSInteractionMatrixAndResidu();
813 
814  bool reStartFromLastIncrement = false;
815  computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
816  if (reStartFromLastIncrement) {
817  ctTc0 = ctTc0_Prev;
818  }
819 
820  if (!reStartFromLastIncrement) {
821  vpMbTracker::computeVVSWeights(m_robust_klt, m_error_klt, m_w_klt);
822 
823  if (computeCovariance) {
824  L_true = m_L_klt;
825  if (!isoJoIdentity) {
827  cVo.buildFrom(m_cMo);
828  LVJ_true = (m_L_klt * cVo * oJo);
829  }
830  }
831 
832  normRes_1 = normRes;
833  normRes = 0.0;
834 
835  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
836  m_weightedError_klt[i] = m_error_klt[i] * m_w_klt[i];
837  normRes += m_weightedError_klt[i];
838  }
839 
840  if ((iter == 0) || m_computeInteraction) {
841  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
842  for (unsigned int j = 0; j < 6; j++) {
843  m_L_klt[i][j] *= m_w_klt[i];
844  }
845  }
846  }
847 
848  computeVVSPoseEstimation(m_isoJoIdentity, iter, m_L_klt, LTL, m_weightedError_klt, m_error_klt, error_prev, LTR, mu, v);
849 
850  cMoPrev = m_cMo;
851  ctTc0_Prev = ctTc0;
852  ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
853  m_cMo = ctTc0 * c0Mo;
854  } // endif(!reStartFromLastIncrement)
855 
856  iter++;
857  }
858 
859  computeCovarianceMatrixVVS(m_isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
860 }
861 
862 void vpMbKltTracker::computeVVSInit()
863 {
864  unsigned int nbFeatures = 2 * m_nbInfos;
865 
866  m_L_klt.resize(nbFeatures, 6, false, false);
867  m_error_klt.resize(nbFeatures, false);
868 
869  m_weightedError_klt.resize(nbFeatures, false);
870  m_w_klt.resize(nbFeatures, false);
871  m_w_klt = 1;
872 
873  m_robust_klt.setMinMedianAbsoluteDeviation(2 / m_cam.get_px());
874 }
875 
876 void vpMbKltTracker::computeVVSInteractionMatrixAndResidu()
877 {
878  unsigned int shift = 0;
879  vpHomography H;
880 
881  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
882  vpMbtDistanceKltPoints *kltpoly = *it;
883  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
884  kltpoly->hasEnoughPoints()) {
885  vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
886  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
887 
888  try {
889  kltpoly->computeHomography(ctTc0, H);
890  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
891  }
892  catch (...) {
893  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
894  }
895 
896  shift += 2 * kltpoly->getCurrentNumberPoints();
897  }
898  }
899 
900  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
901  ++it) {
902  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
903 
904  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
905  vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
906  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
907 
908  try {
909  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
910  }
911  catch (...) {
912  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
913  }
914 
915  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
916  }
917  }
918 }
919 
927 void vpMbKltTracker::track(const vpImage<unsigned char> &I)
928 {
929  preTracking(I);
930 
931  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
932  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
933  }
934 
935  computeVVS();
936 
937  if (postTracking(I, m_w_klt))
938  reinit(I);
939 
940  if (displayFeatures) {
941  m_featuresToBeDisplayedKlt = getFeaturesForDisplayKlt();
942  }
943 }
944 
952 void vpMbKltTracker::track(const vpImage<vpRGBa> &I_color)
953 {
954  vpImageConvert::convert(I_color, m_I);
955  preTracking(m_I);
956 
957  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
958  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
959  }
960 
961  computeVVS();
962 
963  if (postTracking(m_I, m_w_klt))
964  reinit(m_I);
965 }
966 
1010 void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
1011 {
1012 #if defined(VISP_HAVE_PUGIXML)
1013 // Load projection error config
1014  vpMbTracker::loadConfigFile(configFile, verbose);
1015 
1017  xmlp.setVerbose(verbose);
1018  xmlp.setKltMaxFeatures(10000);
1019  xmlp.setKltWindowSize(5);
1020  xmlp.setKltQuality(0.01);
1021  xmlp.setKltMinDistance(5);
1022  xmlp.setKltHarrisParam(0.01);
1023  xmlp.setKltBlockSize(3);
1024  xmlp.setKltPyramidLevels(3);
1025  xmlp.setKltMaskBorder(maskBorder);
1026  xmlp.setAngleAppear(vpMath::deg(angleAppears));
1027  xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
1028 
1029  try {
1030  if (verbose) {
1031  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1032  }
1033  xmlp.parse(configFile.c_str());
1034  }
1035  catch (...) {
1036  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1037  throw vpException(vpException::ioError, "problem to parse configuration file.");
1038  }
1039 
1040  vpCameraParameters camera;
1041  xmlp.getCameraParameters(camera);
1042  setCameraParameters(camera);
1043 
1044  tracker.setMaxFeatures((int)xmlp.getKltMaxFeatures());
1045  tracker.setWindowSize((int)xmlp.getKltWindowSize());
1046  tracker.setQuality(xmlp.getKltQuality());
1047  tracker.setMinDistance(xmlp.getKltMinDistance());
1048  tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
1049  tracker.setBlockSize((int)xmlp.getKltBlockSize());
1050  tracker.setPyramidLevels((int)xmlp.getKltPyramidLevels());
1051  maskBorder = xmlp.getKltMaskBorder();
1052  angleAppears = vpMath::rad(xmlp.getAngleAppear());
1053  angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
1054 
1055  // if(useScanLine)
1056  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1057 
1058  if (xmlp.hasNearClippingDistance())
1059  setNearClippingDistance(xmlp.getNearClippingDistance());
1060 
1061  if (xmlp.hasFarClippingDistance())
1062  setFarClippingDistance(xmlp.getFarClippingDistance());
1063 
1064  if (xmlp.getFovClipping())
1065  setClipping(clippingFlag = clippingFlag | vpPolygon3D::FOV_CLIPPING);
1066 
1067  useLodGeneral = xmlp.getLodState();
1068  minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
1069  minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();
1070 
1071  applyLodSettingInConfig = false;
1072  if (this->getNbPolygon() > 0) {
1073  applyLodSettingInConfig = true;
1074  setLod(useLodGeneral);
1075  setMinLineLengthThresh(minLineLengthThresholdGeneral);
1076  setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
1077  }
1078 #else
1079  (void)configFile;
1080  (void)verbose;
1081  throw(vpException(vpException::ioError, "vpMbKltTracker::loadConfigFile() needs pugixml built-in 3rdparty"));
1082 #endif
1083 }
1084 
1096 void vpMbKltTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
1097  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1098  bool displayFullModel)
1099 {
1100  std::vector<std::vector<double> > models =
1101  vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1102 
1103  for (size_t i = 0; i < models.size(); i++) {
1104  if (vpMath::equal(models[i][0], 0)) {
1105  vpImagePoint ip1(models[i][1], models[i][2]);
1106  vpImagePoint ip2(models[i][3], models[i][4]);
1107  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1108  }
1109  else if (vpMath::equal(models[i][0], 1)) {
1110  vpImagePoint center(models[i][1], models[i][2]);
1111  double n20 = models[i][3];
1112  double n11 = models[i][4];
1113  double n02 = models[i][5];
1114  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1115  }
1116  }
1117 
1118  if (displayFeatures) {
1119  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1120  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1121  vpImagePoint ip1(m_featuresToBeDisplayedKlt[i][1], m_featuresToBeDisplayedKlt[i][2]);
1123 
1124  vpImagePoint ip2(m_featuresToBeDisplayedKlt[i][3], m_featuresToBeDisplayedKlt[i][4]);
1125  double id = m_featuresToBeDisplayedKlt[i][5];
1126  std::stringstream ss;
1127  ss << id;
1128  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1129  }
1130  }
1131  }
1132 
1133 #ifdef VISP_HAVE_OGRE
1134  if (useOgre)
1135  faces.displayOgre(cMo);
1136 #endif
1137 }
1138 
1150 void vpMbKltTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
1151  const vpColor &col, unsigned int thickness, bool displayFullModel)
1152 {
1153  std::vector<std::vector<double> > models =
1154  vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1155 
1156  for (size_t i = 0; i < models.size(); i++) {
1157  if (vpMath::equal(models[i][0], 0)) {
1158  vpImagePoint ip1(models[i][1], models[i][2]);
1159  vpImagePoint ip2(models[i][3], models[i][4]);
1160  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1161  }
1162  else if (vpMath::equal(models[i][0], 1)) {
1163  vpImagePoint center(models[i][1], models[i][2]);
1164  double n20 = models[i][3];
1165  double n11 = models[i][4];
1166  double n02 = models[i][5];
1167  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1168  }
1169  }
1170 
1171  if (displayFeatures) {
1172  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1173  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1174  vpImagePoint ip1(m_featuresToBeDisplayedKlt[i][1], m_featuresToBeDisplayedKlt[i][2]);
1176 
1177  vpImagePoint ip2(m_featuresToBeDisplayedKlt[i][3], m_featuresToBeDisplayedKlt[i][4]);
1178  double id = m_featuresToBeDisplayedKlt[i][5];
1179  std::stringstream ss;
1180  ss << id;
1181  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1182  }
1183  }
1184  }
1185 
1186 #ifdef VISP_HAVE_OGRE
1187  if (useOgre)
1188  faces.displayOgre(cMo);
1189 #endif
1190 }
1191 
1192 std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1193 {
1194  std::vector<std::vector<double> > features;
1195 
1196  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1197  vpMbtDistanceKltPoints *kltpoly = *it;
1198 
1199  if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1200  std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1201  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1202  }
1203  }
1204 
1205  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1206  ++it) {
1207  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1208 
1209  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1210  std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1211  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1212  }
1213  }
1214 
1215  return features;
1216 }
1217 
1233 std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1234  const vpHomogeneousMatrix &cMo,
1235  const vpCameraParameters &cam,
1236  bool displayFullModel)
1237 {
1238  std::vector<std::vector<double> > models;
1239 
1240  vpCameraParameters c = cam;
1241 
1242  if (clippingFlag > 3) // Contains at least one FOV constraint
1243  c.computeFov(width, height);
1244 
1245  // vpMbtDistanceKltPoints *kltpoly;
1246  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1247 
1248  // Previous version 12/08/2015
1249  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1250  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1251  // kltpoly = *it;
1252  // kltpoly->polygon->changeFrame(cMo_);
1253  // kltpoly->polygon->computePolygonClipped(c);
1254  // }
1255  faces.computeClippedPolygons(cMo, c);
1256 
1257  if (useScanLine && !displayFullModel)
1258  faces.computeScanLineRender(m_cam, width, height);
1259 
1260  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1261  vpMbtDistanceKltPoints *kltpoly = *it;
1262  std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(cam, displayFullModel);
1263  models.insert(models.end(), modelLines.begin(), modelLines.end());
1264  }
1265 
1266  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1267  ++it) {
1268  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1269  std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo, cam);
1270  models.insert(models.end(), modelLines.begin(), modelLines.end());
1271  }
1272 
1273  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1274  vpMbtDistanceCircle *displayCircle = *it;
1275  std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo, cam, displayFullModel);
1276  if (!paramsCircle.empty()) {
1277  models.push_back(paramsCircle);
1278  }
1279  }
1280 
1281  return models;
1282 }
1283 
1290 void vpMbKltTracker::testTracking()
1291 {
1292  unsigned int nbTotalPoints = 0;
1293  // for (unsigned int i = 0; i < faces.size(); i += 1){
1294  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1295  vpMbtDistanceKltPoints *kltpoly = *it;
1296  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1297  kltpoly->hasEnoughPoints()) {
1298  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1299  }
1300  }
1301 
1302  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1303  ++it) {
1304  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1305  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1306  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1307  }
1308 
1309  if (nbTotalPoints < 10) {
1310  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1312  "test tracking failed (too few points to realize a good tracking).");
1313  }
1314 }
1315 
1326 void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1327  const std::string & /*name*/)
1328 {
1329  vpMbtDistanceKltCylinder *kltPoly = new vpMbtDistanceKltCylinder();
1330  kltPoly->setCameraParameters(m_cam);
1331 
1332  kltPoly->buildFrom(p1, p2, radius);
1333 
1334  // Add the Cylinder BBox to the list of polygons
1335  kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1336  kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1337  kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1338  kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1339 
1340  kltPoly->hiddenface = &faces;
1341  kltPoly->useScanLine = useScanLine;
1342  kltCylinders.push_back(kltPoly);
1343 }
1344 
1356 void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int /*idFace*/,
1357  const std::string &name)
1358 {
1359  addCircle(p1, p2, p3, radius, name);
1360 }
1361 
1370 void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
1371  const std::string &name)
1372 {
1373  bool already_here = false;
1374 
1375  // for(std::list<vpMbtDistanceCircle*>::const_iterator
1376  // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1377  // ci = *it;
1378  // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1379  // samePoint(*(ci->p3),P3)) ||
1380  // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1381  // samePoint(*(ci->p3),P2)) ){
1382  // already_here = (std::fabs(ci->radius - r) <
1383  // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1384  // r));
1385  // }
1386  // }
1387 
1388  if (!already_here) {
1390 
1391  ci->setCameraParameters(m_cam);
1392  ci->setName(name);
1393  ci->buildFrom(P1, P2, P3, r);
1394  circles_disp.push_back(ci);
1395  }
1396 }
1397 
1410 void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1411  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1412 {
1413  m_cMo.eye();
1414 
1415  firstInitialisation = true;
1416 
1417  // delete the Klt Polygon features
1418  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1419  vpMbtDistanceKltPoints *kltpoly = *it;
1420  if (kltpoly != nullptr) {
1421  delete kltpoly;
1422  }
1423  kltpoly = nullptr;
1424  }
1425  kltPolygons.clear();
1426 
1427  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1428  ++it) {
1429  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1430  if (kltPolyCylinder != nullptr) {
1431  delete kltPolyCylinder;
1432  }
1433  kltPolyCylinder = nullptr;
1434  }
1435  kltCylinders.clear();
1436 
1437  // delete the structures used to display circles
1438  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1439  vpMbtDistanceCircle *ci = *it;
1440  if (ci != nullptr) {
1441  delete ci;
1442  }
1443  ci = nullptr;
1444  }
1445 
1446  faces.reset();
1447 
1448  loadModel(cad_name, verbose, T);
1449  initFromPose(I, cMo);
1450 }
1451 
1459 void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1460 {
1461  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1462  vpMbtDistanceKltPoints *kltpoly = *it;
1463  if (kltpoly->polygon->getName() == name) {
1464  kltpoly->setTracked(useKltTracking);
1465  }
1466  }
1467 }
1468 END_VISP_NAMESPACE
1469 #elif !defined(VISP_BUILD_SHARED_LIBS)
1470 // Work around to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no symbols
1471 void dummy_vpMbKltTracker() { };
1472 #endif // VISP_HAVE_OPENCV
void vpGEMM(const vpArray2D< double > &A, const vpArray2D< double > &B, const double &alpha, const vpArray2D< double > &C, const double &beta, vpArray2D< double > &D, const unsigned int &ops=0)
Definition: vpGEMM.h:414
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:191
vpColVector & normalize()
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:217
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:60
@ ioError
I/O error.
Definition: vpException.h:67
@ fatalError
Fatal error.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
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:174
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:242
unsigned int getCols() const
Definition: vpImage.h:171
unsigned int getHeight() const
Definition: vpImage.h:181
unsigned int getRows() const
Definition: vpImage.h:212
void init(unsigned int h, unsigned int w, Type value)
Definition: vpImage.h:376
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:74
double getQuality() const
Definition: vpKltOpencv.h:211
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:193
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:213
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
Definition: vpKltOpencv.h:189
double getMinDistance() const
Definition: vpKltOpencv.h:196
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:169
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:208
static double rad(double deg)
Definition: vpMath.h:129
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
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 used by the model-based tracker.
Definition: vpMbtPolygon.h:60
Parse an Xml file to extract configuration parameters of a mbtConfig object.
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
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:57
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)