Visual Servoing Platform  version 3.3.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_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_featuresToBeDisplayedDepthNormal(), 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, 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 = m_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(unsigned int width, unsigned int height)
133 {
134  bool changed = false;
135  faces.setVisible(width, height, m_cam, m_cMo, angleAppears, angleDisappears, changed);
136 
137  if (useScanLine) {
138  // if (clippingFlag <= 2) {
139  // cam.computeFov(width, height);
140  // }
141 
143  faces.computeScanLineRender(m_cam, width, height);
144  }
145 
146  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
147  it != m_depthNormalFaces.end(); ++it) {
148  vpMbtFaceDepthNormal *face_normal = *it;
149  face_normal->computeVisibility();
150  }
151 }
152 
154 {
155  double normRes = 0;
156  double normRes_1 = -1;
157  unsigned int iter = 0;
158 
159  computeVVSInit();
160  unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size());
161 
162  vpColVector error_prev(nb_features);
163  vpMatrix LTL;
164  vpColVector LTR, v;
165 
166  double mu = m_initialMu;
167  vpHomogeneousMatrix cMo_prev;
168 
169  bool isoJoIdentity_ = true;
171  vpMatrix L_true, LVJ_true;
172 
173  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
175 
176  bool reStartFromLastIncrement = false;
177  computeVVSCheckLevenbergMarquardt(iter, m_error_depthNormal, error_prev, cMo_prev, mu, reStartFromLastIncrement);
178 
179  if (!reStartFromLastIncrement) {
182 
183  if (computeCovariance) {
184  L_true = m_L_depthNormal;
185  if (!isoJoIdentity_) {
186  cVo.buildFrom(m_cMo);
187  LVJ_true = (m_L_depthNormal * (cVo * oJo));
188  }
189  }
190 
191  // Compute DoF only once
192  if (iter == 0) {
193  isoJoIdentity_ = true;
194  oJo.eye();
195 
196  // If all the 6 dof should be estimated, we check if the interaction
197  // matrix is full rank. If not we remove automatically the dof that
198  // cannot be estimated This is particularly useful when consering
199  // circles (rank 5) and cylinders (rank 4)
200  if (isoJoIdentity_) {
201  cVo.buildFrom(m_cMo);
202 
203  vpMatrix K; // kernel
204  unsigned int rank = (m_L_depthNormal * cVo).kernel(K);
205  if (rank == 0) {
206  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
207  }
208 
209  if (rank != 6) {
210  vpMatrix I; // Identity
211  I.eye(6);
212  oJo = I - K.AtA();
213 
214  isoJoIdentity_ = false;
215  }
216  }
217  }
218 
219  double num = 0.0, den = 0.0;
220  for (unsigned int i = 0; i < m_L_depthNormal.getRows(); i++) {
221  // Compute weighted errors and stop criteria
224  den += m_w_depthNormal[i];
225 
226  // weight interaction matrix
227  for (unsigned int j = 0; j < 6; j++) {
228  m_L_depthNormal[i][j] *= m_w_depthNormal[i];
229  }
230  }
231 
233  m_error_depthNormal, error_prev, LTR, mu, v);
234 
235  cMo_prev = m_cMo;
237 
238  normRes_1 = normRes;
239  normRes = sqrt(num / den);
240  }
241 
242  iter++;
243  }
244 
245  computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthNormal, cMo_prev, L_true, LVJ_true, m_error_depthNormal);
246 }
247 
249 {
250  unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size());
251 
252  m_L_depthNormal.resize(nb_features, 6, false, false);
253  m_error_depthNormal.resize(nb_features, false);
254  m_weightedError_depthNormal.resize(nb_features, false);
255 
256  m_w_depthNormal.resize(nb_features, false);
257  m_w_depthNormal = 1;
258 
259  m_robust_depthNormal.resize(nb_features);
261 }
262 
264 {
265  unsigned int cpt = 0;
266  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalListOfActiveFaces.begin();
267  it != m_depthNormalListOfActiveFaces.end(); ++it) {
268  vpMatrix L_face;
269  vpColVector features_face;
270  (*it)->computeInteractionMatrix(m_cMo, L_face, features_face);
271 
272  vpColVector face_error = features_face - m_depthNormalListOfDesiredFeatures[(size_t)cpt];
273 
274  m_error_depthNormal.insert(cpt * 3, face_error);
275  m_L_depthNormal.insert(L_face, cpt * 3, 0);
276 
277  cpt++;
278  }
279 }
280 
282  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
283  bool displayFullModel)
284 {
285  std::vector<std::vector<double> > models = vpMbDepthNormalTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
286 
287  for (size_t i = 0; i < models.size(); i++) {
288  if (vpMath::equal(models[i][0], 0)) {
289  vpImagePoint ip1(models[i][1], models[i][2]);
290  vpImagePoint ip2(models[i][3], models[i][4]);
291  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
292  }
293  }
294 
295  if (displayFeatures) {
296  std::vector<std::vector<double> > features = getFeaturesForDisplayDepthNormal();
297  for (size_t i = 0; i < features.size(); i++) {
298  vpImagePoint im_centroid(features[i][1], features[i][2]);
299  vpImagePoint im_extremity(features[i][3], features[i][4]);
300  bool desired = vpMath::equal(features[i][0], 2);
301  vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
302  }
303  }
304 }
305 
307  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
308  bool displayFullModel)
309 {
310  std::vector<std::vector<double> > models = vpMbDepthNormalTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
311 
312  for (size_t i = 0; i < models.size(); i++) {
313  if (vpMath::equal(models[i][0], 0)) {
314  vpImagePoint ip1(models[i][1], models[i][2]);
315  vpImagePoint ip2(models[i][3], models[i][4]);
316  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
317  }
318  }
319 
320  if (displayFeatures) {
321  std::vector<std::vector<double> > features = getFeaturesForDisplayDepthNormal();
322  for (size_t i = 0; i < features.size(); i++) {
323  vpImagePoint im_centroid(features[i][1], features[i][2]);
324  vpImagePoint im_extremity(features[i][3], features[i][4]);
325  bool desired = vpMath::equal(features[i][0], 2);
326  vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
327  }
328  }
329 }
330 
331 std::vector<std::vector<double> > vpMbDepthNormalTracker::getFeaturesForDisplayDepthNormal() {
332  std::vector<std::vector<double> > features;
333 
334  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
335  it != m_depthNormalFaces.end(); ++it) {
336  vpMbtFaceDepthNormal *face_normal = *it;
337  std::vector<std::vector<double> > currentFeatures = face_normal->getFeaturesForDisplay(m_cMo, m_cam);
338  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
339  }
340 
341  return features;
342 }
343 
357 std::vector<std::vector<double> > vpMbDepthNormalTracker::getModelForDisplay(unsigned int width, unsigned int height,
358  const vpHomogeneousMatrix &cMo,
359  const vpCameraParameters &cam,
360  bool displayFullModel)
361 {
362  std::vector<std::vector<double> > models;
363 
364  vpCameraParameters c = cam;
365 
366  bool changed = false;
368 
369  if (useScanLine) {
370  c.computeFov(width, height);
371 
374  }
375 
376  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
377  it != m_depthNormalFaces.end(); ++it) {
378  vpMbtFaceDepthNormal *face_normal = *it;
379  std::vector<std::vector<double> > modelLines = face_normal->getModelForDisplay(width, height, cMo, cam, displayFullModel);
380  models.insert(models.end(), modelLines.begin(), modelLines.end());
381  }
382 
383  return models;
384 }
385 
387 {
388  if (!modelInitialised) {
389  throw vpException(vpException::fatalError, "model not initialized");
390  }
391 
392  bool reInitialisation = false;
393  if (!useOgre) {
394  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
395  } else {
396 #ifdef VISP_HAVE_OGRE
397  if (!faces.isOgreInitialised()) {
401  // Turn off Ogre config dialog display for the next call to this
402  // function since settings are saved in the ogre.cfg file and used
403  // during the next call
404  ogreShowConfigDialog = false;
405  }
406 
408 #else
409  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
410 #endif
411  }
412 
413  if (useScanLine || clippingFlag > 3)
415 
417 }
418 
419 void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile)
420 {
421 #ifdef VISP_HAVE_PUGIXML
423 
427 
434 
435  try {
436  std::cout << " *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
437  xmlp.parse(configFile);
438  } catch (const vpException &e) {
439  std::cerr << "Exception: " << e.what() << std::endl;
440  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
441  }
442 
443  vpCameraParameters camera;
444  xmlp.getCameraParameters(camera);
445  setCameraParameters(camera);
446 
449 
450  if (xmlp.hasNearClippingDistance())
452 
453  if (xmlp.hasFarClippingDistance())
455 
456  if (xmlp.getFovClipping())
458 
464 #else
465  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
466 #endif
467 }
468 
469 void vpMbDepthNormalTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
470  const vpHomogeneousMatrix &cMo, bool verbose)
471 {
472  m_cMo.eye();
473 
474  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
475  delete m_depthNormalFaces[i];
476  m_depthNormalFaces[i] = NULL;
477  }
478 
479  m_depthNormalFaces.clear();
480 
481  loadModel(cad_name, verbose);
482  initFromPose(I, cMo);
483 }
484 
485 #if defined(VISP_HAVE_PCL)
486 void vpMbDepthNormalTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
487  const std::string &cad_name, const vpHomogeneousMatrix &cMo,
488  bool verbose)
489 {
490  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
491  reInitModel(I_dummy, cad_name, cMo, verbose);
492 }
493 
494 #endif
495 
497 {
498  m_cMo.eye();
499 
500  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
501  ++it) {
502  vpMbtFaceDepthNormal *normal_face = *it;
503  delete normal_face;
504  normal_face = NULL;
505  }
506 
507  m_depthNormalFaces.clear();
508 
509  m_computeInteraction = true;
510  computeCovariance = false;
511 
514 
516 
517  m_lambda = 1.0;
518 
519  faces.reset();
520 
522 
523  useScanLine = false;
524 
525 #ifdef VISP_HAVE_OGRE
526  useOgre = false;
527 #endif
528 
531 }
532 
534 {
536 #ifdef VISP_HAVE_OGRE
537  faces.getOgreContext()->setWindowName("MBT Depth");
538 #endif
539 }
540 
542 {
543  m_cMo = cdMo;
544  init(I);
545 }
546 
548 {
549  m_cMo = cdMo;
550  vpImageConvert::convert(I_color, m_I);
551  init(m_I);
552 }
553 
554 #if defined(VISP_HAVE_PCL)
555 void vpMbDepthNormalTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
556  const vpHomogeneousMatrix &cdMo)
557 {
558  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
559  m_cMo = cdMo;
560  init(I_dummy);
561 }
562 #endif
563 
565 {
567 
568  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
569  it != m_depthNormalFaces.end(); ++it) {
570  (*it)->setScanLineVisibilityTest(v);
571  }
572 }
573 
574 void vpMbDepthNormalTracker::setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
575 {
576  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
577  it != m_depthNormalFaces.end(); ++it) {
578  vpMbtFaceDepthNormal *face = *it;
579  if (face->m_polygon->getName() == name) {
580  face->setTracked(useDepthNormalTracking);
581  }
582  }
583 }
584 
586 
587 #ifdef VISP_HAVE_PCL
588 void vpMbDepthNormalTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
589 {
592 
593 #if DEBUG_DISPLAY_DEPTH_NORMAL
594  if (!m_debugDisp_depthNormal->isInitialised()) {
595  m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
596  m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker");
597  }
598 
599  m_debugImage_depthNormal = 0;
600  std::vector<std::vector<vpImagePoint> > roiPts_vec;
601 #endif
602 
603  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
604  ++it) {
605  vpMbtFaceDepthNormal *face = *it;
606 
607  if (face->isVisible() && face->isTracked()) {
608  vpColVector desired_features;
609 
610 #if DEBUG_DISPLAY_DEPTH_NORMAL
611  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
612 #endif
613  if (face->computeDesiredFeatures(m_cMo, point_cloud->width, point_cloud->height, point_cloud, desired_features,
615 #if DEBUG_DISPLAY_DEPTH_NORMAL
616  ,
617  m_debugImage_depthNormal, roiPts_vec_
618 #endif
619  , m_mask
620  )) {
621  m_depthNormalListOfDesiredFeatures.push_back(desired_features);
622  m_depthNormalListOfActiveFaces.push_back(face);
623 
624 #if DEBUG_DISPLAY_DEPTH_NORMAL
625  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
626 #endif
627  }
628  }
629  }
630 
631 #if DEBUG_DISPLAY_DEPTH_NORMAL
632  vpDisplay::display(m_debugImage_depthNormal);
633 
634  for (size_t i = 0; i < roiPts_vec.size(); i++) {
635  if (roiPts_vec[i].empty())
636  continue;
637 
638  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
639  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
640  }
641  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
642  vpColor::red, 2);
643  }
644 
645  vpDisplay::flush(m_debugImage_depthNormal);
646 #endif
647 }
648 #endif
649 
650 void vpMbDepthNormalTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width,
651  unsigned int height)
652 {
655 
656 #if DEBUG_DISPLAY_DEPTH_NORMAL
657  if (!m_debugDisp_depthNormal->isInitialised()) {
658  m_debugImage_depthNormal.resize(height, width);
659  m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker");
660  }
661 
662  m_debugImage_depthNormal = 0;
663  std::vector<std::vector<vpImagePoint> > roiPts_vec;
664 #endif
665 
666  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
667  ++it) {
668  vpMbtFaceDepthNormal *face = *it;
669 
670  if (face->isVisible() && face->isTracked()) {
671  vpColVector desired_features;
672 
673 #if DEBUG_DISPLAY_DEPTH_NORMAL
674  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
675 #endif
676 
677  if (face->computeDesiredFeatures(m_cMo, width, height, point_cloud, desired_features, m_depthNormalSamplingStepX,
679 #if DEBUG_DISPLAY_DEPTH_NORMAL
680  ,
681  m_debugImage_depthNormal, roiPts_vec_
682 #endif
683  , m_mask
684  )) {
685  m_depthNormalListOfDesiredFeatures.push_back(desired_features);
686  m_depthNormalListOfActiveFaces.push_back(face);
687 
688 #if DEBUG_DISPLAY_DEPTH_NORMAL
689  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
690 #endif
691  }
692  }
693  }
694 
695 #if DEBUG_DISPLAY_DEPTH_NORMAL
696  vpDisplay::display(m_debugImage_depthNormal);
697 
698  for (size_t i = 0; i < roiPts_vec.size(); i++) {
699  if (roiPts_vec[i].empty())
700  continue;
701 
702  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
703  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
704  }
705  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
706  vpColor::red, 2);
707  }
708 
709  vpDisplay::flush(m_debugImage_depthNormal);
710 #endif
711 }
712 
714 {
715  m_cam = cam;
716 
717  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
718  it != m_depthNormalFaces.end(); ++it) {
719  (*it)->setCameraParameters(cam);
720  }
721 }
722 
724 {
725  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
726  it != m_depthNormalFaces.end(); ++it) {
727  (*it)->setFaceCentroidMethod(method);
728  }
729 }
730 
733 {
735 
736  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
737  it != m_depthNormalFaces.end(); ++it) {
738  (*it)->setFeatureEstimationMethod(method);
739  }
740 }
741 
743 {
745 
746  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
747  it != m_depthNormalFaces.end(); ++it) {
748  (*it)->setPclPlaneEstimationMethod(method);
749  }
750 }
751 
753 {
755 
756  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
757  it != m_depthNormalFaces.end(); ++it) {
758  (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
759  }
760 }
761 
763 {
765 
766  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
767  it != m_depthNormalFaces.end(); ++it) {
768  (*it)->setPclPlaneEstimationRansacThreshold(threshold);
769  }
770 }
771 
772 void vpMbDepthNormalTracker::setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
773 {
774  if (stepX == 0 || stepY == 0) {
775  std::cerr << "stepX and stepY must be greater than zero!" << std::endl;
776  return;
777  }
778 
781 }
782 
783 // void vpMbDepthNormalTracker::setDepthNormalUseRobust(bool use) {
784 // m_depthNormalUseRobust = use;
785 //}
786 
788 {
789  throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
790 }
791 
793 {
794  throw vpException(vpException::fatalError, "Cannot track with a color image!");
795 }
796 
797 #ifdef VISP_HAVE_PCL
798 void vpMbDepthNormalTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
799 {
800  segmentPointCloud(point_cloud);
801 
802  computeVVS();
803 
804  computeVisibility(point_cloud->width, point_cloud->height);
805 }
806 #endif
807 
808 void vpMbDepthNormalTracker::track(const std::vector<vpColVector> &point_cloud, unsigned int width,
809  unsigned int height)
810 {
811  segmentPointCloud(point_cloud, width, height);
812 
813  computeVVS();
814 
815  computeVisibility(width, height);
816 }
817 
818 void vpMbDepthNormalTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
819  const double /*radius*/, const int /*idFace*/, const std::string & /*name*/)
820 {
821  throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCircle() should not be called!");
822 }
823 
824 void vpMbDepthNormalTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
825  const int /*idFace*/, const std::string & /*name*/)
826 {
827  throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCylinder() should not be called!");
828 }
829 
831 
vpMbtFaceDepthNormal::getFeaturesForDisplay
std::vector< std::vector< double > > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05)
Definition: vpMbtFaceDepthNormal.cpp:1493
vpMbtFaceDepthNormal::setTracked
void setTracked(bool tracked)
Definition: vpMbtFaceDepthNormal.h:166
vpMbtFaceDepthNormal::setPclPlaneEstimationRansacMaxIter
void setPclPlaneEstimationRansacMaxIter(int maxIter)
Definition: vpMbtFaceDepthNormal.h:157
vpMbHiddenFaces::setVisible
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
Definition: vpMbHiddenFaces.h:662
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
vpMbtXmlGenericParser::setAngleAppear
void setAngleAppear(const double &aappear)
Definition: vpMbtXmlGenericParser.cpp:1718
vpMbtFaceDepthNormal::vpFeatureEstimationType
vpFeatureEstimationType
Definition: vpMbtFaceDepthNormal.h:61
vpMbHiddenFaces::computeClippedPolygons
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Definition: vpMbHiddenFaces.h:441
vpMbDepthNormalTracker::m_depthNormalUseRobust
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
Definition: vpMbDepthNormalTracker.h:179
vpMbTracker::m_computeInteraction
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMbtFaceDepthNormal::m_hiddenFace
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
Definition: vpMbtFaceDepthNormal.h:78
vpMbTracker::distNearClip
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:114
vpMbDepthNormalTracker::display
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
Definition: vpMbDepthNormalTracker.cpp:281
vpMbTracker::m_cMo
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
vpMbHiddenFaces::reset
void reset()
Definition: vpMbHiddenFaces.h:398
vpDisplay::displayLine
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Definition: vpDisplay_uchar.cpp:400
vpMbDepthNormalTracker::track
virtual void track(const vpImage< unsigned char > &)
Definition: vpMbDepthNormalTracker.cpp:787
vpMbTracker::m_cam
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
vpMbDepthNormalTracker::initCylinder
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
Definition: vpMbDepthNormalTracker.cpp:824
vpMbtXmlGenericParser::setAngleDisappear
void setAngleDisappear(const double &adisappear)
Definition: vpMbtXmlGenericParser.cpp:1728
vpMbTracker::useOgre
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
vpMbDepthNormalTracker::m_weightedError_depthNormal
vpColVector m_weightedError_depthNormal
Weighted error.
Definition: vpMbDepthNormalTracker.h:191
vpMbTracker::computeVVSCheckLevenbergMarquardt
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)
Definition: vpMbTracker.cpp:2903
vpImageConvert::convert
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition: vpImageConvert.cpp:79
vpMbDepthNormalTracker::setDepthNormalSamplingStep
virtual void setDepthNormalSamplingStep(unsigned int stepX, unsigned int stepY)
Definition: vpMbDepthNormalTracker.cpp:772
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:108
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:693
vpMatrix::insert
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:4909
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:234
vpMbtFaceDepthNormal::m_cam
vpCameraParameters m_cam
Camera intrinsic parameters.
Definition: vpMbtFaceDepthNormal.h:70
vpMbtXmlGenericParser::getDepthNormalPclPlaneEstimationMethod
int getDepthNormalPclPlaneEstimationMethod() const
Definition: vpMbtXmlGenericParser.cpp:1531
vpMbtXmlGenericParser::getAngleDisappear
double getAngleDisappear() const
Definition: vpMbtXmlGenericParser.cpp:1486
vpMbDepthNormalTracker::resetTracker
virtual void resetTracker()
Definition: vpMbDepthNormalTracker.cpp:496
vpMbTracker::GAUSS_NEWTON_OPT
@ GAUSS_NEWTON_OPT
Definition: vpMbTracker.h:107
vpMbHiddenFaces::setVisibleOgre
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
Definition: vpMbHiddenFaces.h:775
vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationMethod
virtual void setDepthNormalPclPlaneEstimationMethod(int method)
Definition: vpMbDepthNormalTracker.cpp:742
vpMath::deg
static double deg(double rad)
Definition: vpMath.h:101
vpMbtFaceDepthNormal::setFeatureEstimationMethod
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
Definition: vpMbtFaceDepthNormal.h:153
vpMbDepthNormalTracker::testTracking
virtual void testTracking()
Definition: vpMbDepthNormalTracker.cpp:585
vpMbDepthNormalTracker::setDepthNormalFeatureEstimationMethod
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
Definition: vpMbDepthNormalTracker.cpp:731
vpHomogeneousMatrix::eye
void eye()
Definition: vpHomogeneousMatrix.cpp:662
vpException::fatalError
@ fatalError
Fatal error.
Definition: vpException.h:96
vpMbDepthNormalTracker::initFaceFromCorners
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
Definition: vpMbDepthNormalTracker.cpp:830
vpMbtXmlGenericParser::getDepthNormalSamplingStepX
unsigned int getDepthNormalSamplingStepX() const
Definition: vpMbtXmlGenericParser.cpp:1555
vpMbtFaceDepthNormal::m_distNearClip
double m_distNearClip
Distance for near clipping.
Definition: vpMbtFaceDepthNormal.h:76
vpMbTracker::computeCovariance
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
vpException::ioError
@ ioError
I/O error.
Definition: vpException.h:91
vpMbTracker::modelInitialised
bool modelInitialised
Definition: vpMbTracker.h:123
vpDisplay::displayArrow
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:61
vpMbTracker::faces
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacThreshold
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(double thresold)
Definition: vpMbDepthNormalTracker.cpp:762
vpMbtXmlGenericParser::setDepthNormalPclPlaneEstimationRansacThreshold
void setDepthNormalPclPlaneEstimationRansacThreshold(double threshold)
Definition: vpMbtXmlGenericParser.cpp:1798
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
vpMbDepthNormalTracker::m_depthNormalFeatureEstimationMethod
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
Definition: vpMbDepthNormalTracker.h:159
vpMbtFaceDepthNormal::m_useScanLine
bool m_useScanLine
Scan line visibility.
Definition: vpMbtFaceDepthNormal.h:84
vpColor::blue
static const vpColor blue
Definition: vpColor.h:185
vpMbDepthNormalTracker::getFeaturesForDisplayDepthNormal
virtual std::vector< std::vector< double > > getFeaturesForDisplayDepthNormal()
Definition: vpMbDepthNormalTracker.cpp:331
vpMbDepthNormalTracker::m_error_depthNormal
vpColVector m_error_depthNormal
(s - s*)
Definition: vpMbDepthNormalTracker.h:181
vpMbtXmlGenericParser::setDepthNormalFeatureEstimationMethod
void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
Definition: vpMbtXmlGenericParser.cpp:1768
vpPolygon3D::FOV_CLIPPING
@ FOV_CLIPPING
Definition: vpPolygon3D.h:70
vpMbtFaceDepthNormal::computeVisibility
void computeVisibility()
Definition: vpMbtFaceDepthNormal.cpp:719
vpMbDepthNormalTracker::m_robust_depthNormal
vpRobust m_robust_depthNormal
Robust.
Definition: vpMbDepthNormalTracker.h:187
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:222
vpMbDepthNormalTracker::m_depthNormalSamplingStepY
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
Definition: vpMbDepthNormalTracker.h:177
vpMbtPolygon::getName
std::string getName() const
Definition: vpMbtPolygon.h:108
vpMbDepthNormalTracker::m_w_depthNormal
vpColVector m_w_depthNormal
Robust weights.
Definition: vpMbDepthNormalTracker.h:189
vpMbDepthNormalTracker::loadConfigFile
virtual void loadConfigFile(const std::string &configFile)
Definition: vpMbDepthNormalTracker.cpp:419
vpMbDepthNormalTracker::computeVVSInit
virtual void computeVVSInit()
Definition: vpMbDepthNormalTracker.cpp:248
vpMbtXmlGenericParser::getFovClipping
bool getFovClipping() const
Definition: vpMbtXmlGenericParser.cpp:1579
vpMbDepthNormalTracker::initFaceFromLines
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Definition: vpMbDepthNormalTracker.cpp:832
vpMbtXmlGenericParser::getDepthNormalFeatureEstimationMethod
vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
Definition: vpMbtXmlGenericParser.cpp:1523
vpMbDepthNormalTracker::m_depthNormalFaces
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
Definition: vpMbDepthNormalTracker.h:167
vpMbDepthNormalTracker::initCircle
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
Definition: vpMbDepthNormalTracker.cpp:818
vpPolygon3D::p
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
vpMbDepthNormalTracker::m_depthNormalPclPlaneEstimationMethod
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
Definition: vpMbDepthNormalTracker.h:169
vpMbtFaceDepthNormal::computeDesiredFeatures
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=NULL)
Definition: vpMbtFaceDepthNormal.cpp:150
vpMbtXmlGenericParser::getDepthNormalPclPlaneEstimationRansacThreshold
double getDepthNormalPclPlaneEstimationRansacThreshold() const
Definition: vpMbtXmlGenericParser.cpp:1547
vpMbDepthNormalTracker::setUseDepthNormalTracking
void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
Definition: vpMbDepthNormalTracker.cpp:574
vpMbDepthNormalTracker::setPose
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
Definition: vpMbDepthNormalTracker.cpp:541
vpMbtFaceDepthNormal::setPclPlaneEstimationRansacThreshold
void setPclPlaneEstimationRansacThreshold(double threshold)
Definition: vpMbtFaceDepthNormal.h:159
vpMbTracker::m_lambda
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpMbtFaceDepthNormal::setPclPlaneEstimationMethod
void setPclPlaneEstimationMethod(int method)
Definition: vpMbtFaceDepthNormal.h:155
vpMbTracker::useScanLine
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpMbtXmlGenericParser
Parse an Xml file to extract configuration parameters of a mbtConfig object.
Definition: vpMbtXmlGenericParser.h:63
vpMbtXmlGenericParser::DEPTH_NORMAL_PARSER
@ DEPTH_NORMAL_PARSER
Definition: vpMbtXmlGenericParser.h:69
vpMbtXmlGenericParser::getDepthNormalSamplingStepY
unsigned int getDepthNormalSamplingStepY() const
Definition: vpMbtXmlGenericParser.cpp:1563
vpAROgre::setWindowName
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
vpMbDepthNormalTracker::m_depthNormalListOfDesiredFeatures
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
Definition: vpMbDepthNormalTracker.h:165
vpMbtFaceDepthNormal::m_polygon
vpMbtPolygon * m_polygon
Polygon defining the face.
Definition: vpMbtFaceDepthNormal.h:82
vpMbTracker::initFromPose
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
Definition: vpMbTracker.cpp:1135
vpMbHiddenFaces::setBackgroundSizeOgre
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
Definition: vpMbHiddenFaces.h:211
vpMbtXmlGenericParser::getDepthNormalPclPlaneEstimationRansacMaxIter
int getDepthNormalPclPlaneEstimationRansacMaxIter() const
Definition: vpMbtXmlGenericParser.cpp:1539
vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacMaxIter
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
Definition: vpMbDepthNormalTracker.cpp:752
vpMbtXmlGenericParser::getNearClippingDistance
double getNearClippingDistance() const
Definition: vpMbtXmlGenericParser.cpp:1675
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:165
vpRobust::setThreshold
void setThreshold(double noise_threshold)
Definition: vpRobust.h:115
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:280
vpMbtPolygon
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:67
vpMbDepthNormalTracker::setScanLineVisibilityTest
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbDepthNormalTracker.cpp:564
vpColVector::insert
void insert(unsigned int i, const vpColVector &v)
Definition: vpColVector.cpp:1362
vpMbDepthNormalTracker::m_depthNormalPclPlaneEstimationRansacMaxIter
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
Definition: vpMbDepthNormalTracker.h:171
vpMbDepthNormalTracker::setDepthNormalFaceCentroidMethod
virtual void setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
Definition: vpMbDepthNormalTracker.cpp:723
vpMbtFaceDepthNormal::m_clippingFlag
unsigned int m_clippingFlag
Flags specifying which clipping to used.
Definition: vpMbtFaceDepthNormal.h:72
vpMbtFaceDepthNormal::m_distFarClip
double m_distFarClip
Distance for near clipping.
Definition: vpMbtFaceDepthNormal.h:74
vpMbTracker::m_initialMu
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpMbtFaceDepthNormal::isVisible
bool isVisible() const
Definition: vpMbtFaceDepthNormal.h:147
vpMbtPolygon::getIndex
int getIndex() const
Definition: vpMbtPolygon.h:101
vpMbDepthNormalTracker::getModelForDisplay
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
Definition: vpMbDepthNormalTracker.cpp:357
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:740
vpMbDepthNormalTracker::segmentPointCloud
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
Definition: vpMbDepthNormalTracker.cpp:588
vpArray2D::resize
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
vpMbDepthNormalTracker::computeVisibility
void computeVisibility(unsigned int width, unsigned int height)
Definition: vpMbDepthNormalTracker.cpp:132
vpMbTracker::distFarClip
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
vpMbTracker::m_optimizationMethod
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
vpMbtXmlGenericParser::parse
void parse(const std::string &filename)
Definition: vpMbtXmlGenericParser.cpp:1470
vpMbTracker::computeVVSPoseEstimation
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)
Definition: vpMbTracker.cpp:2925
vpMbTracker::displayFeatures
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
vpMbDepthNormalTracker::computeVVS
void computeVVS()
Definition: vpMbDepthNormalTracker.cpp:153
vpMbTracker::oJo
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
vpException::what
const char * what() const
Definition: vpException.cpp:102
vpMbtXmlGenericParser::getAngleAppear
double getAngleAppear() const
Definition: vpMbtXmlGenericParser.cpp:1478
vpMbHiddenFaces::getOgreContext
vpAROgre * getOgreContext()
Definition: vpMbHiddenFaces.h:155
vpMbDepthNormalTracker::m_depthNormalPclPlaneEstimationRansacThreshold
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
Definition: vpMbDepthNormalTracker.h:173
vpMbDepthNormalTracker::setCameraParameters
virtual void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbDepthNormalTracker.cpp:713
vpMbTracker::computeVVSWeights
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
Definition: vpMbTracker.cpp:2988
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:167
vpMbDepthNormalTracker::~vpMbDepthNormalTracker
virtual ~vpMbDepthNormalTracker()
Definition: vpMbDepthNormalTracker.cpp:78
vpMath::equal
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
vpMbDepthNormalTracker::init
virtual void init(const vpImage< unsigned char > &I)
Definition: vpMbDepthNormalTracker.cpp:386
vpMbTracker::angleAppears
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
vpMbtXmlGenericParser::getCameraParameters
void getCameraParameters(vpCameraParameters &cam) const
Definition: vpMbtXmlGenericParser.cpp:1491
vpMbHiddenFaces::setOgreShowConfigDialog
void setOgreShowConfigDialog(bool showConfigDialog)
Definition: vpMbHiddenFaces.h:254
vpMbDepthNormalTracker::vpMbDepthNormalTracker
vpMbDepthNormalTracker()
Definition: vpMbDepthNormalTracker.cpp:55
vpPolygon3D::nbpt
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:76
vpMbtXmlGenericParser::setDepthNormalPclPlaneEstimationRansacMaxIter
void setDepthNormalPclPlaneEstimationRansacMaxIter(int maxIter)
Definition: vpMbtXmlGenericParser.cpp:1788
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
vpMbHiddenFaces::isOgreInitialised
bool isOgreInitialised()
Definition: vpMbHiddenFaces.h:177
vpMbtXmlGenericParser::setDepthNormalPclPlaneEstimationMethod
void setDepthNormalPclPlaneEstimationMethod(int method)
Definition: vpMbtXmlGenericParser.cpp:1778
vpMbDepthNormalTracker::m_L_depthNormal
vpMatrix m_L_depthNormal
Interaction matrix.
Definition: vpMbDepthNormalTracker.h:185
vpMbtFaceDepthNormal::addLine
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
Definition: vpMbtFaceDepthNormal.cpp:89
vpMbtXmlGenericParser::setCameraParameters
void setCameraParameters(const vpCameraParameters &cam)
Definition: vpMbtXmlGenericParser.cpp:1738
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:59
vpVelocityTwistMatrix::buildFrom
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpVelocityTwistMatrix.cpp:364
vpPolygon3D::getNbPoint
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
vpMbHiddenFaces::computeScanLineRender
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
Definition: vpMbHiddenFaces.h:464
vpMbtXmlGenericParser::getFarClippingDistance
double getFarClippingDistance() const
Definition: vpMbtXmlGenericParser.cpp:1571
vpMbtXmlGenericParser::hasFarClippingDistance
bool hasFarClippingDistance() const
Definition: vpMbtXmlGenericParser.cpp:1698
vpMbtFaceDepthNormal
Definition: vpMbtFaceDepthNormal.h:54
vpMbTracker::m_mask
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
vpColVector::resize
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpMbtXmlGenericParser::hasNearClippingDistance
bool hasNearClippingDistance() const
Definition: vpMbtXmlGenericParser.cpp:1708
vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu
virtual void computeVVSInteractionMatrixAndResidu()
Definition: vpMbDepthNormalTracker.cpp:263
vpRobust::resize
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
vpMbTracker::setOgreVisibilityTest
virtual void setOgreVisibilityTest(const bool &v)
Definition: vpMbTracker.cpp:2692
vpMbtFaceDepthNormal::m_planeObject
vpPlane m_planeObject
Plane equation described in the object frame.
Definition: vpMbtFaceDepthNormal.h:80
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:59
vpMbTracker::setClipping
virtual void setClipping(const unsigned int &flags)
Definition: vpMbTracker.cpp:2816
vpMbTracker::m_maxIter
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
vpMbTracker::ogreShowConfigDialog
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
vpMbDepthNormalTracker::reInitModel
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
Definition: vpMbDepthNormalTracker.cpp:469
vpMbtFaceDepthNormal::vpFaceCentroidType
vpFaceCentroidType
Definition: vpMbtFaceDepthNormal.h:56
vpMbDepthNormalTracker::m_depthNormalSamplingStepX
unsigned int m_depthNormalSamplingStepX
Sampling step in x-direction.
Definition: vpMbDepthNormalTracker.h:175
vpMbTracker::clippingFlag
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
vpPlane::object_frame
@ object_frame
Definition: vpPlane.h:70
vpMbDepthNormalTracker::addFace
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
Definition: vpMbDepthNormalTracker.cpp:85
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:716
vpMbTracker::setNearClippingDistance
virtual void setNearClippingDistance(const double &dist)
Definition: vpMbTracker.cpp:2789
vpMbTracker::setFarClippingDistance
virtual void setFarClippingDistance(const double &dist)
Definition: vpMbTracker.cpp:2710
vpMbtFaceDepthNormal::isTracked
bool isTracked() const
Definition: vpMbtFaceDepthNormal.h:145
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:2823
vpImage< unsigned char >
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:641
vpColor::red
static const vpColor red
Definition: vpColor.h:179
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:59
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:150
vpMbTracker::m_stopCriteriaEpsilon
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
Definition: vpMbTracker.h:191
vpMbtXmlGenericParser::setDepthNormalSamplingStepX
void setDepthNormalSamplingStepX(unsigned int stepX)
Definition: vpMbtXmlGenericParser.cpp:1808
vpMbTracker::setScanLineVisibilityTest
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:597
vpMbDepthNormalTracker::m_depthNormalListOfActiveFaces
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
Definition: vpMbDepthNormalTracker.h:163
vpMbtXmlGenericParser::setDepthNormalSamplingStepY
void setDepthNormalSamplingStepY(unsigned int stepY)
Definition: vpMbtXmlGenericParser.cpp:1818
vpMbDepthNormalTracker::m_depthNormalHiddenFacesDisplay
vpMbHiddenFaces< vpMbtPolygon > m_depthNormalHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
Definition: vpMbDepthNormalTracker.h:161
vpMbDepthNormalTracker::setOgreVisibilityTest
virtual void setOgreVisibilityTest(const bool &v)
Definition: vpMbDepthNormalTracker.cpp:533
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:72
vpCameraParameters::computeFov
void computeFov(const unsigned int &w, const unsigned int &h)
Definition: vpCameraParameters.cpp:432
vpPolygon3D::NO_CLIPPING
@ NO_CLIPPING
Definition: vpPolygon3D.h:63
vpMbHiddenFaces::initOgre
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
Definition: vpMbHiddenFaces.h:715
vpMbtFaceDepthNormal::getModelForDisplay
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
Definition: vpMbtFaceDepthNormal.cpp:1594
vpMbTracker::loadModel
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Definition: vpMbTracker.cpp:1466
vpMbTracker::angleDisappears
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
vpMbTracker::m_I
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
vpMatrix::eye
void eye()
Definition: vpMatrix.cpp:492
vpArray2D::getRows
unsigned int getRows() const
Definition: vpArray2D.h:289