Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpMbDepthDenseTracker.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 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_depthDenseListOfActiveFaces(),
57  m_denseDepthNbFeatures(0), m_depthDenseFaces(), 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_depthDenseFaces.size(); i++) {
78  delete m_depthDenseFaces[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_depthDenseFaces.push_back(normal_face);
127 }
128 
129 void vpMbDepthDenseTracker::computeVisibility(const unsigned int width, const unsigned int height)
130 {
131  bool changed = false;
132  faces.setVisible(width, height, cam, cMo, angleAppears, angleDisappears, changed);
133 
134  if (useScanLine) {
135  // if (clippingFlag <= 2) {
136  // cam.computeFov(width, height);
137  // }
138 
140  faces.computeScanLineRender(cam, width, height);
141  }
142 
143  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
144  it != m_depthDenseFaces.end(); ++it) {
145  vpMbtFaceDepthDense *face_normal = *it;
146  face_normal->computeVisibility();
147  }
148 }
149 
151 {
152  double normRes = 0;
153  double normRes_1 = -1;
154  unsigned int iter = 0;
155 
156  computeVVSInit();
157 
159  vpMatrix LTL;
160  vpColVector LTR, v;
161 
162  double mu = m_initialMu;
163  vpHomogeneousMatrix cMo_prev;
164 
165  bool isoJoIdentity_ = true;
167  vpMatrix L_true, LVJ_true;
168 
169  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
171 
172  bool reStartFromLastIncrement = false;
173  computeVVSCheckLevenbergMarquardt(iter, m_error_depthDense, error_prev, cMo_prev, mu, reStartFromLastIncrement);
174 
175  if (!reStartFromLastIncrement) {
177 
178  if (computeCovariance) {
179  L_true = m_L_depthDense;
180  if (!isoJoIdentity_) {
182  cVo.buildFrom(cMo);
183  LVJ_true = (m_L_depthDense * cVo * oJo);
184  }
185  }
186 
187  // Compute DoF only once
188  if (iter == 0) {
189  isoJoIdentity_ = true;
190  oJo.eye();
191 
192  // If all the 6 dof should be estimated, we check if the interaction
193  // matrix is full rank. If not we remove automatically the dof that
194  // cannot be estimated This is particularly useful when consering
195  // circles (rank 5) and cylinders (rank 4)
196  if (isoJoIdentity_) {
197  cVo.buildFrom(cMo);
198 
199  vpMatrix K; // kernel
200  unsigned int rank = (m_L_depthDense * cVo).kernel(K);
201  if (rank == 0) {
202  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
203  }
204 
205  if (rank != 6) {
206  vpMatrix I; // Identity
207  I.eye(6);
208  oJo = I - K.AtA();
209 
210  isoJoIdentity_ = false;
211  }
212  }
213  }
214 
215  double num = 0.0, den = 0.0;
216  for (unsigned int i = 0; i < m_L_depthDense.getRows(); i++) {
217  // Compute weighted errors and stop criteria
219  num += m_w_depthDense[i] * vpMath::sqr(m_error_depthDense[i]);
220  den += m_w_depthDense[i];
221 
222  // weight interaction matrix
223  for (unsigned int j = 0; j < 6; j++) {
224  m_L_depthDense[i][j] *= m_w_depthDense[i];
225  }
226  }
227 
229  m_error_depthDense, error_prev, LTR, mu, v);
230 
231  cMo_prev = cMo;
233 
234  normRes_1 = normRes;
235  normRes = sqrt(num / den);
236  }
237 
238  iter++;
239  }
240 
241  computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthDense, cMo_prev, L_true, LVJ_true, m_error_depthDense);
242 }
243 
245 {
247 
248  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
249  it != m_depthDenseListOfActiveFaces.end(); ++it) {
250  vpMbtFaceDepthDense *face = *it;
252  }
253 
254  m_L_depthDense.resize(m_denseDepthNbFeatures, 6, false, false);
257 
259  m_w_depthDense = 1;
260 }
261 
263 {
264  unsigned int start_index = 0;
265  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
266  it != m_depthDenseListOfActiveFaces.end(); ++it) {
267  vpMbtFaceDepthDense *face = *it;
268 
269  vpMatrix L_face;
270  vpColVector error;
271 
272  face->computeInteractionMatrixAndResidu(cMo, L_face, error);
273 
274  m_error_depthDense.insert(start_index, error);
275  m_L_depthDense.insert(L_face, start_index, 0);
276 
277  start_index += error.getRows();
278  }
279 }
280 
282 {
284 }
285 
287  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
288  const bool displayFullModel)
289 {
290  std::vector<std::vector<double> > models = vpMbDepthDenseTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, cam_, displayFullModel);
291 
292  for (size_t i = 0; i < models.size(); i++) {
293  if (vpMath::equal(models[i][0], 0)) {
294  vpImagePoint ip1(models[i][1], models[i][2]);
295  vpImagePoint ip2(models[i][3], models[i][4]);
296  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
297  }
298  }
299 }
300 
302  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
303  const bool displayFullModel)
304 {
305  std::vector<std::vector<double> > models = vpMbDepthDenseTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, cam_, displayFullModel);
306 
307  for (size_t i = 0; i < models.size(); i++) {
308  if (vpMath::equal(models[i][0], 0)) {
309  vpImagePoint ip1(models[i][1], models[i][2]);
310  vpImagePoint ip2(models[i][3], models[i][4]);
311  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
312  }
313  }
314 }
315 
329 std::vector<std::vector<double> > vpMbDepthDenseTracker::getModelForDisplay(unsigned int width, unsigned int height,
330  const vpHomogeneousMatrix &cMo_,
331  const vpCameraParameters &cam_,
332  const bool displayFullModel)
333 {
334  std::vector<std::vector<double> > models;
335 
336  vpCameraParameters c = cam_;
337 
338  bool changed = false;
339  m_depthDenseHiddenFacesDisplay.setVisible(width, height, c, cMo_, angleAppears, angleDisappears, changed);
340 
341  if (useScanLine) {
342  c.computeFov(width, height);
343 
346  }
347 
348  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
349  it != m_depthDenseFaces.end(); ++it) {
350  vpMbtFaceDepthDense *face_dense = *it;
351  std::vector<std::vector<double> > modelLines = face_dense->getModelForDisplay(width, height, cMo_, cam_, displayFullModel);
352  models.insert(models.end(), modelLines.begin(), modelLines.end());
353  }
354 
355  return models;
356 }
357 
359 {
360  if (!modelInitialised) {
361  throw vpException(vpException::fatalError, "model not initialized");
362  }
363 
364  bool reInitialisation = false;
365  if (!useOgre) {
366  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
367  } else {
368 #ifdef VISP_HAVE_OGRE
369  if (!faces.isOgreInitialised()) {
372  faces.initOgre(cam);
373  // Turn off Ogre config dialog display for the next call to this
374  // function since settings are saved in the ogre.cfg file and used
375  // during the next call
376  ogreShowConfigDialog = false;
377  }
378 
379  faces.setVisibleOgre(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
380 #else
381  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
382 #endif
383  }
384 
385  if (useScanLine || clippingFlag > 3)
386  cam.computeFov(I.getWidth(), I.getHeight());
387 
389 }
390 
391 void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile)
392 {
393 #ifdef VISP_HAVE_PUGIXML
395 
396  xmlp.setCameraParameters(cam);
399 
402 
403  try {
404  std::cout << " *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
405  xmlp.parse(configFile);
406  } catch (const vpException &e) {
407  std::cerr << "Exception: " << e.what() << std::endl;
408  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
409  }
410 
411  vpCameraParameters camera;
412  xmlp.getCameraParameters(camera);
413  setCameraParameters(camera);
414 
417 
418  if (xmlp.hasNearClippingDistance())
420 
421  if (xmlp.hasFarClippingDistance())
423 
424  if (xmlp.getFovClipping())
426 
428 #else
429  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
430 #endif
431 }
432 
433 void vpMbDepthDenseTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
434  const vpHomogeneousMatrix &cMo_, const bool verbose)
435 {
436  cMo.eye();
437 
438  for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
439  delete m_depthDenseFaces[i];
440  m_depthDenseFaces[i] = NULL;
441  }
442 
443  m_depthDenseFaces.clear();
444 
445  loadModel(cad_name, verbose);
446  initFromPose(I, cMo_);
447 }
448 
449 #if defined(VISP_HAVE_PCL)
450 void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
451  const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
452  const bool verbose)
453 {
454  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
455  reInitModel(I_dummy, cad_name, cMo_, verbose);
456 }
457 
458 #endif
459 
461 {
462  cMo.eye();
463 
464  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin();
465  it != m_depthDenseFaces.end(); ++it) {
466  vpMbtFaceDepthDense *normal_face = *it;
467  delete normal_face;
468  normal_face = NULL;
469  }
470 
471  m_depthDenseFaces.clear();
472 
473  m_computeInteraction = true;
474  computeCovariance = false;
475 
478 
480 
481  m_lambda = 1.0;
482  m_maxIter = 30;
483 
484  faces.reset();
485 
487 
488  useScanLine = false;
489 
490 #ifdef VISP_HAVE_OGRE
491  useOgre = false;
492 #endif
493 
495 }
496 
498 {
499  this->cam = camera;
500 
501  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
502  it != m_depthDenseFaces.end(); ++it) {
503  (*it)->setCameraParameters(camera);
504  }
505 }
506 
508 {
509  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
510  it != m_depthDenseFaces.end(); ++it) {
511  (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
512  }
513 }
514 
516 {
517  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
518  it != m_depthDenseFaces.end(); ++it) {
519  (*it)->setDepthDenseFilteringMethod(method);
520  }
521 }
522 
524 {
525  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
526  it != m_depthDenseFaces.end(); ++it) {
527  (*it)->setDepthDenseFilteringMinDistance(minDistance);
528  }
529 }
530 
532 {
533  if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
534  std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
535  return;
536  }
537 
538  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
539  it != m_depthDenseFaces.end(); ++it) {
540  (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
541  }
542 }
543 
544 #ifdef VISP_HAVE_PCL
545 void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
546 {
548 
549 #if DEBUG_DISPLAY_DEPTH_DENSE
550  if (!m_debugDisp_depthDense->isInitialised()) {
551  m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width);
552  m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
553  }
554 
555  m_debugImage_depthDense = 0;
556  std::vector<std::vector<vpImagePoint> > roiPts_vec;
557 #endif
558 
559  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin();
560  it != m_depthDenseFaces.end(); ++it) {
561  vpMbtFaceDepthDense *face = *it;
562 
563  if (face->isVisible() && face->isTracked()) {
564 #if DEBUG_DISPLAY_DEPTH_DENSE
565  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
566 #endif
568 #if DEBUG_DISPLAY_DEPTH_DENSE
569  ,
570  m_debugImage_depthDense, roiPts_vec_
571 #endif
572  , m_mask
573  )) {
574  m_depthDenseListOfActiveFaces.push_back(*it);
575 
576 #if DEBUG_DISPLAY_DEPTH_DENSE
577  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
578 #endif
579  }
580  }
581  }
582 
583 #if DEBUG_DISPLAY_DEPTH_DENSE
584  vpDisplay::display(m_debugImage_depthDense);
585 
586  for (size_t i = 0; i < roiPts_vec.size(); i++) {
587  if (roiPts_vec[i].empty())
588  continue;
589 
590  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
591  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
592  }
593  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
594  vpColor::red, 2);
595  }
596 
597  vpDisplay::flush(m_debugImage_depthDense);
598 #endif
599 }
600 #endif
601 
602 void vpMbDepthDenseTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, const unsigned int width,
603  const unsigned int height)
604 {
606 
607 #if DEBUG_DISPLAY_DEPTH_DENSE
608  if (!m_debugDisp_depthDense->isInitialised()) {
609  m_debugImage_depthDense.resize(height, width);
610  m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
611  }
612 
613  m_debugImage_depthDense = 0;
614  std::vector<std::vector<vpImagePoint> > roiPts_vec;
615 #endif
616 
617  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin();
618  it != m_depthDenseFaces.end(); ++it) {
619  vpMbtFaceDepthDense *face = *it;
620 
621  if (face->isVisible() && face->isTracked()) {
622 #if DEBUG_DISPLAY_DEPTH_DENSE
623  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
624 #endif
625  if (face->computeDesiredFeatures(cMo, width, height, point_cloud, m_depthDenseSamplingStepX,
627 #if DEBUG_DISPLAY_DEPTH_DENSE
628  ,
629  m_debugImage_depthDense, roiPts_vec_
630 #endif
631  , m_mask
632  )) {
633  m_depthDenseListOfActiveFaces.push_back(*it);
634 
635 #if DEBUG_DISPLAY_DEPTH_DENSE
636  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
637 #endif
638  }
639  }
640  }
641 
642 #if DEBUG_DISPLAY_DEPTH_DENSE
643  vpDisplay::display(m_debugImage_depthDense);
644 
645  for (size_t i = 0; i < roiPts_vec.size(); i++) {
646  if (roiPts_vec[i].empty())
647  continue;
648 
649  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
650  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
651  }
652  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
653  vpColor::red, 2);
654  }
655 
656  vpDisplay::flush(m_debugImage_depthDense);
657 #endif
658 }
659 
661 {
663 #ifdef VISP_HAVE_OGRE
664  faces.getOgreContext()->setWindowName("MBT Depth Dense");
665 #endif
666 }
667 
669 {
670  cMo = cdMo;
671  init(I);
672 }
673 
675 {
676  cMo = cdMo;
677  vpImageConvert::convert(I_color, m_I);
678  init(m_I);
679 }
680 
681 #if defined(VISP_HAVE_PCL)
682 void vpMbDepthDenseTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
683  const vpHomogeneousMatrix &cdMo)
684 {
685  m_I.resize(point_cloud->height, point_cloud->width);
686  cMo = cdMo;
687  init(m_I);
688 }
689 #endif
690 
692 {
694 
695  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
696  it != m_depthDenseFaces.end(); ++it) {
697  (*it)->setScanLineVisibilityTest(v);
698  }
699 }
700 
701 void vpMbDepthDenseTracker::setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
702 {
703  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
704  it != m_depthDenseFaces.end(); ++it) {
705  vpMbtFaceDepthDense *face = *it;
706  if (face->m_polygon->getName() == name) {
707  face->setTracked(useDepthDenseTracking);
708  }
709  }
710 }
711 
713 
715 {
716  throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
717 }
718 
720 {
721  throw vpException(vpException::fatalError, "Cannot track with a color image!");
722 }
723 
724 #ifdef VISP_HAVE_PCL
725 void vpMbDepthDenseTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
726 {
727  segmentPointCloud(point_cloud);
728 
729  computeVVS();
730 
731  computeVisibility(point_cloud->width, point_cloud->height);
732 }
733 #endif
734 
735 void vpMbDepthDenseTracker::track(const std::vector<vpColVector> &point_cloud, const unsigned int width,
736  const unsigned int height)
737 {
738  segmentPointCloud(point_cloud, width, height);
739 
740  computeVVS();
741 
742  computeVisibility(width, height);
743 }
744 
745 void vpMbDepthDenseTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
746  const double /*radius*/, const int /*idFace*/, const std::string & /*name*/)
747 {
748  throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCircle() should not be called!");
749 }
750 
751 void vpMbDepthDenseTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
752  const int /*idFace*/, const std::string & /*name*/)
753 {
754  throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCylinder() should not be called!");
755 }
756 
758 
unsigned int getNbFeatures() const
bool m_computeInteraction
Definition: vpMbTracker.h:185
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:164
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.
double getFarClippingDistance() const
void setTracked(const bool tracked)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
unsigned int getWidth() const
Definition: vpImage.h:237
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:143
double getNearClippingDistance() const
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)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
virtual void track(const vpImage< unsigned char > &)
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:305
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
bool m_useScanLine
Scan line visibility.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:113
vpColVector m_error_depthDense
(s - s*)
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:123
void getCameraParameters(vpCameraParameters &cam) const
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
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)
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
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 setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
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:111
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
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:151
vpAROgre * getOgreContext()
const char * what() const
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:4660
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
void setAngleDisappear(const double &adisappear)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
vpMatrix m_L_depthDense
Interaction matrix.
std::string getName() const
Definition: vpMbtPolygon.h:108
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double sqr(double x)
Definition: vpMath.h:114
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)
int getIndex() const
Definition: vpMbtPolygon.h:101
double m_distFarClip
Distance for near clipping.
Generic class defining intrinsic camera parameters.
virtual void setDepthDenseFilteringMethod(const int method)
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:863
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
unsigned int m_depthDenseSamplingStepX
Sampling step in x-direction.
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
unsigned int getRows() const
Definition: vpArray2D.h:289
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:108
virtual void setOgreVisibilityTest(const bool &v)
void insert(unsigned int i, const vpColVector &v)
void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
virtual void setDepthDenseFilteringMaxDistance(const double maxDistance)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
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:191
static double deg(double rad)
Definition: vpMath.h:101
void computeVisibility(const unsigned int width, const unsigned int height)
virtual void setOgreVisibilityTest(const bool &v)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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)
vpHomogeneousMatrix inverse() const
unsigned int getDepthDenseSamplingStepX() const
vpColVector m_w_depthDense
Robust weights.
static vpHomogeneousMatrix direct(const vpColVector &v)
std::vector< vpMbtFaceDepthDense * > m_depthDenseFaces
List of faces.
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:147
virtual void computeVVSInteractionMatrixAndResidu()
unsigned int getDepthDenseSamplingStepY() const
unsigned int getHeight() const
Definition: vpImage.h:179
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:597
virtual void setClipping(const unsigned int &flags)
virtual void init(const vpImage< unsigned char > &I)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
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:153
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void setFarClippingDistance(const double &dist)
void setCameraParameters(const vpCameraParameters &cam)
virtual void setCameraParameters(const vpCameraParameters &camera)
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
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)
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
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.