Visual Servoing Platform  version 3.5.1 under development (2023-06-06)
vpMbKltTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Model based tracker using only KLT
33  *
34  * Authors:
35  * Romain Tallonneau
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpImageConvert.h>
41 #include <visp3/core/vpTrackingException.h>
42 #include <visp3/core/vpVelocityTwistMatrix.h>
43 #include <visp3/mbt/vpMbKltTracker.h>
44 #include <visp3/mbt/vpMbtXmlGenericParser.h>
45 
46 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
47 
48 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
49 #include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
50 #endif
51 
52 namespace
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 
113  :
114 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
115  cur(),
116 #else
117  cur(NULL),
118 #endif
119  c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
120  kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
121  m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt()
122 {
125  tracker.setMaxFeatures(10000);
127  tracker.setQuality(0.01);
132 
133 #ifdef VISP_HAVE_OGRE
134  faces.getOgreContext()->setWindowName("MBT Klt");
135 #endif
136 
137  m_lambda = 0.8;
138  m_maxIter = 200;
139 }
140 
146 {
147 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
148  if (cur != NULL) {
149  cvReleaseImage(&cur);
150  cur = NULL;
151  }
152 #endif
153 
154  // delete the Klt Polygon features
155  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
156  vpMbtDistanceKltPoints *kltpoly = *it;
157  if (kltpoly != NULL) {
158  delete kltpoly;
159  }
160  kltpoly = NULL;
161  }
162  kltPolygons.clear();
163 
164  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
165  ++it) {
166  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
167  if (kltPolyCylinder != NULL) {
168  delete kltPolyCylinder;
169  }
170  kltPolyCylinder = NULL;
171  }
172  kltCylinders.clear();
173 
174  // delete the structures used to display circles
175  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
176  vpMbtDistanceCircle *ci = *it;
177  if (ci != NULL) {
178  delete ci;
179  }
180  ci = NULL;
181  }
182 
183  circles_disp.clear();
184 }
185 
187 {
188  if (!modelInitialised) {
189  throw vpException(vpException::fatalError, "model not initialized");
190  }
191 
192  bool reInitialisation = false;
193  if (!useOgre)
194  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
195  else {
196 #ifdef VISP_HAVE_OGRE
197  if (!faces.isOgreInitialised()) {
201  // Turn off Ogre config dialog display for the next call to this
202  // function since settings are saved in the ogre.cfg file and used
203  // during the next call
204  ogreShowConfigDialog = false;
205  }
206 
208 
209 #else
210  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
211 #endif
212  }
213  reinit(I);
214 }
215 
217 {
218  c0Mo = m_cMo;
219  ctTc0.eye();
220 
222 
224 
225  if (useScanLine) {
228  }
229 
230 // mask
231 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
232  cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
233 #else
234  IplImage *mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
235  cvZero(mask);
236 #endif
237 
238  vpMbtDistanceKltPoints *kltpoly;
239  vpMbtDistanceKltCylinder *kltPolyCylinder;
240  if (useScanLine) {
242  } else {
243  unsigned char val = 255 /* - i*15*/;
244  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
245  kltpoly = *it;
246  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
247  // need to changeFrame when reinit() is called by postTracking
248  // other solution is
249  kltpoly->polygon->changeFrame(m_cMo);
250  kltpoly->polygon->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
251  kltpoly->updateMask(mask, val, maskBorder);
252  }
253  }
254 
255  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
256  ++it) {
257  kltPolyCylinder = *it;
258 
259  if (kltPolyCylinder->isTracked()) {
260  for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
261  unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
262  if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
263  faces[indCylBBox]->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
264  }
265  }
266 
267  kltPolyCylinder->updateMask(mask, val, maskBorder);
268  }
269  }
270  }
271 
272  tracker.initTracking(cur, mask);
273  // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
274  // the points are valid for tracking and avoid too fast reinitialisations.
275  // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
276  // points" << std::endl;
277 
278  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
279  kltpoly = *it;
280  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
281  kltpoly->init(tracker, m_mask);
282  }
283  }
284 
285  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
286  ++it) {
287  kltPolyCylinder = *it;
288 
289  if (kltPolyCylinder->isTracked())
290  kltPolyCylinder->init(tracker, m_cMo);
291  }
292 
293 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
294  cvReleaseImage(&mask);
295 #endif
296 }
297 
303 {
304  m_cMo.eye();
305 
306 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
307  if (cur != NULL) {
308  cvReleaseImage(&cur);
309  cur = NULL;
310  }
311 #endif
312 
313  // delete the Klt Polygon features
314  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
315  vpMbtDistanceKltPoints *kltpoly = *it;
316  if (kltpoly != NULL) {
317  delete kltpoly;
318  }
319  kltpoly = NULL;
320  }
321  kltPolygons.clear();
322 
323  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
324  ++it) {
325  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
326  if (kltPolyCylinder != NULL) {
327  delete kltPolyCylinder;
328  }
329  kltPolyCylinder = NULL;
330  }
331  kltCylinders.clear();
332 
333  // delete the structures used to display circles
334  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
335  vpMbtDistanceCircle *ci = *it;
336  if (ci != NULL) {
337  delete ci;
338  }
339  ci = NULL;
340  }
341 
342  circles_disp.clear();
343 
344  m_computeInteraction = true;
345  firstInitialisation = true;
346  computeCovariance = false;
347 
350 
351  tracker.setMaxFeatures(10000);
353  tracker.setQuality(0.01);
358 
361 
363 
364  maskBorder = 5;
365  threshold_outlier = 0.5;
366  percentGood = 0.6;
367 
368  m_lambda = 0.8;
369  m_maxIter = 200;
370 
371  faces.reset();
372 
374 
375  useScanLine = false;
376 
377 #ifdef VISP_HAVE_OGRE
378  useOgre = false;
379 #endif
380 }
381 
390 std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
391 {
392  std::vector<vpImagePoint> kltPoints;
393  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
394  long id;
395  float x_tmp, y_tmp;
396  tracker.getFeature((int)i, id, x_tmp, y_tmp);
397  kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
398  }
399 
400  return kltPoints;
401 }
402 
411 std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
412 {
413  std::map<int, vpImagePoint> kltPoints;
414  for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
415  long id;
416  float x_tmp, y_tmp;
417  tracker.getFeature((int)i, id, x_tmp, y_tmp);
418 #if TARGET_OS_IPHONE
419  kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
420 #else
421  kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
422 #endif
423  }
424 
425  return kltPoints;
426 }
427 
434 {
442 }
443 
450 {
451  // for (unsigned int i = 0; i < faces.size(); i += 1){
452  // faces[i]->setCameraParameters(camera);
453  // }
454 
455  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
456  vpMbtDistanceKltPoints *kltpoly = *it;
457  kltpoly->setCameraParameters(cam);
458  }
459 
460  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
461  ++it) {
462  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
463  kltPolyCylinder->setCameraParameters(cam);
464  }
465 
466  m_cam = cam;
467 }
468 
469 void vpMbKltTracker::setPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
470  const vpHomogeneousMatrix &cdMo)
471 {
472  if (I_color) {
473  vpImageConvert::convert(*I_color, m_I);
474  }
475 
476  if (!kltCylinders.empty()) {
477  std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
478  "This feature is not implemented yet."
479  << std::endl;
480  std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
481  m_cMo = cdMo;
482  if (I) {
483  init(*I);
484  } else {
485  init(m_I);
486  }
487  } else {
488  vpMbtDistanceKltPoints *kltpoly;
489 
490 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
491  std::vector<cv::Point2f> init_pts;
492  std::vector<long> init_ids;
493  std::vector<cv::Point2f> guess_pts;
494 #else
495  unsigned int nbp = 0;
496  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
497  kltpoly = *it;
498  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
499  nbp += (*it)->getCurrentNumberPoints();
500  }
501 
502  CvPoint2D32f *init_pts = NULL;
503  init_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(init_pts[0]));
504  long *init_ids = (long *)cvAlloc((unsigned int)tracker.getMaxFeatures() * sizeof(long));
505  unsigned int iter_pts = 0;
506 
507  CvPoint2D32f *guess_pts = NULL;
508  guess_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(guess_pts[0]));
509 #endif
510 
511  vpHomogeneousMatrix cdMc = cdMo * m_cMo.inverse();
512  vpHomogeneousMatrix cMcd = cdMc.inverse();
513 
514  vpRotationMatrix cdRc;
515  vpTranslationVector cdtc;
516 
517  cdMc.extract(cdRc);
518  cdMc.extract(cdtc);
519 
520  // unsigned int nbCur = 0;
521 
522  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
523  kltpoly = *it;
524 
525  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
526  kltpoly->polygon->changeFrame(cdMo);
527 
528  // Get the normal to the face at the current state cMo
529  vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
530  plan.changeFrame(cMcd);
531 
532  vpColVector Nc = plan.getNormal();
533  Nc.normalize();
534 
535  double invDc = 1.0 / plan.getD();
536 
537  // Create the homography
538  vpMatrix cdHc;
539  vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
540  cdHc /= cdHc[2][2];
541 
542  // Compute homography in the pixel space cdGc = K * cdHc * K^{-1}
543  vpMatrix cdGc = homography2collineation(cdHc, m_cam);
544 
545  // Points displacement
546  std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
547  // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
548  for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
549 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
550 #if TARGET_OS_IPHONE
551  if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
552  init_ids.end())
553 #else
554  if (std::find(init_ids.begin(), init_ids.end(),
555  (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
556 #endif
557  {
558  // KLT point already processed (a KLT point can exist in another
559  // vpMbtDistanceKltPoints due to possible overlapping faces)
560  continue;
561  }
562 #endif
563 
564  vpColVector cdp(3);
565  cdp[0] = iter->second.get_j();
566  cdp[1] = iter->second.get_i();
567  cdp[2] = 1.0;
568 
569 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
570  cv::Point2f p((float)cdp[0], (float)cdp[1]);
571  init_pts.push_back(p);
572 #if TARGET_OS_IPHONE
573  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
574 #else
575  init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
576 #endif
577 #else
578  init_pts[iter_pts].x = (float)cdp[0];
579  init_pts[iter_pts].y = (float)cdp[1];
580  init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
581 #endif
582 
583  double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
584 
585  if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
586  cdp[0] = 0.0;
587  cdp[1] = 0.0;
588  throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
589  }
590 
591  cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
592  cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
593 
594 // Set value to the KLT tracker
595 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
596  cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
597  guess_pts.push_back(p_guess);
598 #else
599  guess_pts[iter_pts].x = (float)cdp[0];
600  guess_pts[iter_pts++].y = (float)cdp[1];
601 #endif
602  }
603  }
604  }
605 
606  if (I) {
608  } else {
610  }
611 
612 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
613  tracker.setInitialGuess(init_pts, guess_pts, init_ids);
614 #else
615  tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
616 
617  if (init_pts)
618  cvFree(&init_pts);
619  init_pts = NULL;
620 
621  if (guess_pts)
622  cvFree(&guess_pts);
623  guess_pts = NULL;
624 
625  if (init_ids)
626  cvFree(&init_ids);
627  init_ids = NULL;
628 #endif
629 
630  bool reInitialisation = false;
631  if (!useOgre) {
632  if (I) {
633  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
634  } else {
635  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
636  }
637  } else {
638 #ifdef VISP_HAVE_OGRE
639  if (I) {
641  reInitialisation);
642  } else {
644  reInitialisation);
645  }
646 #else
647  if (I) {
648  faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
649  } else {
650  faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
651  }
652 #endif
653  }
654 
655  m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
656 
657  if (useScanLine) {
660  }
661 
662  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
663  kltpoly = *it;
664  if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
666  kltpoly->init(tracker, m_mask);
667  }
668  }
669 
670  m_cMo = cdMo;
671  c0Mo = m_cMo;
672  ctTc0.eye();
673  }
674 }
675 
686 {
687  vpMbKltTracker::setPose(&I, NULL, cdMo);
688 }
689 
700 {
701  vpMbKltTracker::setPose(NULL, &I_color, cdMo);
702 }
703 
711 {
713  kltPoly->setCameraParameters(m_cam);
714  kltPoly->polygon = &polygon;
715  kltPoly->hiddenface = &faces;
716  kltPoly->useScanLine = useScanLine;
717  kltPolygons.push_back(kltPoly);
718 }
726 {
728  kltPoly->setCameraParameters(m_cam);
729  kltPoly->polygon = &polygon;
730  kltPoly->hiddenface = &faces;
731  kltPoly->useScanLine = useScanLine;
732  kltPolygons.push_back(kltPoly);
733 }
734 
742 {
744  tracker.track(cur);
745 
746  m_nbInfos = 0;
747  m_nbFaceUsed = 0;
748  // for (unsigned int i = 0; i < faces.size(); i += 1){
749  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
750  vpMbtDistanceKltPoints *kltpoly = *it;
751  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
753  // faces[i]->ransac();
754  if (kltpoly->hasEnoughPoints()) {
755  m_nbInfos += kltpoly->getCurrentNumberPoints();
756  m_nbFaceUsed++;
757  }
758  }
759  }
760 
761  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
762  ++it) {
763  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
764 
765  if (kltPolyCylinder->isTracked()) {
766  kltPolyCylinder->computeNbDetectedCurrent(tracker);
767  if (kltPolyCylinder->hasEnoughPoints()) {
768  m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
769  m_nbFaceUsed++;
770  }
771  }
772  }
773 }
774 
779 {
780  // # For a better Post Tracking, tracker should reinitialize if so faces
781  // don't have enough points but are visible. # Here we are not doing it for
782  // more speed performance.
783  bool reInitialisation = false;
784 
785  unsigned int initialNumber = 0;
786  unsigned int currentNumber = 0;
787  unsigned int shift = 0;
788  // for (unsigned int i = 0; i < faces.size(); i += 1){
789  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
790  vpMbtDistanceKltPoints *kltpoly = *it;
791  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
792  initialNumber += kltpoly->getInitialNumberPoint();
793  if (kltpoly->hasEnoughPoints()) {
794  vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
795  shift += 2 * kltpoly->getCurrentNumberPoints();
796  kltpoly->removeOutliers(sub_w, threshold_outlier);
797 
798  currentNumber += kltpoly->getCurrentNumberPoints();
799  }
800  // else{
801  // reInitialisation = true;
802  // break;
803  // }
804  }
805  }
806 
807  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
808  ++it) {
809  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
810 
811  if (kltPolyCylinder->isTracked()) {
812  initialNumber += kltPolyCylinder->getInitialNumberPoint();
813  if (kltPolyCylinder->hasEnoughPoints()) {
814  vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
815  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
816  kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
817 
818  currentNumber += kltPolyCylinder->getCurrentNumberPoints();
819  }
820  }
821  }
822 
823  // if(!reInitialisation){
824  double value = percentGood * (double)initialNumber;
825  if ((double)currentNumber < value) {
826  // std::cout << "Too many point disappear : " << initialNumber << "/"
827  // << currentNumber << std::endl;
828  reInitialisation = true;
829  } else {
830  if (!useOgre)
831  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
832  else {
833 #ifdef VISP_HAVE_OGRE
835 #else
836  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
837 #endif
838  }
839  }
840  // }
841 
842  if (reInitialisation)
843  return true;
844 
845  return false;
846 }
847 
852 {
853  vpMatrix L_true; // interaction matrix without weighting
854  vpMatrix LVJ_true;
855  vpColVector v; // "speed" for VVS
856 
857  vpMatrix LTL;
858  vpColVector LTR;
859  vpHomogeneousMatrix cMoPrev;
860  vpHomogeneousMatrix ctTc0_Prev;
861  vpColVector error_prev;
862  double mu = m_initialMu;
863 
864  double normRes = 0;
865  double normRes_1 = -1;
866  unsigned int iter = 0;
867 
868  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
869  if (isoJoIdentity)
870  oJo.eye();
871 
873 
874  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
876 
877  bool reStartFromLastIncrement = false;
878  computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
879  if (reStartFromLastIncrement) {
880  ctTc0 = ctTc0_Prev;
881  }
882 
883  if (!reStartFromLastIncrement) {
885 
886  if (computeCovariance) {
887  L_true = m_L_klt;
888  if (!isoJoIdentity) {
890  cVo.buildFrom(m_cMo);
891  LVJ_true = (m_L_klt * cVo * oJo);
892  }
893  }
894 
895  normRes_1 = normRes;
896  normRes = 0.0;
897 
898  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
900  normRes += m_weightedError_klt[i];
901  }
902 
903  if ((iter == 0) || m_computeInteraction) {
904  for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
905  for (unsigned int j = 0; j < 6; j++) {
906  m_L_klt[i][j] *= m_w_klt[i];
907  }
908  }
909  }
910 
911  computeVVSPoseEstimation(m_isoJoIdentity, iter, m_L_klt, LTL, m_weightedError_klt, m_error_klt, error_prev, LTR, mu, v);
912 
913  cMoPrev = m_cMo;
914  ctTc0_Prev = ctTc0;
916  m_cMo = ctTc0 * c0Mo;
917  } // endif(!reStartFromLastIncrement)
918 
919  iter++;
920  }
921 
922  computeCovarianceMatrixVVS(m_isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
923 }
924 
926 {
927  unsigned int nbFeatures = 2 * m_nbInfos;
928 
929  m_L_klt.resize(nbFeatures, 6, false, false);
930  m_error_klt.resize(nbFeatures, false);
931 
932  m_weightedError_klt.resize(nbFeatures, false);
933  m_w_klt.resize(nbFeatures, false);
934  m_w_klt = 1;
935 
937 }
938 
940 {
941  unsigned int shift = 0;
942  vpHomography H;
943 
944  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
945  vpMbtDistanceKltPoints *kltpoly = *it;
946  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
947  kltpoly->hasEnoughPoints()) {
948  vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
949  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
950 
951  try {
952  kltpoly->computeHomography(ctTc0, H);
953  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
954  } catch (...) {
955  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
956  }
957 
958  shift += 2 * kltpoly->getCurrentNumberPoints();
959  }
960  }
961 
962  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
963  ++it) {
964  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
965 
966  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
967  vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
968  vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
969 
970  try {
971  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
972  } catch (...) {
973  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
974  }
975 
976  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
977  }
978  }
979 }
980 
989 {
990  preTracking(I);
991 
992  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
993  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
994  }
995 
996  computeVVS();
997 
998  if (postTracking(I, m_w_klt))
999  reinit(I);
1000 
1001  if (displayFeatures) {
1003  }
1004 }
1005 
1014 {
1015  vpImageConvert::convert(I_color, m_I);
1016  preTracking(m_I);
1017 
1018  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
1019  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
1020  }
1021 
1022  computeVVS();
1023 
1024  if (postTracking(m_I, m_w_klt))
1025  reinit(m_I);
1026 }
1027 
1071 void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
1072 {
1073  // Load projection error config
1074  vpMbTracker::loadConfigFile(configFile, verbose);
1075 
1077  xmlp.setVerbose(verbose);
1078  xmlp.setKltMaxFeatures(10000);
1079  xmlp.setKltWindowSize(5);
1080  xmlp.setKltQuality(0.01);
1081  xmlp.setKltMinDistance(5);
1082  xmlp.setKltHarrisParam(0.01);
1083  xmlp.setKltBlockSize(3);
1084  xmlp.setKltPyramidLevels(3);
1088 
1089  try {
1090  if (verbose) {
1091  std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1092  }
1093  xmlp.parse(configFile.c_str());
1094  } catch (...) {
1095  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1096  throw vpException(vpException::ioError, "problem to parse configuration file.");
1097  }
1098 
1099  vpCameraParameters camera;
1100  xmlp.getCameraParameters(camera);
1101  setCameraParameters(camera);
1102 
1104  tracker.setWindowSize((int)xmlp.getKltWindowSize());
1108  tracker.setBlockSize((int)xmlp.getKltBlockSize());
1110  maskBorder = xmlp.getKltMaskBorder();
1113 
1114  // if(useScanLine)
1115  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1116 
1117  if (xmlp.hasNearClippingDistance())
1119 
1120  if (xmlp.hasFarClippingDistance())
1122 
1123  if (xmlp.getFovClipping())
1125 
1126  useLodGeneral = xmlp.getLodState();
1129 
1130  applyLodSettingInConfig = false;
1131  if (this->getNbPolygon() > 0) {
1132  applyLodSettingInConfig = true;
1136  }
1137 }
1138 
1151  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1152  bool displayFullModel)
1153 {
1154  std::vector<std::vector<double> > models =
1155  vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1156 
1157  for (size_t i = 0; i < models.size(); i++) {
1158  if (vpMath::equal(models[i][0], 0)) {
1159  vpImagePoint ip1(models[i][1], models[i][2]);
1160  vpImagePoint ip2(models[i][3], models[i][4]);
1161  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
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)) {
1176 
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 
1204  const vpColor &col, unsigned int thickness, bool displayFullModel)
1205 {
1206  std::vector<std::vector<double> > models =
1207  vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1208 
1209  for (size_t i = 0; i < models.size(); i++) {
1210  if (vpMath::equal(models[i][0], 0)) {
1211  vpImagePoint ip1(models[i][1], models[i][2]);
1212  vpImagePoint ip2(models[i][3], models[i][4]);
1213  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1214  } else if (vpMath::equal(models[i][0], 1)) {
1215  vpImagePoint center(models[i][1], models[i][2]);
1216  double n20 = models[i][3];
1217  double n11 = models[i][4];
1218  double n02 = models[i][5];
1219  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1220  }
1221  }
1222 
1223  if (displayFeatures) {
1224  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1225  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1228 
1230  double id = m_featuresToBeDisplayedKlt[i][5];
1231  std::stringstream ss;
1232  ss << id;
1233  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1234  }
1235  }
1236  }
1237 
1238 #ifdef VISP_HAVE_OGRE
1239  if (useOgre)
1240  faces.displayOgre(cMo);
1241 #endif
1242 }
1243 
1244 std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1245 {
1246  std::vector<std::vector<double> > features;
1247 
1248  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1249  vpMbtDistanceKltPoints *kltpoly = *it;
1250 
1251  if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1252  std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1253  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1254  }
1255  }
1256 
1257  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1258  ++it) {
1259  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1260 
1261  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1262  std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1263  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1264  }
1265  }
1266 
1267  return features;
1268 }
1269 
1285 std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1286  const vpHomogeneousMatrix &cMo,
1287  const vpCameraParameters &cam,
1288  bool displayFullModel)
1289 {
1290  std::vector<std::vector<double> > models;
1291 
1292  vpCameraParameters c = cam;
1293 
1294  if (clippingFlag > 3) // Contains at least one FOV constraint
1295  c.computeFov(width, height);
1296 
1297  // vpMbtDistanceKltPoints *kltpoly;
1298  // vpMbtDistanceKltCylinder *kltPolyCylinder;
1299 
1300  // Previous version 12/08/2015
1301  // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1302  // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1303  // kltpoly = *it;
1304  // kltpoly->polygon->changeFrame(cMo_);
1305  // kltpoly->polygon->computePolygonClipped(c);
1306  // }
1307  faces.computeClippedPolygons(cMo, c);
1308 
1309  if (useScanLine && !displayFullModel)
1310  faces.computeScanLineRender(m_cam, width, height);
1311 
1312  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1313  vpMbtDistanceKltPoints *kltpoly = *it;
1314  std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(cam, displayFullModel);
1315  models.insert(models.end(), modelLines.begin(), modelLines.end());
1316  }
1317 
1318  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1319  ++it) {
1320  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1321  std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo, cam);
1322  models.insert(models.end(), modelLines.begin(), modelLines.end());
1323  }
1324 
1325  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1326  vpMbtDistanceCircle *displayCircle = *it;
1327  std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo, cam, displayFullModel);
1328  if (!paramsCircle.empty()) {
1329  models.push_back(paramsCircle);
1330  }
1331  }
1332 
1333  return models;
1334 }
1335 
1343 {
1344  unsigned int nbTotalPoints = 0;
1345  // for (unsigned int i = 0; i < faces.size(); i += 1){
1346  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1347  vpMbtDistanceKltPoints *kltpoly = *it;
1348  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1349  kltpoly->hasEnoughPoints()) {
1350  nbTotalPoints += kltpoly->getCurrentNumberPoints();
1351  }
1352  }
1353 
1354  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1355  ++it) {
1356  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1357  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1358  nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1359  }
1360 
1361  if (nbTotalPoints < 10) {
1362  std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1364  "test tracking failed (too few points to realize a good tracking).");
1365  }
1366 }
1367 
1378 void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1379  const std::string & /*name*/)
1380 {
1382  kltPoly->setCameraParameters(m_cam);
1383 
1384  kltPoly->buildFrom(p1, p2, radius);
1385 
1386  // Add the Cylinder BBox to the list of polygons
1387  kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1388  kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1389  kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1390  kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1391 
1392  kltPoly->hiddenface = &faces;
1393  kltPoly->useScanLine = useScanLine;
1394  kltCylinders.push_back(kltPoly);
1395 }
1396 
1408 void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int /*idFace*/,
1409  const std::string &name)
1410 {
1411  addCircle(p1, p2, p3, radius, name);
1412 }
1413 
1422 void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
1423  const std::string &name)
1424 {
1425  bool already_here = false;
1426 
1427  // for(std::list<vpMbtDistanceCircle*>::const_iterator
1428  // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1429  // ci = *it;
1430  // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1431  // samePoint(*(ci->p3),P3)) ||
1432  // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1433  // samePoint(*(ci->p3),P2)) ){
1434  // already_here = (std::fabs(ci->radius - r) <
1435  // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1436  // r));
1437  // }
1438  // }
1439 
1440  if (!already_here) {
1442 
1444  ci->setName(name);
1445  ci->buildFrom(P1, P2, P3, r);
1446  circles_disp.push_back(ci);
1447  }
1448 }
1449 
1462 void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1463  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1464 {
1465  m_cMo.eye();
1466 
1467 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1468  if (cur != NULL) {
1469  cvReleaseImage(&cur);
1470  cur = NULL;
1471  }
1472 #endif
1473 
1474  firstInitialisation = true;
1475 
1476  // delete the Klt Polygon features
1477  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1478  vpMbtDistanceKltPoints *kltpoly = *it;
1479  if (kltpoly != NULL) {
1480  delete kltpoly;
1481  }
1482  kltpoly = NULL;
1483  }
1484  kltPolygons.clear();
1485 
1486  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1487  ++it) {
1488  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1489  if (kltPolyCylinder != NULL) {
1490  delete kltPolyCylinder;
1491  }
1492  kltPolyCylinder = NULL;
1493  }
1494  kltCylinders.clear();
1495 
1496  // delete the structures used to display circles
1497  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1498  vpMbtDistanceCircle *ci = *it;
1499  if (ci != NULL) {
1500  delete ci;
1501  }
1502  ci = NULL;
1503  }
1504 
1505  faces.reset();
1506 
1507  loadModel(cad_name, verbose, T);
1508  initFromPose(I, cMo);
1509 }
1510 
1518 void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1519 {
1520  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1521  vpMbtDistanceKltPoints *kltpoly = *it;
1522  if (kltpoly->polygon->getName() == name) {
1523  kltpoly->setTracked(useKltTracking);
1524  }
1525  }
1526 }
1527 
1528 #elif !defined(VISP_BUILD_SHARED_LIBS)
1529 // Work around to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
1530 // symbols
1531 void dummy_vpMbKltTracker(){};
1532 #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:307
unsigned int getRows() const
Definition: vpArray2D.h:292
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:172
vpColVector & normalize()
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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 emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ fatalError
Fatal error.
Definition: vpException.h:96
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
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:175
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:89
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getCols() const
Definition: vpImage.h:180
unsigned int getHeight() const
Definition: vpImage.h:189
unsigned int getRows() const
Definition: vpImage.h:219
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
double getQuality() const
Definition: vpKltOpencv.h:133
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:115
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void track(const cv::Mat &I)
void setTrackerId(int tid)
Definition: vpKltOpencv.h:155
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:135
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
void setHarrisFreeParameter(double harris_k)
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:111
void setMaxFeatures(int maxCount)
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
double getMinDistance() const
Definition: vpKltOpencv.h:118
void setMinDistance(double minDistance)
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:102
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:130
void setUseHarris(int useHarrisDetector)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition: vpMath.h:116
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:367
static double deg(double rad)
Definition: vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:447
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.
virtual void track(const vpImage< unsigned char > &I)
vpHomogeneousMatrix ctTc0
std::list< vpMbtDistanceKltPoints * > kltPolygons
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setKltOpencv(const vpKltOpencv &t)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
cv::Mat cur
Temporary OpenCV image for fast conversion.
std::map< int, vpImagePoint > getKltImagePointsWithId() const
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
virtual void testTracking()
vpColVector m_weightedError_klt
Weighted error.
virtual ~vpMbKltTracker()
vpKltOpencv tracker
Points tracker.
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
double threshold_outlier
void preTracking(const vpImage< unsigned char > &I)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, const std::string &name="")
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="")
unsigned int m_nbInfos
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
vpRobust m_robust_klt
Robust.
virtual void computeVVSInteractionMatrixAndResidu()
virtual void reinit(const vpImage< unsigned char > &I)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
std::vector< vpImagePoint > getKltImagePoints() const
unsigned int maskBorder
Erosion of the mask.
unsigned int m_nbFaceUsed
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
vpColVector m_w_klt
Robust weights.
void setCameraParameters(const vpCameraParameters &cam)
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
vpMatrix m_L_klt
Interaction matrix.
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual void computeVVSInit()
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void init(const vpImage< unsigned char > &I)
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 computeVVSPoseEstimation(const bool isoJoIdentity, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
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
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
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
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
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)
std::vector< std::vector< double > > getFeaturesForDisplay()
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
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:67
std::string getName() const
Definition: vpMbtPolygon.h:108
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:59
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:364
double getD() const
Definition: vpPlane.h:111
vpColVector getNormal() const
Definition: vpPlane.cpp:248
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void changeFrame(const vpHomogeneousMatrix &cMo)
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Implementation of a rotation matrix and operations on such kind of matrices.
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:63
Error that can be emited by the vpTracker class and its derivates.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:393