Visual Servoing Platform  version 3.5.1 under development (2022-12-06)
vpMbEdgeKltTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
33  *
34  * Authors:
35  * Romain Tallonneau
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
40 //#define VP_DEBUG_MODE 1 // Activate debug level 1
41 
42 #include <visp3/core/vpDebug.h>
43 #include <visp3/core/vpTrackingException.h>
44 #include <visp3/core/vpVelocityTwistMatrix.h>
45 #include <visp3/mbt/vpMbEdgeKltTracker.h>
46 #include <visp3/mbt/vpMbtXmlGenericParser.h>
47 
48 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
49 
51  : m_thresholdKLT(2.), m_thresholdMBT(2.), m_maxIterKlt(30), m_w_mbt(), m_w_klt(), m_error_hybrid(), m_w_hybrid()
52 {
53  computeCovariance = false;
54 
55 #ifdef VISP_HAVE_OGRE
56  faces.getOgreContext()->setWindowName("MBT Hybrid");
57 #endif
58 
59  m_lambda = 0.8;
60  m_maxIter = 200;
61 }
62 
68 
76 {
78 
80 
82 
83  unsigned int i = (unsigned int)scales.size();
84  do {
85  i--;
86  if (scales[i]) {
87  downScale(i);
89  upScale(i);
90  }
91  } while (i != 0);
92 
94 }
95 
106 {
107  vpMbKltTracker::setPose(I, cdMo);
108 
109  resetMovingEdge();
110 
111  if (useScanLine) {
115  }
116 
117  initPyramid(I, Ipyramid);
118 
119  unsigned int i = (unsigned int)scales.size();
120  do {
121  i--;
122  if (scales[i]) {
123  downScale(i);
125  upScale(i);
126  }
127  } while (i != 0);
128 
130 }
131 
142 {
143  vpImageConvert::convert(I_color, m_I);
145 
146  resetMovingEdge();
147 
148  if (useScanLine) {
152  }
153 
155 
156  unsigned int i = (unsigned int)scales.size();
157  do {
158  i--;
159  if (scales[i]) {
160  downScale(i);
162  upScale(i);
163  }
164  } while (i != 0);
165 
167 }
168 
174 {
177 }
178 
179 unsigned int vpMbEdgeKltTracker::initMbtTracking(unsigned int lvl)
180 {
181  if (lvl >= scales.size() || !scales[lvl]) {
182  throw vpException(vpException::dimensionError, "lvl not used.");
183  }
184 
185  unsigned int nbrow = 0;
186  for (std::list<vpMbtDistanceLine *>::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
187  vpMbtDistanceLine *l = *it;
188 
189  if (l->isTracked()) {
191  nbrow += l->nbFeatureTotal;
192  }
193  }
194 
195  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
196  ++it) {
197  vpMbtDistanceCylinder *cy = *it;
198 
199  if (cy->isTracked()) {
201  nbrow += cy->nbFeature;
202  }
203  }
204 
205  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
206  vpMbtDistanceCircle *ci = *it;
207 
208  if (ci->isTracked()) {
210  nbrow += ci->nbFeature;
211  }
212  }
213 
214  return nbrow;
215 }
216 
277 void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
278 {
279  // Load projection error config
280  vpMbTracker::loadConfigFile(configFile, verbose);
281 
283  xmlp.setVerbose(verbose);
287 
288  xmlp.setEdgeMe(me);
289 
290  xmlp.setKltMaxFeatures(10000);
291  xmlp.setKltWindowSize(5);
292  xmlp.setKltQuality(0.01);
293  xmlp.setKltMinDistance(5);
294  xmlp.setKltHarrisParam(0.01);
295  xmlp.setKltBlockSize(3);
296  xmlp.setKltPyramidLevels(3);
298 
299  try {
300  if (verbose) {
301  std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
302  }
303  xmlp.parse(configFile.c_str());
304  } catch (...) {
305  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
306  throw vpException(vpException::ioError, "problem to parse configuration file.");
307  }
308 
309  vpCameraParameters camera;
310  xmlp.getCameraParameters(camera);
311  setCameraParameters(camera);
312 
315 
316  if (xmlp.hasNearClippingDistance())
318 
319  if (xmlp.hasFarClippingDistance())
321 
322  if (xmlp.getFovClipping()) {
324  }
325 
326  useLodGeneral = xmlp.getLodState();
329 
330  applyLodSettingInConfig = false;
331  if (this->getNbPolygon() > 0) {
336  }
337 
338  vpMe meParser;
339  xmlp.getEdgeMe(meParser);
341 
347  tracker.setBlockSize((int)xmlp.getKltBlockSize());
349  maskBorder = xmlp.getKltMaskBorder();
350 
351  // if(useScanLine)
352  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
353 }
354 
359  unsigned int lvl)
360 {
361  postTrackingMbt(w_mbt, lvl);
362 
363  if (displayFeatures) {
364  if (lvl == 0) {
365  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
366  vpMbtDistanceLine *l = *it;
367  if (l->isVisible() && l->isTracked()) {
368  l->displayMovingEdges(I);
369  }
370  }
371 
372  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
373  ++it) {
374  vpMbtDistanceCylinder *cy = *it;
375  // A cylinder is always visible: #FIXME AY: Still valid?
376  if (cy->isTracked())
377  cy->displayMovingEdges(I);
378  }
379 
380  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
381  vpMbtDistanceCircle *ci = *it;
382  if (ci->isVisible() && ci->isTracked()) {
383  ci->displayMovingEdges(I);
384  }
385  }
386  }
387  }
388 
389  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
390 
391  if (useScanLine) {
395  }
396 
398 
401 
402  if (computeProjError)
404 
405  if (reInit)
406  return true;
407 
408  return false;
409 }
410 
415  unsigned int lvl)
416 {
417  postTrackingMbt(w_mbt, lvl);
418 
419  if (displayFeatures) {
420  if (lvl == 0) {
421  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
422  vpMbtDistanceLine *l = *it;
423  if (l->isVisible() && l->isTracked()) {
424  l->displayMovingEdges(I_color);
425  }
426  }
427 
428  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
429  ++it) {
430  vpMbtDistanceCylinder *cy = *it;
431  // A cylinder is always visible: #FIXME AY: Still valid?
432  if (cy->isTracked())
433  cy->displayMovingEdges(m_I);
434  }
435 
436  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
437  vpMbtDistanceCircle *ci = *it;
438  if (ci->isVisible() && ci->isTracked()) {
439  ci->displayMovingEdges(m_I);
440  }
441  }
442  }
443  }
444 
445  bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
446 
447  if (useScanLine) {
451  }
452 
454 
457 
458  if (computeProjError)
460 
461  if (reInit)
462  return true;
463 
464  return false;
465 }
466 
478 {
479  if (lvl >= scales.size() || !scales[lvl]) {
480  throw vpException(vpException::dimensionError, "_lvl not used.");
481  }
482 
483  unsigned int n = 0;
485  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
486  if ((*it)->isTracked()) {
487  l = *it;
488  unsigned int indexLine = 0;
489  double wmean = 0;
490 
491  for (size_t a = 0; a < l->meline.size(); a++) {
492  std::list<vpMeSite>::iterator itListLine;
493  if (l->nbFeature[a] > 0)
494  itListLine = l->meline[a]->getMeList().begin();
495 
496  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
497  wmean += w[n + indexLine];
498  vpMeSite p = *itListLine;
499  if (w[n + indexLine] < 0.5) {
501  *itListLine = p;
502  }
503 
504  ++itListLine;
505  indexLine++;
506  }
507  }
508 
509  n += l->nbFeatureTotal;
510 
511  if (l->nbFeatureTotal != 0)
512  wmean /= l->nbFeatureTotal;
513  else
514  wmean = 1;
515 
516  l->setMeanWeight(wmean);
517 
518  if (wmean < 0.8)
519  l->Reinit = true;
520  }
521  }
522 
523  // Same thing with cylinders as with lines
525  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
526  ++it) {
527  if ((*it)->isTracked()) {
528  cy = *it;
529  double wmean = 0;
530  std::list<vpMeSite>::iterator itListCyl1;
531  std::list<vpMeSite>::iterator itListCyl2;
532  if (cy->nbFeature > 0) {
533  itListCyl1 = cy->meline1->getMeList().begin();
534  itListCyl2 = cy->meline2->getMeList().begin();
535  }
536 
537  wmean = 0;
538  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
539  wmean += w[n + i];
540  vpMeSite p = *itListCyl1;
541  if (w[n + i] < 0.5) {
543 
544  *itListCyl1 = p;
545  }
546 
547  ++itListCyl1;
548  }
549 
550  if (cy->nbFeaturel1 != 0)
551  wmean /= cy->nbFeaturel1;
552  else
553  wmean = 1;
554 
555  cy->setMeanWeight1(wmean);
556 
557  if (wmean < 0.8) {
558  cy->Reinit = true;
559  }
560 
561  wmean = 0;
562  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
563  wmean += w[n + i];
564  vpMeSite p = *itListCyl2;
565  if (w[n + i] < 0.5) {
567 
568  *itListCyl2 = p;
569  }
570 
571  ++itListCyl2;
572  }
573 
574  if (cy->nbFeaturel2 != 0)
575  wmean /= cy->nbFeaturel2;
576  else
577  wmean = 1;
578 
579  cy->setMeanWeight2(wmean);
580 
581  if (wmean < 0.8) {
582  cy->Reinit = true;
583  }
584 
585  n += cy->nbFeature;
586  }
587  }
588 
589  // Same thing with circles as with lines
591  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
592  if ((*it)->isTracked()) {
593  ci = *it;
594  double wmean = 0;
595  std::list<vpMeSite>::iterator itListCir;
596 
597  if (ci->nbFeature > 0) {
598  itListCir = ci->meEllipse->getMeList().begin();
599  }
600 
601  wmean = 0;
602  for (unsigned int i = 0; i < ci->nbFeature; i++) {
603  wmean += w[n + i];
604  vpMeSite p = *itListCir;
605  if (w[n + i] < 0.5) {
607 
608  *itListCir = p;
609  }
610 
611  ++itListCir;
612  }
613 
614  if (ci->nbFeature != 0)
615  wmean /= ci->nbFeature;
616  else
617  wmean = 1;
618 
619  ci->setMeanWeight(wmean);
620 
621  if (wmean < 0.8) {
622  ci->Reinit = true;
623  }
624 
625  n += ci->nbFeature;
626  }
627  }
628 }
629 
640 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
641  unsigned int lvl, double *edge_residual, double *klt_residual)
642 {
643  vpColVector factor;
644  nbrow = trackFirstLoop(I, factor, lvl);
645 
646  if (nbrow < 4 && nbInfos < 4) {
647  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
648  } else if (nbrow < 4)
649  nbrow = 0;
650 
651  unsigned int totalNbRows = nbrow + 2 * nbInfos;
652  double residu = 0;
653  double residu_1 = -1;
654  unsigned int iter = 0;
655 
656  vpMatrix L(totalNbRows, 6);
657  vpMatrix L_mbt, L_klt; // interaction matrix
658  vpColVector weighted_error(totalNbRows);
659  vpColVector R_mbt, R_klt; // residu
660  vpMatrix L_true;
661  vpMatrix LVJ_true;
662 
663  if (nbrow != 0) {
664  L_mbt.resize(nbrow, 6, false, false);
665  R_mbt.resize(nbrow, false);
666  }
667 
668  if (nbInfos != 0) {
669  L_klt.resize(2 * nbInfos, 6, false, false);
670  R_klt.resize(2 * nbInfos, false);
671  }
672 
673  vpColVector v; // "speed" for VVS
674  vpRobust robust_mbt, robust_klt;
675  vpHomography H;
676 
677  vpMatrix LTL;
678  vpColVector LTR;
679 
680  double factorMBT; // = 1.0;
681  double factorKLT; // = 1.0;
682 
683  // More efficient weight repartition for hybrid tracker should come soon...
684  // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
685  // factorKLT = 1.0 - factorMBT;
686  factorMBT = 0.35;
687  factorKLT = 0.65;
688 
689  if (nbrow < 4)
690  factorKLT = 1.;
691  if (nbInfos < 4)
692  factorMBT = 1.;
693 
694  if (edge_residual != NULL)
695  *edge_residual = 0;
696  if (klt_residual != NULL)
697  *klt_residual = 0;
698 
699  vpHomogeneousMatrix cMoPrev;
700  vpHomogeneousMatrix ctTc0_Prev;
701  vpColVector m_error_prev;
702  vpColVector m_w_prev;
703 
704  // Init size
705  m_error_hybrid.resize(totalNbRows, false);
706  m_w_hybrid.resize(totalNbRows, false);
707 
708  if (nbrow != 0) {
709  m_w_mbt.resize(nbrow, false);
710  m_w_mbt = 1; // needed in vpRobust::psiTukey()
711  }
712 
713  if (nbInfos != 0) {
714  m_w_klt.resize(2 * nbInfos, false);
715  m_w_klt = 1; // needed in vpRobust::psiTukey()
716  }
717 
718  double mu = m_initialMu;
719 
720  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
721  if (nbrow >= 4)
722  trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
723 
724  if (nbInfos >= 4) {
725  unsigned int shift = 0;
726 
727  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
728  it != vpMbKltTracker::kltPolygons.end(); ++it) {
729  vpMbtDistanceKltPoints *kltpoly = *it;
730  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
731  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
732  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
733  kltpoly->computeHomography(ctTc0, H);
734  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
735  shift += 2 * kltpoly->getCurrentNumberPoints();
736  }
737  }
738 
739  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
740  ++it) {
741  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
742 
743  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
744  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
745  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
746  try {
747  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
748  } catch (...) {
749  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
750  }
751 
752  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
753  }
754  }
755  }
756 
757  /* residuals */
758  if (nbrow > 3) {
759  m_error_hybrid.insert(0, R_mbt);
760  }
761 
762  if (nbInfos > 3) {
763  m_error_hybrid.insert(nbrow, R_klt);
764  }
765 
766  unsigned int cpt = 0;
767  while (cpt < (nbrow + 2 * nbInfos)) {
768  if (cpt < (unsigned)nbrow) {
769  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
770  } else {
771  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
772  }
773  cpt++;
774  }
775 
776  bool reStartFromLastIncrement = false;
777  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
778  &m_w_prev);
779  if (reStartFromLastIncrement) {
780  ctTc0 = ctTc0_Prev;
781  }
782 
783  if (!reStartFromLastIncrement) {
784  /* robust */
785  if (nbrow > 3) {
786  if (edge_residual != NULL) {
787  *edge_residual = 0;
788  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
789  *edge_residual += fabs(R_mbt[i]);
790  *edge_residual /= R_mbt.getRows();
791  }
792 
794  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
795 
796  L.insert(L_mbt, 0, 0);
797  }
798 
799  if (nbInfos > 3) {
800  if (klt_residual != NULL) {
801  *klt_residual = 0;
802  for (unsigned int i = 0; i < R_klt.getRows(); i++)
803  *klt_residual += fabs(R_klt[i]);
804  *klt_residual /= R_klt.getRows();
805  }
806 
808  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
809 
810  L.insert(L_klt, nbrow, 0);
811  }
812 
813  cpt = 0;
814  while (cpt < (nbrow + 2 * nbInfos)) {
815  if (cpt < (unsigned)nbrow) {
816  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
817  } else {
818  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
819  }
820  cpt++;
821  }
822 
823  if (computeCovariance) {
824  L_true = L;
825  if (!isoJoIdentity) {
827  cVo.buildFrom(m_cMo);
828  LVJ_true = (L * cVo * oJo);
829  }
830  }
831 
832  residu_1 = residu;
833  residu = 0;
834  double num = 0;
835  double den = 0;
836 
837  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
838  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
839  den += m_w_hybrid[i];
840 
841  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
842  if (m_computeInteraction) {
843  for (unsigned int j = 0; j < 6; j += 1) {
844  L[i][j] *= m_w_hybrid[i];
845  }
846  }
847  }
848 
849  residu = sqrt(num / den);
850 
851  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
852  &m_w_hybrid, &m_w_prev);
853 
854  cMoPrev = m_cMo;
855  ctTc0_Prev = ctTc0;
857  m_cMo = ctTc0 * c0Mo;
858  }
859 
860  iter++;
861  }
862 
864 }
865 
867 {
868  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
869 }
870 
872 {
873  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
874  "computeVVSInteractionMatrixAndR"
875  "esidu() should not be called!");
876 }
877 
886 {
887  try {
889  } catch (...) {
890  }
891 
892  if (m_nbInfos >= 4) {
893  unsigned int old_maxIter = m_maxIter;
896  m_maxIter = old_maxIter;
897  } else {
898  m_nbInfos = 0;
899  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
900  }
901 
903 
904  unsigned int nbrow = 0;
905  computeVVS(I, m_nbInfos, nbrow);
906 
907  if (postTracking(I, m_w_mbt, m_w_klt)) {
909 
910  // AY : Removed as edge tracked, if necessary, is reinitialized in
911  // postTracking()
912 
913  // initPyramid(I, Ipyramid);
914 
915  // unsigned int i = (unsigned int)scales.size();
916  // do {
917  // i--;
918  // if(scales[i]){
919  // downScale(i);
920  // initMovingEdge(*Ipyramid[i], cMo);
921  // upScale(i);
922  // }
923  // } while(i != 0);
924 
925  // cleanPyramid(Ipyramid);
926  }
927 
928  if (displayFeatures) {
930  }
931 }
932 
941 {
942  vpImageConvert::convert(I_color, m_I);
943  try {
945  } catch (...) {
946  }
947 
948  if (m_nbInfos >= 4) {
949  unsigned int old_maxIter = m_maxIter;
952  m_maxIter = old_maxIter;
953  } else {
954  m_nbInfos = 0;
955  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
956  }
957 
959 
960  unsigned int nbrow = 0;
961  computeVVS(m_I, m_nbInfos, nbrow);
962 
963  if (postTracking(I_color, m_w_mbt, m_w_klt)) {
965 
966  // AY : Removed as edge tracked, if necessary, is reinitialized in
967  // postTracking()
968 
969  // initPyramid(I, Ipyramid);
970 
971  // unsigned int i = (unsigned int)scales.size();
972  // do {
973  // i--;
974  // if(scales[i]){
975  // downScale(i);
976  // initMovingEdge(*Ipyramid[i], cMo);
977  // upScale(i);
978  // }
979  // } while(i != 0);
980 
981  // cleanPyramid(Ipyramid);
982  }
983 
984  if (displayFeatures) {
986  }
987 }
988 
989 unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor, unsigned int lvl)
990 {
994 
995  if (lvl >= scales.size() || !scales[lvl]) {
996  throw vpException(vpException::dimensionError, "_lvl not used.");
997  }
998 
999  unsigned int nbrow = initMbtTracking(lvl);
1000 
1001  if (nbrow == 0) {
1002  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
1003  // "Error: not enough features in the interaction matrix...");
1004  return nbrow;
1005  }
1006 
1007  factor.resize(nbrow, false);
1008  factor = 1;
1009 
1010  unsigned int n = 0;
1011  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1012  if ((*it)->isTracked()) {
1013  l = *it;
1015 
1016  double fac = 1;
1017  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1018  ++itindex) {
1019  int index = *itindex;
1020  if (l->hiddenface->isAppearing((unsigned int)index)) {
1021  fac = 0.2;
1022  break;
1023  }
1024  if (l->closeToImageBorder(I, 10)) {
1025  fac = 0.1;
1026  break;
1027  }
1028  }
1029 
1030  unsigned int indexFeature = 0;
1031  for (size_t a = 0; a < l->meline.size(); a++) {
1032  std::list<vpMeSite>::const_iterator itListLine;
1033  if (l->meline[a] != NULL) {
1034  itListLine = l->meline[a]->getMeList().begin();
1035 
1036  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1037  factor[n + i] = fac;
1038  vpMeSite site = *itListLine;
1039  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1040  factor[n + i] = 0.2;
1041  ++itListLine;
1042  indexFeature++;
1043  }
1044  n += l->nbFeature[a];
1045  }
1046  }
1047  }
1048  }
1049 
1050  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1051  ++it) {
1052  if ((*it)->isTracked()) {
1053  cy = *it;
1055  double fac = 1.0;
1056 
1057  std::list<vpMeSite>::const_iterator itCyl1;
1058  std::list<vpMeSite>::const_iterator itCyl2;
1059  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1060  itCyl1 = cy->meline1->getMeList().begin();
1061  itCyl2 = cy->meline2->getMeList().begin();
1062  }
1063 
1064  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1065  factor[n + i] = fac;
1066  vpMeSite site;
1067  if (i < cy->nbFeaturel1) {
1068  site = *itCyl1;
1069  ++itCyl1;
1070  } else {
1071  site = *itCyl2;
1072  ++itCyl2;
1073  }
1074  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1075  factor[n + i] = 0.2;
1076  }
1077 
1078  n += cy->nbFeature;
1079  }
1080  }
1081 
1082  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1083  if ((*it)->isTracked()) {
1084  ci = *it;
1086  double fac = 1.0;
1087 
1088  std::list<vpMeSite>::const_iterator itCir;
1089  if (ci->meEllipse != NULL) {
1090  itCir = ci->meEllipse->getMeList().begin();
1091  }
1092 
1093  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1094  factor[n + i] = fac;
1095  vpMeSite site = *itCir;
1096  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1097  factor[n + i] = 0.2;
1098  ++itCir;
1099  }
1100 
1101  n += ci->nbFeature;
1102  }
1103  }
1104 
1105  return nbrow;
1106 }
1107 
1109  const vpHomogeneousMatrix &cMo, unsigned int lvl)
1110 {
1111  vpMbtDistanceLine *l;
1113  vpMbtDistanceCircle *ci;
1114 
1115  unsigned int n = 0;
1116  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1117  if ((*it)->isTracked()) {
1118  l = *it;
1120  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1121  for (unsigned int j = 0; j < 6; j++) {
1122  L[n + i][j] = l->L[i][j];
1123  error[n + i] = l->error[i];
1124  }
1125  }
1126  n += l->nbFeatureTotal;
1127  }
1128  }
1129 
1130  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1131  ++it) {
1132  if ((*it)->isTracked()) {
1133  cy = *it;
1134  cy->computeInteractionMatrixError(cMo, I);
1135  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1136  for (unsigned int j = 0; j < 6; j++) {
1137  L[n + i][j] = cy->L[i][j];
1138  error[n + i] = cy->error[i];
1139  }
1140  }
1141  n += cy->nbFeature;
1142  }
1143  }
1144  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1145  if ((*it)->isTracked()) {
1146  ci = *it;
1148  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1149  for (unsigned int j = 0; j < 6; j++) {
1150  L[n + i][j] = ci->L[i][j];
1151  error[n + i] = ci->error[i];
1152  }
1153  }
1154 
1155  n += ci->nbFeature;
1156  }
1157  }
1158 }
1159 
1166 {
1167  m_cam = cam;
1168 
1171 }
1172 
1180 {
1183 }
1191 {
1194 }
1195 
1206 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1207  const std::string &name)
1208 {
1209  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1210 }
1211 
1222 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1223  const std::string &name)
1224 {
1225  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1226  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1227 }
1228 
1241  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1242  bool displayFullModel)
1243 {
1244  std::vector<std::vector<double> > models =
1245  vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1246 
1247  for (size_t i = 0; i < models.size(); i++) {
1248  if (vpMath::equal(models[i][0], 0)) {
1249  vpImagePoint ip1(models[i][1], models[i][2]);
1250  vpImagePoint ip2(models[i][3], models[i][4]);
1251  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1252  } else if (vpMath::equal(models[i][0], 1)) {
1253  vpImagePoint center(models[i][1], models[i][2]);
1254  double n20 = models[i][3];
1255  double n11 = models[i][4];
1256  double n02 = models[i][5];
1257  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1258  }
1259  }
1260 
1261  if (displayFeatures) {
1262  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1263  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1266 
1268  double id = m_featuresToBeDisplayedKlt[i][5];
1269  std::stringstream ss;
1270  ss << id;
1271  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1272  }
1273  }
1274  }
1275 
1276 #ifdef VISP_HAVE_OGRE
1277  if (useOgre)
1278  faces.displayOgre(cMo);
1279 #endif
1280 }
1281 
1294  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1295  bool displayFullModel)
1296 {
1297  std::vector<std::vector<double> > models =
1298  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1299 
1300  for (size_t i = 0; i < models.size(); i++) {
1301  if (vpMath::equal(models[i][0], 0)) {
1302  vpImagePoint ip1(models[i][1], models[i][2]);
1303  vpImagePoint ip2(models[i][3], models[i][4]);
1304  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1305  } else if (vpMath::equal(models[i][0], 1)) {
1306  vpImagePoint center(models[i][1], models[i][2]);
1307  double n20 = models[i][3];
1308  double n11 = models[i][4];
1309  double n02 = models[i][5];
1310  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1311  }
1312  }
1313 
1314  if (displayFeatures) {
1315  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1316  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1319 
1321  double id = m_featuresToBeDisplayedKlt[i][5];
1322  std::stringstream ss;
1323  ss << id;
1324  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1325  }
1326  }
1327  }
1328 
1329 #ifdef VISP_HAVE_OGRE
1330  if (useOgre)
1331  faces.displayOgre(cMo);
1332 #endif
1333 }
1334 
1335 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1336  const vpHomogeneousMatrix &cMo,
1337  const vpCameraParameters &cam,
1338  bool displayFullModel)
1339 {
1340  std::vector<std::vector<double> > models;
1341 
1342  for (unsigned int i = 0; i < scales.size(); i += 1) {
1343  if (scales[i]) {
1344  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1345  ++it) {
1346  std::vector<std::vector<double> > currentModel =
1347  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1348  models.insert(models.end(), currentModel.begin(), currentModel.end());
1349  }
1350 
1351  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1352  it != cylinders[scaleLevel].end(); ++it) {
1353  std::vector<std::vector<double> > currentModel =
1354  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1355  models.insert(models.end(), currentModel.begin(), currentModel.end());
1356  }
1357 
1358  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1359  it != circles[scaleLevel].end(); ++it) {
1360  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1361  models.push_back(paramsCircle);
1362  }
1363 
1364  break; // displaying model on one scale only
1365  }
1366  }
1367 
1368 #ifdef VISP_HAVE_OGRE
1369  if (useOgre)
1370  faces.displayOgre(cMo);
1371 #endif
1372 
1373  return models;
1374 }
1375 
1388 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1389  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1390 {
1391 // Reinit klt
1392 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1393  if (cur != NULL) {
1394  cvReleaseImage(&cur);
1395  cur = NULL;
1396  }
1397 #endif
1398 
1399  // delete the Klt Polygon features
1400  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1401  vpMbtDistanceKltPoints *kltpoly = *it;
1402  if (kltpoly != NULL) {
1403  delete kltpoly;
1404  }
1405  kltpoly = NULL;
1406  }
1407  kltPolygons.clear();
1408 
1409  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1410  ++it) {
1411  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1412  if (kltPolyCylinder != NULL) {
1413  delete kltPolyCylinder;
1414  }
1415  kltPolyCylinder = NULL;
1416  }
1417  kltCylinders.clear();
1418 
1419  // delete the structures used to display circles
1420  vpMbtDistanceCircle *ci;
1421  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1422  ci = *it;
1423  if (ci != NULL) {
1424  delete ci;
1425  }
1426  ci = NULL;
1427  }
1428 
1429  circles_disp.clear();
1430 
1431  firstInitialisation = true;
1432 
1433  // Reinit edge
1434  vpMbtDistanceLine *l;
1436 
1437  for (unsigned int i = 0; i < scales.size(); i += 1) {
1438  if (scales[i]) {
1439  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1440  l = *it;
1441  if (l != NULL)
1442  delete l;
1443  l = NULL;
1444  }
1445 
1446  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1447  ++it) {
1448  cy = *it;
1449  if (cy != NULL)
1450  delete cy;
1451  cy = NULL;
1452  }
1453 
1454  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1455  ci = *it;
1456  if (ci != NULL)
1457  delete ci;
1458  ci = NULL;
1459  }
1460 
1461  lines[i].clear();
1462  cylinders[i].clear();
1463  circles[i].clear();
1464  }
1465  }
1466 
1467  // compute_interaction=1;
1468  nline = 0;
1469  ncylinder = 0;
1470  ncircle = 0;
1471  // lambda = 1;
1472  nbvisiblepolygone = 0;
1473 
1474  // Reinit common parts
1475  faces.reset();
1476 
1477  loadModel(cad_name, verbose, T);
1478 
1479  m_cMo = cMo;
1480  init(I);
1481 }
1482 
1483 #elif !defined(VISP_BUILD_SHARED_LIBS)
1484 // Work around to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1485 // no symbols
1486 void dummy_vpMbEdgeKltTracker(){};
1487 #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:306
unsigned int getRows() const
Definition: vpArray2D.h:291
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:131
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
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:175
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition: vpMath.h:117
static double sqr(double x)
Definition: vpMath.h:123
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:364
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
double m_thresholdKLT
The threshold used in the robust estimation of KLT.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
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 setClipping(const unsigned int &flags)
virtual void setFarClippingDistance(const double &dist)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void initCylinder(const vpPoint &, const vpPoint &, double r, int idFace, const std::string &name="")
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void computeVVSInit()
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)
unsigned int initMbtTracking(unsigned int level=0)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void init(const vpImage< unsigned char > &I)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpColVector m_error_hybrid
(s - s*)
virtual void setNearClippingDistance(const double &dist)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double r, int idFace=0, const std::string &name="")
vpColVector m_w_klt
Robust weights for KLT.
virtual void track(const vpImage< unsigned char > &I)
double m_thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
unsigned int m_maxIterKlt
The maximum iteration of the virtual visual servoing stage.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpColVector m_w_hybrid
Robust weights.
void upScale(const unsigned int _scale)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
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)
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
unsigned int scaleLevel
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="")
unsigned int ncircle
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
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)
void setMovingEdge(const vpMe &me)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
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 setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
cv::Mat cur
Temporary OpenCV image for fast conversion.
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 reinit(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void setCameraParameters(const vpCameraParameters &cam)
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void init(const vpImage< unsigned char > &I)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
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
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
bool computeProjError
Definition: vpMbTracker.h:133
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
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 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:67
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:72
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
Definition: vpMe.h:61
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:63
Error that can be emited by the vpTracker class and its derivates.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:393