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