Visual Servoing Platform  version 3.4.1 under development (2021-09-21)
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
bool m_computeInteraction
Definition: vpMbTracker.h:185
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
bool computeProjError
Definition: vpMbTracker.h:133
unsigned int ncylinder
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
double m_thresholdMBT
The threshold used in the robust estimation of MBT.
void displayMovingEdges(const vpImage< unsigned char > &I)
void setMovingEdge(const vpMe &me)
void setMaxFeatures(int maxCount)
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
void postTrackingMbt(vpColVector &w, unsigned int level=0)
vpMeSiteState getState() const
Definition: vpMeSite.h:190
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int nbFeature
The number of moving edges.
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
unsigned int nbFeatureTotal
The number of moving edges.
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)
void upScale(const unsigned int _scale)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setHarrisFreeParameter(double harris_k)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setKltQuality(const double &q)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
bool Reinit
Indicates if the line has to be reinitialized.
void getCameraParameters(vpCameraParameters &cam) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setKltPyramidLevels(const unsigned int &pL)
unsigned int initMbtTracking(unsigned int level=0)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
vpColVector m_w_mbt
Robust weights for Edge.
unsigned int scaleLevel
std::list< int > Lindex_polygon
Index of the faces which contain the line.
vpMatrix L
The interaction matrix.
bool isTracked() const
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
#define vpERROR_TRACE
Definition: vpDebug.h:393
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:293
std::list< vpMbtDistanceKltCylinder * > kltCylinders
unsigned int m_maxIterKlt
The maximum iteration of the virtual visual servoing stage.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int ncircle
std::vector< const vpImage< unsigned char > * > Ipyramid
vpColVector error
The error vector.
void setMinDistance(double minDistance)
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
vpMbScanLine & getMbScanLineRenderer()
unsigned int getRows() const
Definition: vpArray2D.h:289
Definition: vpMe.h:60
vpHomogeneousMatrix inverse() const
Manage the line of a polygon used in the model-based tracker.
unsigned int nbFeature
The number of moving edges.
virtual void setClipping(const unsigned int &flags)
vpColVector m_w_hybrid
Robust weights.
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
unsigned int getKltBlockSize() const
void setKltMaskBorder(const unsigned int &mb)
void setEdgeMe(const vpMe &ecm)
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:62
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
vpMe me
The moving edges parameters.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
void updateMovingEdge(const vpImage< unsigned char > &I)
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setLod(bool useLod, const std::string &name="")
virtual void setFarClippingDistance(const double &dist)
void downScale(const unsigned int _scale)
virtual void initCylinder(const vpPoint &, const vpPoint &, double r, int idFace, const std::string &name="")
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
vpMatrix L
The interaction matrix.
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void setQuality(double qualityLevel)
virtual void init(const vpImage< unsigned char > &I)
vpHomogeneousMatrix ctTc0
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
unsigned int nbFeaturel1
The number of moving edges on line 1.
void trackSecondLoop(const vpImage< unsigned char > &I, vpMatrix &L, vpColVector &_error, const vpHomogeneousMatrix &cMo, unsigned int lvl=0)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
virtual void computeVVSInit()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
bool Reinit
Indicates if the circle has to be reinitialized.
Error that can be emited by the vpTracker class and its derivates.
Point used by the tracker.
Definition: vpMeSite.h:78
void setMeanWeight(double w_mean)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setAngleDisappear(const double &adisappear)
bool isAppearing(unsigned int i)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setKltMinDistance(const double &mD)
static double sqr(double x)
Definition: vpMath.h:116
void setPyramidLevels(int pyrMaxLevel)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
void setMeanWeight1(double wmean)
vpColVector error
The error vector.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void track(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
Generic class defining intrinsic camera parameters.
unsigned int getKltWindowSize() const
void cleanPyramid(std::vector< const vpImage< unsigned char > *> &_pyramid)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
double m_thresholdKLT
The threshold used in the robust estimation of KLT.
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)
unsigned int getKltMaxFeatures() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void setMeanWeight(double _wmean)
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setWindowSize(int winSize)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
void setMeanWeight2(double wmean)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
vpKltOpencv tracker
Points tracker.
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
void setKltWindowSize(const unsigned int &w)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
unsigned int getCurrentNumberPoints() const
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:110
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
virtual void setCameraParameters(const vpCameraParameters &cam)
void computeProjectionError(const vpImage< unsigned char > &_I)
void insert(unsigned int i, const vpColVector &v)
cv::Mat cur
Temporary OpenCV image for fast conversion.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w_mbt, vpColVector &w_klt, unsigned int lvl=0)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
unsigned int getCurrentNumberPoints() const
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void getEdgeMe(vpMe &ecm) const
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)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
bool isVisible() const
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:103
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
unsigned int getHeight() const
Definition: vpImage.h:188
void setCameraParameters(const vpCameraParameters &cam)
void preTracking(const vpImage< unsigned char > &I)
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpColVector m_error_hybrid
(s - s*)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
double getLodMinLineLengthThreshold() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
Tukey influence function.
Definition: vpRobust.h:93
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
unsigned int getKltMaskBorder() const
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
unsigned int m_nbInfos
bool Reinit
Indicates if the line has to be reinitialized.
virtual void init(const vpImage< unsigned char > &I)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
unsigned int trackFirstLoop(const vpImage< unsigned char > &I, vpColVector &factor, unsigned int lvl=0)
vpColVector m_w_klt
Robust weights for KLT.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
std::vector< vpMbtMeLine * > meline
The moving edge container.
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
unsigned int nline
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > *> &_pyramid)
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
unsigned int nbFeaturel2
The number of moving edges on line 2.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
void setKltBlockSize(const unsigned int &bs)
void setKltMaxFeatures(const unsigned int &mF)
void setCameraParameters(const vpCameraParameters &cam)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double r, int idFace=0, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:246
void setBlockSize(int blockSize)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
vpMatrix L
The interaction matrix.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
std::list< vpMbtDistanceKltPoints * > kltPolygons
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
vpHomogeneousMatrix c0Mo
Initial pose.
double getLodMinPolygonAreaThreshold() const
virtual void setNearClippingDistance(const double &dist)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
unsigned int getKltPyramidLevels() const
void computeFov(const unsigned int &w, const unsigned int &h)