Visual Servoing Platform  version 3.3.1 under development (2020-12-02)
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 
276 void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile)
277 {
278  // Load projection error config
279  vpMbTracker::loadConfigFile(configFile);
280 
285 
286  xmlp.setEdgeMe(me);
287 
288  xmlp.setKltMaxFeatures(10000);
289  xmlp.setKltWindowSize(5);
290  xmlp.setKltQuality(0.01);
291  xmlp.setKltMinDistance(5);
292  xmlp.setKltHarrisParam(0.01);
293  xmlp.setKltBlockSize(3);
294  xmlp.setKltPyramidLevels(3);
296 
297  try {
298  std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
299  xmlp.parse(configFile.c_str());
300  } catch (...) {
301  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
302  throw vpException(vpException::ioError, "problem to parse configuration file.");
303  }
304 
305  vpCameraParameters camera;
306  xmlp.getCameraParameters(camera);
307  setCameraParameters(camera);
308 
311 
312  if (xmlp.hasNearClippingDistance())
314 
315  if (xmlp.hasFarClippingDistance())
317 
318  if (xmlp.getFovClipping()) {
320  }
321 
322  useLodGeneral = xmlp.getLodState();
325 
326  applyLodSettingInConfig = false;
327  if (this->getNbPolygon() > 0) {
332  }
333 
334  vpMe meParser;
335  xmlp.getEdgeMe(meParser);
337 
343  tracker.setBlockSize((int)xmlp.getKltBlockSize());
345  maskBorder = xmlp.getKltMaskBorder();
346 
347  // if(useScanLine)
348  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
349 }
350 
355  unsigned int lvl)
356 {
357  postTrackingMbt(w_mbt, lvl);
358 
359  if (displayFeatures) {
360  if (lvl == 0) {
361  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
362  vpMbtDistanceLine *l = *it;
363  if (l->isVisible() && l->isTracked()) {
364  l->displayMovingEdges(I);
365  }
366  }
367 
368  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
369  ++it) {
370  vpMbtDistanceCylinder *cy = *it;
371  // A cylinder is always visible: #FIXME AY: Still valid?
372  if (cy->isTracked())
373  cy->displayMovingEdges(I);
374  }
375 
376  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
377  vpMbtDistanceCircle *ci = *it;
378  if (ci->isVisible() && ci->isTracked()) {
379  ci->displayMovingEdges(I);
380  }
381  }
382  }
383  }
384 
385  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
386 
387  if (useScanLine) {
391  }
392 
394 
397 
398  if (computeProjError)
400 
401  if (reInit)
402  return true;
403 
404  return false;
405 }
406 
411  unsigned int lvl)
412 {
413  postTrackingMbt(w_mbt, lvl);
414 
415  if (displayFeatures) {
416  if (lvl == 0) {
417  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
418  vpMbtDistanceLine *l = *it;
419  if (l->isVisible() && l->isTracked()) {
420  l->displayMovingEdges(I_color);
421  }
422  }
423 
424  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
425  ++it) {
426  vpMbtDistanceCylinder *cy = *it;
427  // A cylinder is always visible: #FIXME AY: Still valid?
428  if (cy->isTracked())
429  cy->displayMovingEdges(m_I);
430  }
431 
432  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
433  vpMbtDistanceCircle *ci = *it;
434  if (ci->isVisible() && ci->isTracked()) {
435  ci->displayMovingEdges(m_I);
436  }
437  }
438  }
439  }
440 
441  bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
442 
443  if (useScanLine) {
447  }
448 
450 
453 
454  if (computeProjError)
456 
457  if (reInit)
458  return true;
459 
460  return false;
461 }
462 
474 {
475  if (lvl >= scales.size() || !scales[lvl]) {
476  throw vpException(vpException::dimensionError, "_lvl not used.");
477  }
478 
479  unsigned int n = 0;
481  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
482  if ((*it)->isTracked()) {
483  l = *it;
484  unsigned int indexLine = 0;
485  double wmean = 0;
486 
487  for (size_t a = 0; a < l->meline.size(); a++) {
488  std::list<vpMeSite>::iterator itListLine;
489  if (l->nbFeature[a] > 0)
490  itListLine = l->meline[a]->getMeList().begin();
491 
492  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
493  wmean += w[n + indexLine];
494  vpMeSite p = *itListLine;
495  if (w[n + indexLine] < 0.5) {
497  *itListLine = p;
498  }
499 
500  ++itListLine;
501  indexLine++;
502  }
503  }
504 
505  n += l->nbFeatureTotal;
506 
507  if (l->nbFeatureTotal != 0)
508  wmean /= l->nbFeatureTotal;
509  else
510  wmean = 1;
511 
512  l->setMeanWeight(wmean);
513 
514  if (wmean < 0.8)
515  l->Reinit = true;
516  }
517  }
518 
519  // Same thing with cylinders as with lines
521  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
522  ++it) {
523  if ((*it)->isTracked()) {
524  cy = *it;
525  double wmean = 0;
526  std::list<vpMeSite>::iterator itListCyl1;
527  std::list<vpMeSite>::iterator itListCyl2;
528  if (cy->nbFeature > 0) {
529  itListCyl1 = cy->meline1->getMeList().begin();
530  itListCyl2 = cy->meline2->getMeList().begin();
531  }
532 
533  wmean = 0;
534  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
535  wmean += w[n + i];
536  vpMeSite p = *itListCyl1;
537  if (w[n + i] < 0.5) {
539 
540  *itListCyl1 = p;
541  }
542 
543  ++itListCyl1;
544  }
545 
546  if (cy->nbFeaturel1 != 0)
547  wmean /= cy->nbFeaturel1;
548  else
549  wmean = 1;
550 
551  cy->setMeanWeight1(wmean);
552 
553  if (wmean < 0.8) {
554  cy->Reinit = true;
555  }
556 
557  wmean = 0;
558  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
559  wmean += w[n + i];
560  vpMeSite p = *itListCyl2;
561  if (w[n + i] < 0.5) {
563 
564  *itListCyl2 = p;
565  }
566 
567  ++itListCyl2;
568  }
569 
570  if (cy->nbFeaturel2 != 0)
571  wmean /= cy->nbFeaturel2;
572  else
573  wmean = 1;
574 
575  cy->setMeanWeight2(wmean);
576 
577  if (wmean < 0.8) {
578  cy->Reinit = true;
579  }
580 
581  n += cy->nbFeature;
582  }
583  }
584 
585  // Same thing with circles as with lines
587  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
588  if ((*it)->isTracked()) {
589  ci = *it;
590  double wmean = 0;
591  std::list<vpMeSite>::iterator itListCir;
592 
593  if (ci->nbFeature > 0) {
594  itListCir = ci->meEllipse->getMeList().begin();
595  }
596 
597  wmean = 0;
598  for (unsigned int i = 0; i < ci->nbFeature; i++) {
599  wmean += w[n + i];
600  vpMeSite p = *itListCir;
601  if (w[n + i] < 0.5) {
603 
604  *itListCir = p;
605  }
606 
607  ++itListCir;
608  }
609 
610  if (ci->nbFeature != 0)
611  wmean /= ci->nbFeature;
612  else
613  wmean = 1;
614 
615  ci->setMeanWeight(wmean);
616 
617  if (wmean < 0.8) {
618  ci->Reinit = true;
619  }
620 
621  n += ci->nbFeature;
622  }
623  }
624 }
625 
634 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
635  unsigned int lvl)
636 {
637  vpColVector factor;
638  nbrow = trackFirstLoop(I, factor, lvl);
639 
640  if (nbrow < 4 && nbInfos < 4) {
641  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
642  } else if (nbrow < 4)
643  nbrow = 0;
644 
645  unsigned int totalNbRows = nbrow + 2 * nbInfos;
646  double residu = 0;
647  double residu_1 = -1;
648  unsigned int iter = 0;
649 
650  vpMatrix L(totalNbRows, 6);
651  vpMatrix L_mbt, L_klt; // interaction matrix
652  vpColVector weighted_error(totalNbRows);
653  vpColVector R_mbt, R_klt; // residu
654  vpMatrix L_true;
655  vpMatrix LVJ_true;
656 
657  if (nbrow != 0) {
658  L_mbt.resize(nbrow, 6, false, false);
659  R_mbt.resize(nbrow, false);
660  }
661 
662  if (nbInfos != 0) {
663  L_klt.resize(2 * nbInfos, 6, false, false);
664  R_klt.resize(2 * nbInfos, false);
665  }
666 
667  vpColVector v; // "speed" for VVS
668  vpRobust robust_mbt, robust_klt;
669  vpHomography H;
670 
671  vpMatrix LTL;
672  vpColVector LTR;
673 
674  double factorMBT; // = 1.0;
675  double factorKLT; // = 1.0;
676 
677  // More efficient weight repartition for hybrid tracker should come soon...
678  // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
679  // factorKLT = 1.0 - factorMBT;
680  factorMBT = 0.35;
681  factorKLT = 0.65;
682 
683  if (nbrow < 4)
684  factorKLT = 1.;
685  if (nbInfos < 4)
686  factorMBT = 1.;
687 
688  double residuMBT = 0;
689  double residuKLT = 0;
690 
691  vpHomogeneousMatrix cMoPrev;
692  vpHomogeneousMatrix ctTc0_Prev;
693  vpColVector m_error_prev;
694  vpColVector m_w_prev;
695 
696  // Init size
697  m_error_hybrid.resize(totalNbRows, false);
698  m_w_hybrid.resize(totalNbRows, false);
699 
700  if (nbrow != 0) {
701  m_w_mbt.resize(nbrow, false);
702  m_w_mbt = 1; // needed in vpRobust::psiTukey()
703  }
704 
705  if (nbInfos != 0) {
706  m_w_klt.resize(2 * nbInfos, false);
707  m_w_klt = 1; // needed in vpRobust::psiTukey()
708  }
709 
710  double mu = m_initialMu;
711 
712  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
713  if (nbrow >= 4)
714  trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
715 
716  if (nbInfos >= 4) {
717  unsigned int shift = 0;
718 
719  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
720  it != vpMbKltTracker::kltPolygons.end(); ++it) {
721  vpMbtDistanceKltPoints *kltpoly = *it;
722  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
723  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
724  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
725  kltpoly->computeHomography(ctTc0, H);
726  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
727  shift += 2 * kltpoly->getCurrentNumberPoints();
728  }
729  }
730 
731  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
732  ++it) {
733  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
734 
735  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
736  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
737  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
738  try {
739  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
740  } catch (...) {
741  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
742  }
743 
744  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
745  }
746  }
747  }
748 
749  /* residuals */
750  if (nbrow > 3) {
751  m_error_hybrid.insert(0, R_mbt);
752  }
753 
754  if (nbInfos > 3) {
755  m_error_hybrid.insert(nbrow, R_klt);
756  }
757 
758  unsigned int cpt = 0;
759  while (cpt < (nbrow + 2 * nbInfos)) {
760  if (cpt < (unsigned)nbrow) {
761  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
762  } else {
763  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
764  }
765  cpt++;
766  }
767 
768  bool reStartFromLastIncrement = false;
769  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
770  &m_w_prev);
771  if (reStartFromLastIncrement) {
772  ctTc0 = ctTc0_Prev;
773  }
774 
775  if (!reStartFromLastIncrement) {
776  /* robust */
777  if (nbrow > 3) {
778  residuMBT = 0;
779  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
780  residuMBT += fabs(R_mbt[i]);
781  residuMBT /= R_mbt.getRows();
782 
784  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
785 
786  L.insert(L_mbt, 0, 0);
787  }
788 
789  if (nbInfos > 3) {
790  residuKLT = 0;
791  for (unsigned int i = 0; i < R_klt.getRows(); i++)
792  residuKLT += fabs(R_klt[i]);
793  residuKLT /= R_klt.getRows();
794 
796  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
797 
798  L.insert(L_klt, nbrow, 0);
799  }
800 
801  cpt = 0;
802  while (cpt < (nbrow + 2 * nbInfos)) {
803  if (cpt < (unsigned)nbrow) {
804  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
805  } else {
806  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
807  }
808  cpt++;
809  }
810 
811  if (computeCovariance) {
812  L_true = L;
813  if (!isoJoIdentity) {
815  cVo.buildFrom(m_cMo);
816  LVJ_true = (L * cVo * oJo);
817  }
818  }
819 
820  residu_1 = residu;
821  residu = 0;
822  double num = 0;
823  double den = 0;
824 
825  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
826  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
827  den += m_w_hybrid[i];
828 
829  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
830  if (m_computeInteraction) {
831  for (unsigned int j = 0; j < 6; j += 1) {
832  L[i][j] *= m_w_hybrid[i];
833  }
834  }
835  }
836 
837  residu = sqrt(num / den);
838 
839  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
840  &m_w_hybrid, &m_w_prev);
841 
842  cMoPrev = m_cMo;
843  ctTc0_Prev = ctTc0;
845  m_cMo = ctTc0 * c0Mo;
846  }
847 
848  iter++;
849  }
850 
852 }
853 
855 {
856  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
857 }
858 
860 {
861  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
862  "computeVVSInteractionMatrixAndR"
863  "esidu() should not be called!");
864 }
865 
874 {
875  try {
877  } catch (...) {
878  }
879 
880  if (m_nbInfos >= 4) {
881  unsigned int old_maxIter = m_maxIter;
884  m_maxIter = old_maxIter;
885  } else {
886  m_nbInfos = 0;
887  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
888  }
889 
891 
892  unsigned int nbrow = 0;
893  computeVVS(I, m_nbInfos, nbrow);
894 
895  if (postTracking(I, m_w_mbt, m_w_klt)) {
897 
898  // AY : Removed as edge tracked, if necessary, is reinitialized in
899  // postTracking()
900 
901  // initPyramid(I, Ipyramid);
902 
903  // unsigned int i = (unsigned int)scales.size();
904  // do {
905  // i--;
906  // if(scales[i]){
907  // downScale(i);
908  // initMovingEdge(*Ipyramid[i], cMo);
909  // upScale(i);
910  // }
911  // } while(i != 0);
912 
913  // cleanPyramid(Ipyramid);
914  }
915 
916  if (displayFeatures) {
918  }
919 }
920 
929 {
930  vpImageConvert::convert(I_color, m_I);
931  try {
933  } catch (...) {
934  }
935 
936  if (m_nbInfos >= 4) {
937  unsigned int old_maxIter = m_maxIter;
940  m_maxIter = old_maxIter;
941  } else {
942  m_nbInfos = 0;
943  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
944  }
945 
947 
948  unsigned int nbrow = 0;
949  computeVVS(m_I, m_nbInfos, nbrow);
950 
951  if (postTracking(I_color, m_w_mbt, m_w_klt)) {
953 
954  // AY : Removed as edge tracked, if necessary, is reinitialized in
955  // postTracking()
956 
957  // initPyramid(I, Ipyramid);
958 
959  // unsigned int i = (unsigned int)scales.size();
960  // do {
961  // i--;
962  // if(scales[i]){
963  // downScale(i);
964  // initMovingEdge(*Ipyramid[i], cMo);
965  // upScale(i);
966  // }
967  // } while(i != 0);
968 
969  // cleanPyramid(Ipyramid);
970  }
971 
972  if (displayFeatures) {
974  }
975 }
976 
978  unsigned int lvl)
979 {
983 
984  if (lvl >= scales.size() || !scales[lvl]) {
985  throw vpException(vpException::dimensionError, "_lvl not used.");
986  }
987 
988  unsigned int nbrow = initMbtTracking(lvl);
989 
990  if (nbrow == 0) {
991  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
992  // "Error: not enough features in the interaction matrix...");
993  return nbrow;
994  }
995 
996  factor.resize(nbrow, false);
997  factor = 1;
998 
999  unsigned int n = 0;
1000  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1001  if ((*it)->isTracked()) {
1002  l = *it;
1004 
1005  double fac = 1;
1006  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1007  ++itindex) {
1008  int index = *itindex;
1009  if (l->hiddenface->isAppearing((unsigned int)index)) {
1010  fac = 0.2;
1011  break;
1012  }
1013  if (l->closeToImageBorder(I, 10)) {
1014  fac = 0.1;
1015  break;
1016  }
1017  }
1018 
1019  unsigned int indexFeature = 0;
1020  for (size_t a = 0; a < l->meline.size(); a++) {
1021  std::list<vpMeSite>::const_iterator itListLine;
1022  if (l->meline[a] != NULL) {
1023  itListLine = l->meline[a]->getMeList().begin();
1024 
1025  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1026  factor[n + i] = fac;
1027  vpMeSite site = *itListLine;
1028  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1029  factor[n + i] = 0.2;
1030  ++itListLine;
1031  indexFeature++;
1032  }
1033  n += l->nbFeature[a];
1034  }
1035  }
1036  }
1037  }
1038 
1039  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1040  ++it) {
1041  if ((*it)->isTracked()) {
1042  cy = *it;
1044  double fac = 1.0;
1045 
1046  std::list<vpMeSite>::const_iterator itCyl1;
1047  std::list<vpMeSite>::const_iterator itCyl2;
1048  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1049  itCyl1 = cy->meline1->getMeList().begin();
1050  itCyl2 = cy->meline2->getMeList().begin();
1051  }
1052 
1053  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1054  factor[n + i] = fac;
1055  vpMeSite site;
1056  if (i < cy->nbFeaturel1) {
1057  site = *itCyl1;
1058  ++itCyl1;
1059  } else {
1060  site = *itCyl2;
1061  ++itCyl2;
1062  }
1063  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1064  factor[n + i] = 0.2;
1065  }
1066 
1067  n += cy->nbFeature;
1068  }
1069  }
1070 
1071  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1072  if ((*it)->isTracked()) {
1073  ci = *it;
1075  double fac = 1.0;
1076 
1077  std::list<vpMeSite>::const_iterator itCir;
1078  if (ci->meEllipse != NULL) {
1079  itCir = ci->meEllipse->getMeList().begin();
1080  }
1081 
1082  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1083  factor[n + i] = fac;
1084  vpMeSite site = *itCir;
1085  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1086  factor[n + i] = 0.2;
1087  ++itCir;
1088  }
1089 
1090  n += ci->nbFeature;
1091  }
1092  }
1093 
1094  return nbrow;
1095 }
1096 
1098  const vpHomogeneousMatrix &cMo, unsigned int lvl)
1099 {
1100  vpMbtDistanceLine *l;
1102  vpMbtDistanceCircle *ci;
1103 
1104  unsigned int n = 0;
1105  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1106  if ((*it)->isTracked()) {
1107  l = *it;
1109  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1110  for (unsigned int j = 0; j < 6; j++) {
1111  L[n + i][j] = l->L[i][j];
1112  error[n + i] = l->error[i];
1113  }
1114  }
1115  n += l->nbFeatureTotal;
1116  }
1117  }
1118 
1119  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1120  ++it) {
1121  if ((*it)->isTracked()) {
1122  cy = *it;
1123  cy->computeInteractionMatrixError(cMo, I);
1124  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1125  for (unsigned int j = 0; j < 6; j++) {
1126  L[n + i][j] = cy->L[i][j];
1127  error[n + i] = cy->error[i];
1128  }
1129  }
1130  n += cy->nbFeature;
1131  }
1132  }
1133  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1134  if ((*it)->isTracked()) {
1135  ci = *it;
1137  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1138  for (unsigned int j = 0; j < 6; j++) {
1139  L[n + i][j] = ci->L[i][j];
1140  error[n + i] = ci->error[i];
1141  }
1142  }
1143 
1144  n += ci->nbFeature;
1145  }
1146  }
1147 }
1148 
1155 {
1156  m_cam = cam;
1157 
1160 }
1161 
1169 {
1172 }
1180 {
1183 }
1184 
1195 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1196  int idFace, const std::string &name)
1197 {
1198  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1199 }
1200 
1211 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1212  const std::string &name)
1213 {
1214  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1215  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1216 }
1217 
1230  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1231  bool displayFullModel)
1232 {
1233  std::vector<std::vector<double> > models = vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1234 
1235  for (size_t i = 0; i < models.size(); i++) {
1236  if (vpMath::equal(models[i][0], 0)) {
1237  vpImagePoint ip1(models[i][1], models[i][2]);
1238  vpImagePoint ip2(models[i][3], models[i][4]);
1239  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1240  } else if (vpMath::equal(models[i][0], 1)) {
1241  vpImagePoint center(models[i][1], models[i][2]);
1242  double n20 = models[i][3];
1243  double n11 = models[i][4];
1244  double n02 = models[i][5];
1245  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1246  }
1247  }
1248 
1249  if (displayFeatures) {
1250  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1251  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1254 
1256  double id = m_featuresToBeDisplayedKlt[i][5];
1257  std::stringstream ss;
1258  ss << id;
1259  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1260  }
1261  }
1262  }
1263 
1264 #ifdef VISP_HAVE_OGRE
1265  if (useOgre)
1266  faces.displayOgre(cMo);
1267 #endif
1268 }
1269 
1282  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1283  bool displayFullModel)
1284 {
1285  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1286 
1287  for (size_t i = 0; i < models.size(); i++) {
1288  if (vpMath::equal(models[i][0], 0)) {
1289  vpImagePoint ip1(models[i][1], models[i][2]);
1290  vpImagePoint ip2(models[i][3], models[i][4]);
1291  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1292  } else if (vpMath::equal(models[i][0], 1)) {
1293  vpImagePoint center(models[i][1], models[i][2]);
1294  double n20 = models[i][3];
1295  double n11 = models[i][4];
1296  double n02 = models[i][5];
1297  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1298  }
1299  }
1300 
1301  if (displayFeatures) {
1302  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1303  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1306 
1308  double id = m_featuresToBeDisplayedKlt[i][5];
1309  std::stringstream ss;
1310  ss << id;
1311  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1312  }
1313  }
1314  }
1315 
1316 #ifdef VISP_HAVE_OGRE
1317  if (useOgre)
1318  faces.displayOgre(cMo);
1319 #endif
1320 }
1321 
1322 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1323  const vpHomogeneousMatrix &cMo,
1324  const vpCameraParameters &cam,
1325  bool displayFullModel)
1326 {
1327  std::vector<std::vector<double> > models;
1328 
1329  for (unsigned int i = 0; i < scales.size(); i += 1) {
1330  if (scales[i]) {
1331  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1332  ++it) {
1333  std::vector<std::vector<double> > currentModel =
1334  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1335  models.insert(models.end(), currentModel.begin(), currentModel.end());
1336  }
1337 
1338  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1339  it != cylinders[scaleLevel].end(); ++it) {
1340  std::vector<std::vector<double> > currentModel =
1341  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1342  models.insert(models.end(), currentModel.begin(), currentModel.end());
1343  }
1344 
1345  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1346  it != circles[scaleLevel].end(); ++it) {
1347  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1348  models.push_back(paramsCircle);
1349  }
1350 
1351  break; // displaying model on one scale only
1352  }
1353  }
1354 
1355 #ifdef VISP_HAVE_OGRE
1356  if (useOgre)
1357  faces.displayOgre(cMo);
1358 #endif
1359 
1360  return models;
1361 }
1362 
1375 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1376  const vpHomogeneousMatrix &cMo, bool verbose,
1377  const vpHomogeneousMatrix &T)
1378 {
1379  // Reinit klt
1380  #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1381  if (cur != NULL) {
1382  cvReleaseImage(&cur);
1383  cur = NULL;
1384  }
1385  #endif
1386 
1387  // delete the Klt Polygon features
1388  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1389  vpMbtDistanceKltPoints *kltpoly = *it;
1390  if (kltpoly != NULL) {
1391  delete kltpoly;
1392  }
1393  kltpoly = NULL;
1394  }
1395  kltPolygons.clear();
1396 
1397  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1398  ++it) {
1399  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1400  if (kltPolyCylinder != NULL) {
1401  delete kltPolyCylinder;
1402  }
1403  kltPolyCylinder = NULL;
1404  }
1405  kltCylinders.clear();
1406 
1407  // delete the structures used to display circles
1408  vpMbtDistanceCircle *ci;
1409  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1410  ci = *it;
1411  if (ci != NULL) {
1412  delete ci;
1413  }
1414  ci = NULL;
1415  }
1416 
1417  circles_disp.clear();
1418 
1419  firstInitialisation = true;
1420 
1421  // Reinit edge
1422  vpMbtDistanceLine *l;
1424 
1425  for (unsigned int i = 0; i < scales.size(); i += 1) {
1426  if (scales[i]) {
1427  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1428  l = *it;
1429  if (l != NULL)
1430  delete l;
1431  l = NULL;
1432  }
1433 
1434  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1435  ++it) {
1436  cy = *it;
1437  if (cy != NULL)
1438  delete cy;
1439  cy = NULL;
1440  }
1441 
1442  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1443  ci = *it;
1444  if (ci != NULL)
1445  delete ci;
1446  ci = NULL;
1447  }
1448 
1449  lines[i].clear();
1450  cylinders[i].clear();
1451  circles[i].clear();
1452  }
1453  }
1454 
1455  // compute_interaction=1;
1456  nline = 0;
1457  ncylinder = 0;
1458  ncircle = 0;
1459  // lambda = 1;
1460  nbvisiblepolygone = 0;
1461 
1462  // Reinit common parts
1463  faces.reset();
1464 
1465  loadModel(cad_name, verbose, T);
1466 
1467  m_cMo = cMo;
1468  init(I);
1469 }
1470 
1471 #elif !defined(VISP_BUILD_SHARED_LIBS)
1472 // Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1473 // no symbols
1474 void dummy_vpMbEdgeKltTracker(){};
1475 #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:156
double m_thresholdMBT
The threshold used in the robust estimation of MBT.
void displayMovingEdges(const vpImage< unsigned char > &I)
void setMovingEdge(const vpMe &me)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
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
virtual void loadConfigFile(const std::string &configFile)
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
virtual void loadConfigFile(const std::string &configFile)
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:300
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="")
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.
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:366
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)
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:6204
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)