Visual Servoing Platform  version 3.6.1 under development (2023-12-06)
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  // Load projection error config
277  vpMbTracker::loadConfigFile(configFile, verbose);
278 
280  xmlp.setVerbose(verbose);
284 
285  xmlp.setEdgeMe(me);
286 
287  xmlp.setKltMaxFeatures(10000);
288  xmlp.setKltWindowSize(5);
289  xmlp.setKltQuality(0.01);
290  xmlp.setKltMinDistance(5);
291  xmlp.setKltHarrisParam(0.01);
292  xmlp.setKltBlockSize(3);
293  xmlp.setKltPyramidLevels(3);
295 
296  try {
297  if (verbose) {
298  std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
299  }
300  xmlp.parse(configFile.c_str());
301  }
302  catch (...) {
303  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
304  throw vpException(vpException::ioError, "problem to parse configuration file.");
305  }
306 
307  vpCameraParameters camera;
308  xmlp.getCameraParameters(camera);
309  setCameraParameters(camera);
310 
313 
314  if (xmlp.hasNearClippingDistance())
316 
317  if (xmlp.hasFarClippingDistance())
319 
320  if (xmlp.getFovClipping()) {
322  }
323 
324  useLodGeneral = xmlp.getLodState();
327 
328  applyLodSettingInConfig = false;
329  if (this->getNbPolygon() > 0) {
334  }
335 
336  vpMe meParser;
337  xmlp.getEdgeMe(meParser);
339 
345  tracker.setBlockSize((int)xmlp.getKltBlockSize());
347  maskBorder = xmlp.getKltMaskBorder();
348 
349  // if(useScanLine)
350  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
351 }
352 
357  unsigned int lvl)
358 {
359  postTrackingMbt(w_mbt, lvl);
360 
361  if (displayFeatures) {
362  if (lvl == 0) {
363  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
364  vpMbtDistanceLine *l = *it;
365  if (l->isVisible() && l->isTracked()) {
366  l->displayMovingEdges(I);
367  }
368  }
369 
370  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
371  ++it) {
372  vpMbtDistanceCylinder *cy = *it;
373  // A cylinder is always visible: #FIXME AY: Still valid?
374  if (cy->isTracked())
375  cy->displayMovingEdges(I);
376  }
377 
378  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
379  vpMbtDistanceCircle *ci = *it;
380  if (ci->isVisible() && ci->isTracked()) {
381  ci->displayMovingEdges(I);
382  }
383  }
384  }
385  }
386 
387  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
388 
389  if (useScanLine) {
393  }
394 
396 
399 
400  if (computeProjError)
402 
403  if (reInit)
404  return true;
405 
406  return false;
407 }
408 
413  unsigned int lvl)
414 {
415  postTrackingMbt(w_mbt, lvl);
416 
417  if (displayFeatures) {
418  if (lvl == 0) {
419  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
420  vpMbtDistanceLine *l = *it;
421  if (l->isVisible() && l->isTracked()) {
422  l->displayMovingEdges(I_color);
423  }
424  }
425 
426  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
427  ++it) {
428  vpMbtDistanceCylinder *cy = *it;
429  // A cylinder is always visible: #FIXME AY: Still valid?
430  if (cy->isTracked())
431  cy->displayMovingEdges(m_I);
432  }
433 
434  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
435  vpMbtDistanceCircle *ci = *it;
436  if (ci->isVisible() && ci->isTracked()) {
437  ci->displayMovingEdges(m_I);
438  }
439  }
440  }
441  }
442 
443  bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
444 
445  if (useScanLine) {
449  }
450 
452 
455 
456  if (computeProjError)
458 
459  if (reInit)
460  return true;
461 
462  return false;
463 }
464 
476 {
477  if (lvl >= scales.size() || !scales[lvl]) {
478  throw vpException(vpException::dimensionError, "_lvl not used.");
479  }
480 
481  unsigned int n = 0;
483  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
484  if ((*it)->isTracked()) {
485  l = *it;
486  unsigned int indexLine = 0;
487  double wmean = 0;
488 
489  for (size_t a = 0; a < l->meline.size(); a++) {
490  std::list<vpMeSite>::iterator itListLine;
491  if (l->nbFeature[a] > 0)
492  itListLine = l->meline[a]->getMeList().begin();
493 
494  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
495  wmean += w[n + indexLine];
496  vpMeSite p = *itListLine;
497  if (w[n + indexLine] < 0.5) {
499  *itListLine = p;
500  }
501 
502  ++itListLine;
503  indexLine++;
504  }
505  }
506 
507  n += l->nbFeatureTotal;
508 
509  if (l->nbFeatureTotal != 0)
510  wmean /= l->nbFeatureTotal;
511  else
512  wmean = 1;
513 
514  l->setMeanWeight(wmean);
515 
516  if (wmean < 0.8)
517  l->Reinit = true;
518  }
519  }
520 
521  // Same thing with cylinders as with lines
523  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
524  ++it) {
525  if ((*it)->isTracked()) {
526  cy = *it;
527  double wmean = 0;
528  std::list<vpMeSite>::iterator itListCyl1;
529  std::list<vpMeSite>::iterator itListCyl2;
530  if (cy->nbFeature > 0) {
531  itListCyl1 = cy->meline1->getMeList().begin();
532  itListCyl2 = cy->meline2->getMeList().begin();
533  }
534 
535  wmean = 0;
536  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
537  wmean += w[n + i];
538  vpMeSite p = *itListCyl1;
539  if (w[n + i] < 0.5) {
541 
542  *itListCyl1 = p;
543  }
544 
545  ++itListCyl1;
546  }
547 
548  if (cy->nbFeaturel1 != 0)
549  wmean /= cy->nbFeaturel1;
550  else
551  wmean = 1;
552 
553  cy->setMeanWeight1(wmean);
554 
555  if (wmean < 0.8) {
556  cy->Reinit = true;
557  }
558 
559  wmean = 0;
560  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
561  wmean += w[n + i];
562  vpMeSite p = *itListCyl2;
563  if (w[n + i] < 0.5) {
565 
566  *itListCyl2 = p;
567  }
568 
569  ++itListCyl2;
570  }
571 
572  if (cy->nbFeaturel2 != 0)
573  wmean /= cy->nbFeaturel2;
574  else
575  wmean = 1;
576 
577  cy->setMeanWeight2(wmean);
578 
579  if (wmean < 0.8) {
580  cy->Reinit = true;
581  }
582 
583  n += cy->nbFeature;
584  }
585  }
586 
587  // Same thing with circles as with lines
589  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
590  if ((*it)->isTracked()) {
591  ci = *it;
592  double wmean = 0;
593  std::list<vpMeSite>::iterator itListCir;
594 
595  if (ci->nbFeature > 0) {
596  itListCir = ci->meEllipse->getMeList().begin();
597  }
598 
599  wmean = 0;
600  for (unsigned int i = 0; i < ci->nbFeature; i++) {
601  wmean += w[n + i];
602  vpMeSite p = *itListCir;
603  if (w[n + i] < 0.5) {
605 
606  *itListCir = p;
607  }
608 
609  ++itListCir;
610  }
611 
612  if (ci->nbFeature != 0)
613  wmean /= ci->nbFeature;
614  else
615  wmean = 1;
616 
617  ci->setMeanWeight(wmean);
618 
619  if (wmean < 0.8) {
620  ci->Reinit = true;
621  }
622 
623  n += ci->nbFeature;
624  }
625  }
626 }
627 
638 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
639  unsigned int lvl, double *edge_residual, double *klt_residual)
640 {
641  vpColVector factor;
642  nbrow = trackFirstLoop(I, factor, lvl);
643 
644  if (nbrow < 4 && nbInfos < 4) {
645  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
646  }
647  else if (nbrow < 4)
648  nbrow = 0;
649 
650  unsigned int totalNbRows = nbrow + 2 * nbInfos;
651  double residu = 0;
652  double residu_1 = -1;
653  unsigned int iter = 0;
654 
655  vpMatrix L(totalNbRows, 6);
656  vpMatrix L_mbt, L_klt; // interaction matrix
657  vpColVector weighted_error(totalNbRows);
658  vpColVector R_mbt, R_klt; // residu
659  vpMatrix L_true;
660  vpMatrix LVJ_true;
661 
662  if (nbrow != 0) {
663  L_mbt.resize(nbrow, 6, false, false);
664  R_mbt.resize(nbrow, false);
665  }
666 
667  if (nbInfos != 0) {
668  L_klt.resize(2 * nbInfos, 6, false, false);
669  R_klt.resize(2 * nbInfos, false);
670  }
671 
672  vpColVector v; // "speed" for VVS
673  vpRobust robust_mbt, robust_klt;
674  vpHomography H;
675 
676  vpMatrix LTL;
677  vpColVector LTR;
678 
679  double factorMBT; // = 1.0;
680  double factorKLT; // = 1.0;
681 
682  // More efficient weight repartition for hybrid tracker should come soon...
683  // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
684  // factorKLT = 1.0 - factorMBT;
685  factorMBT = 0.35;
686  factorKLT = 0.65;
687 
688  if (nbrow < 4)
689  factorKLT = 1.;
690  if (nbInfos < 4)
691  factorMBT = 1.;
692 
693  if (edge_residual != nullptr)
694  *edge_residual = 0;
695  if (klt_residual != nullptr)
696  *klt_residual = 0;
697 
698  vpHomogeneousMatrix cMoPrev;
699  vpHomogeneousMatrix ctTc0_Prev;
700  vpColVector m_error_prev;
701  vpColVector m_w_prev;
702 
703  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
704  if (isoJoIdentity)
705  oJo.eye();
706 
707  // Init size
708  m_error_hybrid.resize(totalNbRows, false);
709  m_w_hybrid.resize(totalNbRows, false);
710 
711  if (nbrow != 0) {
712  m_w_mbt.resize(nbrow, false);
713  m_w_mbt = 1; // needed in vpRobust::psiTukey()
714  }
715 
716  if (nbInfos != 0) {
717  m_w_klt.resize(2 * nbInfos, false);
718  m_w_klt = 1; // needed in vpRobust::psiTukey()
719  }
720 
721  double mu = m_initialMu;
722 
723  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
724  if (nbrow >= 4)
725  trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
726 
727  if (nbInfos >= 4) {
728  unsigned int shift = 0;
729 
730  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
731  it != vpMbKltTracker::kltPolygons.end(); ++it) {
732  vpMbtDistanceKltPoints *kltpoly = *it;
733  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
734  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
735  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
736  kltpoly->computeHomography(ctTc0, H);
737  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
738  shift += 2 * kltpoly->getCurrentNumberPoints();
739  }
740  }
741 
742  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
743  ++it) {
744  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
745 
746  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
747  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
748  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
749  try {
750  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
751  }
752  catch (...) {
753  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
754  }
755 
756  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
757  }
758  }
759  }
760 
761  /* residuals */
762  if (nbrow > 3) {
763  m_error_hybrid.insert(0, R_mbt);
764  }
765 
766  if (nbInfos > 3) {
767  m_error_hybrid.insert(nbrow, R_klt);
768  }
769 
770  unsigned int cpt = 0;
771  while (cpt < (nbrow + 2 * nbInfos)) {
772  if (cpt < (unsigned)nbrow) {
773  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
774  }
775  else {
776  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
777  }
778  cpt++;
779  }
780 
781  bool reStartFromLastIncrement = false;
782  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
783  &m_w_prev);
784  if (reStartFromLastIncrement) {
785  ctTc0 = ctTc0_Prev;
786  }
787 
788  if (!reStartFromLastIncrement) {
789  /* robust */
790  if (nbrow > 3) {
791  if (edge_residual != nullptr) {
792  *edge_residual = 0;
793  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
794  *edge_residual += fabs(R_mbt[i]);
795  *edge_residual /= R_mbt.getRows();
796  }
797 
799  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
800 
801  L.insert(L_mbt, 0, 0);
802  }
803 
804  if (nbInfos > 3) {
805  if (klt_residual != nullptr) {
806  *klt_residual = 0;
807  for (unsigned int i = 0; i < R_klt.getRows(); i++)
808  *klt_residual += fabs(R_klt[i]);
809  *klt_residual /= R_klt.getRows();
810  }
811 
813  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
814 
815  L.insert(L_klt, nbrow, 0);
816  }
817 
818  cpt = 0;
819  while (cpt < (nbrow + 2 * nbInfos)) {
820  if (cpt < (unsigned)nbrow) {
821  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
822  }
823  else {
824  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
825  }
826  cpt++;
827  }
828 
829  if (computeCovariance) {
830  L_true = L;
831  if (!isoJoIdentity) {
833  cVo.buildFrom(m_cMo);
834  LVJ_true = (L * cVo * oJo);
835  }
836  }
837 
838  residu_1 = residu;
839  residu = 0;
840  double num = 0;
841  double den = 0;
842 
843  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
844  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
845  den += m_w_hybrid[i];
846 
847  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
848  if (m_computeInteraction) {
849  for (unsigned int j = 0; j < 6; j += 1) {
850  L[i][j] *= m_w_hybrid[i];
851  }
852  }
853  }
854 
855  residu = sqrt(num / den);
856 
857  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
858  &m_w_hybrid, &m_w_prev);
859 
860  cMoPrev = m_cMo;
861  ctTc0_Prev = ctTc0;
863  m_cMo = ctTc0 * c0Mo;
864  }
865 
866  iter++;
867  }
868 
869  computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybrid, cMoPrev, L_true, LVJ_true, m_error_hybrid);
870 }
871 
873 {
874  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
875 }
876 
878 {
879  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
880  "computeVVSInteractionMatrixAndR"
881  "esidu() should not be called!");
882 }
883 
892 {
893  try {
895  }
896  catch (...) {
897  }
898 
899  if (m_nbInfos >= 4) {
900  unsigned int old_maxIter = m_maxIter;
903  m_maxIter = old_maxIter;
904  }
905  else {
906  m_nbInfos = 0;
907  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
908  }
909 
911 
912  unsigned int nbrow = 0;
913  computeVVS(I, m_nbInfos, nbrow);
914 
915  if (postTracking(I, m_w_mbt, m_w_klt)) {
917 
918  // AY : Removed as edge tracked, if necessary, is reinitialized in
919  // postTracking()
920 
921  // initPyramid(I, Ipyramid);
922 
923  // unsigned int i = (unsigned int)scales.size();
924  // do {
925  // i--;
926  // if(scales[i]){
927  // downScale(i);
928  // initMovingEdge(*Ipyramid[i], cMo);
929  // upScale(i);
930  // }
931  // } while(i != 0);
932 
933  // cleanPyramid(Ipyramid);
934  }
935 
936  if (displayFeatures) {
938  }
939 }
940 
949 {
950  vpImageConvert::convert(I_color, m_I);
951  try {
953  }
954  catch (...) {
955  }
956 
957  if (m_nbInfos >= 4) {
958  unsigned int old_maxIter = m_maxIter;
961  m_maxIter = old_maxIter;
962  }
963  else {
964  m_nbInfos = 0;
965  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
966  }
967 
969 
970  unsigned int nbrow = 0;
971  computeVVS(m_I, m_nbInfos, nbrow);
972 
973  if (postTracking(I_color, m_w_mbt, m_w_klt)) {
975 
976  // AY : Removed as edge tracked, if necessary, is reinitialized in
977  // postTracking()
978 
979  // initPyramid(I, Ipyramid);
980 
981  // unsigned int i = (unsigned int)scales.size();
982  // do {
983  // i--;
984  // if(scales[i]){
985  // downScale(i);
986  // initMovingEdge(*Ipyramid[i], cMo);
987  // upScale(i);
988  // }
989  // } while(i != 0);
990 
991  // cleanPyramid(Ipyramid);
992  }
993 
994  if (displayFeatures) {
996  }
997 }
998 
999 unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor, unsigned int lvl)
1000 {
1001  vpMbtDistanceLine *l;
1003  vpMbtDistanceCircle *ci;
1004 
1005  if (lvl >= scales.size() || !scales[lvl]) {
1006  throw vpException(vpException::dimensionError, "_lvl not used.");
1007  }
1008 
1009  unsigned int nbrow = initMbtTracking(lvl);
1010 
1011  if (nbrow == 0) {
1012  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
1013  // "Error: not enough features in the interaction matrix...");
1014  return nbrow;
1015  }
1016 
1017  factor.resize(nbrow, false);
1018  factor = 1;
1019 
1020  unsigned int n = 0;
1021  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1022  if ((*it)->isTracked()) {
1023  l = *it;
1025 
1026  double fac = 1;
1027  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1028  ++itindex) {
1029  int index = *itindex;
1030  if (l->hiddenface->isAppearing((unsigned int)index)) {
1031  fac = 0.2;
1032  break;
1033  }
1034  if (l->closeToImageBorder(I, 10)) {
1035  fac = 0.1;
1036  break;
1037  }
1038  }
1039 
1040  for (size_t a = 0; a < l->meline.size(); a++) {
1041  std::list<vpMeSite>::const_iterator itListLine;
1042  if (l->meline[a] != nullptr) {
1043  itListLine = l->meline[a]->getMeList().begin();
1044 
1045  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1046  factor[n + i] = fac;
1047  vpMeSite site = *itListLine;
1048  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1049  factor[n + i] = 0.2;
1050  ++itListLine;
1051  }
1052  n += l->nbFeature[a];
1053  }
1054  }
1055  }
1056  }
1057 
1058  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1059  ++it) {
1060  if ((*it)->isTracked()) {
1061  cy = *it;
1063  double fac = 1.0;
1064 
1065  std::list<vpMeSite>::const_iterator itCyl1;
1066  std::list<vpMeSite>::const_iterator itCyl2;
1067  if ((cy->meline1 != nullptr || cy->meline2 != nullptr)) {
1068  itCyl1 = cy->meline1->getMeList().begin();
1069  itCyl2 = cy->meline2->getMeList().begin();
1070  }
1071 
1072  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1073  factor[n + i] = fac;
1074  vpMeSite site;
1075  if (i < cy->nbFeaturel1) {
1076  site = *itCyl1;
1077  ++itCyl1;
1078  }
1079  else {
1080  site = *itCyl2;
1081  ++itCyl2;
1082  }
1083  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1084  factor[n + i] = 0.2;
1085  }
1086 
1087  n += cy->nbFeature;
1088  }
1089  }
1090 
1091  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1092  if ((*it)->isTracked()) {
1093  ci = *it;
1095  double fac = 1.0;
1096 
1097  std::list<vpMeSite>::const_iterator itCir;
1098  if (ci->meEllipse != nullptr) {
1099  itCir = ci->meEllipse->getMeList().begin();
1100  }
1101 
1102  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1103  factor[n + i] = fac;
1104  vpMeSite site = *itCir;
1105  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1106  factor[n + i] = 0.2;
1107  ++itCir;
1108  }
1109 
1110  n += ci->nbFeature;
1111  }
1112  }
1113 
1114  return nbrow;
1115 }
1116 
1118  const vpHomogeneousMatrix &cMo, unsigned int lvl)
1119 {
1120  vpMbtDistanceLine *l;
1122  vpMbtDistanceCircle *ci;
1123 
1124  unsigned int n = 0;
1125  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1126  if ((*it)->isTracked()) {
1127  l = *it;
1129  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1130  for (unsigned int j = 0; j < 6; j++) {
1131  L[n + i][j] = l->L[i][j];
1132  error[n + i] = l->error[i];
1133  }
1134  }
1135  n += l->nbFeatureTotal;
1136  }
1137  }
1138 
1139  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1140  ++it) {
1141  if ((*it)->isTracked()) {
1142  cy = *it;
1143  cy->computeInteractionMatrixError(cMo, I);
1144  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1145  for (unsigned int j = 0; j < 6; j++) {
1146  L[n + i][j] = cy->L[i][j];
1147  error[n + i] = cy->error[i];
1148  }
1149  }
1150  n += cy->nbFeature;
1151  }
1152  }
1153  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1154  if ((*it)->isTracked()) {
1155  ci = *it;
1157  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1158  for (unsigned int j = 0; j < 6; j++) {
1159  L[n + i][j] = ci->L[i][j];
1160  error[n + i] = ci->error[i];
1161  }
1162  }
1163 
1164  n += ci->nbFeature;
1165  }
1166  }
1167 }
1168 
1175 {
1176  m_cam = cam;
1177 
1180 }
1181 
1189 {
1192 }
1200 {
1203 }
1204 
1215 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1216  const std::string &name)
1217 {
1218  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1219 }
1220 
1231 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1232  const std::string &name)
1233 {
1234  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1235  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1236 }
1237 
1250  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1251  bool displayFullModel)
1252 {
1253  std::vector<std::vector<double> > models =
1254  vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1255 
1256  for (size_t i = 0; i < models.size(); i++) {
1257  if (vpMath::equal(models[i][0], 0)) {
1258  vpImagePoint ip1(models[i][1], models[i][2]);
1259  vpImagePoint ip2(models[i][3], models[i][4]);
1260  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1261  }
1262  else if (vpMath::equal(models[i][0], 1)) {
1263  vpImagePoint center(models[i][1], models[i][2]);
1264  double n20 = models[i][3];
1265  double n11 = models[i][4];
1266  double n02 = models[i][5];
1267  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1268  }
1269  }
1270 
1271  if (displayFeatures) {
1272  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1273  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1276 
1278  double id = m_featuresToBeDisplayedKlt[i][5];
1279  std::stringstream ss;
1280  ss << id;
1281  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1282  }
1283  }
1284  }
1285 
1286 #ifdef VISP_HAVE_OGRE
1287  if (useOgre)
1288  faces.displayOgre(cMo);
1289 #endif
1290 }
1291 
1304  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1305  bool displayFullModel)
1306 {
1307  std::vector<std::vector<double> > models =
1308  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1309 
1310  for (size_t i = 0; i < models.size(); i++) {
1311  if (vpMath::equal(models[i][0], 0)) {
1312  vpImagePoint ip1(models[i][1], models[i][2]);
1313  vpImagePoint ip2(models[i][3], models[i][4]);
1314  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1315  }
1316  else if (vpMath::equal(models[i][0], 1)) {
1317  vpImagePoint center(models[i][1], models[i][2]);
1318  double n20 = models[i][3];
1319  double n11 = models[i][4];
1320  double n02 = models[i][5];
1321  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1322  }
1323  }
1324 
1325  if (displayFeatures) {
1326  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1327  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1330 
1332  double id = m_featuresToBeDisplayedKlt[i][5];
1333  std::stringstream ss;
1334  ss << id;
1335  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1336  }
1337  }
1338  }
1339 
1340 #ifdef VISP_HAVE_OGRE
1341  if (useOgre)
1342  faces.displayOgre(cMo);
1343 #endif
1344 }
1345 
1346 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1347  const vpHomogeneousMatrix &cMo,
1348  const vpCameraParameters &cam,
1349  bool displayFullModel)
1350 {
1351  std::vector<std::vector<double> > models;
1352 
1353  for (unsigned int i = 0; i < scales.size(); i += 1) {
1354  if (scales[i]) {
1355  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1356  ++it) {
1357  std::vector<std::vector<double> > currentModel =
1358  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1359  models.insert(models.end(), currentModel.begin(), currentModel.end());
1360  }
1361 
1362  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1363  it != cylinders[scaleLevel].end(); ++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<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1370  it != circles[scaleLevel].end(); ++it) {
1371  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1372  models.push_back(paramsCircle);
1373  }
1374 
1375  break; // displaying model on one scale only
1376  }
1377  }
1378 
1379 #ifdef VISP_HAVE_OGRE
1380  if (useOgre)
1381  faces.displayOgre(cMo);
1382 #endif
1383 
1384  return models;
1385 }
1386 
1399 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1400  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1401 {
1402  // Reinit klt
1403 
1404  // delete the Klt Polygon features
1405  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1406  vpMbtDistanceKltPoints *kltpoly = *it;
1407  if (kltpoly != nullptr) {
1408  delete kltpoly;
1409  }
1410  kltpoly = nullptr;
1411  }
1412  kltPolygons.clear();
1413 
1414  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1415  ++it) {
1416  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1417  if (kltPolyCylinder != nullptr) {
1418  delete kltPolyCylinder;
1419  }
1420  kltPolyCylinder = nullptr;
1421  }
1422  kltCylinders.clear();
1423 
1424  // delete the structures used to display circles
1425  vpMbtDistanceCircle *ci;
1426  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1427  ci = *it;
1428  if (ci != nullptr) {
1429  delete ci;
1430  }
1431  ci = nullptr;
1432  }
1433 
1434  circles_disp.clear();
1435 
1436  firstInitialisation = true;
1437 
1438  // Reinit edge
1439  vpMbtDistanceLine *l;
1441 
1442  for (unsigned int i = 0; i < scales.size(); i += 1) {
1443  if (scales[i]) {
1444  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1445  l = *it;
1446  if (l != nullptr)
1447  delete l;
1448  l = nullptr;
1449  }
1450 
1451  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1452  ++it) {
1453  cy = *it;
1454  if (cy != nullptr)
1455  delete cy;
1456  cy = nullptr;
1457  }
1458 
1459  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1460  ci = *it;
1461  if (ci != nullptr)
1462  delete ci;
1463  ci = nullptr;
1464  }
1465 
1466  lines[i].clear();
1467  cylinders[i].clear();
1468  circles[i].clear();
1469  }
1470  }
1471 
1472  // compute_interaction=1;
1473  nline = 0;
1474  ncylinder = 0;
1475  ncircle = 0;
1476  // lambda = 1;
1477  nbvisiblepolygone = 0;
1478 
1479  // Reinit common parts
1480  faces.reset();
1481 
1482  loadModel(cad_name, verbose, T);
1483 
1484  m_cMo = cMo;
1485  init(I);
1486 }
1487 
1488 #elif !defined(VISP_BUILD_SHARED_LIBS)
1489 // Work around to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1490 // no symbols
1491 void dummy_vpMbEdgeKltTracker() { };
1492 #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:282
unsigned int getRows() const
Definition: vpArray2D.h:267
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:1049
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:79
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ fatalError
Fatal error.
Definition: vpException.h:84
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:240
unsigned int getHeight() const
Definition: vpImage.h:182
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:449
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:437
double m_thresholdKLT
The threshold used in the robust estimation of KLT.
virtual void initFaceFromLines(vpMbtPolygon &polygon) override
virtual void setCameraParameters(const vpCameraParameters &cam) override
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double r, int idFace=0, const std::string &name="") override
void resetTracker() 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) override
virtual void initCylinder(const vpPoint &, const vpPoint &, double r, int idFace, const std::string &name="") override
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) override
virtual void track(const vpImage< unsigned char > &I) override
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) override
virtual void initFaceFromCorners(vpMbtPolygon &polygon) override
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) override
virtual void setClipping(const unsigned int &flags) override
vpColVector m_w_mbt
Robust weights for Edge.
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)
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 computeVVSInteractionMatrixAndResidu() 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 setNearClippingDistance(const double &dist) override
vpColVector m_error_hybrid
(s - s*)
virtual void computeVVSInit() override
vpColVector m_w_klt
Robust weights for KLT.
double m_thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setFarClippingDistance(const double &dist) override
unsigned int m_maxIterKlt
The maximum iteration of the virtual visual servoing stage.
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix()) override
virtual void init(const vpImage< unsigned char > &I) 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)
virtual void initFaceFromLines(vpMbtPolygon &polygon) override
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)
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
virtual void setCameraParameters(const vpCameraParameters &cam) 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 initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="") override
void trackMovingEdge(const vpImage< unsigned char > &I)
std::vector< const vpImage< unsigned char > * > Ipyramid
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="") override
unsigned int ncircle
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
virtual void initFaceFromCorners(vpMbtPolygon &polygon) override
void updateMovingEdge(const vpImage< unsigned char > &I)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setMovingEdge(const vpMe &me)
void resetTracker() override
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)
virtual void initFaceFromLines(vpMbtPolygon &polygon) override
std::list< vpMbtDistanceKltCylinder * > kltCylinders
vpHomogeneousMatrix c0Mo
Initial pose.
vpHomogeneousMatrix ctTc0
std::list< vpMbtDistanceKltPoints * > kltPolygons
void resetTracker() override
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="") override
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 reinit(const vpImage< unsigned char > &I)
virtual void initFaceFromCorners(vpMbtPolygon &polygon) override
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) override
unsigned int maskBorder
Erosion of the mask.
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
void setCameraParameters(const vpCameraParameters &cam) override
virtual void init(const vpImage< unsigned char > &I) 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 removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:89
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:83
vpMeSiteState getState() const
Definition: vpMeSite.h:251
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:241
Definition: vpMe.h:120
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:134
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:154
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:58
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)
#define vpERROR_TRACE
Definition: vpDebug.h:388