Visual Servoing Platform  version 3.3.1 under development (2020-09-18)
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 
281 #ifdef VISP_HAVE_PUGIXML
283 
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  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) {
397  }
398 
400 
403 
404  if (computeProjError)
406 
407  if (reInit)
408  return true;
409 
410  return false;
411 }
412 
417  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 
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  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  m_w_mbt.resize(nbrow, false);
708  m_w_mbt = 1; // needed in vpRobust::psiTukey()
709  robust_mbt.resize(nbrow);
710  }
711 
712  if (nbInfos != 0) {
713  m_w_klt.resize(2 * nbInfos, false);
714  m_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, m_cMo, lvl);
723 
724  if (nbInfos >= 4) {
725  unsigned int shift = 0;
726 
727  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
728  it != vpMbKltTracker::kltPolygons.end(); ++it) {
729  vpMbtDistanceKltPoints *kltpoly = *it;
730  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
731  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
732  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
733  kltpoly->computeHomography(ctTc0, H);
734  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
735  shift += 2 * kltpoly->getCurrentNumberPoints();
736  }
737  }
738 
739  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
740  ++it) {
741  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
742 
743  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
744  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
745  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
746  try {
747  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
748  } catch (...) {
749  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
750  }
751 
752  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
753  }
754  }
755  }
756 
757  /* residuals */
758  if (nbrow > 3) {
759  m_error_hybrid.insert(0, R_mbt);
760  }
761 
762  if (nbInfos > 3) {
763  m_error_hybrid.insert(nbrow, R_klt);
764  }
765 
766  unsigned int cpt = 0;
767  while (cpt < (nbrow + 2 * nbInfos)) {
768  if (cpt < (unsigned)nbrow) {
769  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
770  } else {
771  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
772  }
773  cpt++;
774  }
775 
776  bool reStartFromLastIncrement = false;
777  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
778  &m_w_prev);
779  if (reStartFromLastIncrement) {
780  ctTc0 = ctTc0_Prev;
781  }
782 
783  if (!reStartFromLastIncrement) {
784  /* robust */
785  if (nbrow > 3) {
786  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(m_thresholdMBT / m_cam.get_px());
792  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_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(m_thresholdKLT / m_cam.get_px());
804  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
805 
806  L.insert(L_klt, nbrow, 0);
807  }
808 
809  cpt = 0;
810  while (cpt < (nbrow + 2 * nbInfos)) {
811  if (cpt < (unsigned)nbrow) {
812  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
813  } else {
814  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
815  }
816  cpt++;
817  }
818 
819  if (computeCovariance) {
820  L_true = L;
821  if (!isoJoIdentity) {
823  cVo.buildFrom(m_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 = m_cMo;
851  ctTc0_Prev = ctTc0;
853  m_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, m_w_mbt, m_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, m_w_mbt, m_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  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  const vpHomogeneousMatrix &cMo, 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  m_cam = cam;
1165 
1168 }
1169 
1177 {
1180 }
1188 {
1191 }
1192 
1203 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1204  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, double radius, 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 &cam, const vpColor &col, unsigned int thickness,
1239  bool displayFullModel)
1240 {
1241  std::vector<std::vector<double> > models = vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, 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 n20 = models[i][3];
1251  double n11 = models[i][4];
1252  double n02 = models[i][5];
1253  vpDisplay::displayEllipse(I, center, n20, n11, n02, 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 &cam, const vpColor &col, unsigned int thickness,
1291  bool displayFullModel)
1292 {
1293  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, 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 n20 = models[i][3];
1303  double n11 = models[i][4];
1304  double n02 = models[i][5];
1305  vpDisplay::displayEllipse(I, center, n20, n11, n02, 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 &cam,
1333  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, cam, 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, cam, 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, cam, 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, 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  m_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
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:305
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)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
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.
void setThreshold(double noise_threshold)
Definition: vpRobust.h:115
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:364
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:58
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
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 insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:4729
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 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()
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)