Visual Servoing Platform  version 3.5.1 under development (2022-05-22)
vpMbEdgeKltTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
33  *
34  * Authors:
35  * Romain Tallonneau
36  * Aurelien Yol
37  *
38  *****************************************************************************/
39 
40 //#define VP_DEBUG_MODE 1 // Activate debug level 1
41 
42 #include <visp3/core/vpDebug.h>
43 #include <visp3/core/vpTrackingException.h>
44 #include <visp3/core/vpVelocityTwistMatrix.h>
45 #include <visp3/mbt/vpMbEdgeKltTracker.h>
46 #include <visp3/mbt/vpMbtXmlGenericParser.h>
47 
48 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
49 
51  : m_thresholdKLT(2.), m_thresholdMBT(2.), m_maxIterKlt(30), m_w_mbt(), m_w_klt(), m_error_hybrid(), m_w_hybrid()
52 {
53  computeCovariance = false;
54 
55 #ifdef VISP_HAVE_OGRE
56  faces.getOgreContext()->setWindowName("MBT Hybrid");
57 #endif
58 
59  m_lambda = 0.8;
60  m_maxIter = 200;
61 }
62 
68 
76 {
78 
80 
82 
83  unsigned int i = (unsigned int)scales.size();
84  do {
85  i--;
86  if (scales[i]) {
87  downScale(i);
89  upScale(i);
90  }
91  } while (i != 0);
92 
94 }
95 
106 {
107  vpMbKltTracker::setPose(I, cdMo);
108 
109  resetMovingEdge();
110 
111  if (useScanLine) {
115  }
116 
117  initPyramid(I, Ipyramid);
118 
119  unsigned int i = (unsigned int)scales.size();
120  do {
121  i--;
122  if (scales[i]) {
123  downScale(i);
125  upScale(i);
126  }
127  } while (i != 0);
128 
130 }
131 
142 {
143  vpImageConvert::convert(I_color, m_I);
145 
146  resetMovingEdge();
147 
148  if (useScanLine) {
152  }
153 
155 
156  unsigned int i = (unsigned int)scales.size();
157  do {
158  i--;
159  if (scales[i]) {
160  downScale(i);
162  upScale(i);
163  }
164  } while (i != 0);
165 
167 }
168 
174 {
177 }
178 
179 unsigned int vpMbEdgeKltTracker::initMbtTracking(unsigned int lvl)
180 {
181  if (lvl >= scales.size() || !scales[lvl]) {
182  throw vpException(vpException::dimensionError, "lvl not used.");
183  }
184 
185  unsigned int nbrow = 0;
186  for (std::list<vpMbtDistanceLine *>::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
187  vpMbtDistanceLine *l = *it;
188 
189  if (l->isTracked()) {
191  nbrow += l->nbFeatureTotal;
192  }
193  }
194 
195  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
196  ++it) {
197  vpMbtDistanceCylinder *cy = *it;
198 
199  if (cy->isTracked()) {
201  nbrow += cy->nbFeature;
202  }
203  }
204 
205  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
206  vpMbtDistanceCircle *ci = *it;
207 
208  if (ci->isTracked()) {
210  nbrow += ci->nbFeature;
211  }
212  }
213 
214  return nbrow;
215 }
216 
277 void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
278 {
279  // Load projection error config
280  vpMbTracker::loadConfigFile(configFile, verbose);
281 
283  xmlp.setVerbose(verbose);
287 
288  xmlp.setEdgeMe(me);
289 
290  xmlp.setKltMaxFeatures(10000);
291  xmlp.setKltWindowSize(5);
292  xmlp.setKltQuality(0.01);
293  xmlp.setKltMinDistance(5);
294  xmlp.setKltHarrisParam(0.01);
295  xmlp.setKltBlockSize(3);
296  xmlp.setKltPyramidLevels(3);
298 
299  try {
300  if (verbose) {
301  std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
302  }
303  xmlp.parse(configFile.c_str());
304  } catch (...) {
305  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
306  throw vpException(vpException::ioError, "problem to parse configuration file.");
307  }
308 
309  vpCameraParameters camera;
310  xmlp.getCameraParameters(camera);
311  setCameraParameters(camera);
312 
315 
316  if (xmlp.hasNearClippingDistance())
318 
319  if (xmlp.hasFarClippingDistance())
321 
322  if (xmlp.getFovClipping()) {
324  }
325 
326  useLodGeneral = xmlp.getLodState();
329 
330  applyLodSettingInConfig = false;
331  if (this->getNbPolygon() > 0) {
336  }
337 
338  vpMe meParser;
339  xmlp.getEdgeMe(meParser);
341 
347  tracker.setBlockSize((int)xmlp.getKltBlockSize());
349  maskBorder = xmlp.getKltMaskBorder();
350 
351  // if(useScanLine)
352  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
353 }
354 
359  unsigned int lvl)
360 {
361  postTrackingMbt(w_mbt, lvl);
362 
363  if (displayFeatures) {
364  if (lvl == 0) {
365  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
366  vpMbtDistanceLine *l = *it;
367  if (l->isVisible() && l->isTracked()) {
368  l->displayMovingEdges(I);
369  }
370  }
371 
372  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
373  ++it) {
374  vpMbtDistanceCylinder *cy = *it;
375  // A cylinder is always visible: #FIXME AY: Still valid?
376  if (cy->isTracked())
377  cy->displayMovingEdges(I);
378  }
379 
380  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
381  vpMbtDistanceCircle *ci = *it;
382  if (ci->isVisible() && ci->isTracked()) {
383  ci->displayMovingEdges(I);
384  }
385  }
386  }
387  }
388 
389  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
390 
391  if (useScanLine) {
395  }
396 
398 
401 
402  if (computeProjError)
404 
405  if (reInit)
406  return true;
407 
408  return false;
409 }
410 
415  unsigned int lvl)
416 {
417  postTrackingMbt(w_mbt, lvl);
418 
419  if (displayFeatures) {
420  if (lvl == 0) {
421  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
422  vpMbtDistanceLine *l = *it;
423  if (l->isVisible() && l->isTracked()) {
424  l->displayMovingEdges(I_color);
425  }
426  }
427 
428  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
429  ++it) {
430  vpMbtDistanceCylinder *cy = *it;
431  // A cylinder is always visible: #FIXME AY: Still valid?
432  if (cy->isTracked())
433  cy->displayMovingEdges(m_I);
434  }
435 
436  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
437  vpMbtDistanceCircle *ci = *it;
438  if (ci->isVisible() && ci->isTracked()) {
439  ci->displayMovingEdges(m_I);
440  }
441  }
442  }
443  }
444 
445  bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
446 
447  if (useScanLine) {
451  }
452 
454 
457 
458  if (computeProjError)
460 
461  if (reInit)
462  return true;
463 
464  return false;
465 }
466 
478 {
479  if (lvl >= scales.size() || !scales[lvl]) {
480  throw vpException(vpException::dimensionError, "_lvl not used.");
481  }
482 
483  unsigned int n = 0;
485  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
486  if ((*it)->isTracked()) {
487  l = *it;
488  unsigned int indexLine = 0;
489  double wmean = 0;
490 
491  for (size_t a = 0; a < l->meline.size(); a++) {
492  std::list<vpMeSite>::iterator itListLine;
493  if (l->nbFeature[a] > 0)
494  itListLine = l->meline[a]->getMeList().begin();
495 
496  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
497  wmean += w[n + indexLine];
498  vpMeSite p = *itListLine;
499  if (w[n + indexLine] < 0.5) {
501  *itListLine = p;
502  }
503 
504  ++itListLine;
505  indexLine++;
506  }
507  }
508 
509  n += l->nbFeatureTotal;
510 
511  if (l->nbFeatureTotal != 0)
512  wmean /= l->nbFeatureTotal;
513  else
514  wmean = 1;
515 
516  l->setMeanWeight(wmean);
517 
518  if (wmean < 0.8)
519  l->Reinit = true;
520  }
521  }
522 
523  // Same thing with cylinders as with lines
525  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
526  ++it) {
527  if ((*it)->isTracked()) {
528  cy = *it;
529  double wmean = 0;
530  std::list<vpMeSite>::iterator itListCyl1;
531  std::list<vpMeSite>::iterator itListCyl2;
532  if (cy->nbFeature > 0) {
533  itListCyl1 = cy->meline1->getMeList().begin();
534  itListCyl2 = cy->meline2->getMeList().begin();
535  }
536 
537  wmean = 0;
538  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
539  wmean += w[n + i];
540  vpMeSite p = *itListCyl1;
541  if (w[n + i] < 0.5) {
543 
544  *itListCyl1 = p;
545  }
546 
547  ++itListCyl1;
548  }
549 
550  if (cy->nbFeaturel1 != 0)
551  wmean /= cy->nbFeaturel1;
552  else
553  wmean = 1;
554 
555  cy->setMeanWeight1(wmean);
556 
557  if (wmean < 0.8) {
558  cy->Reinit = true;
559  }
560 
561  wmean = 0;
562  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
563  wmean += w[n + i];
564  vpMeSite p = *itListCyl2;
565  if (w[n + i] < 0.5) {
567 
568  *itListCyl2 = p;
569  }
570 
571  ++itListCyl2;
572  }
573 
574  if (cy->nbFeaturel2 != 0)
575  wmean /= cy->nbFeaturel2;
576  else
577  wmean = 1;
578 
579  cy->setMeanWeight2(wmean);
580 
581  if (wmean < 0.8) {
582  cy->Reinit = true;
583  }
584 
585  n += cy->nbFeature;
586  }
587  }
588 
589  // Same thing with circles as with lines
591  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
592  if ((*it)->isTracked()) {
593  ci = *it;
594  double wmean = 0;
595  std::list<vpMeSite>::iterator itListCir;
596 
597  if (ci->nbFeature > 0) {
598  itListCir = ci->meEllipse->getMeList().begin();
599  }
600 
601  wmean = 0;
602  for (unsigned int i = 0; i < ci->nbFeature; i++) {
603  wmean += w[n + i];
604  vpMeSite p = *itListCir;
605  if (w[n + i] < 0.5) {
607 
608  *itListCir = p;
609  }
610 
611  ++itListCir;
612  }
613 
614  if (ci->nbFeature != 0)
615  wmean /= ci->nbFeature;
616  else
617  wmean = 1;
618 
619  ci->setMeanWeight(wmean);
620 
621  if (wmean < 0.8) {
622  ci->Reinit = true;
623  }
624 
625  n += ci->nbFeature;
626  }
627  }
628 }
629 
640 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
641  unsigned int lvl, double *edge_residual, double *klt_residual)
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, robust_klt;
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  if (edge_residual != NULL)
695  *edge_residual = 0;
696  if (klt_residual != NULL)
697  *klt_residual = 0;
698 
699  vpHomogeneousMatrix cMoPrev;
700  vpHomogeneousMatrix ctTc0_Prev;
701  vpColVector m_error_prev;
702  vpColVector m_w_prev;
703 
704  // Init size
705  m_error_hybrid.resize(totalNbRows, false);
706  m_w_hybrid.resize(totalNbRows, false);
707 
708  if (nbrow != 0) {
709  m_w_mbt.resize(nbrow, false);
710  m_w_mbt = 1; // needed in vpRobust::psiTukey()
711  }
712 
713  if (nbInfos != 0) {
714  m_w_klt.resize(2 * nbInfos, false);
715  m_w_klt = 1; // needed in vpRobust::psiTukey()
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  if (edge_residual != NULL) {
787  *edge_residual = 0;
788  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
789  *edge_residual += fabs(R_mbt[i]);
790  *edge_residual /= R_mbt.getRows();
791  }
792 
794  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
795 
796  L.insert(L_mbt, 0, 0);
797  }
798 
799  if (nbInfos > 3) {
800  if (klt_residual != NULL) {
801  *klt_residual = 0;
802  for (unsigned int i = 0; i < R_klt.getRows(); i++)
803  *klt_residual += fabs(R_klt[i]);
804  *klt_residual /= R_klt.getRows();
805  }
806 
808  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
809 
810  L.insert(L_klt, nbrow, 0);
811  }
812 
813  cpt = 0;
814  while (cpt < (nbrow + 2 * nbInfos)) {
815  if (cpt < (unsigned)nbrow) {
816  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
817  } else {
818  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
819  }
820  cpt++;
821  }
822 
823  if (computeCovariance) {
824  L_true = L;
825  if (!isoJoIdentity) {
827  cVo.buildFrom(m_cMo);
828  LVJ_true = (L * cVo * oJo);
829  }
830  }
831 
832  residu_1 = residu;
833  residu = 0;
834  double num = 0;
835  double den = 0;
836 
837  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
838  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
839  den += m_w_hybrid[i];
840 
841  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
842  if (m_computeInteraction) {
843  for (unsigned int j = 0; j < 6; j += 1) {
844  L[i][j] *= m_w_hybrid[i];
845  }
846  }
847  }
848 
849  residu = sqrt(num / den);
850 
851  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
852  &m_w_hybrid, &m_w_prev);
853 
854  cMoPrev = m_cMo;
855  ctTc0_Prev = ctTc0;
857  m_cMo = ctTc0 * c0Mo;
858  }
859 
860  iter++;
861  }
862 
864 }
865 
867 {
868  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
869 }
870 
872 {
873  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
874  "computeVVSInteractionMatrixAndR"
875  "esidu() should not be called!");
876 }
877 
886 {
887  try {
889  } catch (...) {
890  }
891 
892  if (m_nbInfos >= 4) {
893  unsigned int old_maxIter = m_maxIter;
896  m_maxIter = old_maxIter;
897  } else {
898  m_nbInfos = 0;
899  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
900  }
901 
903 
904  unsigned int nbrow = 0;
905  computeVVS(I, m_nbInfos, nbrow);
906 
907  if (postTracking(I, m_w_mbt, m_w_klt)) {
909 
910  // AY : Removed as edge tracked, if necessary, is reinitialized in
911  // postTracking()
912 
913  // initPyramid(I, Ipyramid);
914 
915  // unsigned int i = (unsigned int)scales.size();
916  // do {
917  // i--;
918  // if(scales[i]){
919  // downScale(i);
920  // initMovingEdge(*Ipyramid[i], cMo);
921  // upScale(i);
922  // }
923  // } while(i != 0);
924 
925  // cleanPyramid(Ipyramid);
926  }
927 
928  if (displayFeatures) {
930  }
931 }
932 
941 {
942  vpImageConvert::convert(I_color, m_I);
943  try {
945  } catch (...) {
946  }
947 
948  if (m_nbInfos >= 4) {
949  unsigned int old_maxIter = m_maxIter;
952  m_maxIter = old_maxIter;
953  } else {
954  m_nbInfos = 0;
955  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
956  }
957 
959 
960  unsigned int nbrow = 0;
961  computeVVS(m_I, m_nbInfos, nbrow);
962 
963  if (postTracking(I_color, m_w_mbt, m_w_klt)) {
965 
966  // AY : Removed as edge tracked, if necessary, is reinitialized in
967  // postTracking()
968 
969  // initPyramid(I, Ipyramid);
970 
971  // unsigned int i = (unsigned int)scales.size();
972  // do {
973  // i--;
974  // if(scales[i]){
975  // downScale(i);
976  // initMovingEdge(*Ipyramid[i], cMo);
977  // upScale(i);
978  // }
979  // } while(i != 0);
980 
981  // cleanPyramid(Ipyramid);
982  }
983 
984  if (displayFeatures) {
986  }
987 }
988 
989 unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor, unsigned int lvl)
990 {
994 
995  if (lvl >= scales.size() || !scales[lvl]) {
996  throw vpException(vpException::dimensionError, "_lvl not used.");
997  }
998 
999  unsigned int nbrow = initMbtTracking(lvl);
1000 
1001  if (nbrow == 0) {
1002  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
1003  // "Error: not enough features in the interaction matrix...");
1004  return nbrow;
1005  }
1006 
1007  factor.resize(nbrow, false);
1008  factor = 1;
1009 
1010  unsigned int n = 0;
1011  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1012  if ((*it)->isTracked()) {
1013  l = *it;
1015 
1016  double fac = 1;
1017  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1018  ++itindex) {
1019  int index = *itindex;
1020  if (l->hiddenface->isAppearing((unsigned int)index)) {
1021  fac = 0.2;
1022  break;
1023  }
1024  if (l->closeToImageBorder(I, 10)) {
1025  fac = 0.1;
1026  break;
1027  }
1028  }
1029 
1030  unsigned int indexFeature = 0;
1031  for (size_t a = 0; a < l->meline.size(); a++) {
1032  std::list<vpMeSite>::const_iterator itListLine;
1033  if (l->meline[a] != NULL) {
1034  itListLine = l->meline[a]->getMeList().begin();
1035 
1036  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1037  factor[n + i] = fac;
1038  vpMeSite site = *itListLine;
1039  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1040  factor[n + i] = 0.2;
1041  ++itListLine;
1042  indexFeature++;
1043  }
1044  n += l->nbFeature[a];
1045  }
1046  }
1047  }
1048  }
1049 
1050  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1051  ++it) {
1052  if ((*it)->isTracked()) {
1053  cy = *it;
1055  double fac = 1.0;
1056 
1057  std::list<vpMeSite>::const_iterator itCyl1;
1058  std::list<vpMeSite>::const_iterator itCyl2;
1059  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1060  itCyl1 = cy->meline1->getMeList().begin();
1061  itCyl2 = cy->meline2->getMeList().begin();
1062  }
1063 
1064  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1065  factor[n + i] = fac;
1066  vpMeSite site;
1067  if (i < cy->nbFeaturel1) {
1068  site = *itCyl1;
1069  ++itCyl1;
1070  } else {
1071  site = *itCyl2;
1072  ++itCyl2;
1073  }
1074  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1075  factor[n + i] = 0.2;
1076  }
1077 
1078  n += cy->nbFeature;
1079  }
1080  }
1081 
1082  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1083  if ((*it)->isTracked()) {
1084  ci = *it;
1086  double fac = 1.0;
1087 
1088  std::list<vpMeSite>::const_iterator itCir;
1089  if (ci->meEllipse != NULL) {
1090  itCir = ci->meEllipse->getMeList().begin();
1091  }
1092 
1093  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1094  factor[n + i] = fac;
1095  vpMeSite site = *itCir;
1096  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1097  factor[n + i] = 0.2;
1098  ++itCir;
1099  }
1100 
1101  n += ci->nbFeature;
1102  }
1103  }
1104 
1105  return nbrow;
1106 }
1107 
1109  const vpHomogeneousMatrix &cMo, unsigned int lvl)
1110 {
1111  vpMbtDistanceLine *l;
1113  vpMbtDistanceCircle *ci;
1114 
1115  unsigned int n = 0;
1116  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1117  if ((*it)->isTracked()) {
1118  l = *it;
1120  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1121  for (unsigned int j = 0; j < 6; j++) {
1122  L[n + i][j] = l->L[i][j];
1123  error[n + i] = l->error[i];
1124  }
1125  }
1126  n += l->nbFeatureTotal;
1127  }
1128  }
1129 
1130  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1131  ++it) {
1132  if ((*it)->isTracked()) {
1133  cy = *it;
1134  cy->computeInteractionMatrixError(cMo, I);
1135  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1136  for (unsigned int j = 0; j < 6; j++) {
1137  L[n + i][j] = cy->L[i][j];
1138  error[n + i] = cy->error[i];
1139  }
1140  }
1141  n += cy->nbFeature;
1142  }
1143  }
1144  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1145  if ((*it)->isTracked()) {
1146  ci = *it;
1148  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1149  for (unsigned int j = 0; j < 6; j++) {
1150  L[n + i][j] = ci->L[i][j];
1151  error[n + i] = ci->error[i];
1152  }
1153  }
1154 
1155  n += ci->nbFeature;
1156  }
1157  }
1158 }
1159 
1166 {
1167  m_cam = cam;
1168 
1171 }
1172 
1180 {
1183 }
1191 {
1194 }
1195 
1206 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1207  const std::string &name)
1208 {
1209  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1210 }
1211 
1222 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1223  const std::string &name)
1224 {
1225  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1226  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1227 }
1228 
1241  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1242  bool displayFullModel)
1243 {
1244  std::vector<std::vector<double> > models =
1245  vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1246 
1247  for (size_t i = 0; i < models.size(); i++) {
1248  if (vpMath::equal(models[i][0], 0)) {
1249  vpImagePoint ip1(models[i][1], models[i][2]);
1250  vpImagePoint ip2(models[i][3], models[i][4]);
1251  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1252  } else if (vpMath::equal(models[i][0], 1)) {
1253  vpImagePoint center(models[i][1], models[i][2]);
1254  double n20 = models[i][3];
1255  double n11 = models[i][4];
1256  double n02 = models[i][5];
1257  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1258  }
1259  }
1260 
1261  if (displayFeatures) {
1262  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1263  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1266 
1268  double id = m_featuresToBeDisplayedKlt[i][5];
1269  std::stringstream ss;
1270  ss << id;
1271  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1272  }
1273  }
1274  }
1275 
1276 #ifdef VISP_HAVE_OGRE
1277  if (useOgre)
1278  faces.displayOgre(cMo);
1279 #endif
1280 }
1281 
1294  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1295  bool displayFullModel)
1296 {
1297  std::vector<std::vector<double> > models =
1298  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1299 
1300  for (size_t i = 0; i < models.size(); i++) {
1301  if (vpMath::equal(models[i][0], 0)) {
1302  vpImagePoint ip1(models[i][1], models[i][2]);
1303  vpImagePoint ip2(models[i][3], models[i][4]);
1304  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1305  } else if (vpMath::equal(models[i][0], 1)) {
1306  vpImagePoint center(models[i][1], models[i][2]);
1307  double n20 = models[i][3];
1308  double n11 = models[i][4];
1309  double n02 = models[i][5];
1310  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1311  }
1312  }
1313 
1314  if (displayFeatures) {
1315  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1316  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1319 
1321  double id = m_featuresToBeDisplayedKlt[i][5];
1322  std::stringstream ss;
1323  ss << id;
1324  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1325  }
1326  }
1327  }
1328 
1329 #ifdef VISP_HAVE_OGRE
1330  if (useOgre)
1331  faces.displayOgre(cMo);
1332 #endif
1333 }
1334 
1335 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1336  const vpHomogeneousMatrix &cMo,
1337  const vpCameraParameters &cam,
1338  bool displayFullModel)
1339 {
1340  std::vector<std::vector<double> > models;
1341 
1342  for (unsigned int i = 0; i < scales.size(); i += 1) {
1343  if (scales[i]) {
1344  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1345  ++it) {
1346  std::vector<std::vector<double> > currentModel =
1347  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1348  models.insert(models.end(), currentModel.begin(), currentModel.end());
1349  }
1350 
1351  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1352  it != cylinders[scaleLevel].end(); ++it) {
1353  std::vector<std::vector<double> > currentModel =
1354  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1355  models.insert(models.end(), currentModel.begin(), currentModel.end());
1356  }
1357 
1358  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1359  it != circles[scaleLevel].end(); ++it) {
1360  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1361  models.push_back(paramsCircle);
1362  }
1363 
1364  break; // displaying model on one scale only
1365  }
1366  }
1367 
1368 #ifdef VISP_HAVE_OGRE
1369  if (useOgre)
1370  faces.displayOgre(cMo);
1371 #endif
1372 
1373  return models;
1374 }
1375 
1388 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1389  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1390 {
1391 // Reinit klt
1392 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1393  if (cur != NULL) {
1394  cvReleaseImage(&cur);
1395  cur = NULL;
1396  }
1397 #endif
1398 
1399  // delete the Klt Polygon features
1400  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1401  vpMbtDistanceKltPoints *kltpoly = *it;
1402  if (kltpoly != NULL) {
1403  delete kltpoly;
1404  }
1405  kltpoly = NULL;
1406  }
1407  kltPolygons.clear();
1408 
1409  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1410  ++it) {
1411  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1412  if (kltPolyCylinder != NULL) {
1413  delete kltPolyCylinder;
1414  }
1415  kltPolyCylinder = NULL;
1416  }
1417  kltCylinders.clear();
1418 
1419  // delete the structures used to display circles
1420  vpMbtDistanceCircle *ci;
1421  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1422  ci = *it;
1423  if (ci != NULL) {
1424  delete ci;
1425  }
1426  ci = NULL;
1427  }
1428 
1429  circles_disp.clear();
1430 
1431  firstInitialisation = true;
1432 
1433  // Reinit edge
1434  vpMbtDistanceLine *l;
1436 
1437  for (unsigned int i = 0; i < scales.size(); i += 1) {
1438  if (scales[i]) {
1439  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1440  l = *it;
1441  if (l != NULL)
1442  delete l;
1443  l = NULL;
1444  }
1445 
1446  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1447  ++it) {
1448  cy = *it;
1449  if (cy != NULL)
1450  delete cy;
1451  cy = NULL;
1452  }
1453 
1454  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1455  ci = *it;
1456  if (ci != NULL)
1457  delete ci;
1458  ci = NULL;
1459  }
1460 
1461  lines[i].clear();
1462  cylinders[i].clear();
1463  circles[i].clear();
1464  }
1465  }
1466 
1467  // compute_interaction=1;
1468  nline = 0;
1469  ncylinder = 0;
1470  ncircle = 0;
1471  // lambda = 1;
1472  nbvisiblepolygone = 0;
1473 
1474  // Reinit common parts
1475  faces.reset();
1476 
1477  loadModel(cad_name, verbose, T);
1478 
1479  m_cMo = cMo;
1480  init(I);
1481 }
1482 
1483 #elif !defined(VISP_BUILD_SHARED_LIBS)
1484 // Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1485 // no symbols
1486 void dummy_vpMbEdgeKltTracker(){};
1487 #endif // VISP_HAVE_OPENCV
bool m_computeInteraction
Definition: vpMbTracker.h:185
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
bool computeProjError
Definition: vpMbTracker.h:133
unsigned int ncylinder
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
double m_thresholdMBT
The threshold used in the robust estimation of MBT.
void displayMovingEdges(const vpImage< unsigned char > &I)
void setMovingEdge(const vpMe &me)
void setMaxFeatures(int maxCount)
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
void postTrackingMbt(vpColVector &w, unsigned int level=0)
vpMeSiteState getState() const
Definition: vpMeSite.h:190
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int nbFeature
The number of moving edges.
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
unsigned int nbFeatureTotal
The number of moving edges.
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
void upScale(const unsigned int _scale)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setHarrisFreeParameter(double harris_k)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setKltQuality(const double &q)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
bool Reinit
Indicates if the line has to be reinitialized.
void getCameraParameters(vpCameraParameters &cam) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setKltPyramidLevels(const unsigned int &pL)
unsigned int initMbtTracking(unsigned int level=0)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
vpColVector m_w_mbt
Robust weights for Edge.
unsigned int scaleLevel
std::list< int > Lindex_polygon
Index of the faces which contain the line.
vpMatrix L
The interaction matrix.
bool isTracked() const
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
#define vpERROR_TRACE
Definition: vpDebug.h:393
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:367
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:291
Definition: vpMe.h:60
vpHomogeneousMatrix inverse() const
Manage the line of a polygon used in the model-based tracker.
unsigned int nbFeature
The number of moving edges.
virtual void setClipping(const unsigned int &flags)
vpColVector m_w_hybrid
Robust weights.
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
unsigned int getKltBlockSize() const
void setKltMaskBorder(const unsigned int &mb)
void setEdgeMe(const vpMe &ecm)
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:62
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
vpMe me
The moving edges parameters.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
void updateMovingEdge(const vpImage< unsigned char > &I)
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setLod(bool useLod, const std::string &name="")
virtual void setFarClippingDistance(const double &dist)
void downScale(const unsigned int _scale)
virtual void initCylinder(const vpPoint &, const vpPoint &, double r, int idFace, const std::string &name="")
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
vpMatrix L
The interaction matrix.
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void setQuality(double qualityLevel)
virtual void init(const vpImage< unsigned char > &I)
vpHomogeneousMatrix ctTc0
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
unsigned int nbFeaturel1
The number of moving edges on line 1.
void trackSecondLoop(const vpImage< unsigned char > &I, vpMatrix &L, vpColVector &_error, const vpHomogeneousMatrix &cMo, unsigned int lvl=0)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpAROgre * getOgreContext()
void setKltHarrisParam(const double &hp)
virtual void computeVVSInit()
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
bool Reinit
Indicates if the circle has to be reinitialized.
Error that can be emited by the vpTracker class and its derivates.
Point used by the tracker.
Definition: vpMeSite.h:78
void setMeanWeight(double w_mean)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setAngleDisappear(const double &adisappear)
bool isAppearing(unsigned int i)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setKltMinDistance(const double &mD)
static double sqr(double x)
Definition: vpMath.h:123
void setPyramidLevels(int pyrMaxLevel)
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
void setMeanWeight1(double wmean)
vpColVector error
The error vector.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void track(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
Generic class defining intrinsic camera parameters.
unsigned int getKltWindowSize() const
void cleanPyramid(std::vector< const vpImage< unsigned char > *> &_pyramid)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
double m_thresholdKLT
The threshold used in the robust estimation of KLT.
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
unsigned int getKltMaxFeatures() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void setMeanWeight(double _wmean)
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setWindowSize(int winSize)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
void setMeanWeight2(double wmean)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
vpKltOpencv tracker
Points tracker.
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
void setKltWindowSize(const unsigned int &w)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
unsigned int getCurrentNumberPoints() const
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:117
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:110
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
unsigned int getHeight() const
Definition: vpImage.h:188
void setCameraParameters(const vpCameraParameters &cam)
void preTracking(const vpImage< unsigned char > &I)
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpColVector m_error_hybrid
(s - s*)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
double getLodMinLineLengthThreshold() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
Tukey influence function.
Definition: vpRobust.h:93
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
unsigned int getKltMaskBorder() const
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
unsigned int m_nbInfos
bool Reinit
Indicates if the line has to be reinitialized.
virtual void init(const vpImage< unsigned char > &I)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int trackFirstLoop(const vpImage< unsigned char > &I, vpColVector &factor, unsigned int lvl=0)
vpColVector m_w_klt
Robust weights for KLT.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
std::vector< vpMbtMeLine * > meline
The moving edge container.
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
unsigned int nline
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5980
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > *> &_pyramid)
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
unsigned int nbFeaturel2
The number of moving edges on line 2.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
void setKltBlockSize(const unsigned int &bs)
void setKltMaxFeatures(const unsigned int &mF)
void setCameraParameters(const vpCameraParameters &cam)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double r, int idFace=0, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:246
void setBlockSize(int blockSize)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
vpMatrix L
The interaction matrix.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
std::list< vpMbtDistanceKltPoints * > kltPolygons
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
vpHomogeneousMatrix c0Mo
Initial pose.
double getLodMinPolygonAreaThreshold() const
virtual void setNearClippingDistance(const double &dist)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
unsigned int getKltPyramidLevels() const
void computeFov(const unsigned int &w, const unsigned int &h)