Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
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  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
705  if (isoJoIdentity)
706  oJo.eye();
707 
708  // Init size
709  m_error_hybrid.resize(totalNbRows, false);
710  m_w_hybrid.resize(totalNbRows, false);
711 
712  if (nbrow != 0) {
713  m_w_mbt.resize(nbrow, false);
714  m_w_mbt = 1; // needed in vpRobust::psiTukey()
715  }
716 
717  if (nbInfos != 0) {
718  m_w_klt.resize(2 * nbInfos, false);
719  m_w_klt = 1; // needed in vpRobust::psiTukey()
720  }
721 
722  double mu = m_initialMu;
723 
724  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
725  if (nbrow >= 4)
726  trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
727 
728  if (nbInfos >= 4) {
729  unsigned int shift = 0;
730 
731  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
732  it != vpMbKltTracker::kltPolygons.end(); ++it) {
733  vpMbtDistanceKltPoints *kltpoly = *it;
734  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
735  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
736  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
737  kltpoly->computeHomography(ctTc0, H);
738  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
739  shift += 2 * kltpoly->getCurrentNumberPoints();
740  }
741  }
742 
743  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
744  ++it) {
745  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
746 
747  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
748  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
749  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
750  try {
751  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
752  } catch (...) {
753  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
754  }
755 
756  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
757  }
758  }
759  }
760 
761  /* residuals */
762  if (nbrow > 3) {
763  m_error_hybrid.insert(0, R_mbt);
764  }
765 
766  if (nbInfos > 3) {
767  m_error_hybrid.insert(nbrow, R_klt);
768  }
769 
770  unsigned int cpt = 0;
771  while (cpt < (nbrow + 2 * nbInfos)) {
772  if (cpt < (unsigned)nbrow) {
773  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
774  } else {
775  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
776  }
777  cpt++;
778  }
779 
780  bool reStartFromLastIncrement = false;
781  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
782  &m_w_prev);
783  if (reStartFromLastIncrement) {
784  ctTc0 = ctTc0_Prev;
785  }
786 
787  if (!reStartFromLastIncrement) {
788  /* robust */
789  if (nbrow > 3) {
790  if (edge_residual != NULL) {
791  *edge_residual = 0;
792  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
793  *edge_residual += fabs(R_mbt[i]);
794  *edge_residual /= R_mbt.getRows();
795  }
796 
798  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
799 
800  L.insert(L_mbt, 0, 0);
801  }
802 
803  if (nbInfos > 3) {
804  if (klt_residual != NULL) {
805  *klt_residual = 0;
806  for (unsigned int i = 0; i < R_klt.getRows(); i++)
807  *klt_residual += fabs(R_klt[i]);
808  *klt_residual /= R_klt.getRows();
809  }
810 
812  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
813 
814  L.insert(L_klt, nbrow, 0);
815  }
816 
817  cpt = 0;
818  while (cpt < (nbrow + 2 * nbInfos)) {
819  if (cpt < (unsigned)nbrow) {
820  m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
821  } else {
822  m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
823  }
824  cpt++;
825  }
826 
827  if (computeCovariance) {
828  L_true = L;
829  if (!isoJoIdentity) {
831  cVo.buildFrom(m_cMo);
832  LVJ_true = (L * cVo * oJo);
833  }
834  }
835 
836  residu_1 = residu;
837  residu = 0;
838  double num = 0;
839  double den = 0;
840 
841  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
842  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
843  den += m_w_hybrid[i];
844 
845  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
846  if (m_computeInteraction) {
847  for (unsigned int j = 0; j < 6; j += 1) {
848  L[i][j] *= m_w_hybrid[i];
849  }
850  }
851  }
852 
853  residu = sqrt(num / den);
854 
855  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
856  &m_w_hybrid, &m_w_prev);
857 
858  cMoPrev = m_cMo;
859  ctTc0_Prev = ctTc0;
861  m_cMo = ctTc0 * c0Mo;
862  }
863 
864  iter++;
865  }
866 
867  computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybrid, cMoPrev, L_true, LVJ_true, m_error_hybrid);
868 }
869 
871 {
872  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
873 }
874 
876 {
877  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
878  "computeVVSInteractionMatrixAndR"
879  "esidu() should not be called!");
880 }
881 
890 {
891  try {
893  } catch (...) {
894  }
895 
896  if (m_nbInfos >= 4) {
897  unsigned int old_maxIter = m_maxIter;
900  m_maxIter = old_maxIter;
901  } else {
902  m_nbInfos = 0;
903  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
904  }
905 
907 
908  unsigned int nbrow = 0;
909  computeVVS(I, m_nbInfos, nbrow);
910 
911  if (postTracking(I, m_w_mbt, m_w_klt)) {
913 
914  // AY : Removed as edge tracked, if necessary, is reinitialized in
915  // postTracking()
916 
917  // initPyramid(I, Ipyramid);
918 
919  // unsigned int i = (unsigned int)scales.size();
920  // do {
921  // i--;
922  // if(scales[i]){
923  // downScale(i);
924  // initMovingEdge(*Ipyramid[i], cMo);
925  // upScale(i);
926  // }
927  // } while(i != 0);
928 
929  // cleanPyramid(Ipyramid);
930  }
931 
932  if (displayFeatures) {
934  }
935 }
936 
945 {
946  vpImageConvert::convert(I_color, m_I);
947  try {
949  } catch (...) {
950  }
951 
952  if (m_nbInfos >= 4) {
953  unsigned int old_maxIter = m_maxIter;
956  m_maxIter = old_maxIter;
957  } else {
958  m_nbInfos = 0;
959  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
960  }
961 
963 
964  unsigned int nbrow = 0;
965  computeVVS(m_I, m_nbInfos, nbrow);
966 
967  if (postTracking(I_color, m_w_mbt, m_w_klt)) {
969 
970  // AY : Removed as edge tracked, if necessary, is reinitialized in
971  // postTracking()
972 
973  // initPyramid(I, Ipyramid);
974 
975  // unsigned int i = (unsigned int)scales.size();
976  // do {
977  // i--;
978  // if(scales[i]){
979  // downScale(i);
980  // initMovingEdge(*Ipyramid[i], cMo);
981  // upScale(i);
982  // }
983  // } while(i != 0);
984 
985  // cleanPyramid(Ipyramid);
986  }
987 
988  if (displayFeatures) {
990  }
991 }
992 
993 unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor, unsigned int lvl)
994 {
998 
999  if (lvl >= scales.size() || !scales[lvl]) {
1000  throw vpException(vpException::dimensionError, "_lvl not used.");
1001  }
1002 
1003  unsigned int nbrow = initMbtTracking(lvl);
1004 
1005  if (nbrow == 0) {
1006  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
1007  // "Error: not enough features in the interaction matrix...");
1008  return nbrow;
1009  }
1010 
1011  factor.resize(nbrow, false);
1012  factor = 1;
1013 
1014  unsigned int n = 0;
1015  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1016  if ((*it)->isTracked()) {
1017  l = *it;
1019 
1020  double fac = 1;
1021  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1022  ++itindex) {
1023  int index = *itindex;
1024  if (l->hiddenface->isAppearing((unsigned int)index)) {
1025  fac = 0.2;
1026  break;
1027  }
1028  if (l->closeToImageBorder(I, 10)) {
1029  fac = 0.1;
1030  break;
1031  }
1032  }
1033 
1034  for (size_t a = 0; a < l->meline.size(); a++) {
1035  std::list<vpMeSite>::const_iterator itListLine;
1036  if (l->meline[a] != NULL) {
1037  itListLine = l->meline[a]->getMeList().begin();
1038 
1039  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1040  factor[n + i] = fac;
1041  vpMeSite site = *itListLine;
1042  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1043  factor[n + i] = 0.2;
1044  ++itListLine;
1045  }
1046  n += l->nbFeature[a];
1047  }
1048  }
1049  }
1050  }
1051 
1052  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1053  ++it) {
1054  if ((*it)->isTracked()) {
1055  cy = *it;
1057  double fac = 1.0;
1058 
1059  std::list<vpMeSite>::const_iterator itCyl1;
1060  std::list<vpMeSite>::const_iterator itCyl2;
1061  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1062  itCyl1 = cy->meline1->getMeList().begin();
1063  itCyl2 = cy->meline2->getMeList().begin();
1064  }
1065 
1066  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1067  factor[n + i] = fac;
1068  vpMeSite site;
1069  if (i < cy->nbFeaturel1) {
1070  site = *itCyl1;
1071  ++itCyl1;
1072  } else {
1073  site = *itCyl2;
1074  ++itCyl2;
1075  }
1076  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1077  factor[n + i] = 0.2;
1078  }
1079 
1080  n += cy->nbFeature;
1081  }
1082  }
1083 
1084  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1085  if ((*it)->isTracked()) {
1086  ci = *it;
1088  double fac = 1.0;
1089 
1090  std::list<vpMeSite>::const_iterator itCir;
1091  if (ci->meEllipse != NULL) {
1092  itCir = ci->meEllipse->getMeList().begin();
1093  }
1094 
1095  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1096  factor[n + i] = fac;
1097  vpMeSite site = *itCir;
1098  if (site.getState() != vpMeSite::NO_SUPPRESSION)
1099  factor[n + i] = 0.2;
1100  ++itCir;
1101  }
1102 
1103  n += ci->nbFeature;
1104  }
1105  }
1106 
1107  return nbrow;
1108 }
1109 
1111  const vpHomogeneousMatrix &cMo, unsigned int lvl)
1112 {
1113  vpMbtDistanceLine *l;
1115  vpMbtDistanceCircle *ci;
1116 
1117  unsigned int n = 0;
1118  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1119  if ((*it)->isTracked()) {
1120  l = *it;
1122  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1123  for (unsigned int j = 0; j < 6; j++) {
1124  L[n + i][j] = l->L[i][j];
1125  error[n + i] = l->error[i];
1126  }
1127  }
1128  n += l->nbFeatureTotal;
1129  }
1130  }
1131 
1132  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1133  ++it) {
1134  if ((*it)->isTracked()) {
1135  cy = *it;
1136  cy->computeInteractionMatrixError(cMo, I);
1137  for (unsigned int i = 0; i < cy->nbFeature; i++) {
1138  for (unsigned int j = 0; j < 6; j++) {
1139  L[n + i][j] = cy->L[i][j];
1140  error[n + i] = cy->error[i];
1141  }
1142  }
1143  n += cy->nbFeature;
1144  }
1145  }
1146  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1147  if ((*it)->isTracked()) {
1148  ci = *it;
1150  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1151  for (unsigned int j = 0; j < 6; j++) {
1152  L[n + i][j] = ci->L[i][j];
1153  error[n + i] = ci->error[i];
1154  }
1155  }
1156 
1157  n += ci->nbFeature;
1158  }
1159  }
1160 }
1161 
1168 {
1169  m_cam = cam;
1170 
1173 }
1174 
1182 {
1185 }
1193 {
1196 }
1197 
1208 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1209  const std::string &name)
1210 {
1211  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1212 }
1213 
1224 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1225  const std::string &name)
1226 {
1227  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1228  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1229 }
1230 
1243  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1244  bool displayFullModel)
1245 {
1246  std::vector<std::vector<double> > models =
1247  vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1248 
1249  for (size_t i = 0; i < models.size(); i++) {
1250  if (vpMath::equal(models[i][0], 0)) {
1251  vpImagePoint ip1(models[i][1], models[i][2]);
1252  vpImagePoint ip2(models[i][3], models[i][4]);
1253  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1254  } else if (vpMath::equal(models[i][0], 1)) {
1255  vpImagePoint center(models[i][1], models[i][2]);
1256  double n20 = models[i][3];
1257  double n11 = models[i][4];
1258  double n02 = models[i][5];
1259  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1260  }
1261  }
1262 
1263  if (displayFeatures) {
1264  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1265  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1268 
1270  double id = m_featuresToBeDisplayedKlt[i][5];
1271  std::stringstream ss;
1272  ss << id;
1273  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1274  }
1275  }
1276  }
1277 
1278 #ifdef VISP_HAVE_OGRE
1279  if (useOgre)
1280  faces.displayOgre(cMo);
1281 #endif
1282 }
1283 
1296  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1297  bool displayFullModel)
1298 {
1299  std::vector<std::vector<double> > models =
1300  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1301 
1302  for (size_t i = 0; i < models.size(); i++) {
1303  if (vpMath::equal(models[i][0], 0)) {
1304  vpImagePoint ip1(models[i][1], models[i][2]);
1305  vpImagePoint ip2(models[i][3], models[i][4]);
1306  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1307  } else if (vpMath::equal(models[i][0], 1)) {
1308  vpImagePoint center(models[i][1], models[i][2]);
1309  double n20 = models[i][3];
1310  double n11 = models[i][4];
1311  double n02 = models[i][5];
1312  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1313  }
1314  }
1315 
1316  if (displayFeatures) {
1317  for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1318  if (vpMath::equal(m_featuresToBeDisplayedKlt[i][0], 1)) {
1321 
1323  double id = m_featuresToBeDisplayedKlt[i][5];
1324  std::stringstream ss;
1325  ss << id;
1326  vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1327  }
1328  }
1329  }
1330 
1331 #ifdef VISP_HAVE_OGRE
1332  if (useOgre)
1333  faces.displayOgre(cMo);
1334 #endif
1335 }
1336 
1337 std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1338  const vpHomogeneousMatrix &cMo,
1339  const vpCameraParameters &cam,
1340  bool displayFullModel)
1341 {
1342  std::vector<std::vector<double> > models;
1343 
1344  for (unsigned int i = 0; i < scales.size(); i += 1) {
1345  if (scales[i]) {
1346  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1347  ++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<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1354  it != cylinders[scaleLevel].end(); ++it) {
1355  std::vector<std::vector<double> > currentModel =
1356  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1357  models.insert(models.end(), currentModel.begin(), currentModel.end());
1358  }
1359 
1360  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1361  it != circles[scaleLevel].end(); ++it) {
1362  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1363  models.push_back(paramsCircle);
1364  }
1365 
1366  break; // displaying model on one scale only
1367  }
1368  }
1369 
1370 #ifdef VISP_HAVE_OGRE
1371  if (useOgre)
1372  faces.displayOgre(cMo);
1373 #endif
1374 
1375  return models;
1376 }
1377 
1390 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1391  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1392 {
1393 // Reinit klt
1394 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1395  if (cur != NULL) {
1396  cvReleaseImage(&cur);
1397  cur = NULL;
1398  }
1399 #endif
1400 
1401  // delete the Klt Polygon features
1402  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1403  vpMbtDistanceKltPoints *kltpoly = *it;
1404  if (kltpoly != NULL) {
1405  delete kltpoly;
1406  }
1407  kltpoly = NULL;
1408  }
1409  kltPolygons.clear();
1410 
1411  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1412  ++it) {
1413  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1414  if (kltPolyCylinder != NULL) {
1415  delete kltPolyCylinder;
1416  }
1417  kltPolyCylinder = NULL;
1418  }
1419  kltCylinders.clear();
1420 
1421  // delete the structures used to display circles
1422  vpMbtDistanceCircle *ci;
1423  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1424  ci = *it;
1425  if (ci != NULL) {
1426  delete ci;
1427  }
1428  ci = NULL;
1429  }
1430 
1431  circles_disp.clear();
1432 
1433  firstInitialisation = true;
1434 
1435  // Reinit edge
1436  vpMbtDistanceLine *l;
1438 
1439  for (unsigned int i = 0; i < scales.size(); i += 1) {
1440  if (scales[i]) {
1441  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1442  l = *it;
1443  if (l != NULL)
1444  delete l;
1445  l = NULL;
1446  }
1447 
1448  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1449  ++it) {
1450  cy = *it;
1451  if (cy != NULL)
1452  delete cy;
1453  cy = NULL;
1454  }
1455 
1456  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1457  ci = *it;
1458  if (ci != NULL)
1459  delete ci;
1460  ci = NULL;
1461  }
1462 
1463  lines[i].clear();
1464  cylinders[i].clear();
1465  circles[i].clear();
1466  }
1467  }
1468 
1469  // compute_interaction=1;
1470  nline = 0;
1471  ncylinder = 0;
1472  ncircle = 0;
1473  // lambda = 1;
1474  nbvisiblepolygone = 0;
1475 
1476  // Reinit common parts
1477  faces.reset();
1478 
1479  loadModel(cad_name, verbose, T);
1480 
1481  m_cMo = cMo;
1482  init(I);
1483 }
1484 
1485 #elif !defined(VISP_BUILD_SHARED_LIBS)
1486 // Work around to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1487 // no symbols
1488 void dummy_vpMbEdgeKltTracker(){};
1489 #endif // VISP_HAVE_OPENCV
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:307
unsigned int getRows() const
Definition: vpArray2D.h:292
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getHeight() const
Definition: vpImage.h:189
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition: vpMath.h:116
static double sqr(double x)
Definition: vpMath.h:122
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:367
static double deg(double rad)
Definition: vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:447
double m_thresholdKLT
The threshold used in the robust estimation of KLT.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setClipping(const unsigned int &flags)
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)
virtual void setFarClippingDistance(const double &dist)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void initCylinder(const vpPoint &, const vpPoint &, double r, int idFace, const std::string &name="")
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void computeVVSInit()
vpColVector m_w_mbt
Robust weights for Edge.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w_mbt, vpColVector &w_klt, unsigned int lvl=0)
unsigned int trackFirstLoop(const vpImage< unsigned char > &I, vpColVector &factor, unsigned int lvl=0)
void postTrackingMbt(vpColVector &w, unsigned int level=0)
void trackSecondLoop(const vpImage< unsigned char > &I, vpMatrix &L, vpColVector &_error, const vpHomogeneousMatrix &cMo, unsigned int lvl=0)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
unsigned int initMbtTracking(unsigned int level=0)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void init(const vpImage< unsigned char > &I)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpColVector m_error_hybrid
(s - s*)
virtual void setNearClippingDistance(const double &dist)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double r, int idFace=0, const std::string &name="")
vpColVector m_w_klt
Robust weights for KLT.
virtual void track(const vpImage< unsigned char > &I)
double m_thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
unsigned int m_maxIterKlt
The maximum iteration of the virtual visual servoing stage.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpColVector m_w_hybrid
Robust weights.
void upScale(const unsigned int _scale)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
std::vector< std::list< vpMbtDistanceLine * > > lines
vpMe me
The moving edges parameters.
void computeProjectionError(const vpImage< unsigned char > &_I)
unsigned int ncylinder
void downScale(const unsigned int _scale)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
unsigned int scaleLevel
void trackMovingEdge(const vpImage< unsigned char > &I)
std::vector< const vpImage< unsigned char > * > Ipyramid
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
unsigned int ncircle
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
void updateMovingEdge(const vpImage< unsigned char > &I)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setMovingEdge(const vpMe &me)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int nline
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool isAppearing(unsigned int i)
vpMbScanLine & getMbScanLineRenderer()
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void displayOgre(const vpHomogeneousMatrix &cMo)
std::list< vpMbtDistanceKltCylinder * > kltCylinders
vpHomogeneousMatrix c0Mo
Initial pose.
vpHomogeneousMatrix ctTc0
std::list< vpMbtDistanceKltPoints * > kltPolygons
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
cv::Mat cur
Temporary OpenCV image for fast conversion.
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
vpKltOpencv tracker
Points tracker.
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
void preTracking(const vpImage< unsigned char > &I)
unsigned int m_nbInfos
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
virtual void reinit(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void setCameraParameters(const vpCameraParameters &cam)
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void init(const vpImage< unsigned char > &I)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
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)
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
bool computeProjError
Definition: vpMbTracker.h:133
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
virtual void setLod(bool useLod, const std::string &name="")
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
vpColVector error
The error vector.
unsigned int nbFeature
The number of moving edges.
vpMatrix L
The interaction matrix.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void setMeanWeight(double _wmean)
void displayMovingEdges(const vpImage< unsigned char > &I)
bool Reinit
Indicates if the circle has to be reinitialized.
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void setMeanWeight1(double wmean)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(double wmean)
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int nbFeature
The number of moving edges.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
unsigned int getCurrentNumberPoints() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
unsigned int getCurrentNumberPoints() const
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
Manage the line of a polygon used in the model-based tracker.
std::vector< unsigned int > nbFeature
The number of moving edges.
void displayMovingEdges(const vpImage< unsigned char > &I)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isVisible() const
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
bool isTracked() const
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setMeanWeight(double w_mean)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:67
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
Parse an Xml file to extract configuration parameters of a mbtConfig object.
unsigned int getKltMaxFeatures() const
void setKltMinDistance(const double &mD)
unsigned int getKltBlockSize() const
void getCameraParameters(vpCameraParameters &cam) const
void setEdgeMe(const vpMe &ecm)
void setKltMaskBorder(const unsigned int &mb)
void getEdgeMe(vpMe &ecm) const
double getLodMinLineLengthThreshold() const
unsigned int getKltMaskBorder() const
void setAngleDisappear(const double &adisappear)
void setKltPyramidLevels(const unsigned int &pL)
void setKltWindowSize(const unsigned int &w)
void setKltMaxFeatures(const unsigned int &mF)
void setAngleAppear(const double &aappear)
void setKltBlockSize(const unsigned int &bs)
void setKltHarrisParam(const double &hp)
void parse(const std::string &filename)
void setKltQuality(const double &q)
unsigned int getKltPyramidLevels() const
unsigned int getKltWindowSize() const
void setCameraParameters(const vpCameraParameters &cam)
double getLodMinPolygonAreaThreshold() const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:67
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:80
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:74
vpMeSiteState getState() const
Definition: vpMeSite.h:189
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:175
Definition: vpMe.h:119
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:63
Error that can be emited by the vpTracker class and its derivates.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:393