Visual Servoing Platform  version 3.5.0
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 
638 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
639  unsigned int lvl)
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  } else if (nbrow < 4)
647  nbrow = 0;
648 
649  unsigned int totalNbRows = nbrow + 2 * nbInfos;
650  double residu = 0;
651  double residu_1 = -1;
652  unsigned int iter = 0;
653 
654  vpMatrix L(totalNbRows, 6);
655  vpMatrix L_mbt, L_klt; // interaction matrix
656  vpColVector weighted_error(totalNbRows);
657  vpColVector R_mbt, R_klt; // residu
658  vpMatrix L_true;
659  vpMatrix LVJ_true;
660 
661  if (nbrow != 0) {
662  L_mbt.resize(nbrow, 6, false, false);
663  R_mbt.resize(nbrow, false);
664  }
665 
666  if (nbInfos != 0) {
667  L_klt.resize(2 * nbInfos, 6, false, false);
668  R_klt.resize(2 * nbInfos, false);
669  }
670 
671  vpColVector v; // "speed" for VVS
672  vpRobust robust_mbt, robust_klt;
673  vpHomography H;
674 
675  vpMatrix LTL;
676  vpColVector LTR;
677 
678  double factorMBT; // = 1.0;
679  double factorKLT; // = 1.0;
680 
681  // More efficient weight repartition for hybrid tracker should come soon...
682  // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
683  // factorKLT = 1.0 - factorMBT;
684  factorMBT = 0.35;
685  factorKLT = 0.65;
686 
687  if (nbrow < 4)
688  factorKLT = 1.;
689  if (nbInfos < 4)
690  factorMBT = 1.;
691 
692  double residuMBT = 0;
693  double residuKLT = 0;
694 
695  vpHomogeneousMatrix cMoPrev;
696  vpHomogeneousMatrix ctTc0_Prev;
697  vpColVector m_error_prev;
698  vpColVector m_w_prev;
699 
700  // Init size
701  m_error_hybrid.resize(totalNbRows, false);
702  m_w_hybrid.resize(totalNbRows, false);
703 
704  if (nbrow != 0) {
705  m_w_mbt.resize(nbrow, false);
706  m_w_mbt = 1; // needed in vpRobust::psiTukey()
707  }
708 
709  if (nbInfos != 0) {
710  m_w_klt.resize(2 * nbInfos, false);
711  m_w_klt = 1; // needed in vpRobust::psiTukey()
712  }
713 
714  double mu = m_initialMu;
715 
716  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
717  if (nbrow >= 4)
718  trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
719 
720  if (nbInfos >= 4) {
721  unsigned int shift = 0;
722 
723  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
724  it != vpMbKltTracker::kltPolygons.end(); ++it) {
725  vpMbtDistanceKltPoints *kltpoly = *it;
726  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
727  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
728  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
729  kltpoly->computeHomography(ctTc0, H);
730  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
731  shift += 2 * kltpoly->getCurrentNumberPoints();
732  }
733  }
734 
735  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
736  ++it) {
737  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
738 
739  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
740  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
741  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
742  try {
743  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
744  } catch (...) {
745  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
746  }
747 
748  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
749  }
750  }
751  }
752 
753  /* residuals */
754  if (nbrow > 3) {
755  m_error_hybrid.insert(0, R_mbt);
756  }
757 
758  if (nbInfos > 3) {
759  m_error_hybrid.insert(nbrow, R_klt);
760  }
761 
762  unsigned int cpt = 0;
763  while (cpt < (nbrow + 2 * nbInfos)) {
764  if (cpt < (unsigned)nbrow) {
765  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
766  } else {
767  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
768  }
769  cpt++;
770  }
771 
772  bool reStartFromLastIncrement = false;
773  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
774  &m_w_prev);
775  if (reStartFromLastIncrement) {
776  ctTc0 = ctTc0_Prev;
777  }
778 
779  if (!reStartFromLastIncrement) {
780  /* robust */
781  if (nbrow > 3) {
782  residuMBT = 0;
783  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
784  residuMBT += fabs(R_mbt[i]);
785  residuMBT /= R_mbt.getRows();
786 
788  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
789 
790  L.insert(L_mbt, 0, 0);
791  }
792 
793  if (nbInfos > 3) {
794  residuKLT = 0;
795  for (unsigned int i = 0; i < R_klt.getRows(); i++)
796  residuKLT += fabs(R_klt[i]);
797  residuKLT /= R_klt.getRows();
798 
800  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
801 
802  L.insert(L_klt, nbrow, 0);
803  }
804 
805  cpt = 0;
806  while (cpt < (nbrow + 2 * nbInfos)) {
807  if (cpt < (unsigned)nbrow) {
808  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
809  } else {
810  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
811  }
812  cpt++;
813  }
814 
815  if (computeCovariance) {
816  L_true = L;
817  if (!isoJoIdentity) {
819  cVo.buildFrom(m_cMo);
820  LVJ_true = (L * cVo * oJo);
821  }
822  }
823 
824  residu_1 = residu;
825  residu = 0;
826  double num = 0;
827  double den = 0;
828 
829  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
830  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
831  den += m_w_hybrid[i];
832 
833  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
834  if (m_computeInteraction) {
835  for (unsigned int j = 0; j < 6; j += 1) {
836  L[i][j] *= m_w_hybrid[i];
837  }
838  }
839  }
840 
841  residu = sqrt(num / den);
842 
843  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
844  &m_w_hybrid, &m_w_prev);
845 
846  cMoPrev = m_cMo;
847  ctTc0_Prev = ctTc0;
849  m_cMo = ctTc0 * c0Mo;
850  }
851 
852  iter++;
853  }
854 
856 }
857 
859 {
860  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
861 }
862 
864 {
865  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
866  "computeVVSInteractionMatrixAndR"
867  "esidu() should not be called!");
868 }
869 
878 {
879  try {
881  } catch (...) {
882  }
883 
884  if (m_nbInfos >= 4) {
885  unsigned int old_maxIter = m_maxIter;
888  m_maxIter = old_maxIter;
889  } else {
890  m_nbInfos = 0;
891  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
892  }
893 
895 
896  unsigned int nbrow = 0;
897  computeVVS(I, m_nbInfos, nbrow);
898 
899  if (postTracking(I, m_w_mbt, m_w_klt)) {
901 
902  // AY : Removed as edge tracked, if necessary, is reinitialized in
903  // postTracking()
904 
905  // initPyramid(I, Ipyramid);
906 
907  // unsigned int i = (unsigned int)scales.size();
908  // do {
909  // i--;
910  // if(scales[i]){
911  // downScale(i);
912  // initMovingEdge(*Ipyramid[i], cMo);
913  // upScale(i);
914  // }
915  // } while(i != 0);
916 
917  // cleanPyramid(Ipyramid);
918  }
919 
920  if (displayFeatures) {
922  }
923 }
924 
933 {
934  vpImageConvert::convert(I_color, m_I);
935  try {
937  } catch (...) {
938  }
939 
940  if (m_nbInfos >= 4) {
941  unsigned int old_maxIter = m_maxIter;
944  m_maxIter = old_maxIter;
945  } else {
946  m_nbInfos = 0;
947  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
948  }
949 
951 
952  unsigned int nbrow = 0;
953  computeVVS(m_I, m_nbInfos, nbrow);
954 
955  if (postTracking(I_color, m_w_mbt, m_w_klt)) {
957 
958  // AY : Removed as edge tracked, if necessary, is reinitialized in
959  // postTracking()
960 
961  // initPyramid(I, Ipyramid);
962 
963  // unsigned int i = (unsigned int)scales.size();
964  // do {
965  // i--;
966  // if(scales[i]){
967  // downScale(i);
968  // initMovingEdge(*Ipyramid[i], cMo);
969  // upScale(i);
970  // }
971  // } while(i != 0);
972 
973  // cleanPyramid(Ipyramid);
974  }
975 
976  if (displayFeatures) {
978  }
979 }
980 
982  unsigned int lvl)
983 {
987 
988  if (lvl >= scales.size() || !scales[lvl]) {
989  throw vpException(vpException::dimensionError, "_lvl not used.");
990  }
991 
992  unsigned int nbrow = initMbtTracking(lvl);
993 
994  if (nbrow == 0) {
995  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
996  // "Error: not enough features in the interaction matrix...");
997  return nbrow;
998  }
999 
1000  factor.resize(nbrow, false);
1001  factor = 1;
1002 
1003  unsigned int n = 0;
1004  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1005  if ((*it)->isTracked()) {
1006  l = *it;
1008 
1009  double fac = 1;
1010  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1011  ++itindex) {
1012  int index = *itindex;
1013  if (l->hiddenface->isAppearing((unsigned int)index)) {
1014  fac = 0.2;
1015  break;
1016  }
1017  if (l->closeToImageBorder(I, 10)) {
1018  fac = 0.1;
1019  break;
1020  }
1021  }
1022 
1023  unsigned int indexFeature = 0;
1024  for (size_t a = 0; a < l->meline.size(); a++) {
1025  std::list<vpMeSite>::const_iterator itListLine;
1026  if (l->meline[a] != NULL) {
1027  itListLine = l->meline[a]->getMeList().begin();
1028 
1029  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1030  factor[n + i] = fac;
1031  vpMeSite site = *itListLine;
1032  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1033  factor[n + i] = 0.2;
1034  ++itListLine;
1035  indexFeature++;
1036  }
1037  n += l->nbFeature[a];
1038  }
1039  }
1040  }
1041  }
1042 
1043  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1044  ++it) {
1045  if ((*it)->isTracked()) {
1046  cy = *it;
1048  double fac = 1.0;
1049 
1050  std::list<vpMeSite>::const_iterator itCyl1;
1051  std::list<vpMeSite>::const_iterator itCyl2;
1052  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1053  itCyl1 = cy->meline1->getMeList().begin();
1054  itCyl2 = cy->meline2->getMeList().begin();
1055  }
1056 
1057  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1058  factor[n + i] = fac;
1059  vpMeSite site;
1060  if (i < cy->nbFeaturel1) {
1061  site = *itCyl1;
1062  ++itCyl1;
1063  } else {
1064  site = *itCyl2;
1065  ++itCyl2;
1066  }
1067  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1068  factor[n + i] = 0.2;
1069  }
1070 
1071  n += cy->nbFeature;
1072  }
1073  }
1074 
1075  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1076  if ((*it)->isTracked()) {
1077  ci = *it;
1079  double fac = 1.0;
1080 
1081  std::list<vpMeSite>::const_iterator itCir;
1082  if (ci->meEllipse != NULL) {
1083  itCir = ci->meEllipse->getMeList().begin();
1084  }
1085 
1086  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1087  factor[n + i] = fac;
1088  vpMeSite site = *itCir;
1089  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1090  factor[n + i] = 0.2;
1091  ++itCir;
1092  }
1093 
1094  n += ci->nbFeature;
1095  }
1096  }
1097 
1098  return nbrow;
1099 }
1100 
1102  const vpHomogeneousMatrix &cMo, unsigned int lvl)
1103 {
1104  vpMbtDistanceLine *l;
1106  vpMbtDistanceCircle *ci;
1107 
1108  unsigned int n = 0;
1109  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1110  if ((*it)->isTracked()) {
1111  l = *it;
1113  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1114  for (unsigned int j = 0; j < 6; j++) {
1115  L[n + i][j] = l->L[i][j];
1116  error[n + i] = l->error[i];
1117  }
1118  }
1119  n += l->nbFeatureTotal;
1120  }
1121  }
1122 
1123  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1124  ++it) {
1125  if ((*it)->isTracked()) {
1126  cy = *it;
1127  cy->computeInteractionMatrixError(cMo, I);
1128  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1129  for (unsigned int j = 0; j < 6; j++) {
1130  L[n + i][j] = cy->L[i][j];
1131  error[n + i] = cy->error[i];
1132  }
1133  }
1134  n += cy->nbFeature;
1135  }
1136  }
1137  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1138  if ((*it)->isTracked()) {
1139  ci = *it;
1141  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1142  for (unsigned int j = 0; j < 6; j++) {
1143  L[n + i][j] = ci->L[i][j];
1144  error[n + i] = ci->error[i];
1145  }
1146  }
1147 
1148  n += ci->nbFeature;
1149  }
1150  }
1151 }
1152 
1159 {
1160  m_cam = cam;
1161 
1164 }
1165 
1173 {
1176 }
1184 {
1187 }
1188 
1199 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1200  int idFace, const std::string &name)
1201 {
1202  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1203 }
1204 
1215 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1216  const std::string &name)
1217 {
1218  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1219  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1220 }
1221 
1234  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1235  bool displayFullModel)
1236 {
1237  std::vector<std::vector<double> > models = vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1238 
1239  for (size_t i = 0; i < models.size(); i++) {
1240  if (vpMath::equal(models[i][0], 0)) {
1241  vpImagePoint ip1(models[i][1], models[i][2]);
1242  vpImagePoint ip2(models[i][3], models[i][4]);
1243  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1244  } else if (vpMath::equal(models[i][0], 1)) {
1245  vpImagePoint center(models[i][1], models[i][2]);
1246  double n20 = models[i][3];
1247  double n11 = models[i][4];
1248  double n02 = models[i][5];
1249  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1250  }
1251  }
1252 
1253  if (displayFeatures) {
1254  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1255  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1258 
1260  double id = m_featuresToBeDisplayedKlt[i][5];
1261  std::stringstream ss;
1262  ss << id;
1263  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1264  }
1265  }
1266  }
1267 
1268 #ifdef VISP_HAVE_OGRE
1269  if (useOgre)
1270  faces.displayOgre(cMo);
1271 #endif
1272 }
1273 
1286  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1287  bool displayFullModel)
1288 {
1289  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1290 
1291  for (size_t i = 0; i < models.size(); i++) {
1292  if (vpMath::equal(models[i][0], 0)) {
1293  vpImagePoint ip1(models[i][1], models[i][2]);
1294  vpImagePoint ip2(models[i][3], models[i][4]);
1295  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1296  } else if (vpMath::equal(models[i][0], 1)) {
1297  vpImagePoint center(models[i][1], models[i][2]);
1298  double n20 = models[i][3];
1299  double n11 = models[i][4];
1300  double n02 = models[i][5];
1301  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1302  }
1303  }
1304 
1305  if (displayFeatures) {
1306  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1307  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1310 
1312  double id = m_featuresToBeDisplayedKlt[i][5];
1313  std::stringstream ss;
1314  ss << id;
1315  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1316  }
1317  }
1318  }
1319 
1320 #ifdef VISP_HAVE_OGRE
1321  if (useOgre)
1322  faces.displayOgre(cMo);
1323 #endif
1324 }
1325 
1326 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1327  const vpHomogeneousMatrix &cMo,
1328  const vpCameraParameters &cam,
1329  bool displayFullModel)
1330 {
1331  std::vector<std::vector<double> > models;
1332 
1333  for (unsigned int i = 0; i < scales.size(); i += 1) {
1334  if (scales[i]) {
1335  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1336  ++it) {
1337  std::vector<std::vector<double> > currentModel =
1338  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1339  models.insert(models.end(), currentModel.begin(), currentModel.end());
1340  }
1341 
1342  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1343  it != cylinders[scaleLevel].end(); ++it) {
1344  std::vector<std::vector<double> > currentModel =
1345  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1346  models.insert(models.end(), currentModel.begin(), currentModel.end());
1347  }
1348 
1349  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1350  it != circles[scaleLevel].end(); ++it) {
1351  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1352  models.push_back(paramsCircle);
1353  }
1354 
1355  break; // displaying model on one scale only
1356  }
1357  }
1358 
1359 #ifdef VISP_HAVE_OGRE
1360  if (useOgre)
1361  faces.displayOgre(cMo);
1362 #endif
1363 
1364  return models;
1365 }
1366 
1379 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1380  const vpHomogeneousMatrix &cMo, bool verbose,
1381  const vpHomogeneousMatrix &T)
1382 {
1383  // Reinit klt
1384  #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1385  if (cur != NULL) {
1386  cvReleaseImage(&cur);
1387  cur = NULL;
1388  }
1389  #endif
1390 
1391  // delete the Klt Polygon features
1392  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1393  vpMbtDistanceKltPoints *kltpoly = *it;
1394  if (kltpoly != NULL) {
1395  delete kltpoly;
1396  }
1397  kltpoly = NULL;
1398  }
1399  kltPolygons.clear();
1400 
1401  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1402  ++it) {
1403  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1404  if (kltPolyCylinder != NULL) {
1405  delete kltPolyCylinder;
1406  }
1407  kltPolyCylinder = NULL;
1408  }
1409  kltCylinders.clear();
1410 
1411  // delete the structures used to display circles
1412  vpMbtDistanceCircle *ci;
1413  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1414  ci = *it;
1415  if (ci != NULL) {
1416  delete ci;
1417  }
1418  ci = NULL;
1419  }
1420 
1421  circles_disp.clear();
1422 
1423  firstInitialisation = true;
1424 
1425  // Reinit edge
1426  vpMbtDistanceLine *l;
1428 
1429  for (unsigned int i = 0; i < scales.size(); i += 1) {
1430  if (scales[i]) {
1431  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1432  l = *it;
1433  if (l != NULL)
1434  delete l;
1435  l = NULL;
1436  }
1437 
1438  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1439  ++it) {
1440  cy = *it;
1441  if (cy != NULL)
1442  delete cy;
1443  cy = NULL;
1444  }
1445 
1446  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1447  ci = *it;
1448  if (ci != NULL)
1449  delete ci;
1450  ci = NULL;
1451  }
1452 
1453  lines[i].clear();
1454  cylinders[i].clear();
1455  circles[i].clear();
1456  }
1457  }
1458 
1459  // compute_interaction=1;
1460  nline = 0;
1461  ncylinder = 0;
1462  ncircle = 0;
1463  // lambda = 1;
1464  nbvisiblepolygone = 0;
1465 
1466  // Reinit common parts
1467  faces.reset();
1468 
1469  loadModel(cad_name, verbose, T);
1470 
1471  m_cMo = cMo;
1472  init(I);
1473 }
1474 
1475 #elif !defined(VISP_BUILD_SHARED_LIBS)
1476 // Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1477 // no symbols
1478 void dummy_vpMbEdgeKltTracker(){};
1479 #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:304
unsigned int getRows() const
Definition: vpArray2D.h:289
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:310
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:88
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:110
static double sqr(double x)
Definition: vpMath.h:116
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static double deg(double rad)
Definition: vpMath.h:103
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