Visual Servoing Platform  version 3.1.0
vpMbDepthDenseTracker.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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 dense 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/vpMbDepthDenseTracker.h>
48 #include <visp3/mbt/vpMbtXmlGenericParser.h>
49 
50 #if DEBUG_DISPLAY_DEPTH_DENSE
51 #include <visp3/gui/vpDisplayGDI.h>
52 #include <visp3/gui/vpDisplayX.h>
53 #endif
54 
56  : m_depthDenseHiddenFacesDisplay(), m_depthDenseI_dummyVisibility(), m_depthDenseListOfActiveFaces(),
57  m_denseDepthNbFeatures(0), m_depthDenseNormalFaces(), m_depthDenseSamplingStepX(2), m_depthDenseSamplingStepY(2),
58  m_error_depthDense(), m_L_depthDense(), m_robust_depthDense(), m_w_depthDense(), m_weightedError_depthDense()
59 #if DEBUG_DISPLAY_DEPTH_DENSE
60  ,
61  m_debugDisp_depthDense(NULL), m_debugImage_depthDense()
62 #endif
63 {
64 #ifdef VISP_HAVE_OGRE
65  faces.getOgreContext()->setWindowName("MBT Depth Dense");
66 #endif
67 
68 #if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_DENSE
69  m_debugDisp_depthDense = new vpDisplayX;
70 #elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_DENSE
71  m_debugDisp_depthDense = new vpDisplayGDI;
72 #endif
73 }
74 
76 {
77  for (size_t i = 0; i < m_depthDenseNormalFaces.size(); i++) {
78  delete m_depthDenseNormalFaces[i];
79  }
80 
81 #if DEBUG_DISPLAY_DEPTH_DENSE
82  delete m_debugDisp_depthDense;
83 #endif
84 }
85 
86 void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, const bool alreadyClose)
87 {
88  if (polygon.nbpt < 3) {
89  return;
90  }
91 
92  // Copy hidden faces
94 
95  vpMbtFaceDepthDense *normal_face = new vpMbtFaceDepthDense;
96  normal_face->m_hiddenFace = &faces;
97  normal_face->m_polygon = &polygon;
98  normal_face->m_cam = cam;
99  normal_face->m_useScanLine = useScanLine;
100  normal_face->m_clippingFlag = clippingFlag;
101  normal_face->m_distNearClip = distNearClip;
102  normal_face->m_distFarClip = distFarClip;
103 
104  // Add lines that compose the face
105  unsigned int nbpt = polygon.getNbPoint();
106  if (nbpt > 0) {
107  for (unsigned int i = 0; i < nbpt - 1; i++) {
108  normal_face->addLine(polygon.p[i], polygon.p[i + 1], &m_depthDenseHiddenFacesDisplay, polygon.getIndex(),
109  polygon.getName());
110  }
111 
112  if (!alreadyClose) {
113  // Add last line that closes the face
114  normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], &m_depthDenseHiddenFacesDisplay, polygon.getIndex(),
115  polygon.getName());
116  }
117  }
118 
119  // Construct a vpPlane in object frame
120  vpPoint pts[3];
121  pts[0] = polygon.p[0];
122  pts[1] = polygon.p[1];
123  pts[2] = polygon.p[2];
124  normal_face->m_planeObject = vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame);
125 
126  m_depthDenseNormalFaces.push_back(normal_face);
127 }
128 
129 void vpMbDepthDenseTracker::computeVisibility(const unsigned int width, const unsigned int height)
130 {
131  m_depthDenseI_dummyVisibility.resize(height, width);
132 
133  bool changed = false;
135 
136  if (useScanLine) {
137  // if (clippingFlag <= 2) {
138  // cam.computeFov(m_depthDenseI_dummyVisibility.getWidth(),
139  // m_depthDenseI_dummyVisibility.getHeight());
140  // }
141 
145  }
146 
147  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
148  it != m_depthDenseNormalFaces.end(); ++it) {
149  vpMbtFaceDepthDense *face_normal = *it;
150  face_normal->computeVisibility();
151  }
152 }
153 
155 {
156  double normRes = 0;
157  double normRes_1 = -1;
158  unsigned int iter = 0;
159 
160  computeVVSInit();
161 
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_depthDense, error_prev, cMo_prev, mu, reStartFromLastIncrement);
178 
179  if (!reStartFromLastIncrement) {
181 
182  if (computeCovariance) {
183  L_true = m_L_depthDense;
184  if (!isoJoIdentity_) {
186  cVo.buildFrom(cMo);
187  LVJ_true = (m_L_depthDense * 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(cMo);
202 
203  vpMatrix K; // kernel
204  unsigned int rank = (m_L_depthDense * 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_depthDense.getRows(); i++) {
221  // Compute weighted errors and stop criteria
223  num += m_w_depthDense[i] * vpMath::sqr(m_error_depthDense[i]);
224  den += m_w_depthDense[i];
225 
226  // weight interaction matrix
227  for (unsigned int j = 0; j < 6; j++) {
228  m_L_depthDense[i][j] *= m_w_depthDense[i];
229  }
230  }
231 
233  m_error_depthDense, error_prev, LTR, mu, v);
234 
235  cMo_prev = cMo;
237 
238  normRes_1 = normRes;
239  normRes = sqrt(num / den);
240  }
241 
242  iter++;
243  }
244 
245  computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthDense, cMo_prev, L_true, LVJ_true, m_error_depthDense);
246 }
247 
249 {
251 
252  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
253  it != m_depthDenseListOfActiveFaces.end(); ++it) {
254  vpMbtFaceDepthDense *face = *it;
256  }
257 
258  m_L_depthDense.resize(m_denseDepthNbFeatures, 6, false, false);
261 
263  m_w_depthDense = 1;
264 }
265 
267 {
268  unsigned int start_index = 0;
269  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
270  it != m_depthDenseListOfActiveFaces.end(); ++it) {
271  vpMbtFaceDepthDense *face = *it;
272 
273  vpMatrix L_face;
274  vpColVector error;
275 
276  face->computeInteractionMatrixAndResidu(cMo, L_face, error);
277 
278  m_error_depthDense.insert(start_index, error);
279  m_L_depthDense.insert(L_face, start_index, 0);
280 
281  start_index += error.getRows();
282  }
283 }
284 
286 {
288 }
289 
291  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
292  const bool displayFullModel)
293 {
294  vpCameraParameters c = cam_;
295 
296  bool changed = false;
298 
299  if (useScanLine) {
300  c.computeFov(I.getWidth(), I.getHeight());
301 
304  }
305 
306  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
307  it != m_depthDenseNormalFaces.end(); ++it) {
308  vpMbtFaceDepthDense *face_normal = *it;
309  face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
310 
311  if (displayFeatures) {
312  face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
313  }
314  }
315 }
316 
318  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
319  const bool displayFullModel)
320 {
321  vpCameraParameters c = cam_;
322 
323  bool changed = false;
324  vpImage<unsigned char> I_dummy;
325  vpImageConvert::convert(I, I_dummy);
327 
328  if (useScanLine) {
329  c.computeFov(I.getWidth(), I.getHeight());
330 
333  }
334 
335  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
336  it != m_depthDenseNormalFaces.end(); ++it) {
337  vpMbtFaceDepthDense *face_normal = *it;
338  face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
339 
340  if (displayFeatures) {
341  face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
342  }
343  }
344 }
345 
347 {
348  if (!modelInitialised) {
349  throw vpException(vpException::fatalError, "model not initialized");
350  }
351 
352  bool reInitialisation = false;
353  if (!useOgre) {
354  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
355  } else {
356 #ifdef VISP_HAVE_OGRE
357  if (!faces.isOgreInitialised()) {
360  faces.initOgre(cam);
361  // Turn off Ogre config dialog display for the next call to this
362  // function since settings are saved in the ogre.cfg file and used
363  // during the next call
364  ogreShowConfigDialog = false;
365  }
366 
367  faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
368 #else
369  faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
370 #endif
371  }
372 
373  if (useScanLine || clippingFlag > 3)
374  cam.computeFov(I.getWidth(), I.getHeight());
375 
377 }
378 
379 void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile)
380 {
381 #ifdef VISP_HAVE_XML2
383 
384  xmlp.setCameraParameters(cam);
387 
390 
391  try {
392  std::cout << " *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
393  xmlp.parse(configFile);
394  } catch (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 
416 #else
417  std::cerr << "You need the libXML2 to read the config file " << configFile << std::endl;
418 #endif
419 }
420 
421 void vpMbDepthDenseTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
422  const vpHomogeneousMatrix &cMo_, const bool verbose)
423 {
424  cMo.eye();
425 
426  for (size_t i = 0; i < m_depthDenseNormalFaces.size(); i++) {
427  delete m_depthDenseNormalFaces[i];
428  m_depthDenseNormalFaces[i] = NULL;
429  }
430 
431  m_depthDenseNormalFaces.clear();
432 
433  loadModel(cad_name, verbose);
434  initFromPose(I, cMo_);
435 }
436 
437 #if defined(VISP_HAVE_PCL)
438 void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
439  const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
440  const bool verbose)
441 {
442  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
443  reInitModel(I_dummy, cad_name, cMo_, verbose);
444 }
445 
446 #endif
447 
449 {
450  cMo.eye();
451 
452  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseNormalFaces.begin();
453  it != m_depthDenseNormalFaces.end(); ++it) {
454  vpMbtFaceDepthDense *normal_face = *it;
455  delete normal_face;
456  normal_face = NULL;
457  }
458 
459  m_depthDenseNormalFaces.clear();
460 
461  m_computeInteraction = true;
462  computeCovariance = false;
463 
466 
468 
469  m_lambda = 1.0;
470  m_maxIter = 30;
471 
472  faces.reset();
473 
475 
476  useScanLine = false;
477 
478 #ifdef VISP_HAVE_OGRE
479  useOgre = false;
480 #endif
481 
483 }
484 
486 {
488 #ifdef VISP_HAVE_OGRE
489  faces.getOgreContext()->setWindowName("MBT Depth Dense");
490 #endif
491 }
492 
494 {
495  cMo = cdMo;
496  init(I);
497 }
498 
499 #if defined(VISP_HAVE_PCL)
500 void vpMbDepthDenseTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
501  const vpHomogeneousMatrix &cdMo)
502 {
503  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
504  cMo = cdMo;
505  init(I_dummy);
506 }
507 #endif
508 
510 {
512 
513  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
514  it != m_depthDenseNormalFaces.end(); ++it) {
515  (*it)->setScanLineVisibilityTest(v);
516  }
517 }
518 
520 
521 #ifdef VISP_HAVE_PCL
522 void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
523 {
525 
526 #if DEBUG_DISPLAY_DEPTH_DENSE
527  if (!m_debugDisp_depthDense->isInitialised()) {
528  m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width);
529  m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
530  }
531 
532  m_debugImage_depthDense = 0;
533  std::vector<std::vector<vpImagePoint> > roiPts_vec;
534 #endif
535 
536  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseNormalFaces.begin();
537  it != m_depthDenseNormalFaces.end(); ++it) {
538  vpMbtFaceDepthDense *face = *it;
539 
540  if (face->isVisible()) {
541 #if DEBUG_DISPLAY_DEPTH_DENSE
542  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
543 #endif
545 #if DEBUG_DISPLAY_DEPTH_DENSE
546  ,
547  m_debugImage_depthDense, roiPts_vec_
548 #endif
549  )) {
550  m_depthDenseListOfActiveFaces.push_back(*it);
551 
552 #if DEBUG_DISPLAY_DEPTH_DENSE
553  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
554 #endif
555  }
556  }
557  }
558 
559 #if DEBUG_DISPLAY_DEPTH_DENSE
560  vpDisplay::display(m_debugImage_depthDense);
561 
562  for (size_t i = 0; i < roiPts_vec.size(); i++) {
563  if (roiPts_vec[i].empty())
564  continue;
565 
566  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
567  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
568  }
569  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
570  vpColor::red, 2);
571  }
572 
573  vpDisplay::flush(m_debugImage_depthDense);
574 #endif
575 }
576 #endif
577 
578 void vpMbDepthDenseTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, const unsigned int width,
579  const unsigned int height)
580 {
582 
583 #if DEBUG_DISPLAY_DEPTH_DENSE
584  if (!m_debugDisp_depthDense->isInitialised()) {
585  m_debugImage_depthDense.resize(height, width);
586  m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
587  }
588 
589  m_debugImage_depthDense = 0;
590  std::vector<std::vector<vpImagePoint> > roiPts_vec;
591 #endif
592 
593  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseNormalFaces.begin();
594  it != m_depthDenseNormalFaces.end(); ++it) {
595  vpMbtFaceDepthDense *face = *it;
596 
597  if (face->isVisible()) {
598 #if DEBUG_DISPLAY_DEPTH_DENSE
599  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
600 #endif
601  if (face->computeDesiredFeatures(cMo, width, height, point_cloud, m_depthDenseSamplingStepX,
603 #if DEBUG_DISPLAY_DEPTH_DENSE
604  ,
605  m_debugImage_depthDense, roiPts_vec_
606 #endif
607  )) {
608  m_depthDenseListOfActiveFaces.push_back(*it);
609 
610 #if DEBUG_DISPLAY_DEPTH_DENSE
611  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
612 #endif
613  }
614  }
615  }
616 
617 #if DEBUG_DISPLAY_DEPTH_DENSE
618  vpDisplay::display(m_debugImage_depthDense);
619 
620  for (size_t i = 0; i < roiPts_vec.size(); i++) {
621  if (roiPts_vec[i].empty())
622  continue;
623 
624  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
625  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
626  }
627  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
628  vpColor::red, 2);
629  }
630 
631  vpDisplay::flush(m_debugImage_depthDense);
632 #endif
633 }
634 
636 {
637  this->cam = camera;
638 
639  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
640  it != m_depthDenseNormalFaces.end(); ++it) {
641  (*it)->setCameraParameters(camera);
642  }
643 }
644 
646 {
647  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
648  it != m_depthDenseNormalFaces.end(); ++it) {
649  (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
650  }
651 }
652 
654 {
655  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
656  it != m_depthDenseNormalFaces.end(); ++it) {
657  (*it)->setDepthDenseFilteringMethod(method);
658  }
659 }
660 
662 {
663  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
664  it != m_depthDenseNormalFaces.end(); ++it) {
665  (*it)->setDepthDenseFilteringMinDistance(minDistance);
666  }
667 }
668 
670 {
671  if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
672  std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
673  return;
674  }
675 
676  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
677  it != m_depthDenseNormalFaces.end(); ++it) {
678  (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
679  }
680 }
681 
683 {
684  throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
685 }
686 
687 #ifdef VISP_HAVE_PCL
688 void vpMbDepthDenseTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
689 {
690  segmentPointCloud(point_cloud);
691 
692  computeVVS();
693 
694  computeVisibility(point_cloud->width, point_cloud->height);
695 }
696 #endif
697 
698 void vpMbDepthDenseTracker::track(const std::vector<vpColVector> &point_cloud, const unsigned int width,
699  const unsigned int height)
700 {
701  segmentPointCloud(point_cloud, width, height);
702 
703  computeVVS();
704 
705  computeVisibility(width, height);
706 }
707 
708 void vpMbDepthDenseTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
709  const double /*radius*/, const int /*idFace*/, const std::string & /*name*/)
710 {
711  throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCircle() should not be called!");
712 }
713 
714 void vpMbDepthDenseTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
715  const int /*idFace*/, const std::string & /*name*/)
716 {
717  throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCylinder() should not be called!");
718 }
719 
721 
bool m_computeInteraction
Definition: vpMbTracker.h:187
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
vpMbtTukeyEstimator< double > m_robust_depthDense
Tukey M-Estimator.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
double m_distNearClip
Distance for near clipping.
virtual void setScanLineVisibilityTest(const bool &v)
vpMbHiddenFaces< vpMbtPolygon > m_depthDenseHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
int getIndex() const
Definition: vpMbtPolygon.h:101
vpCameraParameters m_cam
Camera intrinsic parameters.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:145
vpColVector m_weightedError_depthDense
Weighted error.
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void loadConfigFile(const std::string &configFile)
std::string getName() const
Definition: vpMbtPolygon.h:108
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void track(const vpImage< unsigned char > &)
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
bool m_useScanLine
Scan line visibility.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
double getFarClippingDistance() const
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:115
vpColVector m_error_depthDense
(s - s*)
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, const unsigned int stepX, const unsigned int stepY)
void setOgreShowConfigDialog(const bool showConfigDialog)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
bool modelInitialised
Definition: vpMbTracker.h:125
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getRows() const
Definition: vpArray2D.h:156
vpHomogeneousMatrix inverse() const
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:157
void getCameraParameters(vpCameraParameters &_cam) const
double getNearClippingDistance() const
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)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:130
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
static void flush(const vpImage< unsigned char > &I)
unsigned int getDepthDenseSamplingStepY() const
void addFace(vpMbtPolygon &polygon, const bool alreadyClose)
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:113
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPlane m_planeObject
Plane equation described in the object frame.
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:153
vpAROgre * getOgreContext()
unsigned int m_denseDepthNbFeatures
Nb features.
void setDepthDenseSamplingStepX(const unsigned int stepX)
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4512
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:117
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setAngleDisappear(const double &adisappear)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
vpMatrix m_L_depthDense
Interaction matrix.
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)
virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:160
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
Definition: vpMath.h:108
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
static void display(const vpImage< unsigned char > &I)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
double m_distFarClip
Distance for near clipping.
Generic class defining intrinsic camera parameters.
virtual void setDepthDenseFilteringMethod(const int method)
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:856
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:195
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
unsigned int m_depthDenseSamplingStepX
Sampling step in x-direction.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:142
vpImage< unsigned char > m_depthDenseI_dummyVisibility
Dummy image used to compute the visibility.
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:147
unsigned int getNbFeatures() const
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:191
void setAngleAppear(const double &aappear)
const char * what() const
static double rad(double deg)
Definition: vpMath.h:102
virtual void setOgreVisibilityTest(const bool &v)
void insert(unsigned int i, const vpColVector &v)
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
virtual void setDepthDenseFilteringMaxDistance(const double maxDistance)
void setDepthDenseSamplingStepY(const unsigned int stepY)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:76
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
Definition: vpMbTracker.h:193
static double deg(double rad)
Definition: vpMath.h:95
void computeVisibility(const unsigned int width, const unsigned int height)
virtual void setOgreVisibilityTest(const bool &v)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:140
virtual void loadModel(const char *modelFile, const bool verbose=false)
unsigned int getHeight() const
Definition: vpImage.h:178
bool ogreShowConfigDialog
Definition: vpMbTracker.h:158
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
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)
vpColVector m_w_depthDense
Robust weights.
static vpHomogeneousMatrix direct(const vpColVector &v)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:149
unsigned int getDepthDenseSamplingStepX() const
virtual void computeVVSInteractionMatrixAndResidu()
std::vector< vpMbtFaceDepthDense * > m_depthDenseNormalFaces
List of faces.
void setCameraParameters(const vpCameraParameters &_cam)
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:533
virtual void setClipping(const unsigned int &flags)
virtual void init(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:155
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setFarClippingDistance(const double &dist)
virtual void setCameraParameters(const vpCameraParameters &camera)
unsigned int getWidth() const
Definition: vpImage.h:229
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:151
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void eye()
Definition: vpMatrix.cpp:360
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)
virtual void setDepthDenseFilteringMinDistance(const double minDistance)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:241
unsigned int m_depthDenseSamplingStepY
Sampling step in y-direction.
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)
std::vector< vpMbtFaceDepthDense * > m_depthDenseListOfActiveFaces
List of current active (visible and features extracted) faces.