Visual Servoing Platform  version 3.2.0 under development (2019-01-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 
47 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
48 
50  : thresholdKLT(2.), thresholdMBT(2.), m_maxIterKlt(30), w_mbt(), w_klt(), m_error_hybrid(), m_w_hybrid()
51 {
52  computeCovariance = false;
53 
56 
57 #ifdef VISP_HAVE_OGRE
58  faces.getOgreContext()->setWindowName("MBT Hybrid");
59 #endif
60 
61  m_lambda = 0.8;
62  m_maxIter = 200;
63 }
64 
70 
78 {
80 
82 
84 
85  unsigned int i = (unsigned int)scales.size();
86  do {
87  i--;
88  if (scales[i]) {
89  downScale(i);
91  upScale(i);
92  }
93  } while (i != 0);
94 
96 }
97 
108 {
109  vpMbKltTracker::setPose(I, cdMo);
110 
111  resetMovingEdge();
112 
113  if (useScanLine) {
114  cam.computeFov(I.getWidth(), I.getHeight());
117  }
118 
119  initPyramid(I, Ipyramid);
120 
121  unsigned int i = (unsigned int)scales.size();
122  do {
123  i--;
124  if (scales[i]) {
125  downScale(i);
127  upScale(i);
128  }
129  } while (i != 0);
130 
132 }
133 
139 {
142 }
143 
144 unsigned int vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
145 {
146  if (lvl >= scales.size() || !scales[lvl]) {
147  throw vpException(vpException::dimensionError, "lvl not used.");
148  }
149 
150  unsigned int nbrow = 0;
151  for (std::list<vpMbtDistanceLine *>::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
152  vpMbtDistanceLine *l = *it;
153 
154  if (l->isTracked()) {
156  nbrow += l->nbFeatureTotal;
157  }
158  }
159 
160  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
161  ++it) {
162  vpMbtDistanceCylinder *cy = *it;
163 
164  if (cy->isTracked()) {
166  nbrow += cy->nbFeature;
167  }
168  }
169 
170  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
171  vpMbtDistanceCircle *ci = *it;
172 
173  if (ci->isTracked()) {
175  nbrow += ci->nbFeature;
176  }
177  }
178 
179  return nbrow;
180 }
181 
246 void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile)
247 {
248  // Load projection error config
249  vpMbTracker::loadConfigFile(configFile);
250 
251 #ifdef VISP_HAVE_XML2
253 
254  xmlp.setCameraParameters(cam);
257 
258  xmlp.setMovingEdge(me);
259 
260  xmlp.setMaxFeatures(10000);
261  xmlp.setWindowSize(5);
262  xmlp.setQuality(0.01);
263  xmlp.setMinDistance(5);
264  xmlp.setHarrisParam(0.01);
265  xmlp.setBlockSize(3);
266  xmlp.setPyramidLevels(3);
268 
269  try {
270  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
271  xmlp.parse(configFile.c_str());
272  } catch (...) {
273  vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
274  throw vpException(vpException::ioError, "problem to parse configuration file.");
275  }
276 
277  vpCameraParameters camera;
278  xmlp.getCameraParameters(camera);
279  setCameraParameters(camera);
280 
283 
284  if (xmlp.hasNearClippingDistance())
286 
287  if (xmlp.hasFarClippingDistance())
289 
290  if (xmlp.getFovClipping()) {
292  }
293 
294  useLodGeneral = xmlp.getLodState();
297 
298  applyLodSettingInConfig = false;
299  if (this->getNbPolygon() > 0) {
304  }
305 
306  vpMe meParser;
307  xmlp.getMe(meParser);
309 
311  tracker.setWindowSize((int)xmlp.getWindowSize());
312  tracker.setQuality(xmlp.getQuality());
315  tracker.setBlockSize((int)xmlp.getBlockSize());
317  maskBorder = xmlp.getMaskBorder();
318 
319  // if(useScanLine)
320  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
321 
322 #else
323  vpTRACE("You need the libXML2 to read the config file %s", configFile.c_str());
324 #endif
325 }
326 
331  const unsigned int lvl)
332 {
333  postTrackingMbt(w_mbt, lvl);
334 
335  if (displayFeatures) {
336  if (lvl == 0) {
337  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
338  vpMbtDistanceLine *l = *it;
339  if (l->isVisible() && l->isTracked()) {
340  l->displayMovingEdges(I);
341  }
342  }
343 
344  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
345  ++it) {
346  vpMbtDistanceCylinder *cy = *it;
347  // A cylinder is always visible: #FIXME AY: Still valid?
348  if (cy->isTracked())
349  cy->displayMovingEdges(I);
350  }
351 
352  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
353  vpMbtDistanceCircle *ci = *it;
354  if (ci->isVisible() && ci->isTracked()) {
355  ci->displayMovingEdges(I);
356  }
357  }
358  }
359  }
360 
361  bool reInit = vpMbKltTracker::postTracking(I, w_klt);
362 
363  if (useScanLine) {
364  cam.computeFov(I.getWidth(), I.getHeight());
367  }
368 
370 
373 
374  if (computeProjError)
376 
377  if (reInit)
378  return true;
379 
380  return false;
381 }
382 
393 void vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
394 {
395  if (lvl >= scales.size() || !scales[lvl]) {
396  throw vpException(vpException::dimensionError, "_lvl not used.");
397  }
398 
399  unsigned int n = 0;
401  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
402  if ((*it)->isTracked()) {
403  l = *it;
404  unsigned int indexLine = 0;
405  double wmean = 0;
406 
407  for (size_t a = 0; a < l->meline.size(); a++) {
408  std::list<vpMeSite>::iterator itListLine;
409  if (l->nbFeature[a] > 0)
410  itListLine = l->meline[a]->getMeList().begin();
411 
412  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
413  wmean += w[n + indexLine];
414  vpMeSite p = *itListLine;
415  if (w[n + indexLine] < 0.5) {
417  *itListLine = p;
418  }
419 
420  ++itListLine;
421  indexLine++;
422  }
423  }
424 
425  n += l->nbFeatureTotal;
426 
427  if (l->nbFeatureTotal != 0)
428  wmean /= l->nbFeatureTotal;
429  else
430  wmean = 1;
431 
432  l->setMeanWeight(wmean);
433 
434  if (wmean < 0.8)
435  l->Reinit = true;
436  }
437  }
438 
439  // Same thing with cylinders as with lines
441  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
442  ++it) {
443  if ((*it)->isTracked()) {
444  cy = *it;
445  double wmean = 0;
446  std::list<vpMeSite>::iterator itListCyl1;
447  std::list<vpMeSite>::iterator itListCyl2;
448  if (cy->nbFeature > 0) {
449  itListCyl1 = cy->meline1->getMeList().begin();
450  itListCyl2 = cy->meline2->getMeList().begin();
451  }
452 
453  wmean = 0;
454  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
455  wmean += w[n + i];
456  vpMeSite p = *itListCyl1;
457  if (w[n + i] < 0.5) {
459 
460  *itListCyl1 = p;
461  }
462 
463  ++itListCyl1;
464  }
465 
466  if (cy->nbFeaturel1 != 0)
467  wmean /= cy->nbFeaturel1;
468  else
469  wmean = 1;
470 
471  cy->setMeanWeight1(wmean);
472 
473  if (wmean < 0.8) {
474  cy->Reinit = true;
475  }
476 
477  wmean = 0;
478  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
479  wmean += w[n + i];
480  vpMeSite p = *itListCyl2;
481  if (w[n + i] < 0.5) {
483 
484  *itListCyl2 = p;
485  }
486 
487  ++itListCyl2;
488  }
489 
490  if (cy->nbFeaturel2 != 0)
491  wmean /= cy->nbFeaturel2;
492  else
493  wmean = 1;
494 
495  cy->setMeanWeight2(wmean);
496 
497  if (wmean < 0.8) {
498  cy->Reinit = true;
499  }
500 
501  n += cy->nbFeature;
502  }
503  }
504 
505  // Same thing with circles as with lines
507  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
508  if ((*it)->isTracked()) {
509  ci = *it;
510  double wmean = 0;
511  std::list<vpMeSite>::iterator itListCir;
512 
513  if (ci->nbFeature > 0) {
514  itListCir = ci->meEllipse->getMeList().begin();
515  }
516 
517  wmean = 0;
518  for (unsigned int i = 0; i < ci->nbFeature; i++) {
519  wmean += w[n + i];
520  vpMeSite p = *itListCir;
521  if (w[n + i] < 0.5) {
523 
524  *itListCir = p;
525  }
526 
527  ++itListCir;
528  }
529 
530  if (ci->nbFeature != 0)
531  wmean /= ci->nbFeature;
532  else
533  wmean = 1;
534 
535  ci->setMeanWeight(wmean);
536 
537  if (wmean < 0.8) {
538  ci->Reinit = true;
539  }
540 
541  n += ci->nbFeature;
542  }
543  }
544 }
545 
554 void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
555  const unsigned int lvl)
556 {
557  vpColVector factor;
558  nbrow = trackFirstLoop(I, factor, lvl);
559 
560  if (nbrow < 4 && nbInfos < 4) {
561  throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
562  } else if (nbrow < 4)
563  nbrow = 0;
564 
565  unsigned int totalNbRows = nbrow + 2 * nbInfos;
566  double residu = 0;
567  double residu_1 = -1;
568  unsigned int iter = 0;
569 
570  vpMatrix L(totalNbRows, 6);
571  vpMatrix L_mbt, L_klt; // interaction matrix
572  vpColVector weighted_error(totalNbRows);
573  vpColVector R_mbt, R_klt; // residu
574  vpMatrix L_true;
575  vpMatrix LVJ_true;
576 
577  if (nbrow != 0) {
578  L_mbt.resize(nbrow, 6, false, false);
579  R_mbt.resize(nbrow, false);
580  }
581 
582  if (nbInfos != 0) {
583  L_klt.resize(2 * nbInfos, 6, false, false);
584  R_klt.resize(2 * nbInfos, false);
585  }
586 
587  vpColVector v; // "speed" for VVS
588  vpRobust robust_mbt(0), robust_klt(0);
589  vpHomography H;
590 
591  vpMatrix LTL;
592  vpColVector LTR;
593 
594  double factorMBT; // = 1.0;
595  double factorKLT; // = 1.0;
596 
597  // More efficient weight repartition for hybrid tracker should come soon...
598  // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
599  // factorKLT = 1.0 - factorMBT;
600  factorMBT = 0.35;
601  factorKLT = 0.65;
602 
603  if (nbrow < 4)
604  factorKLT = 1.;
605  if (nbInfos < 4)
606  factorMBT = 1.;
607 
608  double residuMBT = 0;
609  double residuKLT = 0;
610 
611  vpHomogeneousMatrix cMoPrev;
612  vpHomogeneousMatrix ctTc0_Prev;
613  vpColVector m_error_prev;
614  vpColVector m_w_prev;
615 
616  // Init size
617  m_error_hybrid.resize(totalNbRows, false);
618  m_w_hybrid.resize(totalNbRows, false);
619 
620  if (nbrow != 0) {
621  w_mbt.resize(nbrow, false);
622  w_mbt = 1; // needed in vpRobust::psiTukey()
623  robust_mbt.resize(nbrow);
624  }
625 
626  if (nbInfos != 0) {
627  w_klt.resize(2 * nbInfos, false);
628  w_klt = 1; // needed in vpRobust::psiTukey()
629  robust_klt.resize(2 * nbInfos);
630  }
631 
632  double mu = m_initialMu;
633 
634  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
635  if (nbrow >= 4)
636  trackSecondLoop(I, L_mbt, R_mbt, cMo, lvl);
637 
638  if (nbInfos >= 4) {
639  unsigned int shift = 0;
640 
641  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
642  it != vpMbKltTracker::kltPolygons.end(); ++it) {
643  vpMbtDistanceKltPoints *kltpoly = *it;
644  if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
645  vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
646  vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
647  kltpoly->computeHomography(ctTc0, H);
648  kltpoly->computeInteractionMatrixAndResidu(subR, subL);
649  shift += 2 * kltpoly->getCurrentNumberPoints();
650  }
651  }
652 
653  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
654  ++it) {
655  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
656 
657  if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
658  vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
659  vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
660  try {
661  kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
662  } catch (...) {
663  throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
664  }
665 
666  shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
667  }
668  }
669  }
670 
671  /* residuals */
672  if (nbrow > 3) {
673  m_error_hybrid.insert(0, R_mbt);
674  }
675 
676  if (nbInfos > 3) {
677  m_error_hybrid.insert(nbrow, R_klt);
678  }
679 
680  unsigned int cpt = 0;
681  while (cpt < (nbrow + 2 * nbInfos)) {
682  if (cpt < (unsigned)nbrow) {
683  m_w_hybrid[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT);
684  } else {
685  m_w_hybrid[cpt] = (w_klt[cpt - nbrow] * factorKLT);
686  }
687  cpt++;
688  }
689 
690  bool reStartFromLastIncrement = false;
691  computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
692  &m_w_prev);
693  if (reStartFromLastIncrement) {
694  ctTc0 = ctTc0_Prev;
695  }
696 
697  if (!reStartFromLastIncrement) {
698  /* robust */
699  if (nbrow > 3) {
700  residuMBT = 0;
701  for (unsigned int i = 0; i < R_mbt.getRows(); i++)
702  residuMBT += fabs(R_mbt[i]);
703  residuMBT /= R_mbt.getRows();
704 
705  robust_mbt.setThreshold(thresholdMBT / cam.get_px());
706  robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, w_mbt);
707 
708  L.insert(L_mbt, 0, 0);
709  }
710 
711  if (nbInfos > 3) {
712  residuKLT = 0;
713  for (unsigned int i = 0; i < R_klt.getRows(); i++)
714  residuKLT += fabs(R_klt[i]);
715  residuKLT /= R_klt.getRows();
716 
717  robust_klt.setThreshold(thresholdKLT / cam.get_px());
718  robust_klt.MEstimator(vpRobust::TUKEY, R_klt, w_klt);
719 
720  L.insert(L_klt, nbrow, 0);
721  }
722 
723  unsigned int cpt = 0;
724  while (cpt < (nbrow + 2 * nbInfos)) {
725  if (cpt < (unsigned)nbrow) {
726  m_w_hybrid[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT);
727  } else {
728  m_w_hybrid[cpt] = (w_klt[cpt - nbrow] * factorKLT);
729  }
730  cpt++;
731  }
732 
733  if (computeCovariance) {
734  L_true = L;
735  if (!isoJoIdentity) {
737  cVo.buildFrom(cMo);
738  LVJ_true = (L * cVo * oJo);
739  }
740  }
741 
742  residu_1 = residu;
743  residu = 0;
744  double num = 0;
745  double den = 0;
746 
747  for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
748  num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
749  den += m_w_hybrid[i];
750 
751  weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
752  if (m_computeInteraction) {
753  for (unsigned int j = 0; j < 6; j += 1) {
754  L[i][j] *= m_w_hybrid[i];
755  }
756  }
757  }
758 
759  residu = sqrt(num / den);
760 
761  computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
762  &m_w_hybrid, &m_w_prev);
763 
764  cMoPrev = cMo;
765  ctTc0_Prev = ctTc0;
767  cMo = ctTc0 * c0Mo;
768  }
769 
770  iter++;
771  }
772 
774 }
775 
777 {
778  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
779 }
780 
782 {
783  throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
784  "computeVVSInteractionMatrixAndR"
785  "esidu() should not be called!");
786 }
787 
796 {
797  try {
799  } catch (...) {
800  }
801 
802  if (m_nbInfos >= 4) {
803  unsigned int old_maxIter = m_maxIter;
806  m_maxIter = old_maxIter;
807  } else {
808  m_nbInfos = 0;
809  // std::cout << "[Warning] Unable to init with KLT" << std::endl;
810  }
811 
813 
814  unsigned int nbrow = 0;
815  computeVVS(I, m_nbInfos, nbrow);
816 
817  if (postTracking(I, w_mbt, w_klt)) {
819 
820  // AY : Removed as edge tracked, if necessary, is reinitialized in
821  // postTracking()
822 
823  // initPyramid(I, Ipyramid);
824 
825  // unsigned int i = (unsigned int)scales.size();
826  // do {
827  // i--;
828  // if(scales[i]){
829  // downScale(i);
830  // initMovingEdge(*Ipyramid[i], cMo);
831  // upScale(i);
832  // }
833  // } while(i != 0);
834 
835  // cleanPyramid(Ipyramid);
836  }
837 }
838 
840  const unsigned int lvl)
841 {
845 
846  if (lvl >= scales.size() || !scales[lvl]) {
847  throw vpException(vpException::dimensionError, "_lvl not used.");
848  }
849 
850  unsigned int nbrow = initMbtTracking(lvl);
851 
852  if (nbrow == 0) {
853  // throw vpTrackingException(vpTrackingException::notEnoughPointError,
854  // "Error: not enough features in the interaction matrix...");
855  return nbrow;
856  }
857 
858  factor.resize(nbrow, false);
859  factor = 1;
860 
861  unsigned int n = 0;
862  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
863  if ((*it)->isTracked()) {
864  l = *it;
866 
867  double fac = 1;
868  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
869  ++itindex) {
870  int index = *itindex;
871  if (l->hiddenface->isAppearing((unsigned int)index)) {
872  fac = 0.2;
873  break;
874  }
875  if (l->closeToImageBorder(I, 10)) {
876  fac = 0.1;
877  break;
878  }
879  }
880 
881  unsigned int indexFeature = 0;
882  for (size_t a = 0; a < l->meline.size(); a++) {
883  std::list<vpMeSite>::const_iterator itListLine;
884  if (l->meline[a] != NULL) {
885  itListLine = l->meline[a]->getMeList().begin();
886 
887  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
888  factor[n + i] = fac;
889  vpMeSite site = *itListLine;
890  if (site.getState() != vpMeSite::NO_SUPPRESSION)
891  factor[n + i] = 0.2;
892  ++itListLine;
893  indexFeature++;
894  }
895  n += l->nbFeature[a];
896  }
897  }
898  }
899  }
900 
901  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
902  ++it) {
903  if ((*it)->isTracked()) {
904  cy = *it;
906  double fac = 1.0;
907 
908  std::list<vpMeSite>::const_iterator itCyl1;
909  std::list<vpMeSite>::const_iterator itCyl2;
910  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
911  itCyl1 = cy->meline1->getMeList().begin();
912  itCyl2 = cy->meline2->getMeList().begin();
913  }
914 
915  for (unsigned int i = 0; i < cy->nbFeature; i++) {
916  factor[n + i] = fac;
917  vpMeSite site;
918  if (i < cy->nbFeaturel1) {
919  site = *itCyl1;
920  ++itCyl1;
921  } else {
922  site = *itCyl2;
923  ++itCyl2;
924  }
925  if (site.getState() != vpMeSite::NO_SUPPRESSION)
926  factor[n + i] = 0.2;
927  }
928 
929  n += cy->nbFeature;
930  }
931  }
932 
933  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
934  if ((*it)->isTracked()) {
935  ci = *it;
937  double fac = 1.0;
938 
939  std::list<vpMeSite>::const_iterator itCir;
940  if (ci->meEllipse != NULL) {
941  itCir = ci->meEllipse->getMeList().begin();
942  }
943 
944  for (unsigned int i = 0; i < ci->nbFeature; i++) {
945  factor[n + i] = fac;
946  vpMeSite site = *itCir;
947  if (site.getState() != vpMeSite::NO_SUPPRESSION)
948  factor[n + i] = 0.2;
949  ++itCir;
950  }
951 
952  n += ci->nbFeature;
953  }
954  }
955 
956  return nbrow;
957 }
958 
960  vpHomogeneousMatrix &cMo_, const unsigned int lvl)
961 {
965 
966  unsigned int n = 0;
967  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
968  if ((*it)->isTracked()) {
969  l = *it;
971  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
972  for (unsigned int j = 0; j < 6; j++) {
973  L[n + i][j] = l->L[i][j];
974  error[n + i] = l->error[i];
975  }
976  }
977  n += l->nbFeatureTotal;
978  }
979  }
980 
981  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
982  ++it) {
983  if ((*it)->isTracked()) {
984  cy = *it;
985  cy->computeInteractionMatrixError(cMo_, I);
986  for (unsigned int i = 0; i < cy->nbFeature; i++) {
987  for (unsigned int j = 0; j < 6; j++) {
988  L[n + i][j] = cy->L[i][j];
989  error[n + i] = cy->error[i];
990  }
991  }
992  n += cy->nbFeature;
993  }
994  }
995  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
996  if ((*it)->isTracked()) {
997  ci = *it;
999  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1000  for (unsigned int j = 0; j < 6; j++) {
1001  L[n + i][j] = ci->L[i][j];
1002  error[n + i] = ci->error[i];
1003  }
1004  }
1005 
1006  n += ci->nbFeature;
1007  }
1008  }
1009 }
1010 
1017 {
1018  this->cam = camera;
1019 
1022 }
1023 
1031 {
1034 }
1042 {
1045 }
1046 
1057 void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
1058  const int idFace, const std::string &name)
1059 {
1060  vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1061 }
1062 
1073 void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
1074  const std::string &name)
1075 {
1076  vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1077  vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1078 }
1079 
1092  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1093  const bool displayFullModel)
1094 {
1095  for (unsigned int i = 0; i < scales.size(); i += 1) {
1096  if (scales[i]) {
1097  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1098  ++it) {
1099  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1100  }
1101 
1102  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1103  it != cylinders[scaleLevel].end(); ++it) {
1104  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1105  }
1106 
1107  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1108  it != circles[scaleLevel].end(); ++it) {
1109  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1110  }
1111 
1112  break; // displaying model on one scale only
1113  }
1114  }
1115 
1116  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1117  vpMbtDistanceKltPoints *kltpoly = *it;
1118  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
1119  kltpoly->displayPrimitive(I);
1120  }
1121  }
1122 
1123  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1124  ++it) {
1125  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1126  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1127  kltPolyCylinder->displayPrimitive(I);
1128  }
1129 
1130 #ifdef VISP_HAVE_OGRE
1131  if (useOgre)
1132  faces.displayOgre(cMo_);
1133 #endif
1134 }
1135 
1148  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1149  const bool displayFullModel)
1150 {
1151  for (unsigned int i = 0; i < scales.size(); i += 1) {
1152  if (scales[i]) {
1153  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1154  ++it) {
1155  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1156  }
1157 
1158  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1159  it != cylinders[scaleLevel].end(); ++it) {
1160  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1161  }
1162 
1163  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1164  it != circles[scaleLevel].end(); ++it) {
1165  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1166  }
1167 
1168  break; // displaying model on one scale only
1169  }
1170  }
1171 
1172  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1173  vpMbtDistanceKltPoints *kltpoly = *it;
1174  if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
1175  kltpoly->displayPrimitive(I);
1176  }
1177  }
1178 
1179  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1180  ++it) {
1181  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1182  if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1183  kltPolyCylinder->displayPrimitive(I);
1184  }
1185 
1186 #ifdef VISP_HAVE_OGRE
1187  if (useOgre)
1188  faces.displayOgre(cMo_);
1189 #endif
1190 }
1191 
1204 void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1205  const vpHomogeneousMatrix &cMo_, const bool verbose,
1206  const vpHomogeneousMatrix &T)
1207 {
1208  // Reinit klt
1209  #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1210  if (cur != NULL) {
1211  cvReleaseImage(&cur);
1212  cur = NULL;
1213  }
1214  #endif
1215 
1216  // delete the Klt Polygon features
1217  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1218  vpMbtDistanceKltPoints *kltpoly = *it;
1219  if (kltpoly != NULL) {
1220  delete kltpoly;
1221  }
1222  kltpoly = NULL;
1223  }
1224  kltPolygons.clear();
1225 
1226  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1227  ++it) {
1228  vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1229  if (kltPolyCylinder != NULL) {
1230  delete kltPolyCylinder;
1231  }
1232  kltPolyCylinder = NULL;
1233  }
1234  kltCylinders.clear();
1235 
1236  // delete the structures used to display circles
1237  vpMbtDistanceCircle *ci;
1238  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1239  ci = *it;
1240  if (ci != NULL) {
1241  delete ci;
1242  }
1243  ci = NULL;
1244  }
1245 
1246  circles_disp.clear();
1247 
1248  firstInitialisation = true;
1249 
1250  // Reinit edge
1251  vpMbtDistanceLine *l;
1253 
1254  for (unsigned int i = 0; i < scales.size(); i += 1) {
1255  if (scales[i]) {
1256  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1257  l = *it;
1258  if (l != NULL)
1259  delete l;
1260  l = NULL;
1261  }
1262 
1263  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1264  ++it) {
1265  cy = *it;
1266  if (cy != NULL)
1267  delete cy;
1268  cy = NULL;
1269  }
1270 
1271  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1272  ci = *it;
1273  if (ci != NULL)
1274  delete ci;
1275  ci = NULL;
1276  }
1277 
1278  lines[i].clear();
1279  cylinders[i].clear();
1280  circles[i].clear();
1281  }
1282  }
1283 
1284  // compute_interaction=1;
1285  nline = 0;
1286  ncylinder = 0;
1287  ncircle = 0;
1288  // lambda = 1;
1289  nbvisiblepolygone = 0;
1290 
1291  // Reinit common parts
1292  faces.reset();
1293 
1294  loadModel(cad_name, verbose, T);
1295 
1296  this->cMo = cMo_;
1297  init(I);
1298 }
1299 
1300 #elif !defined(VISP_BUILD_SHARED_LIBS)
1301 // Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1302 // no symbols
1303 void dummy_vpMbEdgeKltTracker(){};
1304 #endif // VISP_HAVE_OPENCV
bool m_computeInteraction
Definition: vpMbTracker.h:191
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void trackSecondLoop(const vpImage< unsigned char > &I, vpMatrix &L, vpColVector &_error, vpHomogeneousMatrix &cMo, const unsigned int lvl=0)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double r, const int idFace=0, const std::string &name="")
bool computeProjError
Definition: vpMbTracker.h:139
unsigned int ncylinder
unsigned int getMaskBorder() const
void setQuality(const double &q)
void postTrackingMbt(vpColVector &w, const unsigned int level=0)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void displayMovingEdges(const vpImage< unsigned char > &I)
void displayPrimitive(const vpImage< unsigned char > &_I)
void setMovingEdge(const vpMe &me)
double getHarrisParam() const
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void upScale(const unsigned int _scale)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
void setHarrisFreeParameter(double harris_k)
bool isAppearing(const unsigned int i)
unsigned int getWidth() const
Definition: vpImage.h:239
void getCameraParameters(vpCameraParameters &_cam) const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:149
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(const double wmean)
Implementation of an homogeneous matrix and operations on such kind of matrices.
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
unsigned int scaleLevel
virtual void loadConfigFile(const std::string &configFile)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
vpMatrix L
The interaction matrix.
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
#define vpERROR_TRACE
Definition: vpDebug.h:393
virtual void loadConfigFile(const std::string &configFile)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setMaxFeatures(const int maxCount)
std::list< vpMbtDistanceKltCylinder * > kltCylinders
unsigned int m_maxIterKlt
The maximum iteration of the virtual visual servoing stage.
virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name="")
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:119
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
unsigned int getBlockSize() const
void displayMovingEdges(const vpImage< unsigned char > &I)
double getNearClippingDistance() const
unsigned int ncircle
double thresholdKLT
The threshold used in the robust estimation of KLT.
std::vector< const vpImage< unsigned char > * > Ipyramid
vpColVector error
The error vector.
void getMe(vpMe &_ecm) const
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()
Definition: vpMe.h:60
unsigned int getMaxFeatures() 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:161
bool getLodState() const
virtual void setCameraParameters(const vpCameraParameters &camera)
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:134
vpMe me
The moving edges parameters.
void updateMovingEdge(const vpImage< unsigned char > &I)
vpColVector w_mbt
Robust weights for Edge.
virtual void reinit(const vpImage< unsigned char > &I)
virtual void setFarClippingDistance(const double &dist)
void downScale(const unsigned int _scale)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
bool hasNearClippingDistance() const
Class that defines what is a point.
Definition: vpPoint.h:58
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
Definition: vpMeSite.h:188
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:117
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void setQuality(double qualityLevel)
virtual void init(const vpImage< unsigned char > &I)
vpHomogeneousMatrix ctTc0
double getFarClippingDistance() const
unsigned int nbFeaturel1
The number of moving edges on line 1.
bool hasFarClippingDistance() const
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpAROgre * getOgreContext()
void setPyramidLevels(const unsigned int &pL)
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
virtual void computeVVSInit()
Manage a circle used in the model-based tracker.
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4570
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:121
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.
unsigned int getWindowSize() const
Error that can be emited by the vpTracker class and its derivates.
double getAngleDisappear() const
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
void displayPrimitive(const vpImage< unsigned char > &_I)
void setWindowSize(const unsigned int &w)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:164
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:108
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:183
vpColVector w_klt
Robust weights for KLT.
void setAngleDisappear(const double &adisappear)
double getMinPolygonAreaThreshold() const
vpColVector error
The error vector.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
bool isVisible() const
virtual void track(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Generic class defining intrinsic camera parameters.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
bool isTracked() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:199
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:193
void setHarrisParam(const double &hp)
unsigned int getRows() const
Definition: vpArray2D.h:156
void setMinDistance(const double &mD)
void setCameraParameters(const vpCameraParameters &_cam)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:151
vpKltOpencv tracker
Points tracker.
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), const vpImage< unsigned char > &I=vpImage< unsigned char >())
void setPyramidLevels(const int pyrMaxLevel)
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:174
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:195
double get_px() const
static double rad(double deg)
Definition: vpMath.h:102
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void setMeanWeight1(const double wmean)
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
virtual void setCameraParameters(const vpCameraParameters &cam)
void computeProjectionError(const vpImage< unsigned char > &_I)
void insert(unsigned int i, const vpColVector &v)
cv::Mat cur
Temporary OpenCV image for fast conversion.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
double getMinLineLengthThreshold() const
void setMaxFeatures(const unsigned int &mF)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
void setWindowSize(const int winSize)
double getQuality() const
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:185
static double deg(double rad)
Definition: vpMath.h:95
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:144
virtual void initCylinder(const vpPoint &, const vpPoint &, const double r, const int idFace, const std::string &name="")
void setCameraParameters(const vpCameraParameters &cam)
void preTracking(const vpImage< unsigned char > &I)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:181
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setAngleAppear(const double &aappear)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpColVector m_error_hybrid
(s - s*)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
unsigned int initMbtTracking(const unsigned int level=0)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
vpHomogeneousMatrix inverse() const
unsigned int trackFirstLoop(const vpImage< unsigned char > &I, vpColVector &factor, const unsigned int lvl=0)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setBlockSize(const int blockSize)
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
void setMeanWeight(const double _wmean)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:153
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl=0)
unsigned int getHeight() const
Definition: vpImage.h:178
void setMeanWeight(const double w_mean)
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int m_nbInfos
bool Reinit
Indicates if the line has to be reinitialized.
virtual void init(const vpImage< unsigned char > &I)
unsigned int getCurrentNumberPoints() const
std::vector< vpMbtMeLine * > meline
The moving edge container.
void setBlockSize(const unsigned int &bs)
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:159
unsigned int nline
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
double getAngleAppear() const
unsigned int nbFeaturel2
The number of moving edges on line 2.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
unsigned int getCurrentNumberPoints() const
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
double getMinDistance() const
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
virtual void computeVVSInteractionMatrixAndResidu()
bool getFovClipping() const
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:178
vpMatrix L
The interaction matrix.
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
std::list< vpMbtDistanceKltPoints * > kltPolygons
void setMovingEdge(const vpMe &_ecm)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:123
void setMaskBorder(const unsigned int &mb)
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
vpHomogeneousMatrix c0Mo
Initial pose.
unsigned int getPyramidLevels() const
double thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setNearClippingDistance(const double &dist)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
virtual void setLod(const bool useLod, const std::string &name="")
void parse(const std::string &filename)
void computeFov(const unsigned int &w, const unsigned int &h)