Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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  : thresholdKLT(2.), thresholdMBT(2.), m_maxIterKlt(30), w_mbt(), 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) {
112  cam.computeFov(I.getWidth(), I.getHeight());
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(const 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 
281 #ifdef VISP_HAVE_PUGIXML
283 
284  xmlp.setCameraParameters(cam);
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  std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
301  xmlp.parse(configFile.c_str());
302  } catch (...) {
303  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
304  throw vpException(vpException::ioError, "problem to parse configuration file.");
305  }
306 
307  vpCameraParameters camera;
308  xmlp.getCameraParameters(camera);
309  setCameraParameters(camera);
310 
313 
314  if (xmlp.hasNearClippingDistance())
316 
317  if (xmlp.hasFarClippingDistance())
319 
320  if (xmlp.getFovClipping()) {
322  }
323 
324  useLodGeneral = xmlp.getLodState();
327 
328  applyLodSettingInConfig = false;
329  if (this->getNbPolygon() > 0) {
334  }
335 
336  vpMe meParser;
337  xmlp.getEdgeMe(meParser);
339 
345  tracker.setBlockSize((int)xmlp.getKltBlockSize());
347  maskBorder = xmlp.getKltMaskBorder();
348 
349  // if(useScanLine)
350  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
351 
352 #else
353  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
354 #endif
355 }
356 
361  const unsigned int lvl)
362 {
363  postTrackingMbt(w_mbt, lvl);
364 
365  if (displayFeatures) {
366  if (lvl == 0) {
367  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
368  vpMbtDistanceLine *l = *it;
369  if (l->isVisible() && l->isTracked()) {
370  l->displayMovingEdges(I);
371  }
372  }
373 
374  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
375  ++it) {
376  vpMbtDistanceCylinder *cy = *it;
377  // A cylinder is always visible: #FIXME AY: Still valid?
378  if (cy->isTracked())
379  cy->displayMovingEdges(I);
380  }
381 
382  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
383  vpMbtDistanceCircle *ci = *it;
384  if (ci->isVisible() && ci->isTracked()) {
385  ci->displayMovingEdges(I);
386  }
387  }
388  }
389  }
390 
391  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
392 
393  if (useScanLine) {
394  cam.computeFov(I.getWidth(), I.getHeight());
397  }
398 
400 
403 
404  if (computeProjError)
406 
407  if (reInit)
408  return true;
409 
410  return false;
411 }
412 
417  const unsigned int lvl)
418 {
419  postTrackingMbt(w_mbt, lvl);
420 
421  if (displayFeatures) {
422  if (lvl == 0) {
423  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
424  vpMbtDistanceLine *l = *it;
425  if (l->isVisible() && l->isTracked()) {
426  l->displayMovingEdges(I_color);
427  }
428  }
429 
430  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
431  ++it) {
432  vpMbtDistanceCylinder *cy = *it;
433  // A cylinder is always visible: #FIXME AY: Still valid?
434  if (cy->isTracked())
435  cy->displayMovingEdges(m_I);
436  }
437 
438  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
439  vpMbtDistanceCircle *ci = *it;
440  if (ci->isVisible() && ci->isTracked()) {
441  ci->displayMovingEdges(m_I);
442  }
443  }
444  }
445  }
446 
447  bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
448 
449  if (useScanLine) {
453  }
454 
456 
459 
460  if (computeProjError)
462 
463  if (reInit)
464  return true;
465 
466  return false;
467 }
468 
479 void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
480 {
481  if (lvl >= scales.size() || !scales[lvl]) {
482  throw vpException(vpException::dimensionError, "_lvl not used.");
483  }
484 
485  unsigned int n = 0;
487  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
488  if ((*it)->isTracked()) {
489  l = *it;
490  unsigned int indexLine = 0;
491  double wmean = 0;
492 
493  for (size_t a = 0; a < l->meline.size(); a++) {
494  std::list<vpMeSite>::iterator itListLine;
495  if (l->nbFeature[a] > 0)
496  itListLine = l->meline[a]->getMeList().begin();
497 
498  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
499  wmean += w[n + indexLine];
500  vpMeSite p = *itListLine;
501  if (w[n + indexLine] < 0.5) {
503  *itListLine = p;
504  }
505 
506  ++itListLine;
507  indexLine++;
508  }
509  }
510 
511  n += l->nbFeatureTotal;
512 
513  if (l->nbFeatureTotal != 0)
514  wmean /= l->nbFeatureTotal;
515  else
516  wmean = 1;
517 
518  l->setMeanWeight(wmean);
519 
520  if (wmean < 0.8)
521  l->Reinit = true;
522  }
523  }
524 
525  // Same thing with cylinders as with lines
527  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
528  ++it) {
529  if ((*it)->isTracked()) {
530  cy = *it;
531  double wmean = 0;
532  std::list<vpMeSite>::iterator itListCyl1;
533  std::list<vpMeSite>::iterator itListCyl2;
534  if (cy->nbFeature > 0) {
535  itListCyl1 = cy->meline1->getMeList().begin();
536  itListCyl2 = cy->meline2->getMeList().begin();
537  }
538 
539  wmean = 0;
540  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
541  wmean += w[n + i];
542  vpMeSite p = *itListCyl1;
543  if (w[n + i] < 0.5) {
545 
546  *itListCyl1 = p;
547  }
548 
549  ++itListCyl1;
550  }
551 
552  if (cy->nbFeaturel1 != 0)
553  wmean /= cy->nbFeaturel1;
554  else
555  wmean = 1;
556 
557  cy->setMeanWeight1(wmean);
558 
559  if (wmean < 0.8) {
560  cy->Reinit = true;
561  }
562 
563  wmean = 0;
564  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
565  wmean += w[n + i];
566  vpMeSite p = *itListCyl2;
567  if (w[n + i] < 0.5) {
569 
570  *itListCyl2 = p;
571  }
572 
573  ++itListCyl2;
574  }
575 
576  if (cy->nbFeaturel2 != 0)
577  wmean /= cy->nbFeaturel2;
578  else
579  wmean = 1;
580 
581  cy->setMeanWeight2(wmean);
582 
583  if (wmean < 0.8) {
584  cy->Reinit = true;
585  }
586 
587  n += cy->nbFeature;
588  }
589  }
590 
591  // Same thing with circles as with lines
593  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
594  if ((*it)->isTracked()) {
595  ci = *it;
596  double wmean = 0;
597  std::list<vpMeSite>::iterator itListCir;
598 
599  if (ci->nbFeature > 0) {
600  itListCir = ci->meEllipse->getMeList().begin();
601  }
602 
603  wmean = 0;
604  for (unsigned int i = 0; i < ci->nbFeature; i++) {
605  wmean += w[n + i];
606  vpMeSite p = *itListCir;
607  if (w[n + i] < 0.5) {
609 
610  *itListCir = p;
611  }
612 
613  ++itListCir;
614  }
615 
616  if (ci->nbFeature != 0)
617  wmean /= ci->nbFeature;
618  else
619  wmean = 1;
620 
621  ci->setMeanWeight(wmean);
622 
623  if (wmean < 0.8) {
624  ci->Reinit = true;
625  }
626 
627  n += ci->nbFeature;
628  }
629  }
630 }
631 
640 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
641  const unsigned int lvl)
642 {
643  vpColVector factor;
644  nbrow = trackFirstLoop(I, factor, lvl);
645 
646  if (nbrow < 4 && nbInfos < 4) {
647  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
648  } else if (nbrow < 4)
649  nbrow = 0;
650 
651  unsigned int totalNbRows = nbrow + 2 * nbInfos;
652  double residu = 0;
653  double residu_1 = -1;
654  unsigned int iter = 0;
655 
656  vpMatrix L(totalNbRows, 6);
657  vpMatrix L_mbt, L_klt; // interaction matrix
658  vpColVector weighted_error(totalNbRows);
659  vpColVector R_mbt, R_klt; // residu
660  vpMatrix L_true;
661  vpMatrix LVJ_true;
662 
663  if (nbrow != 0) {
664  L_mbt.resize(nbrow, 6, false, false);
665  R_mbt.resize(nbrow, false);
666  }
667 
668  if (nbInfos != 0) {
669  L_klt.resize(2 * nbInfos, 6, false, false);
670  R_klt.resize(2 * nbInfos, false);
671  }
672 
673  vpColVector v; // "speed" for VVS
674  vpRobust robust_mbt(0), robust_klt(0);
675  vpHomography H;
676 
677  vpMatrix LTL;
678  vpColVector LTR;
679 
680  double factorMBT; // = 1.0;
681  double factorKLT; // = 1.0;
682 
683  // More efficient weight repartition for hybrid tracker should come soon...
684  // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
685  // factorKLT = 1.0 - factorMBT;
686  factorMBT = 0.35;
687  factorKLT = 0.65;
688 
689  if (nbrow < 4)
690  factorKLT = 1.;
691  if (nbInfos < 4)
692  factorMBT = 1.;
693 
694  double residuMBT = 0;
695  double residuKLT = 0;
696 
697  vpHomogeneousMatrix cMoPrev;
698  vpHomogeneousMatrix ctTc0_Prev;
699  vpColVector m_error_prev;
700  vpColVector m_w_prev;
701 
702  // Init size
703  m_error_hybrid.resize(totalNbRows, false);
704  m_w_hybrid.resize(totalNbRows, false);
705 
706  if (nbrow != 0) {
707  w_mbt.resize(nbrow, false);
708  w_mbt = 1; // needed in vpRobust::psiTukey()
709  robust_mbt.resize(nbrow);
710  }
711 
712  if (nbInfos != 0) {
713  w_klt.resize(2 * nbInfos, false);
714  w_klt = 1; // needed in vpRobust::psiTukey()
715  robust_klt.resize(2 * nbInfos);
716  }
717 
718  double mu = m_initialMu;
719 
720  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
721  if (nbrow >= 4)
722  trackSecondLoop(I, L_mbt, R_mbt, cMo, lvl);
723 
724  if (nbInfos >= 4) {
725  unsigned int shift = 0;
726 
727  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
728  it != vpMbKltTracker::kltPolygons.end(); ++it) {
729  vpMbtDistanceKltPoints *kltpoly = *it;
730  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
731  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
732  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
733  kltpoly->computeHomography(ctTc0, H);
734  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
735  shift += 2 * kltpoly->getCurrentNumberPoints();
736  }
737  }
738 
739  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
740  ++it) {
741  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
742 
743  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
744  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
745  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
746  try {
747  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
748  } catch (...) {
749  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
750  }
751 
752  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
753  }
754  }
755  }
756 
757  /* residuals */
758  if (nbrow > 3) {
759  m_error_hybrid.insert(0, R_mbt);
760  }
761 
762  if (nbInfos > 3) {
763  m_error_hybrid.insert(nbrow, R_klt);
764  }
765 
766  unsigned int cpt = 0;
767  while (cpt < (nbrow + 2 * nbInfos)) {
768  if (cpt < (unsigned)nbrow) {
769  m_w_hybrid[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT);
770  } else {
771  m_w_hybrid[cpt] = (w_klt[cpt - nbrow] * factorKLT);
772  }
773  cpt++;
774  }
775 
776  bool reStartFromLastIncrement = false;
777  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
778  &m_w_prev);
779  if (reStartFromLastIncrement) {
780  ctTc0 = ctTc0_Prev;
781  }
782 
783  if (!reStartFromLastIncrement) {
784  /* robust */
785  if (nbrow > 3) {
786  residuMBT = 0;
787  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
788  residuMBT += fabs(R_mbt[i]);
789  residuMBT /= R_mbt.getRows();
790 
791  robust_mbt.setThreshold(thresholdMBT / cam.get_px());
792  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, w_mbt);
793 
794  L.insert(L_mbt, 0, 0);
795  }
796 
797  if (nbInfos > 3) {
798  residuKLT = 0;
799  for (unsigned int i = 0; i < R_klt.getRows(); i++)
800  residuKLT += fabs(R_klt[i]);
801  residuKLT /= R_klt.getRows();
802 
803  robust_klt.setThreshold(thresholdKLT / cam.get_px());
804  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, w_klt);
805 
806  L.insert(L_klt, nbrow, 0);
807  }
808 
809  unsigned int cpt = 0;
810  while (cpt < (nbrow + 2 * nbInfos)) {
811  if (cpt < (unsigned)nbrow) {
812  m_w_hybrid[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT);
813  } else {
814  m_w_hybrid[cpt] = (w_klt[cpt - nbrow] * factorKLT);
815  }
816  cpt++;
817  }
818 
819  if (computeCovariance) {
820  L_true = L;
821  if (!isoJoIdentity) {
823  cVo.buildFrom(cMo);
824  LVJ_true = (L * cVo * oJo);
825  }
826  }
827 
828  residu_1 = residu;
829  residu = 0;
830  double num = 0;
831  double den = 0;
832 
833  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
834  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
835  den += m_w_hybrid[i];
836 
837  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
838  if (m_computeInteraction) {
839  for (unsigned int j = 0; j < 6; j += 1) {
840  L[i][j] *= m_w_hybrid[i];
841  }
842  }
843  }
844 
845  residu = sqrt(num / den);
846 
847  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
848  &m_w_hybrid, &m_w_prev);
849 
850  cMoPrev = cMo;
851  ctTc0_Prev = ctTc0;
853  cMo = ctTc0 * c0Mo;
854  }
855 
856  iter++;
857  }
858 
860 }
861 
863 {
864  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
865 }
866 
868 {
869  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
870  "computeVVSInteractionMatrixAndR"
871  "esidu() should not be called!");
872 }
873 
882 {
883  try {
885  } catch (...) {
886  }
887 
888  if (m_nbInfos >= 4) {
889  unsigned int old_maxIter = m_maxIter;
892  m_maxIter = old_maxIter;
893  } else {
894  m_nbInfos = 0;
895  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
896  }
897 
899 
900  unsigned int nbrow = 0;
901  computeVVS(I, m_nbInfos, nbrow);
902 
903  if (postTracking(I, w_mbt, w_klt)) {
905 
906  // AY : Removed as edge tracked, if necessary, is reinitialized in
907  // postTracking()
908 
909  // initPyramid(I, Ipyramid);
910 
911  // unsigned int i = (unsigned int)scales.size();
912  // do {
913  // i--;
914  // if(scales[i]){
915  // downScale(i);
916  // initMovingEdge(*Ipyramid[i], cMo);
917  // upScale(i);
918  // }
919  // } while(i != 0);
920 
921  // cleanPyramid(Ipyramid);
922  }
923 
924  if (displayFeatures) {
926  }
927 }
928 
937 {
938  vpImageConvert::convert(I_color, m_I);
939  try {
941  } catch (...) {
942  }
943 
944  if (m_nbInfos >= 4) {
945  unsigned int old_maxIter = m_maxIter;
948  m_maxIter = old_maxIter;
949  } else {
950  m_nbInfos = 0;
951  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
952  }
953 
955 
956  unsigned int nbrow = 0;
957  computeVVS(m_I, m_nbInfos, nbrow);
958 
959  if (postTracking(I_color, w_mbt, w_klt)) {
961 
962  // AY : Removed as edge tracked, if necessary, is reinitialized in
963  // postTracking()
964 
965  // initPyramid(I, Ipyramid);
966 
967  // unsigned int i = (unsigned int)scales.size();
968  // do {
969  // i--;
970  // if(scales[i]){
971  // downScale(i);
972  // initMovingEdge(*Ipyramid[i], cMo);
973  // upScale(i);
974  // }
975  // } while(i != 0);
976 
977  // cleanPyramid(Ipyramid);
978  }
979 
980  if (displayFeatures) {
982  }
983 }
984 
986  const unsigned int lvl)
987 {
991 
992  if (lvl >= scales.size() || !scales[lvl]) {
993  throw vpException(vpException::dimensionError, "_lvl not used.");
994  }
995 
996  unsigned int nbrow = initMbtTracking(lvl);
997 
998  if (nbrow == 0) {
999  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
1000  // "Error: not enough features in the interaction matrix...");
1001  return nbrow;
1002  }
1003 
1004  factor.resize(nbrow, false);
1005  factor = 1;
1006 
1007  unsigned int n = 0;
1008  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1009  if ((*it)->isTracked()) {
1010  l = *it;
1012 
1013  double fac = 1;
1014  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1015  ++itindex) {
1016  int index = *itindex;
1017  if (l->hiddenface->isAppearing((unsigned int)index)) {
1018  fac = 0.2;
1019  break;
1020  }
1021  if (l->closeToImageBorder(I, 10)) {
1022  fac = 0.1;
1023  break;
1024  }
1025  }
1026 
1027  unsigned int indexFeature = 0;
1028  for (size_t a = 0; a < l->meline.size(); a++) {
1029  std::list<vpMeSite>::const_iterator itListLine;
1030  if (l->meline[a] != NULL) {
1031  itListLine = l->meline[a]->getMeList().begin();
1032 
1033  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1034  factor[n + i] = fac;
1035  vpMeSite site = *itListLine;
1036  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1037  factor[n + i] = 0.2;
1038  ++itListLine;
1039  indexFeature++;
1040  }
1041  n += l->nbFeature[a];
1042  }
1043  }
1044  }
1045  }
1046 
1047  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1048  ++it) {
1049  if ((*it)->isTracked()) {
1050  cy = *it;
1052  double fac = 1.0;
1053 
1054  std::list<vpMeSite>::const_iterator itCyl1;
1055  std::list<vpMeSite>::const_iterator itCyl2;
1056  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1057  itCyl1 = cy->meline1->getMeList().begin();
1058  itCyl2 = cy->meline2->getMeList().begin();
1059  }
1060 
1061  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1062  factor[n + i] = fac;
1063  vpMeSite site;
1064  if (i < cy->nbFeaturel1) {
1065  site = *itCyl1;
1066  ++itCyl1;
1067  } else {
1068  site = *itCyl2;
1069  ++itCyl2;
1070  }
1071  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1072  factor[n + i] = 0.2;
1073  }
1074 
1075  n += cy->nbFeature;
1076  }
1077  }
1078 
1079  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1080  if ((*it)->isTracked()) {
1081  ci = *it;
1083  double fac = 1.0;
1084 
1085  std::list<vpMeSite>::const_iterator itCir;
1086  if (ci->meEllipse != NULL) {
1087  itCir = ci->meEllipse->getMeList().begin();
1088  }
1089 
1090  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1091  factor[n + i] = fac;
1092  vpMeSite site = *itCir;
1093  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1094  factor[n + i] = 0.2;
1095  ++itCir;
1096  }
1097 
1098  n += ci->nbFeature;
1099  }
1100  }
1101 
1102  return nbrow;
1103 }
1104 
1106  vpHomogeneousMatrix &cMo_, const unsigned int lvl)
1107 {
1108  vpMbtDistanceLine *l;
1110  vpMbtDistanceCircle *ci;
1111 
1112  unsigned int n = 0;
1113  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1114  if ((*it)->isTracked()) {
1115  l = *it;
1117  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1118  for (unsigned int j = 0; j < 6; j++) {
1119  L[n + i][j] = l->L[i][j];
1120  error[n + i] = l->error[i];
1121  }
1122  }
1123  n += l->nbFeatureTotal;
1124  }
1125  }
1126 
1127  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1128  ++it) {
1129  if ((*it)->isTracked()) {
1130  cy = *it;
1131  cy->computeInteractionMatrixError(cMo_, I);
1132  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1133  for (unsigned int j = 0; j < 6; j++) {
1134  L[n + i][j] = cy->L[i][j];
1135  error[n + i] = cy->error[i];
1136  }
1137  }
1138  n += cy->nbFeature;
1139  }
1140  }
1141  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1142  if ((*it)->isTracked()) {
1143  ci = *it;
1145  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1146  for (unsigned int j = 0; j < 6; j++) {
1147  L[n + i][j] = ci->L[i][j];
1148  error[n + i] = ci->error[i];
1149  }
1150  }
1151 
1152  n += ci->nbFeature;
1153  }
1154  }
1155 }
1156 
1163 {
1164  this->cam = camera;
1165 
1168 }
1169 
1177 {
1180 }
1188 {
1191 }
1192 
1203 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
1204  const int idFace, const std::string &name)
1205 {
1206  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1207 }
1208 
1219 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
1220  const std::string &name)
1221 {
1222  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1223  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1224 }
1225 
1238  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1239  const bool displayFullModel)
1240 {
1241  std::vector<std::vector<double> > models = vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, camera, displayFullModel);
1242 
1243  for (size_t i = 0; i < models.size(); i++) {
1244  if (vpMath::equal(models[i][0], 0)) {
1245  vpImagePoint ip1(models[i][1], models[i][2]);
1246  vpImagePoint ip2(models[i][3], models[i][4]);
1247  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1248  } else if (vpMath::equal(models[i][0], 1)) {
1249  vpImagePoint center(models[i][1], models[i][2]);
1250  double mu20 = models[i][3];
1251  double mu11 = models[i][4];
1252  double mu02 = models[i][5];
1253  vpDisplay::displayEllipse(I, center, mu20, mu11, mu02, true, col, thickness);
1254  }
1255  }
1256 
1257  if (displayFeatures) {
1258  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1259  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1262 
1264  double id = m_featuresToBeDisplayedKlt[i][5];
1265  std::stringstream ss;
1266  ss << id;
1267  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1268  }
1269  }
1270  }
1271 
1272 #ifdef VISP_HAVE_OGRE
1273  if (useOgre)
1274  faces.displayOgre(cMo_);
1275 #endif
1276 }
1277 
1290  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1291  const bool displayFullModel)
1292 {
1293  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, camera, displayFullModel);
1294 
1295  for (size_t i = 0; i < models.size(); i++) {
1296  if (vpMath::equal(models[i][0], 0)) {
1297  vpImagePoint ip1(models[i][1], models[i][2]);
1298  vpImagePoint ip2(models[i][3], models[i][4]);
1299  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1300  } else if (vpMath::equal(models[i][0], 1)) {
1301  vpImagePoint center(models[i][1], models[i][2]);
1302  double mu20 = models[i][3];
1303  double mu11 = models[i][4];
1304  double mu02 = models[i][5];
1305  vpDisplay::displayEllipse(I, center, mu20, mu11, mu02, true, col, thickness);
1306  }
1307  }
1308 
1309  if (displayFeatures) {
1310  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1311  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1314 
1316  double id = m_featuresToBeDisplayedKlt[i][5];
1317  std::stringstream ss;
1318  ss << id;
1319  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1320  }
1321  }
1322  }
1323 
1324 #ifdef VISP_HAVE_OGRE
1325  if (useOgre)
1326  faces.displayOgre(cMo_);
1327 #endif
1328 }
1329 
1330 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1331  const vpHomogeneousMatrix &cMo_,
1332  const vpCameraParameters &camera,
1333  const bool displayFullModel)
1334 {
1335  std::vector<std::vector<double> > models;
1336 
1337  for (unsigned int i = 0; i < scales.size(); i += 1) {
1338  if (scales[i]) {
1339  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1340  ++it) {
1341  std::vector<std::vector<double> > currentModel =
1342  (*it)->getModelForDisplay(width, height, cMo_, camera, displayFullModel);
1343  models.insert(models.end(), currentModel.begin(), currentModel.end());
1344  }
1345 
1346  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1347  it != cylinders[scaleLevel].end(); ++it) {
1348  std::vector<std::vector<double> > currentModel =
1349  (*it)->getModelForDisplay(width, height, cMo_, camera, displayFullModel);
1350  models.insert(models.end(), currentModel.begin(), currentModel.end());
1351  }
1352 
1353  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1354  it != circles[scaleLevel].end(); ++it) {
1355  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo_, camera, displayFullModel);
1356  models.push_back(paramsCircle);
1357  }
1358 
1359  break; // displaying model on one scale only
1360  }
1361  }
1362 
1363 #ifdef VISP_HAVE_OGRE
1364  if (useOgre)
1365  faces.displayOgre(cMo_);
1366 #endif
1367 
1368  return models;
1369 }
1370 
1383 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1384  const vpHomogeneousMatrix &cMo_, const bool verbose,
1385  const vpHomogeneousMatrix &T)
1386 {
1387  // Reinit klt
1388  #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1389  if (cur != NULL) {
1390  cvReleaseImage(&cur);
1391  cur = NULL;
1392  }
1393  #endif
1394 
1395  // delete the Klt Polygon features
1396  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1397  vpMbtDistanceKltPoints *kltpoly = *it;
1398  if (kltpoly != NULL) {
1399  delete kltpoly;
1400  }
1401  kltpoly = NULL;
1402  }
1403  kltPolygons.clear();
1404 
1405  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1406  ++it) {
1407  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1408  if (kltPolyCylinder != NULL) {
1409  delete kltPolyCylinder;
1410  }
1411  kltPolyCylinder = NULL;
1412  }
1413  kltCylinders.clear();
1414 
1415  // delete the structures used to display circles
1416  vpMbtDistanceCircle *ci;
1417  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1418  ci = *it;
1419  if (ci != NULL) {
1420  delete ci;
1421  }
1422  ci = NULL;
1423  }
1424 
1425  circles_disp.clear();
1426 
1427  firstInitialisation = true;
1428 
1429  // Reinit edge
1430  vpMbtDistanceLine *l;
1432 
1433  for (unsigned int i = 0; i < scales.size(); i += 1) {
1434  if (scales[i]) {
1435  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1436  l = *it;
1437  if (l != NULL)
1438  delete l;
1439  l = NULL;
1440  }
1441 
1442  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1443  ++it) {
1444  cy = *it;
1445  if (cy != NULL)
1446  delete cy;
1447  cy = NULL;
1448  }
1449 
1450  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1451  ci = *it;
1452  if (ci != NULL)
1453  delete ci;
1454  ci = NULL;
1455  }
1456 
1457  lines[i].clear();
1458  cylinders[i].clear();
1459  circles[i].clear();
1460  }
1461  }
1462 
1463  // compute_interaction=1;
1464  nline = 0;
1465  ncylinder = 0;
1466  ncircle = 0;
1467  // lambda = 1;
1468  nbvisiblepolygone = 0;
1469 
1470  // Reinit common parts
1471  faces.reset();
1472 
1473  loadModel(cad_name, verbose, T);
1474 
1475  this->cMo = cMo_;
1476  init(I);
1477 }
1478 
1479 #elif !defined(VISP_BUILD_SHARED_LIBS)
1480 // Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1481 // no symbols
1482 void dummy_vpMbEdgeKltTracker(){};
1483 #endif // VISP_HAVE_OPENCV
bool m_computeInteraction
Definition: vpMbTracker.h:185
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void trackSecondLoop(const vpImage< unsigned char > &I, vpMatrix &L, vpColVector &_error, vpHomogeneousMatrix &cMo, const unsigned int lvl=0)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double r, const int idFace=0, const std::string &name="")
bool computeProjError
Definition: vpMbTracker.h:133
unsigned int ncylinder
void postTrackingMbt(vpColVector &w, const unsigned int level=0)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
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)
double getFarClippingDistance() const
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void upScale(const unsigned int _scale)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
void setHarrisFreeParameter(double harris_k)
bool isAppearing(const unsigned int i)
unsigned int getWidth() const
Definition: vpImage.h:237
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 setMeanWeight2(const double wmean)
double getNearClippingDistance() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setKltPyramidLevels(const unsigned int &pL)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
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.
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.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int getKltBlockSize() const
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setMaxFeatures(const int maxCount)
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
std::list< vpMbtDistanceKltCylinder * > kltCylinders
unsigned int m_maxIterKlt
The maximum iteration of the virtual visual servoing stage.
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:113
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)
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int ncircle
double thresholdKLT
The threshold used in the robust estimation of KLT.
std::vector< const vpImage< unsigned char > * > Ipyramid
vpColVector error
The error vector.
void setMinDistance(double minDistance)
void getCameraParameters(vpCameraParameters &cam) const
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()
Definition: vpMe.h:60
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
double getLodMinLineLengthThreshold() const
void setKltMaskBorder(const unsigned int &mb)
virtual void setCameraParameters(const vpCameraParameters &camera)
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.
void updateMovingEdge(const vpImage< unsigned char > &I)
vpColVector w_mbt
Robust weights for Edge.
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setFarClippingDistance(const double &dist)
void downScale(const unsigned int _scale)
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:180
Class that defines what is a point.
Definition: vpPoint.h:58
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
Definition: vpMeSite.h:189
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:111
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void setQuality(double qualityLevel)
virtual void init(const vpImage< unsigned char > &I)
vpHomogeneousMatrix ctTc0
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.
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void computeVVSInit()
Manage a circle used in the model-based tracker.
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4660
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.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setAngleDisappear(const double &adisappear)
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:114
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
vpColVector w_klt
Robust weights for KLT.
vpColVector error
The error vector.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
bool isVisible() const
virtual void track(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Generic class defining intrinsic camera parameters.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
bool isTracked() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
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)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
unsigned int getRows() const
Definition: vpArray2D.h:289
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
vpKltOpencv tracker
Points tracker.
void setPyramidLevels(const int pyrMaxLevel)
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:175
void setKltWindowSize(const unsigned int &w)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void getEdgeMe(vpMe &ecm) const
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
double get_px() const
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:108
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void setMeanWeight1(const double wmean)
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.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
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)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
unsigned int getKltPyramidLevels() const
void setWindowSize(const int winSize)
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:101
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
virtual void initCylinder(const vpPoint &, const vpPoint &, const double r, const int idFace, const std::string &name="")
void setCameraParameters(const vpCameraParameters &cam)
void preTracking(const vpImage< unsigned char > &I)
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
unsigned int getKltMaskBorder() const
vpColVector m_error_hybrid
(s - s*)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
unsigned int initMbtTracking(const unsigned int level=0)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const 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)
vpHomogeneousMatrix inverse() const
unsigned int trackFirstLoop(const vpImage< unsigned char > &I, vpColVector &factor, const unsigned int lvl=0)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setBlockSize(const int blockSize)
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)
void setMeanWeight(const double _wmean)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:364
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl=0)
unsigned int getHeight() const
Definition: vpImage.h:179
void setMeanWeight(const double w_mean)
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
unsigned int getKltMaxFeatures() const
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int m_nbInfos
bool Reinit
Indicates if the line has to be reinitialized.
virtual void init(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
unsigned int getCurrentNumberPoints() const
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 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)
unsigned int getCurrentNumberPoints() const
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
void setKltBlockSize(const unsigned int &bs)
void setKltMaxFeatures(const unsigned int &mF)
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
void setCameraParameters(const vpCameraParameters &cam)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
virtual void computeVVSInteractionMatrixAndResidu()
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
double getLodMinPolygonAreaThreshold() const
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()
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
virtual void computeVVSCheckLevenbergMarquardt(const 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)
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.
unsigned int getKltWindowSize() const
double thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setNearClippingDistance(const double &dist)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
virtual void setLod(const bool useLod, const std::string &name="")
void computeFov(const unsigned int &w, const unsigned int &h)