Visual Servoing Platform  version 3.6.1 under development (2024-12-07)
vpMbEdgeKltTracker.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  * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
33  *
34 *****************************************************************************/
35 
36 //#define VP_DEBUG_MODE 1 // Activate debug level 1
37 
38 #include <visp3/core/vpDebug.h>
39 #include <visp3/core/vpTrackingException.h>
40 #include <visp3/core/vpVelocityTwistMatrix.h>
41 #include <visp3/mbt/vpMbEdgeKltTracker.h>
42 #include <visp3/mbt/vpMbtXmlGenericParser.h>
43 
44 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
45 
46 BEGIN_VISP_NAMESPACE
47 vpMbEdgeKltTracker::vpMbEdgeKltTracker()
48  : m_thresholdKLT(2.), m_thresholdMBT(2.), m_maxIterKlt(30), m_w_mbt(), m_w_klt(), m_error_hybrid(), m_w_hybrid()
49 {
50  computeCovariance = false;
51 
52 #ifdef VISP_HAVE_OGRE
53  faces.getOgreContext()->setWindowName("MBT Hybrid");
54 #endif
55 
56  m_lambda = 0.8;
57  m_maxIter = 200;
58 }
59 
64 vpMbEdgeKltTracker::~vpMbEdgeKltTracker() { }
65 
72 void vpMbEdgeKltTracker::init(const vpImage<unsigned char> &I)
73 {
74  vpMbKltTracker::init(I);
75 
76  initPyramid(I, Ipyramid);
77 
79 
80  unsigned int i = (unsigned int)scales.size();
81  do {
82  i--;
83  if (scales[i]) {
84  downScale(i);
85  initMovingEdge(*Ipyramid[i], m_cMo);
86  upScale(i);
87  }
88  } while (i != 0);
89 
90  cleanPyramid(Ipyramid);
91 }
92 
102 void vpMbEdgeKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
103 {
104  vpMbKltTracker::setPose(I, cdMo);
105 
106  resetMovingEdge();
107 
108  if (useScanLine) {
109  m_cam.computeFov(I.getWidth(), I.getHeight());
110  faces.computeClippedPolygons(m_cMo, m_cam);
111  faces.computeScanLineRender(m_cam, I.getWidth(), I.getHeight());
112  }
113 
114  initPyramid(I, Ipyramid);
115 
116  unsigned int i = (unsigned int)scales.size();
117  do {
118  i--;
119  if (scales[i]) {
120  downScale(i);
121  initMovingEdge(*Ipyramid[i], m_cMo);
122  upScale(i);
123  }
124  } while (i != 0);
125 
126  cleanPyramid(Ipyramid);
127 }
128 
138 void vpMbEdgeKltTracker::setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo)
139 {
140  vpImageConvert::convert(I_color, m_I);
141  vpMbKltTracker::setPose(m_I, cdMo);
142 
143  resetMovingEdge();
144 
145  if (useScanLine) {
146  m_cam.computeFov(m_I.getWidth(), m_I.getHeight());
147  faces.computeClippedPolygons(m_cMo, m_cam);
148  faces.computeScanLineRender(m_cam, m_I.getWidth(), m_I.getHeight());
149  }
150 
151  initPyramid(m_I, Ipyramid);
152 
153  unsigned int i = (unsigned int)scales.size();
154  do {
155  i--;
156  if (scales[i]) {
157  downScale(i);
158  initMovingEdge(*Ipyramid[i], m_cMo);
159  upScale(i);
160  }
161  } while (i != 0);
162 
163  cleanPyramid(Ipyramid);
164 }
165 
170 void vpMbEdgeKltTracker::resetTracker()
171 {
173  vpMbKltTracker::resetTracker();
174 }
175 
176 unsigned int vpMbEdgeKltTracker::initMbtTracking(unsigned int lvl)
177 {
178  if (lvl >= scales.size() || !scales[lvl]) {
179  throw vpException(vpException::dimensionError, "lvl not used.");
180  }
181 
182  unsigned int nbrow = 0;
183  for (std::list<vpMbtDistanceLine *>::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
184  vpMbtDistanceLine *l = *it;
185 
186  if (l->isTracked()) {
188  nbrow += l->nbFeatureTotal;
189  }
190  }
191 
192  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
193  ++it) {
194  vpMbtDistanceCylinder *cy = *it;
195 
196  if (cy->isTracked()) {
198  nbrow += cy->nbFeature;
199  }
200  }
201 
202  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
203  vpMbtDistanceCircle *ci = *it;
204 
205  if (ci->isTracked()) {
207  nbrow += ci->nbFeature;
208  }
209  }
210 
211  return nbrow;
212 }
213 
275 void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
276 {
277 #if defined(VISP_HAVE_PUGIXML)
278 // Load projection error config
279  vpMbTracker::loadConfigFile(configFile, verbose);
280 
282  xmlp.setVerbose(verbose);
283  xmlp.setCameraParameters(m_cam);
284  xmlp.setAngleAppear(vpMath::deg(angleAppears));
285  xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
286 
287  xmlp.setEdgeMe(me);
288 
289  xmlp.setKltMaxFeatures(10000);
290  xmlp.setKltWindowSize(5);
291  xmlp.setKltQuality(0.01);
292  xmlp.setKltMinDistance(5);
293  xmlp.setKltHarrisParam(0.01);
294  xmlp.setKltBlockSize(3);
295  xmlp.setKltPyramidLevels(3);
296  xmlp.setKltMaskBorder(maskBorder);
297 
298  try {
299  if (verbose) {
300  std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
301  }
302  xmlp.parse(configFile.c_str());
303  }
304  catch (...) {
305  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
306  throw vpException(vpException::ioError, "problem to parse configuration file.");
307  }
308 
309  vpCameraParameters camera;
310  xmlp.getCameraParameters(camera);
311  setCameraParameters(camera);
312 
313  angleAppears = vpMath::rad(xmlp.getAngleAppear());
314  angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
315 
316  if (xmlp.hasNearClippingDistance())
317  setNearClippingDistance(xmlp.getNearClippingDistance());
318 
319  if (xmlp.hasFarClippingDistance())
320  setFarClippingDistance(xmlp.getFarClippingDistance());
321 
322  if (xmlp.getFovClipping()) {
324  }
325 
326  useLodGeneral = xmlp.getLodState();
327  minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
328  minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();
329 
330  applyLodSettingInConfig = false;
331  if (this->getNbPolygon() > 0) {
332  applyLodSettingInConfig = true;
333  setLod(useLodGeneral);
334  setMinLineLengthThresh(minLineLengthThresholdGeneral);
335  setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
336  }
337 
338  vpMe meParser;
339  xmlp.getEdgeMe(meParser);
341 
342  tracker.setMaxFeatures((int)xmlp.getKltMaxFeatures());
343  tracker.setWindowSize((int)xmlp.getKltWindowSize());
344  tracker.setQuality(xmlp.getKltQuality());
345  tracker.setMinDistance(xmlp.getKltMinDistance());
346  tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
347  tracker.setBlockSize((int)xmlp.getKltBlockSize());
348  tracker.setPyramidLevels((int)xmlp.getKltPyramidLevels());
349  maskBorder = xmlp.getKltMaskBorder();
350 
351  // if(useScanLine)
352  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
353 #else
354  (void)configFile;
355  (void)verbose;
356  throw(vpException(vpException::ioError, "vpMbEdgeKltTracker::loadConfigFile() needs pugixml built-in 3rdparty"));
357 #endif
358 }
359 
363 bool vpMbEdgeKltTracker::postTracking(const vpImage<unsigned char> &I, vpColVector &w_mbt, vpColVector &w_klt,
364  unsigned int lvl)
365 {
366  postTrackingMbt(w_mbt, lvl);
367 
368  if (displayFeatures) {
369  if (lvl == 0) {
370  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
371  vpMbtDistanceLine *l = *it;
372  if (l->isVisible() && l->isTracked()) {
373  l->displayMovingEdges(I);
374  }
375  }
376 
377  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
378  ++it) {
379  vpMbtDistanceCylinder *cy = *it;
380  // A cylinder is always visible: #FIXME AY: Still valid?
381  if (cy->isTracked())
382  cy->displayMovingEdges(I);
383  }
384 
385  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
386  vpMbtDistanceCircle *ci = *it;
387  if (ci->isVisible() && ci->isTracked()) {
388  ci->displayMovingEdges(I);
389  }
390  }
391  }
392  }
393 
394  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
395 
396  if (useScanLine) {
397  m_cam.computeFov(I.getWidth(), I.getHeight());
398  faces.computeClippedPolygons(m_cMo, m_cam);
399  faces.computeScanLineRender(m_cam, I.getWidth(), I.getHeight());
400  }
401 
403 
406 
407  if (computeProjError)
409 
410  if (reInit)
411  return true;
412 
413  return false;
414 }
415 
419 bool vpMbEdgeKltTracker::postTracking(const vpImage<vpRGBa> &I_color, vpColVector &w_mbt, vpColVector &w_klt,
420  unsigned int lvl)
421 {
422  postTrackingMbt(w_mbt, lvl);
423 
424  if (displayFeatures) {
425  if (lvl == 0) {
426  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
427  vpMbtDistanceLine *l = *it;
428  if (l->isVisible() && l->isTracked()) {
429  l->displayMovingEdges(I_color);
430  }
431  }
432 
433  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
434  ++it) {
435  vpMbtDistanceCylinder *cy = *it;
436  // A cylinder is always visible: #FIXME AY: Still valid?
437  if (cy->isTracked())
438  cy->displayMovingEdges(m_I);
439  }
440 
441  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
442  vpMbtDistanceCircle *ci = *it;
443  if (ci->isVisible() && ci->isTracked()) {
444  ci->displayMovingEdges(m_I);
445  }
446  }
447  }
448  }
449 
450  bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
451 
452  if (useScanLine) {
453  m_cam.computeFov(m_I.getWidth(), m_I.getHeight());
454  faces.computeClippedPolygons(m_cMo, m_cam);
455  faces.computeScanLineRender(m_cam, m_I.getWidth(), m_I.getHeight());
456  }
457 
459 
462 
463  if (computeProjError)
465 
466  if (reInit)
467  return true;
468 
469  return false;
470 }
471 
482 void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, unsigned int lvl)
483 {
484  if (lvl >= scales.size() || !scales[lvl]) {
485  throw vpException(vpException::dimensionError, "_lvl not used.");
486  }
487 
488  unsigned int n = 0;
490  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
491  if ((*it)->isTracked()) {
492  l = *it;
493  unsigned int indexLine = 0;
494  double wmean = 0;
495 
496  for (size_t a = 0; a < l->meline.size(); a++) {
497  std::list<vpMeSite>::iterator itListLine;
498  if (l->nbFeature[a] > 0)
499  itListLine = l->meline[a]->getMeList().begin();
500 
501  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
502  wmean += w[n + indexLine];
503  vpMeSite p = *itListLine;
504  if (w[n + indexLine] < 0.5) {
506  *itListLine = p;
507  }
508 
509  ++itListLine;
510  indexLine++;
511  }
512  }
513 
514  n += l->nbFeatureTotal;
515 
516  if (l->nbFeatureTotal != 0)
517  wmean /= l->nbFeatureTotal;
518  else
519  wmean = 1;
520 
521  l->setMeanWeight(wmean);
522 
523  if (wmean < 0.8)
524  l->Reinit = true;
525  }
526  }
527 
528  // Same thing with cylinders as with lines
530  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
531  ++it) {
532  if ((*it)->isTracked()) {
533  cy = *it;
534  double wmean = 0;
535  std::list<vpMeSite>::iterator itListCyl1;
536  std::list<vpMeSite>::iterator itListCyl2;
537  if (cy->nbFeature > 0) {
538  itListCyl1 = cy->meline1->getMeList().begin();
539  itListCyl2 = cy->meline2->getMeList().begin();
540  }
541 
542  wmean = 0;
543  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
544  wmean += w[n + i];
545  vpMeSite p = *itListCyl1;
546  if (w[n + i] < 0.5) {
548 
549  *itListCyl1 = p;
550  }
551 
552  ++itListCyl1;
553  }
554 
555  if (cy->nbFeaturel1 != 0)
556  wmean /= cy->nbFeaturel1;
557  else
558  wmean = 1;
559 
560  cy->setMeanWeight1(wmean);
561 
562  if (wmean < 0.8) {
563  cy->Reinit = true;
564  }
565 
566  wmean = 0;
567  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
568  wmean += w[n + i];
569  vpMeSite p = *itListCyl2;
570  if (w[n + i] < 0.5) {
572 
573  *itListCyl2 = p;
574  }
575 
576  ++itListCyl2;
577  }
578 
579  if (cy->nbFeaturel2 != 0)
580  wmean /= cy->nbFeaturel2;
581  else
582  wmean = 1;
583 
584  cy->setMeanWeight2(wmean);
585 
586  if (wmean < 0.8) {
587  cy->Reinit = true;
588  }
589 
590  n += cy->nbFeature;
591  }
592  }
593 
594  // Same thing with circles as with lines
596  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
597  if ((*it)->isTracked()) {
598  ci = *it;
599  double wmean = 0;
600  std::list<vpMeSite>::iterator itListCir;
601 
602  if (ci->nbFeature > 0) {
603  itListCir = ci->meEllipse->getMeList().begin();
604  }
605 
606  wmean = 0;
607  for (unsigned int i = 0; i < ci->nbFeature; i++) {
608  wmean += w[n + i];
609  vpMeSite p = *itListCir;
610  if (w[n + i] < 0.5) {
612 
613  *itListCir = p;
614  }
615 
616  ++itListCir;
617  }
618 
619  if (ci->nbFeature != 0)
620  wmean /= ci->nbFeature;
621  else
622  wmean = 1;
623 
624  ci->setMeanWeight(wmean);
625 
626  if (wmean < 0.8) {
627  ci->Reinit = true;
628  }
629 
630  n += ci->nbFeature;
631  }
632  }
633 }
634 
645 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
646  unsigned int lvl, double *edge_residual, double *klt_residual)
647 {
648  vpColVector factor;
649  nbrow = trackFirstLoop(I, factor, lvl);
650 
651  if (nbrow < 4 && nbInfos < 4) {
652  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
653  }
654  else if (nbrow < 4)
655  nbrow = 0;
656 
657  unsigned int totalNbRows = nbrow + 2 * nbInfos;
658  double residu = 0;
659  double residu_1 = -1;
660  unsigned int iter = 0;
661 
662  vpMatrix L(totalNbRows, 6);
663  vpMatrix L_mbt, L_klt; // interaction matrix
664  vpColVector weighted_error(totalNbRows);
665  vpColVector R_mbt, R_klt; // residu
666  vpMatrix L_true;
667  vpMatrix LVJ_true;
668 
669  if (nbrow != 0) {
670  L_mbt.resize(nbrow, 6, false, false);
671  R_mbt.resize(nbrow, false);
672  }
673 
674  if (nbInfos != 0) {
675  L_klt.resize(2 * nbInfos, 6, false, false);
676  R_klt.resize(2 * nbInfos, false);
677  }
678 
679  vpColVector v; // "speed" for VVS
680  vpRobust robust_mbt, robust_klt;
681  vpHomography H;
682 
683  vpMatrix LTL;
684  vpColVector LTR;
685 
686  double factorMBT; // = 1.0;
687  double factorKLT; // = 1.0;
688 
689  // More efficient weight repartition for hybrid tracker should come soon...
690  // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
691  // factorKLT = 1.0 - factorMBT;
692  factorMBT = 0.35;
693  factorKLT = 0.65;
694 
695  if (nbrow < 4)
696  factorKLT = 1.;
697  if (nbInfos < 4)
698  factorMBT = 1.;
699 
700  if (edge_residual != nullptr)
701  *edge_residual = 0;
702  if (klt_residual != nullptr)
703  *klt_residual = 0;
704 
705  vpHomogeneousMatrix cMoPrev;
706  vpHomogeneousMatrix ctTc0_Prev;
707  vpColVector m_error_prev;
708  vpColVector m_w_prev;
709 
710  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
711  if (isoJoIdentity)
712  oJo.eye();
713 
714  // Init size
715  m_error_hybrid.resize(totalNbRows, false);
716  m_w_hybrid.resize(totalNbRows, false);
717 
718  if (nbrow != 0) {
719  m_w_mbt.resize(nbrow, false);
720  m_w_mbt = 1; // needed in vpRobust::psiTukey()
721  }
722 
723  if (nbInfos != 0) {
724  m_w_klt.resize(2 * nbInfos, false);
725  m_w_klt = 1; // needed in vpRobust::psiTukey()
726  }
727 
728  double mu = m_initialMu;
729 
730  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
731  if (nbrow >= 4)
732  trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
733 
734  if (nbInfos >= 4) {
735  unsigned int shift = 0;
736 
737  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
738  it != vpMbKltTracker::kltPolygons.end(); ++it) {
739  vpMbtDistanceKltPoints *kltpoly = *it;
740  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
741  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
742  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
743  kltpoly->computeHomography(ctTc0, H);
744  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
745  shift += 2 * kltpoly->getCurrentNumberPoints();
746  }
747  }
748 
749  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
750  ++it) {
751  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
752 
753  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
754  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
755  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
756  try {
757  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
758  }
759  catch (...) {
760  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
761  }
762 
763  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
764  }
765  }
766  }
767 
768  /* residuals */
769  if (nbrow > 3) {
770  m_error_hybrid.insert(0, R_mbt);
771  }
772 
773  if (nbInfos > 3) {
774  m_error_hybrid.insert(nbrow, R_klt);
775  }
776 
777  unsigned int cpt = 0;
778  while (cpt < (nbrow + 2 * nbInfos)) {
779  if (cpt < (unsigned)nbrow) {
780  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
781  }
782  else {
783  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
784  }
785  cpt++;
786  }
787 
788  bool reStartFromLastIncrement = false;
789  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
790  &m_w_prev);
791  if (reStartFromLastIncrement) {
792  ctTc0 = ctTc0_Prev;
793  }
794 
795  if (!reStartFromLastIncrement) {
796  /* robust */
797  if (nbrow > 3) {
798  if (edge_residual != nullptr) {
799  *edge_residual = 0;
800  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
801  *edge_residual += fabs(R_mbt[i]);
802  *edge_residual /= R_mbt.getRows();
803  }
804 
805  robust_mbt.setMinMedianAbsoluteDeviation(m_thresholdMBT / m_cam.get_px());
806  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
807 
808  L.insert(L_mbt, 0, 0);
809  }
810 
811  if (nbInfos > 3) {
812  if (klt_residual != nullptr) {
813  *klt_residual = 0;
814  for (unsigned int i = 0; i < R_klt.getRows(); i++)
815  *klt_residual += fabs(R_klt[i]);
816  *klt_residual /= R_klt.getRows();
817  }
818 
819  robust_klt.setMinMedianAbsoluteDeviation(m_thresholdKLT / m_cam.get_px());
820  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
821 
822  L.insert(L_klt, nbrow, 0);
823  }
824 
825  cpt = 0;
826  while (cpt < (nbrow + 2 * nbInfos)) {
827  if (cpt < (unsigned)nbrow) {
828  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
829  }
830  else {
831  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
832  }
833  cpt++;
834  }
835 
836  if (computeCovariance) {
837  L_true = L;
838  if (!isoJoIdentity) {
840  cVo.buildFrom(m_cMo);
841  LVJ_true = (L * cVo * oJo);
842  }
843  }
844 
845  residu_1 = residu;
846  residu = 0;
847  double num = 0;
848  double den = 0;
849 
850  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
851  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
852  den += m_w_hybrid[i];
853 
854  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
855  if (m_computeInteraction) {
856  for (unsigned int j = 0; j < 6; j += 1) {
857  L[i][j] *= m_w_hybrid[i];
858  }
859  }
860  }
861 
862  residu = sqrt(num / den);
863 
864  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
865  &m_w_hybrid, &m_w_prev);
866 
867  cMoPrev = m_cMo;
868  ctTc0_Prev = ctTc0;
869  ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
870  m_cMo = ctTc0 * c0Mo;
871  }
872 
873  iter++;
874  }
875 
876  computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybrid, cMoPrev, L_true, LVJ_true, m_error_hybrid);
877 }
878 
879 void vpMbEdgeKltTracker::computeVVSInit()
880 {
881  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
882 }
883 
884 void vpMbEdgeKltTracker::computeVVSInteractionMatrixAndResidu()
885 {
886  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
887  "computeVVSInteractionMatrixAndR"
888  "esidu() should not be called!");
889 }
890 
898 void vpMbEdgeKltTracker::track(const vpImage<unsigned char> &I)
899 {
900  try {
901  vpMbKltTracker::preTracking(I);
902  }
903  catch (...) {
904  }
905 
906  if (m_nbInfos >= 4) {
907  unsigned int old_maxIter = m_maxIter;
908  m_maxIter = m_maxIterKlt;
909  vpMbKltTracker::computeVVS();
910  m_maxIter = old_maxIter;
911  }
912  else {
913  m_nbInfos = 0;
914  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
915  }
916 
918 
919  unsigned int nbrow = 0;
920  computeVVS(I, m_nbInfos, nbrow);
921 
922  if (postTracking(I, m_w_mbt, m_w_klt)) {
923  vpMbKltTracker::reinit(I);
924 
925  // AY : Removed as edge tracked, if necessary, is reinitialized in
926  // postTracking()
927 
928  // initPyramid(I, Ipyramid);
929 
930  // unsigned int i = (unsigned int)scales.size();
931  // do {
932  // i--;
933  // if(scales[i]){
934  // downScale(i);
935  // initMovingEdge(*Ipyramid[i], cMo);
936  // upScale(i);
937  // }
938  // } while(i != 0);
939 
940  // cleanPyramid(Ipyramid);
941  }
942 
943  if (displayFeatures) {
944  m_featuresToBeDisplayedKlt = getFeaturesForDisplayKlt();
945  }
946 }
947 
955 void vpMbEdgeKltTracker::track(const vpImage<vpRGBa> &I_color)
956 {
957  vpImageConvert::convert(I_color, m_I);
958  try {
959  vpMbKltTracker::preTracking(m_I);
960  }
961  catch (...) {
962  }
963 
964  if (m_nbInfos >= 4) {
965  unsigned int old_maxIter = m_maxIter;
966  m_maxIter = m_maxIterKlt;
967  vpMbKltTracker::computeVVS();
968  m_maxIter = old_maxIter;
969  }
970  else {
971  m_nbInfos = 0;
972  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
973  }
974 
976 
977  unsigned int nbrow = 0;
978  computeVVS(m_I, m_nbInfos, nbrow);
979 
980  if (postTracking(I_color, m_w_mbt, m_w_klt)) {
981  vpMbKltTracker::reinit(m_I);
982 
983  // AY : Removed as edge tracked, if necessary, is reinitialized in
984  // postTracking()
985 
986  // initPyramid(I, Ipyramid);
987 
988  // unsigned int i = (unsigned int)scales.size();
989  // do {
990  // i--;
991  // if(scales[i]){
992  // downScale(i);
993  // initMovingEdge(*Ipyramid[i], cMo);
994  // upScale(i);
995  // }
996  // } while(i != 0);
997 
998  // cleanPyramid(Ipyramid);
999  }
1000 
1001  if (displayFeatures) {
1002  m_featuresToBeDisplayedKlt = getFeaturesForDisplayKlt();
1003  }
1004 }
1005 
1006 unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor, unsigned int lvl)
1007 {
1008  vpMbtDistanceLine *l;
1010  vpMbtDistanceCircle *ci;
1011 
1012  if (lvl >= scales.size() || !scales[lvl]) {
1013  throw vpException(vpException::dimensionError, "_lvl not used.");
1014  }
1015 
1016  unsigned int nbrow = initMbtTracking(lvl);
1017 
1018  if (nbrow == 0) {
1019  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
1020  // "Error: not enough features in the interaction matrix...");
1021  return nbrow;
1022  }
1023 
1024  factor.resize(nbrow, false);
1025  factor = 1;
1026 
1027  unsigned int n = 0;
1028  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1029  if ((*it)->isTracked()) {
1030  l = *it;
1032 
1033  double fac = 1;
1034  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1035  ++itindex) {
1036  int index = *itindex;
1037  if (l->hiddenface->isAppearing((unsigned int)index)) {
1038  fac = 0.2;
1039  break;
1040  }
1041  if (l->closeToImageBorder(I, 10)) {
1042  fac = 0.1;
1043  break;
1044  }
1045  }
1046 
1047  for (size_t a = 0; a < l->meline.size(); a++) {
1048  std::list<vpMeSite>::const_iterator itListLine;
1049  if (l->meline[a] != nullptr) {
1050  itListLine = l->meline[a]->getMeList().begin();
1051 
1052  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1053  factor[n + i] = fac;
1054  vpMeSite site = *itListLine;
1055  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1056  factor[n + i] = 0.2;
1057  ++itListLine;
1058  }
1059  n += l->nbFeature[a];
1060  }
1061  }
1062  }
1063  }
1064 
1065  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1066  ++it) {
1067  if ((*it)->isTracked()) {
1068  cy = *it;
1069  cy->computeInteractionMatrixError(m_cMo, I);
1070  double fac = 1.0;
1071 
1072  std::list<vpMeSite>::const_iterator itCyl1;
1073  std::list<vpMeSite>::const_iterator itCyl2;
1074  if ((cy->meline1 != nullptr || cy->meline2 != nullptr)) {
1075  itCyl1 = cy->meline1->getMeList().begin();
1076  itCyl2 = cy->meline2->getMeList().begin();
1077  }
1078 
1079  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1080  factor[n + i] = fac;
1081  vpMeSite site;
1082  if (i < cy->nbFeaturel1) {
1083  site = *itCyl1;
1084  ++itCyl1;
1085  }
1086  else {
1087  site = *itCyl2;
1088  ++itCyl2;
1089  }
1090  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1091  factor[n + i] = 0.2;
1092  }
1093 
1094  n += cy->nbFeature;
1095  }
1096  }
1097 
1098  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1099  if ((*it)->isTracked()) {
1100  ci = *it;
1101  ci->computeInteractionMatrixError(m_cMo);
1102  double fac = 1.0;
1103 
1104  std::list<vpMeSite>::const_iterator itCir;
1105  if (ci->meEllipse != nullptr) {
1106  itCir = ci->meEllipse->getMeList().begin();
1107  }
1108 
1109  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1110  factor[n + i] = fac;
1111  vpMeSite site = *itCir;
1112  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1113  factor[n + i] = 0.2;
1114  ++itCir;
1115  }
1116 
1117  n += ci->nbFeature;
1118  }
1119  }
1120 
1121  return nbrow;
1122 }
1123 
1124 void vpMbEdgeKltTracker::trackSecondLoop(const vpImage<unsigned char> &I, vpMatrix &L, vpColVector &error,
1125  const vpHomogeneousMatrix &cMo, unsigned int lvl)
1126 {
1127  vpMbtDistanceLine *l;
1129  vpMbtDistanceCircle *ci;
1130 
1131  unsigned int n = 0;
1132  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1133  if ((*it)->isTracked()) {
1134  l = *it;
1136  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1137  for (unsigned int j = 0; j < 6; j++) {
1138  L[n + i][j] = l->L[i][j];
1139  error[n + i] = l->error[i];
1140  }
1141  }
1142  n += l->nbFeatureTotal;
1143  }
1144  }
1145 
1146  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1147  ++it) {
1148  if ((*it)->isTracked()) {
1149  cy = *it;
1150  cy->computeInteractionMatrixError(cMo, I);
1151  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1152  for (unsigned int j = 0; j < 6; j++) {
1153  L[n + i][j] = cy->L[i][j];
1154  error[n + i] = cy->error[i];
1155  }
1156  }
1157  n += cy->nbFeature;
1158  }
1159  }
1160  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1161  if ((*it)->isTracked()) {
1162  ci = *it;
1164  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1165  for (unsigned int j = 0; j < 6; j++) {
1166  L[n + i][j] = ci->L[i][j];
1167  error[n + i] = ci->error[i];
1168  }
1169  }
1170 
1171  n += ci->nbFeature;
1172  }
1173  }
1174 }
1175 
1181 void vpMbEdgeKltTracker::setCameraParameters(const vpCameraParameters &cam)
1182 {
1183  m_cam = cam;
1184 
1186  vpMbKltTracker::setCameraParameters(m_cam);
1187 }
1188 
1195 void vpMbEdgeKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
1196 {
1198  vpMbKltTracker::initFaceFromCorners(polygon);
1199 }
1206 void vpMbEdgeKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
1207 {
1209  vpMbKltTracker::initFaceFromLines(polygon);
1210 }
1211 
1222 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1223  const std::string &name)
1224 {
1225  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1226 }
1227 
1238 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1239  const std::string &name)
1240 {
1241  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1242  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1243 }
1244 
1256 void vpMbEdgeKltTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
1257  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1258  bool displayFullModel)
1259 {
1260  std::vector<std::vector<double> > models =
1261  vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1262 
1263  for (size_t i = 0; i < models.size(); i++) {
1264  if (vpMath::equal(models[i][0], 0)) {
1265  vpImagePoint ip1(models[i][1], models[i][2]);
1266  vpImagePoint ip2(models[i][3], models[i][4]);
1267  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1268  }
1269  else if (vpMath::equal(models[i][0], 1)) {
1270  vpImagePoint center(models[i][1], models[i][2]);
1271  double n20 = models[i][3];
1272  double n11 = models[i][4];
1273  double n02 = models[i][5];
1274  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1275  }
1276  }
1277 
1278  if (displayFeatures) {
1279  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1280  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1281  vpImagePoint ip1(m_featuresToBeDisplayedKlt[i][1], m_featuresToBeDisplayedKlt[i][2]);
1283 
1284  vpImagePoint ip2(m_featuresToBeDisplayedKlt[i][3], m_featuresToBeDisplayedKlt[i][4]);
1285  double id = m_featuresToBeDisplayedKlt[i][5];
1286  std::stringstream ss;
1287  ss << id;
1288  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1289  }
1290  }
1291  }
1292 
1293 #ifdef VISP_HAVE_OGRE
1294  if (useOgre)
1295  faces.displayOgre(cMo);
1296 #endif
1297 }
1298 
1310 void vpMbEdgeKltTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
1311  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1312  bool displayFullModel)
1313 {
1314  std::vector<std::vector<double> > models =
1315  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1316 
1317  for (size_t i = 0; i < models.size(); i++) {
1318  if (vpMath::equal(models[i][0], 0)) {
1319  vpImagePoint ip1(models[i][1], models[i][2]);
1320  vpImagePoint ip2(models[i][3], models[i][4]);
1321  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1322  }
1323  else if (vpMath::equal(models[i][0], 1)) {
1324  vpImagePoint center(models[i][1], models[i][2]);
1325  double n20 = models[i][3];
1326  double n11 = models[i][4];
1327  double n02 = models[i][5];
1328  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1329  }
1330  }
1331 
1332  if (displayFeatures) {
1333  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1334  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1335  vpImagePoint ip1(m_featuresToBeDisplayedKlt[i][1], m_featuresToBeDisplayedKlt[i][2]);
1337 
1338  vpImagePoint ip2(m_featuresToBeDisplayedKlt[i][3], m_featuresToBeDisplayedKlt[i][4]);
1339  double id = m_featuresToBeDisplayedKlt[i][5];
1340  std::stringstream ss;
1341  ss << id;
1342  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1343  }
1344  }
1345  }
1346 
1347 #ifdef VISP_HAVE_OGRE
1348  if (useOgre)
1349  faces.displayOgre(cMo);
1350 #endif
1351 }
1352 
1353 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1354  const vpHomogeneousMatrix &cMo,
1355  const vpCameraParameters &cam,
1356  bool displayFullModel)
1357 {
1358  std::vector<std::vector<double> > models;
1359 
1360  for (unsigned int i = 0; i < scales.size(); i += 1) {
1361  if (scales[i]) {
1362  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1363  ++it) {
1364  std::vector<std::vector<double> > currentModel =
1365  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1366  models.insert(models.end(), currentModel.begin(), currentModel.end());
1367  }
1368 
1369  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1370  it != cylinders[scaleLevel].end(); ++it) {
1371  std::vector<std::vector<double> > currentModel =
1372  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1373  models.insert(models.end(), currentModel.begin(), currentModel.end());
1374  }
1375 
1376  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1377  it != circles[scaleLevel].end(); ++it) {
1378  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1379  models.push_back(paramsCircle);
1380  }
1381 
1382  break; // displaying model on one scale only
1383  }
1384  }
1385 
1386 #ifdef VISP_HAVE_OGRE
1387  if (useOgre)
1388  faces.displayOgre(cMo);
1389 #endif
1390 
1391  return models;
1392 }
1393 
1406 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1407  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1408 {
1409  // Reinit klt
1410 
1411  // delete the Klt Polygon features
1412  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1413  vpMbtDistanceKltPoints *kltpoly = *it;
1414  if (kltpoly != nullptr) {
1415  delete kltpoly;
1416  }
1417  kltpoly = nullptr;
1418  }
1419  kltPolygons.clear();
1420 
1421  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1422  ++it) {
1423  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1424  if (kltPolyCylinder != nullptr) {
1425  delete kltPolyCylinder;
1426  }
1427  kltPolyCylinder = nullptr;
1428  }
1429  kltCylinders.clear();
1430 
1431  // delete the structures used to display circles
1432  vpMbtDistanceCircle *ci;
1433  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1434  ci = *it;
1435  if (ci != nullptr) {
1436  delete ci;
1437  }
1438  ci = nullptr;
1439  }
1440 
1441  circles_disp.clear();
1442 
1443  firstInitialisation = true;
1444 
1445  // Reinit edge
1446  vpMbtDistanceLine *l;
1448 
1449  for (unsigned int i = 0; i < scales.size(); i += 1) {
1450  if (scales[i]) {
1451  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1452  l = *it;
1453  if (l != nullptr)
1454  delete l;
1455  l = nullptr;
1456  }
1457 
1458  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1459  ++it) {
1460  cy = *it;
1461  if (cy != nullptr)
1462  delete cy;
1463  cy = nullptr;
1464  }
1465 
1466  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1467  ci = *it;
1468  if (ci != nullptr)
1469  delete ci;
1470  ci = nullptr;
1471  }
1472 
1473  lines[i].clear();
1474  cylinders[i].clear();
1475  circles[i].clear();
1476  }
1477  }
1478 
1479  // compute_interaction=1;
1480  nline = 0;
1481  ncylinder = 0;
1482  ncircle = 0;
1483  // lambda = 1;
1484  nbvisiblepolygone = 0;
1485 
1486  // Reinit common parts
1487  faces.reset();
1488 
1489  loadModel(cad_name, verbose, T);
1490 
1491  m_cMo = cMo;
1492  init(I);
1493 }
1494 END_VISP_NAMESPACE
1495 #elif !defined(VISP_BUILD_SHARED_LIBS)
1496 // Work around to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1497 // no symbols
1498 void dummy_vpMbEdgeKltTracker() { };
1499 #endif // VISP_HAVE_OPENCV
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
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
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() 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 getHeight() const
Definition: vpImage.h:181
void init(unsigned int h, unsigned int w, Type value)
Definition: vpImage.h:378
static double rad(double deg)
Definition: vpMath.h:129
static double sqr(double x)
Definition: vpMath.h:203
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
void computeProjectionError(const vpImage< unsigned char > &_I)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
virtual void initFaceFromLines(vpMbtPolygon &polygon) VP_OVERRIDE
void trackMovingEdge(const vpImage< unsigned char > &I)
virtual void initFaceFromCorners(vpMbtPolygon &polygon) VP_OVERRIDE
void updateMovingEdge(const vpImage< unsigned char > &I)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void resetTracker() VP_OVERRIDE
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="") VP_OVERRIDE
void setMovingEdge(const vpMe &me)
virtual void setCameraParameters(const vpCameraParameters &cam) VP_OVERRIDE
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="") VP_OVERRIDE
bool isAppearing(unsigned int i)
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:155
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
vpColVector error
The error vector.
unsigned int nbFeature
The number of moving edges.
vpMatrix L
The interaction matrix.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void setMeanWeight(double _wmean)
void displayMovingEdges(const vpImage< unsigned char > &I)
bool Reinit
Indicates if the circle has to be reinitialized.
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void setMeanWeight1(double wmean)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(double wmean)
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int nbFeature
The number of moving edges.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
std::vector< unsigned int > nbFeature
The number of moving edges.
void displayMovingEdges(const vpImage< unsigned char > &I)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isVisible() const
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
bool isTracked() const
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setMeanWeight(double w_mean)
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.
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:68
@ M_ESTIMATOR
Point detected as an outlier during virtual visual-servoing.
Definition: vpMeSite.h:92
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:86
vpMeSiteState getState() const
Definition: vpMeSite.h:283
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:273
Definition: vpMe.h:134
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
Contains an M-estimator and various influence function.
Definition: vpRobust.h:84
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:89
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:130
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:136
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.
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)