Visual Servoing Platform  version 3.2.0
vpMbDepthNormalTracker.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  * Model-based tracker using depth normal features.
33  *
34  *****************************************************************************/
35 
36 #include <iostream>
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #ifdef VISP_HAVE_PCL
41 #include <pcl/point_cloud.h>
42 #endif
43 
44 #include <visp3/core/vpDisplay.h>
45 #include <visp3/core/vpExponentialMap.h>
46 #include <visp3/core/vpTrackingException.h>
47 #include <visp3/mbt/vpMbDepthNormalTracker.h>
48 #include <visp3/mbt/vpMbtXmlGenericParser.h>
49 
50 #if DEBUG_DISPLAY_DEPTH_NORMAL
51 #include <visp3/gui/vpDisplayGDI.h>
52 #include <visp3/gui/vpDisplayX.h>
53 #endif
54 
56  : m_depthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION),
57  m_depthNormalHiddenFacesDisplay(), m_depthNormalI_dummyVisibility(), m_depthNormalListOfActiveFaces(),
58  m_depthNormalListOfDesiredFeatures(), m_depthNormalFaces(), m_depthNormalPclPlaneEstimationMethod(2),
59  m_depthNormalPclPlaneEstimationRansacMaxIter(200), m_depthNormalPclPlaneEstimationRansacThreshold(0.001),
60  m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2), m_depthNormalUseRobust(false), m_error_depthNormal(),
61  m_L_depthNormal(), m_robust_depthNormal(), m_w_depthNormal(), m_weightedError_depthNormal()
62 #if DEBUG_DISPLAY_DEPTH_NORMAL
63  ,
64  m_debugDisp_depthNormal(NULL), m_debugImage_depthNormal()
65 #endif
66 {
67 #ifdef VISP_HAVE_OGRE
68  faces.getOgreContext()->setWindowName("MBT Depth");
69 #endif
70 
71 #if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_NORMAL
72  m_debugDisp_depthNormal = new vpDisplayX;
73 #elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_NORMAL
74  m_debugDisp_depthNormal = new vpDisplayGDI;
75 #endif
76 }
77 
79 {
80  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
81  delete m_depthNormalFaces[i];
82  }
83 }
84 
85 void vpMbDepthNormalTracker::addFace(vpMbtPolygon &polygon, const bool alreadyClose)
86 {
87  if (polygon.nbpt < 3) {
88  return;
89  }
90 
91  // Copy hidden faces
93 
94  vpMbtFaceDepthNormal *normal_face = new vpMbtFaceDepthNormal;
95  normal_face->m_hiddenFace = &faces;
96  normal_face->m_polygon = &polygon;
97  normal_face->m_cam = cam;
98  normal_face->m_useScanLine = useScanLine;
99  normal_face->m_clippingFlag = clippingFlag;
100  normal_face->m_distNearClip = distNearClip;
101  normal_face->m_distFarClip = distFarClip;
106 
107  // Add lines that compose the face
108  unsigned int nbpt = polygon.getNbPoint();
109  if (nbpt > 0) {
110  for (unsigned int i = 0; i < nbpt - 1; i++) {
111  normal_face->addLine(polygon.p[i], polygon.p[i + 1], &m_depthNormalHiddenFacesDisplay, polygon.getIndex(),
112  polygon.getName());
113  }
114 
115  if (!alreadyClose) {
116  // Add last line that closes the face
117  normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], &m_depthNormalHiddenFacesDisplay, polygon.getIndex(),
118  polygon.getName());
119  }
120  }
121 
122  // Construct a vpPlane in object frame
123  vpPoint pts[3];
124  pts[0] = polygon.p[0];
125  pts[1] = polygon.p[1];
126  pts[2] = polygon.p[2];
127  normal_face->m_planeObject = vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame);
128 
129  m_depthNormalFaces.push_back(normal_face);
130 }
131 
132 void vpMbDepthNormalTracker::computeVisibility(const unsigned int width, const unsigned int height)
133 {
134  m_depthNormalI_dummyVisibility.resize(height, width);
135 
136  bool changed = false;
138 
139  if (useScanLine) {
140  // if (clippingFlag <= 2) {
141  // cam.computeFov(m_depthNormalI_dummyVisibility.getWidth(),
142  // m_depthNormalI_dummyVisibility.getHeight());
143  // }
144 
148  }
149 
150  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
151  it != m_depthNormalFaces.end(); ++it) {
152  vpMbtFaceDepthNormal *face_normal = *it;
153  face_normal->computeVisibility();
154  }
155 }
156 
158 {
159  double normRes = 0;
160  double normRes_1 = -1;
161  unsigned int iter = 0;
162 
163  computeVVSInit();
164  unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size());
165 
166  vpColVector error_prev(nb_features);
167  vpMatrix LTL;
168  vpColVector LTR, v;
169 
170  double mu = m_initialMu;
171  vpHomogeneousMatrix cMo_prev;
172 
173  bool isoJoIdentity_ = true;
175  vpMatrix L_true, LVJ_true;
176 
177  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
179 
180  bool reStartFromLastIncrement = false;
181  computeVVSCheckLevenbergMarquardt(iter, m_error_depthNormal, error_prev, cMo_prev, mu, reStartFromLastIncrement);
182 
183  if (!reStartFromLastIncrement) {
186 
187  if (computeCovariance) {
188  L_true = m_L_depthNormal;
189  if (!isoJoIdentity_) {
191  cVo.buildFrom(cMo);
192  LVJ_true = (m_L_depthNormal * (cVo * oJo));
193  }
194  }
195 
196  // Compute DoF only once
197  if (iter == 0) {
198  isoJoIdentity_ = true;
199  oJo.eye();
200 
201  // If all the 6 dof should be estimated, we check if the interaction
202  // matrix is full rank. If not we remove automatically the dof that
203  // cannot be estimated This is particularly useful when consering
204  // circles (rank 5) and cylinders (rank 4)
205  if (isoJoIdentity_) {
206  cVo.buildFrom(cMo);
207 
208  vpMatrix K; // kernel
209  unsigned int rank = (m_L_depthNormal * cVo).kernel(K);
210  if (rank == 0) {
211  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
212  }
213 
214  if (rank != 6) {
215  vpMatrix I; // Identity
216  I.eye(6);
217  oJo = I - K.AtA();
218 
219  isoJoIdentity_ = false;
220  }
221  }
222  }
223 
224  double num = 0.0, den = 0.0;
225  for (unsigned int i = 0; i < m_L_depthNormal.getRows(); i++) {
226  // Compute weighted errors and stop criteria
229  den += m_w_depthNormal[i];
230 
231  // weight interaction matrix
232  for (unsigned int j = 0; j < 6; j++) {
233  m_L_depthNormal[i][j] *= m_w_depthNormal[i];
234  }
235  }
236 
238  m_error_depthNormal, error_prev, LTR, mu, v);
239 
240  cMo_prev = cMo;
242 
243  normRes_1 = normRes;
244  normRes = sqrt(num / den);
245  }
246 
247  iter++;
248  }
249 
250  computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthNormal, cMo_prev, L_true, LVJ_true, m_error_depthNormal);
251 }
252 
254 {
255  unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size());
256 
257  m_L_depthNormal.resize(nb_features, 6, false, false);
258  m_error_depthNormal.resize(nb_features, false);
259  m_weightedError_depthNormal.resize(nb_features, false);
260 
261  m_w_depthNormal.resize(nb_features, false);
262  m_w_depthNormal = 1;
263 
264  m_robust_depthNormal.resize(nb_features);
266 }
267 
269 {
270  unsigned int cpt = 0;
271  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalListOfActiveFaces.begin();
272  it != m_depthNormalListOfActiveFaces.end(); ++it) {
273  vpMatrix L_face;
274  vpColVector features_face;
275  (*it)->computeInteractionMatrix(cMo, L_face, features_face);
276 
277  vpColVector face_error = features_face - m_depthNormalListOfDesiredFeatures[(size_t)cpt];
278 
279  m_error_depthNormal.insert(cpt * 3, face_error);
280  m_L_depthNormal.insert(L_face, cpt * 3, 0);
281 
282  cpt++;
283  }
284 }
285 
287  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
288  const bool displayFullModel)
289 {
290  vpCameraParameters c = cam_;
291 
292  bool changed = false;
294 
295  if (useScanLine) {
296  c.computeFov(I.getWidth(), I.getHeight());
297 
300  }
301 
302  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
303  it != m_depthNormalFaces.end(); ++it) {
304  vpMbtFaceDepthNormal *face_normal = *it;
305  face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
306 
307  if (displayFeatures) {
308  face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
309  }
310  }
311 }
312 
314  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
315  const bool displayFullModel)
316 {
317  vpCameraParameters c = cam_;
318 
319  bool changed = false;
320  vpImage<unsigned char> I_dummy;
321  vpImageConvert::convert(I, I_dummy);
323 
324  if (useScanLine) {
325  c.computeFov(I.getWidth(), I.getHeight());
326 
329  }
330 
331  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
332  it != m_depthNormalFaces.end(); ++it) {
333  vpMbtFaceDepthNormal *face_normal = *it;
334  face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
335 
336  if (displayFeatures) {
337  face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
338  }
339  }
340 }
341 
343 {
344  if (!modelInitialised) {
345  throw vpException(vpException::fatalError, "model not initialized");
346  }
347 
348  bool reInitialisation = false;
349  if (!useOgre) {
350  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
351  } else {
352 #ifdef VISP_HAVE_OGRE
353  if (!faces.isOgreInitialised()) {
356  faces.initOgre(cam);
357  // Turn off Ogre config dialog display for the next call to this
358  // function since settings are saved in the ogre.cfg file and used
359  // during the next call
360  ogreShowConfigDialog = false;
361  }
362 
363  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
364 #else
365  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
366 #endif
367  }
368 
369  if (useScanLine || clippingFlag > 3)
370  cam.computeFov(I.getWidth(), I.getHeight());
371 
373 }
374 
375 void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile)
376 {
377 #ifdef VISP_HAVE_XML2
379 
380  xmlp.setCameraParameters(cam);
383 
390 
391  try {
392  std::cout << " *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
393  xmlp.parse(configFile);
394  } catch (const vpException &e) {
395  std::cerr << "Exception: " << e.what() << std::endl;
396  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
397  }
398 
399  vpCameraParameters camera;
400  xmlp.getCameraParameters(camera);
401  setCameraParameters(camera);
402 
405 
406  if (xmlp.hasNearClippingDistance())
408 
409  if (xmlp.hasFarClippingDistance())
411 
412  if (xmlp.getFovClipping())
414 
420 #else
421  std::cerr << "You need the libXML2 to read the config file " << configFile << std::endl;
422 #endif
423 }
424 
425 void vpMbDepthNormalTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
426  const vpHomogeneousMatrix &cMo_, const bool verbose)
427 {
428  cMo.eye();
429 
430  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
431  delete m_depthNormalFaces[i];
432  m_depthNormalFaces[i] = NULL;
433  }
434 
435  m_depthNormalFaces.clear();
436 
437  loadModel(cad_name, verbose);
438  initFromPose(I, cMo_);
439 }
440 
441 #if defined(VISP_HAVE_PCL)
442 void vpMbDepthNormalTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
443  const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
444  const bool verbose)
445 {
446  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
447  reInitModel(I_dummy, cad_name, cMo_, verbose);
448 }
449 
450 #endif
451 
453 {
454  cMo.eye();
455 
456  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
457  ++it) {
458  vpMbtFaceDepthNormal *normal_face = *it;
459  delete normal_face;
460  normal_face = NULL;
461  }
462 
463  m_depthNormalFaces.clear();
464 
465  m_computeInteraction = true;
466  computeCovariance = false;
467 
470 
472 
473  m_lambda = 1.0;
474 
475  faces.reset();
476 
478 
479  useScanLine = false;
480 
481 #ifdef VISP_HAVE_OGRE
482  useOgre = false;
483 #endif
484 
487 }
488 
490 {
492 #ifdef VISP_HAVE_OGRE
493  faces.getOgreContext()->setWindowName("MBT Depth");
494 #endif
495 }
496 
498 {
499  cMo = cdMo;
500  init(I);
501 }
502 
503 #if defined(VISP_HAVE_PCL)
504 void vpMbDepthNormalTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
505  const vpHomogeneousMatrix &cdMo)
506 {
507  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
508  cMo = cdMo;
509  init(I_dummy);
510 }
511 #endif
512 
514 {
516 
517  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
518  it != m_depthNormalFaces.end(); ++it) {
519  (*it)->setScanLineVisibilityTest(v);
520  }
521 }
522 
523 void vpMbDepthNormalTracker::setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
524 {
525  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
526  it != m_depthNormalFaces.end(); ++it) {
527  vpMbtFaceDepthNormal *face = *it;
528  if (face->m_polygon->getName() == name) {
529  face->setTracked(useDepthNormalTracking);
530  }
531  }
532 }
533 
535 
536 #ifdef VISP_HAVE_PCL
537 void vpMbDepthNormalTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
538 {
541 
542 #if DEBUG_DISPLAY_DEPTH_NORMAL
543  if (!m_debugDisp_depthNormal->isInitialised()) {
544  m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
545  m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker");
546  }
547 
548  m_debugImage_depthNormal = 0;
549  std::vector<std::vector<vpImagePoint> > roiPts_vec;
550 #endif
551 
552  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
553  ++it) {
554  vpMbtFaceDepthNormal *face = *it;
555 
556  if (face->isVisible() && face->isTracked()) {
557  vpColVector desired_features;
558 
559 #if DEBUG_DISPLAY_DEPTH_NORMAL
560  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
561 #endif
562  if (face->computeDesiredFeatures(cMo, point_cloud->width, point_cloud->height, point_cloud, desired_features,
564 #if DEBUG_DISPLAY_DEPTH_NORMAL
565  ,
566  m_debugImage_depthNormal, roiPts_vec_
567 #endif
568  , m_mask
569  )) {
570  m_depthNormalListOfDesiredFeatures.push_back(desired_features);
571  m_depthNormalListOfActiveFaces.push_back(face);
572 
573 #if DEBUG_DISPLAY_DEPTH_NORMAL
574  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
575 #endif
576  }
577  }
578  }
579 
580 #if DEBUG_DISPLAY_DEPTH_NORMAL
581  vpDisplay::display(m_debugImage_depthNormal);
582 
583  for (size_t i = 0; i < roiPts_vec.size(); i++) {
584  if (roiPts_vec[i].empty())
585  continue;
586 
587  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
588  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
589  }
590  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
591  vpColor::red, 2);
592  }
593 
594  vpDisplay::flush(m_debugImage_depthNormal);
595 #endif
596 }
597 #endif
598 
599 void vpMbDepthNormalTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, const unsigned int width,
600  const unsigned int height)
601 {
604 
605 #if DEBUG_DISPLAY_DEPTH_NORMAL
606  if (!m_debugDisp_depthNormal->isInitialised()) {
607  m_debugImage_depthNormal.resize(height, width);
608  m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker");
609  }
610 
611  m_debugImage_depthNormal = 0;
612  std::vector<std::vector<vpImagePoint> > roiPts_vec;
613 #endif
614 
615  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
616  ++it) {
617  vpMbtFaceDepthNormal *face = *it;
618 
619  if (face->isVisible() && face->isTracked()) {
620  vpColVector desired_features;
621 
622 #if DEBUG_DISPLAY_DEPTH_NORMAL
623  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
624 #endif
625 
626  if (face->computeDesiredFeatures(cMo, width, height, point_cloud, desired_features, m_depthNormalSamplingStepX,
628 #if DEBUG_DISPLAY_DEPTH_NORMAL
629  ,
630  m_debugImage_depthNormal, roiPts_vec_
631 #endif
632  , m_mask
633  )) {
634  m_depthNormalListOfDesiredFeatures.push_back(desired_features);
635  m_depthNormalListOfActiveFaces.push_back(face);
636 
637 #if DEBUG_DISPLAY_DEPTH_NORMAL
638  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
639 #endif
640  }
641  }
642  }
643 
644 #if DEBUG_DISPLAY_DEPTH_NORMAL
645  vpDisplay::display(m_debugImage_depthNormal);
646 
647  for (size_t i = 0; i < roiPts_vec.size(); i++) {
648  if (roiPts_vec[i].empty())
649  continue;
650 
651  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
652  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
653  }
654  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
655  vpColor::red, 2);
656  }
657 
658  vpDisplay::flush(m_debugImage_depthNormal);
659 #endif
660 }
661 
663 {
664  this->cam = camera;
665 
666  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
667  it != m_depthNormalFaces.end(); ++it) {
668  (*it)->setCameraParameters(camera);
669  }
670 }
671 
673 {
674  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
675  it != m_depthNormalFaces.end(); ++it) {
676  (*it)->setFaceCentroidMethod(method);
677  }
678 }
679 
682 {
684 
685  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
686  it != m_depthNormalFaces.end(); ++it) {
687  (*it)->setFeatureEstimationMethod(method);
688  }
689 }
690 
692 {
694 
695  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
696  it != m_depthNormalFaces.end(); ++it) {
697  (*it)->setPclPlaneEstimationMethod(method);
698  }
699 }
700 
702 {
704 
705  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
706  it != m_depthNormalFaces.end(); ++it) {
707  (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
708  }
709 }
710 
712 {
714 
715  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
716  it != m_depthNormalFaces.end(); ++it) {
717  (*it)->setPclPlaneEstimationRansacThreshold(thresold);
718  }
719 }
720 
721 void vpMbDepthNormalTracker::setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
722 {
723  if (stepX == 0 || stepY == 0) {
724  std::cerr << "stepX and stepY must be greater than zero!" << std::endl;
725  return;
726  }
727 
730 }
731 
732 // void vpMbDepthNormalTracker::setDepthNormalUseRobust(const bool use) {
733 // m_depthNormalUseRobust = use;
734 //}
735 
737 {
738  throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
739 }
740 
741 #ifdef VISP_HAVE_PCL
742 void vpMbDepthNormalTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
743 {
744  segmentPointCloud(point_cloud);
745 
746  computeVVS();
747 
748  computeVisibility(point_cloud->width, point_cloud->height);
749 }
750 #endif
751 
752 void vpMbDepthNormalTracker::track(const std::vector<vpColVector> &point_cloud, const unsigned int width,
753  const unsigned int height)
754 {
755  segmentPointCloud(point_cloud, width, height);
756 
757  computeVVS();
758 
759  computeVisibility(width, height);
760 }
761 
762 void vpMbDepthNormalTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
763  const double /*radius*/, const int /*idFace*/, const std::string & /*name*/)
764 {
765  throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCircle() should not be called!");
766 }
767 
768 void vpMbDepthNormalTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
769  const int /*idFace*/, const std::string & /*name*/)
770 {
771  throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCylinder() should not be called!");
772 }
773 
774 void vpMbDepthNormalTracker::initFaceFromCorners(vpMbtPolygon &polygon) { addFace(polygon, false); }
775 
776 void vpMbDepthNormalTracker::initFaceFromLines(vpMbtPolygon &polygon) { addFace(polygon, true); }
vpMbHiddenFaces::setOgreShowConfigDialog
void setOgreShowConfigDialog(const bool showConfigDialog)
Definition: vpMbHiddenFaces.h:253
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
vpMbtXmlGenericParser::setCameraParameters
void setCameraParameters(const vpCameraParameters &_cam)
Definition: vpMbtXmlGenericParser.h:397
vpMbtXmlGenericParser::setAngleAppear
void setAngleAppear(const double &aappear)
Definition: vpMbtXmlGenericParser.h:383
vpMbtXmlGenericParser::getCameraParameters
void getCameraParameters(vpCameraParameters &_cam) const
Definition: vpMbtXmlGenericParser.h:229
vpMbtFaceDepthNormal::vpFeatureEstimationType
vpFeatureEstimationType
Definition: vpMbtFaceDepthNormal.h:60
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:243
vpXmlParser::parse
void parse(const std::string &filename)
Definition: vpXmlParser.cpp:420
vpMbtFaceDepthNormal::display
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)
Definition: vpMbtFaceDepthNormal.cpp:900
vpMbDepthNormalTracker::m_depthNormalUseRobust
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
Definition: vpMbDepthNormalTracker.h:172
vpMbTracker::m_computeInteraction
bool m_computeInteraction
Definition: vpMbTracker.h:190
vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacThreshold
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold)
Definition: vpMbDepthNormalTracker.cpp:710
vpMbtFaceDepthNormal::m_hiddenFace
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
Definition: vpMbtFaceDepthNormal.h:77
vpMbDepthNormalTracker::setDepthNormalSamplingStep
virtual void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
Definition: vpMbDepthNormalTracker.cpp:720
vpMbtXmlGenericParser::setDepthNormalPclPlaneEstimationRansacThreshold
void setDepthNormalPclPlaneEstimationRansacThreshold(const double threshold)
Definition: vpMbtXmlGenericParser.h:448
vpMbTracker::distNearClip
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:154
vpMbDepthNormalTracker::m_depthNormalI_dummyVisibility
vpImage< unsigned char > m_depthNormalI_dummyVisibility
Dummy image used to compute the visibility.
Definition: vpMbDepthNormalTracker.h:154
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:107
vpMbTracker::computeVVSCheckLevenbergMarquardt
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)
Definition: vpMbTracker.cpp:2524
vpMbHiddenFaces::setVisibleOgre
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
Definition: vpMbHiddenFaces.h:768
vpMbDepthNormalTracker::track
virtual void track(const vpImage< unsigned char > &)
Definition: vpMbDepthNormalTracker.cpp:735
vpMbHiddenFaces::reset
void reset()
Definition: vpMbHiddenFaces.h:397
vpRobust::setThreshold
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:114
vpMbtXmlGenericParser::setAngleDisappear
void setAngleDisappear(const double &adisappear)
Definition: vpMbtXmlGenericParser.h:390
vpMbHiddenFaces::computeClippedPolygons
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Definition: vpMbHiddenFaces.h:440
vpMbTracker::useOgre
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:160
vpMbDepthNormalTracker::m_weightedError_depthNormal
vpColVector m_weightedError_depthNormal
Weighted error.
Definition: vpMbDepthNormalTracker.h:182
vpMbTracker::loadModel
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Definition: vpMbTracker.cpp:1147
vpMatrix::insert
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4569
vpImageConvert::convert
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition: vpImageConvert.cpp:78
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:101
vpMbTracker::cMo
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:118
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:523
vpMbDepthNormalTracker::initCylinder
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
Definition: vpMbDepthNormalTracker.cpp:767
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpMbtFaceDepthNormal::m_cam
vpCameraParameters m_cam
Camera intrinsic parameters.
Definition: vpMbtFaceDepthNormal.h:69
vpMbtXmlGenericParser::getDepthNormalPclPlaneEstimationMethod
int getDepthNormalPclPlaneEstimationMethod() const
Definition: vpMbtXmlGenericParser.h:257
vpMbtXmlGenericParser::getAngleDisappear
double getAngleDisappear() const
Definition: vpMbtXmlGenericParser.h:227
vpMbDepthNormalTracker::resetTracker
virtual void resetTracker()
Definition: vpMbDepthNormalTracker.cpp:451
vpMbTracker::GAUSS_NEWTON_OPT
Definition: vpMbTracker.h:112
vpMbtXmlGenericParser::setDepthNormalSamplingStepX
void setDepthNormalSamplingStepX(const unsigned int stepX)
Definition: vpMbtXmlGenericParser.h:458
vpMbtFaceDepthNormal::computeDesiredFeatures
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
Definition: vpMbtFaceDepthNormal.cpp:149
vpMbHiddenFaces::isOgreInitialised
bool isOgreInitialised()
Definition: vpMbHiddenFaces.h:176
vpMath::deg
static double deg(double rad)
Definition: vpMath.h:94
vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationMethod
virtual void setDepthNormalPclPlaneEstimationMethod(const int method)
Definition: vpMbDepthNormalTracker.cpp:690
vpMbtFaceDepthNormal::setFeatureEstimationMethod
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
Definition: vpMbtFaceDepthNormal.h:144
vpMbDepthNormalTracker::testTracking
virtual void testTracking()
Definition: vpMbDepthNormalTracker.cpp:533
vpMbDepthNormalTracker::setDepthNormalFeatureEstimationMethod
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
Definition: vpMbDepthNormalTracker.cpp:679
vpHomogeneousMatrix::eye
void eye()
Definition: vpHomogeneousMatrix.cpp:663
vpException::fatalError
Fatal error.
Definition: vpException.h:95
vpMbtXmlGenericParser::setDepthNormalPclPlaneEstimationRansacMaxIter
void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
Definition: vpMbtXmlGenericParser.h:438
vpMbDepthNormalTracker::initFaceFromCorners
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
Definition: vpMbDepthNormalTracker.cpp:773
vpMbtXmlGenericParser::getDepthNormalSamplingStepX
unsigned int getDepthNormalSamplingStepX() const
Definition: vpMbtXmlGenericParser.h:278
vpMbtFaceDepthNormal::m_distNearClip
double m_distNearClip
Distance for near clipping.
Definition: vpMbtFaceDepthNormal.h:75
vpMbtXmlGenericParser::setDepthNormalPclPlaneEstimationMethod
void setDepthNormalPclPlaneEstimationMethod(const int method)
Definition: vpMbtXmlGenericParser.h:428
vpMbTracker::computeCovariance
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:133
vpException::ioError
I/O error.
Definition: vpException.h:90
vpMbTracker::modelInitialised
bool modelInitialised
Definition: vpMbTracker.h:128
vpMbTracker::faces
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:148
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
vpMbDepthNormalTracker::m_depthNormalFeatureEstimationMethod
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
Definition: vpMbDepthNormalTracker.h:150
vpMbtFaceDepthNormal::m_useScanLine
bool m_useScanLine
Scan line visibility.
Definition: vpMbtFaceDepthNormal.h:83
vpMbDepthNormalTracker::m_error_depthNormal
vpColVector m_error_depthNormal
(s - s*)
Definition: vpMbDepthNormalTracker.h:174
vpMbtXmlGenericParser::setDepthNormalFeatureEstimationMethod
void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
Definition: vpMbtXmlGenericParser.h:418
vpPolygon3D::FOV_CLIPPING
Definition: vpPolygon3D.h:69
vpMbtFaceDepthNormal::computeVisibility
void computeVisibility()
Definition: vpMbtFaceDepthNormal.cpp:713
vpMbDepthNormalTracker::m_robust_depthNormal
vpRobust m_robust_depthNormal
Robust.
Definition: vpMbDepthNormalTracker.h:178
vpMbDepthNormalTracker::m_depthNormalSamplingStepY
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
Definition: vpMbDepthNormalTracker.h:170
vpMbtPolygon::getName
std::string getName() const
Definition: vpMbtPolygon.h:107
vpMbDepthNormalTracker::m_w_depthNormal
vpColVector m_w_depthNormal
Robust weights.
Definition: vpMbDepthNormalTracker.h:180
vpMbDepthNormalTracker::loadConfigFile
virtual void loadConfigFile(const std::string &configFile)
Definition: vpMbDepthNormalTracker.cpp:374
vpMbDepthNormalTracker::computeVVSInit
virtual void computeVVSInit()
Definition: vpMbDepthNormalTracker.cpp:252
vpMbtXmlGenericParser::getFovClipping
bool getFovClipping() const
Definition: vpMbtXmlGenericParser.h:293
vpMbDepthNormalTracker::initFaceFromLines
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Definition: vpMbDepthNormalTracker.cpp:775
vpMbtXmlGenericParser::getDepthNormalFeatureEstimationMethod
vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
Definition: vpMbtXmlGenericParser.h:249
vpMbDepthNormalTracker::m_depthNormalFaces
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
Definition: vpMbDepthNormalTracker.h:160
vpMbtFaceDepthNormal::setPclPlaneEstimationRansacMaxIter
void setPclPlaneEstimationRansacMaxIter(const int maxIter)
Definition: vpMbtFaceDepthNormal.h:148
vpPolygon3D::p
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:80
vpMbDepthNormalTracker::m_depthNormalPclPlaneEstimationMethod
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
Definition: vpMbDepthNormalTracker.h:162
vpMbtXmlGenericParser::getDepthNormalPclPlaneEstimationRansacThreshold
double getDepthNormalPclPlaneEstimationRansacThreshold() const
Definition: vpMbtXmlGenericParser.h:270
vpMbTracker::cam
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:116
vpMbDepthNormalTracker::setUseDepthNormalTracking
void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
Definition: vpMbDepthNormalTracker.cpp:522
vpMbDepthNormalTracker::setPose
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
Definition: vpMbDepthNormalTracker.cpp:496
vpMbTracker::m_lambda
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:192
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:71
vpMbTracker::useScanLine
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:163
vpMbtXmlGenericParser
Parse an Xml file to extract configuration parameters of a mbtConfig object.
Definition: vpMbtXmlGenericParser.h:63
vpMbtXmlGenericParser::DEPTH_NORMAL_PARSER
Definition: vpMbtXmlGenericParser.h:70
vpMbtXmlGenericParser::getDepthNormalSamplingStepY
unsigned int getDepthNormalSamplingStepY() const
Definition: vpMbtXmlGenericParser.h:283
vpAROgre::setWindowName
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:268
vpMbDepthNormalTracker::m_depthNormalListOfDesiredFeatures
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
Definition: vpMbDepthNormalTracker.h:158
vpMbtFaceDepthNormal::m_polygon
vpMbtPolygon * m_polygon
Polygon defining the face.
Definition: vpMbtFaceDepthNormal.h:81
vpMbTracker::initFromPose
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
Definition: vpMbTracker.cpp:841
vpMbtXmlGenericParser::getDepthNormalPclPlaneEstimationRansacMaxIter
int getDepthNormalPclPlaneEstimationRansacMaxIter() const
Definition: vpMbtXmlGenericParser.h:262
vpMbtXmlGenericParser::getNearClippingDistance
double getNearClippingDistance() const
Definition: vpMbtXmlGenericParser.h:353
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:103
vpMbtPolygon
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:65
vpMbDepthNormalTracker::setScanLineVisibilityTest
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbDepthNormalTracker.cpp:512
vpColVector::insert
void insert(unsigned int i, const vpColVector &v)
Definition: vpColVector.cpp:1275
vpMbDepthNormalTracker::m_depthNormalPclPlaneEstimationRansacMaxIter
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
Definition: vpMbDepthNormalTracker.h:164
vpMbDepthNormalTracker::setDepthNormalFaceCentroidMethod
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
Definition: vpMbDepthNormalTracker.cpp:671
vpMbtFaceDepthNormal::m_clippingFlag
unsigned int m_clippingFlag
Flags specifying which clipping to used.
Definition: vpMbtFaceDepthNormal.h:71
vpMbtFaceDepthNormal::m_distFarClip
double m_distFarClip
Distance for near clipping.
Definition: vpMbtFaceDepthNormal.h:73
vpMbTracker::m_initialMu
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:198
vpMbtFaceDepthNormal::isVisible
bool isVisible() const
Definition: vpMbtFaceDepthNormal.h:138
vpMbtPolygon::getIndex
int getIndex() const
Definition: vpMbtPolygon.h:100
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:676
vpMbDepthNormalTracker::initCircle
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
Definition: vpMbDepthNormalTracker.cpp:761
vpMbDepthNormalTracker::segmentPointCloud
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
Definition: vpMbDepthNormalTracker.cpp:536
vpMbTracker::distFarClip
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:156
vpMbTracker::m_optimizationMethod
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:145
vpMbTracker::displayFeatures
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:143
vpMbHiddenFaces::setBackgroundSizeOgre
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
Definition: vpMbHiddenFaces.h:210
vpMbDepthNormalTracker::computeVVS
void computeVVS()
Definition: vpMbDepthNormalTracker.cpp:156
vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacMaxIter
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
Definition: vpMbDepthNormalTracker.cpp:700
vpMbTracker::oJo
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:120
vpMbDepthNormalTracker::computeVisibility
void computeVisibility(const unsigned int width, const unsigned int height)
Definition: vpMbDepthNormalTracker.cpp:131
vpException::what
const char * what() const
Definition: vpException.cpp:101
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:177
vpMbtXmlGenericParser::getAngleAppear
double getAngleAppear() const
Definition: vpMbtXmlGenericParser.h:222
vpMbtFaceDepthNormal::displayFeature
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
Definition: vpMbtFaceDepthNormal.cpp:930
vpMbDepthNormalTracker::m_depthNormalPclPlaneEstimationRansacThreshold
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
Definition: vpMbDepthNormalTracker.h:166
vpMbDepthNormalTracker::setCameraParameters
virtual void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbDepthNormalTracker.cpp:661
vpMbTracker::computeVVSWeights
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
Definition: vpMbTracker.cpp:2609
vpMbtFaceDepthNormal::setPclPlaneEstimationMethod
void setPclPlaneEstimationMethod(const int method)
Definition: vpMbtFaceDepthNormal.h:146
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:113
vpMbDepthNormalTracker::~vpMbDepthNormalTracker
virtual ~vpMbDepthNormalTracker()
Definition: vpMbDepthNormalTracker.cpp:77
vpMbDepthNormalTracker::init
virtual void init(const vpImage< unsigned char > &I)
Definition: vpMbDepthNormalTracker.cpp:341
vpMbTracker::angleAppears
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:150
vpMbtFaceDepthNormal::setTracked
void setTracked(const bool tracked)
Definition: vpMbtFaceDepthNormal.h:157
vpMbDepthNormalTracker::display
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)
Definition: vpMbDepthNormalTracker.cpp:285
vpMbDepthNormalTracker::vpMbDepthNormalTracker
vpMbDepthNormalTracker()
Definition: vpMbDepthNormalTracker.cpp:54
vpPolygon3D::nbpt
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:75
vpMbDepthNormalTracker::addFace
void addFace(vpMbtPolygon &polygon, const bool alreadyClose)
Definition: vpMbDepthNormalTracker.cpp:84
vpMbDepthNormalTracker::m_L_depthNormal
vpMatrix m_L_depthNormal
Interaction matrix.
Definition: vpMbDepthNormalTracker.h:176
vpMbtFaceDepthNormal::addLine
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
Definition: vpMbtFaceDepthNormal.cpp:88
vpMbTracker::computeVVSPoseEstimation
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)
Definition: vpMbTracker.cpp:2546
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:58
vpVelocityTwistMatrix::buildFrom
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpVelocityTwistMatrix.cpp:364
vpMbDepthNormalTracker::reInitModel
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
Definition: vpMbDepthNormalTracker.cpp:424
vpPolygon3D::getNbPoint
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:131
vpMbtXmlGenericParser::getFarClippingDistance
double getFarClippingDistance() const
Definition: vpMbtXmlGenericParser.h:288
vpMbtXmlGenericParser::hasFarClippingDistance
bool hasFarClippingDistance() const
Definition: vpMbtXmlGenericParser.h:367
vpMbtFaceDepthNormal
Definition: vpMbtFaceDepthNormal.h:52
vpMbTracker::m_mask
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:226
vpMbHiddenFaces::initOgre
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
Definition: vpMbHiddenFaces.h:710
vpMbtXmlGenericParser::hasNearClippingDistance
bool hasNearClippingDistance() const
Definition: vpMbtXmlGenericParser.h:374
vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu
virtual void computeVVSInteractionMatrixAndResidu()
Definition: vpMbDepthNormalTracker.cpp:267
vpRobust::resize
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:127
vpMbTracker::setOgreVisibilityTest
virtual void setOgreVisibilityTest(const bool &v)
Definition: vpMbTracker.cpp:2313
vpMbtFaceDepthNormal::m_planeObject
vpPlane m_planeObject
Plane equation described in the object frame.
Definition: vpMbtFaceDepthNormal.h:79
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
vpMbTracker::setClipping
virtual void setClipping(const unsigned int &flags)
Definition: vpMbTracker.cpp:2437
vpMbTracker::m_maxIter
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:194
vpMbTracker::ogreShowConfigDialog
bool ogreShowConfigDialog
Definition: vpMbTracker.h:161
vpMbtFaceDepthNormal::vpFaceCentroidType
vpFaceCentroidType
Definition: vpMbtFaceDepthNormal.h:55
vpMbDepthNormalTracker::m_depthNormalSamplingStepX
unsigned int m_depthNormalSamplingStepX
Sampling step in x-direction.
Definition: vpMbDepthNormalTracker.h:168
vpMbTracker::clippingFlag
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:158
vpPlane::object_frame
Definition: vpPlane.h:69
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:652
vpMbHiddenFaces::getOgreContext
vpAROgre * getOgreContext()
Definition: vpMbHiddenFaces.h:154
vpMbTracker::setNearClippingDistance
virtual void setNearClippingDistance(const double &dist)
Definition: vpMbTracker.cpp:2410
vpMbTracker::setFarClippingDistance
virtual void setFarClippingDistance(const double &dist)
Definition: vpMbTracker.cpp:2331
vpMbtFaceDepthNormal::isTracked
bool isTracked() const
Definition: vpMbtFaceDepthNormal.h:136
vpMbTracker::computeCovarianceMatrixVVS
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
Definition: vpMbTracker.cpp:2444
vpImage< unsigned char >
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:642
vpMbtXmlGenericParser::setDepthNormalSamplingStepY
void setDepthNormalSamplingStepY(const unsigned int stepY)
Definition: vpMbtXmlGenericParser.h:465
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpArray2D::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:170
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
vpDisplay::displayLine
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:391
vpMbHiddenFaces::setVisible
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
Definition: vpMbHiddenFaces.h:659
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:91
vpMbTracker::m_stopCriteriaEpsilon
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
Definition: vpMbTracker.h:196
vpMbTracker::setScanLineVisibilityTest
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:586
vpMbDepthNormalTracker::m_depthNormalListOfActiveFaces
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
Definition: vpMbDepthNormalTracker.h:156
vpMbHiddenFaces::computeScanLineRender
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
Definition: vpMbHiddenFaces.h:463
vpImage::resize
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:865
vpMbDepthNormalTracker::m_depthNormalHiddenFacesDisplay
vpMbHiddenFaces< vpMbtPolygon > m_depthNormalHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
Definition: vpMbDepthNormalTracker.h:152
vpMbDepthNormalTracker::setOgreVisibilityTest
virtual void setOgreVisibilityTest(const bool &v)
Definition: vpMbDepthNormalTracker.cpp:488
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpCameraParameters::computeFov
void computeFov(const unsigned int &w, const unsigned int &h)
Definition: vpCameraParameters.cpp:432
vpPolygon3D::NO_CLIPPING
Definition: vpPolygon3D.h:62
vpColor::red
static const vpColor red
Definition: vpColor.h:179
vpMbtFaceDepthNormal::setPclPlaneEstimationRansacThreshold
void setPclPlaneEstimationRansacThreshold(const double threshold)
Definition: vpMbtFaceDepthNormal.h:150
vpMbTracker::angleDisappears
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:152
vpMatrix::eye
void eye()
Definition: vpMatrix.cpp:359
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:238
vpArray2D::getRows
unsigned int getRows() const
Definition: vpArray2D.h:155