Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpMbDepthDenseTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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(), m_denseDepthNbFeatures(0), m_depthDenseFaces(),
57  m_depthDenseSamplingStepX(2), m_depthDenseSamplingStepY(2), m_error_depthDense(), m_L_depthDense(),
58  m_robust_depthDense(), m_w_depthDense(), m_weightedError_depthDense()
59 #if DEBUG_DISPLAY_DEPTH_DENSE
60  ,
61  m_debugDisp_depthDense(nullptr), 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, 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 = m_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, m_rand, 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, m_rand,
115  polygon.getIndex(), 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(unsigned int width, unsigned int height)
130 {
131  bool changed = false;
132  faces.setVisible(width, height, m_cam, m_cMo, angleAppears, angleDisappears, changed);
133 
134  if (useScanLine) {
135  // if (clippingFlag <= 2) {
136  // cam.computeFov(width, height);
137  // }
138 
140  faces.computeScanLineRender(m_cam, width, height);
141  }
142 
143  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
144  ++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 = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
166  if (isoJoIdentity)
167  oJo.eye();
168 
170  vpMatrix L_true, LVJ_true;
171 
172  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
174 
175  bool reStartFromLastIncrement = false;
176  computeVVSCheckLevenbergMarquardt(iter, m_error_depthDense, error_prev, cMo_prev, mu, reStartFromLastIncrement);
177 
178  if (!reStartFromLastIncrement) {
180 
181  if (computeCovariance) {
182  L_true = m_L_depthDense;
183  if (!isoJoIdentity) {
184  cVo.buildFrom(m_cMo);
185  LVJ_true = (m_L_depthDense * cVo * oJo);
186  }
187  }
188 
189  // Compute DoF only once
190  if (iter == 0) {
191  // If all the 6 dof should be estimated, we check if the interaction
192  // matrix is full rank. If not we remove automatically the dof that
193  // cannot be estimated. This is particularly useful when considering
194  // circles (rank 5) and cylinders (rank 4)
195  if (isoJoIdentity) {
196  cVo.buildFrom(m_cMo);
197 
198  vpMatrix K; // kernel
199  unsigned int rank = (m_L_depthDense * cVo).kernel(K);
200  if (rank == 0) {
201  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
202  }
203 
204  if (rank != 6) {
205  vpMatrix I; // Identity
206  I.eye(6);
207  oJo = I - K.AtA();
208 
209  isoJoIdentity = false;
210  }
211  }
212  }
213 
214  double num = 0.0, den = 0.0;
215  for (unsigned int i = 0; i < m_L_depthDense.getRows(); i++) {
216  // Compute weighted errors and stop criteria
219  den += m_w_depthDense[i];
220 
221  // weight interaction matrix
222  for (unsigned int j = 0; j < 6; j++) {
223  m_L_depthDense[i][j] *= m_w_depthDense[i];
224  }
225  }
226 
228  m_error_depthDense, error_prev, LTR, mu, v);
229 
230  cMo_prev = m_cMo;
232 
233  normRes_1 = normRes;
234  normRes = sqrt(num / den);
235  }
236 
237  iter++;
238  }
239 
240  computeCovarianceMatrixVVS(isoJoIdentity, m_w_depthDense, cMo_prev, L_true, LVJ_true, m_error_depthDense);
241 }
242 
244 {
246 
247  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
248  it != m_depthDenseListOfActiveFaces.end(); ++it) {
249  vpMbtFaceDepthDense *face = *it;
251  }
252 
253  m_L_depthDense.resize(m_denseDepthNbFeatures, 6, false, false);
256 
258  m_w_depthDense = 1;
259 }
260 
262 {
263  unsigned int start_index = 0;
264  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
265  it != m_depthDenseListOfActiveFaces.end(); ++it) {
266  vpMbtFaceDepthDense *face = *it;
267 
268  vpMatrix L_face;
269  vpColVector error;
270 
271  face->computeInteractionMatrixAndResidu(m_cMo, L_face, error);
272 
273  m_error_depthDense.insert(start_index, error);
274  m_L_depthDense.insert(L_face, start_index, 0);
275 
276  start_index += error.getRows();
277  }
278 }
279 
281 {
283 }
284 
286  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
287  bool displayFullModel)
288 {
289  std::vector<std::vector<double> > models =
290  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, unsigned int thickness,
303  bool displayFullModel)
304 {
305  std::vector<std::vector<double> > models =
306  vpMbDepthDenseTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
307 
308  for (size_t i = 0; i < models.size(); i++) {
309  if (vpMath::equal(models[i][0], 0)) {
310  vpImagePoint ip1(models[i][1], models[i][2]);
311  vpImagePoint ip2(models[i][3], models[i][4]);
312  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
313  }
314  }
315 }
316 
332 std::vector<std::vector<double> > vpMbDepthDenseTracker::getModelForDisplay(unsigned int width, unsigned int height,
333  const vpHomogeneousMatrix &cMo,
334  const vpCameraParameters &cam,
335  bool displayFullModel)
336 {
337  std::vector<std::vector<double> > models;
338 
339  vpCameraParameters c = cam;
340 
341  bool changed = false;
342  m_depthDenseHiddenFacesDisplay.setVisible(width, height, c, cMo, angleAppears, angleDisappears, changed);
343 
344  if (useScanLine) {
345  c.computeFov(width, height);
346 
349  }
350 
351  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
352  ++it) {
353  vpMbtFaceDepthDense *face_dense = *it;
354  std::vector<std::vector<double> > modelLines =
355  face_dense->getModelForDisplay(width, height, cMo, cam, displayFullModel);
356  models.insert(models.end(), modelLines.begin(), modelLines.end());
357  }
358 
359  return models;
360 }
361 
363 {
364  if (!modelInitialised) {
365  throw vpException(vpException::fatalError, "model not initialized");
366  }
367 
368  bool reInitialisation = false;
369  if (!useOgre) {
370  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
371  }
372  else {
373 #ifdef VISP_HAVE_OGRE
374  if (!faces.isOgreInitialised()) {
378  // Turn off Ogre config dialog display for the next call to this
379  // function since settings are saved in the ogre.cfg file and used
380  // during the next call
381  ogreShowConfigDialog = false;
382  }
383 
385 #else
386  faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
387 #endif
388  }
389 
390  if (useScanLine || clippingFlag > 3)
392 
394 }
395 
396 void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile, bool verbose)
397 {
398 #if defined(VISP_HAVE_PUGIXML)
400  xmlp.setVerbose(verbose);
404 
407 
408  try {
409  if (verbose) {
410  std::cout << " *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
411  }
412  xmlp.parse(configFile);
413  }
414  catch (const vpException &e) {
415  std::cerr << "Exception: " << e.what() << std::endl;
416  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
417  }
418 
419  vpCameraParameters camera;
420  xmlp.getCameraParameters(camera);
421  setCameraParameters(camera);
422 
425 
426  if (xmlp.hasNearClippingDistance())
428 
429  if (xmlp.hasFarClippingDistance())
431 
432  if (xmlp.getFovClipping())
434 
436 #else
437  (void)configFile;
438  (void)verbose;
439  throw(vpException(vpException::ioError, "vpMbDepthDenseTracker::loadConfigFile() needs pugixml built-in 3rdparty"));
440 #endif
441 }
442 
443 void vpMbDepthDenseTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
444  const vpHomogeneousMatrix &cMo, bool verbose)
445 {
446  m_cMo.eye();
447 
448  for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
449  delete m_depthDenseFaces[i];
450  m_depthDenseFaces[i] = nullptr;
451  }
452 
453  m_depthDenseFaces.clear();
454 
455  loadModel(cad_name, verbose);
456  initFromPose(I, cMo);
457 }
458 
459 #if defined(VISP_HAVE_PCL)
460 void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
461  const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose)
462 {
463  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
464  reInitModel(I_dummy, cad_name, cMo, verbose);
465 }
466 
467 #endif
468 
470 {
471  m_cMo.eye();
472 
473  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
474  ++it) {
475  vpMbtFaceDepthDense *normal_face = *it;
476  delete normal_face;
477  normal_face = nullptr;
478  }
479 
480  m_depthDenseFaces.clear();
481 
482  m_computeInteraction = true;
483  computeCovariance = false;
484 
487 
489 
490  m_lambda = 1.0;
491  m_maxIter = 30;
492 
493  faces.reset();
494 
496 
497  useScanLine = false;
498 
499 #ifdef VISP_HAVE_OGRE
500  useOgre = false;
501 #endif
502 
504 }
505 
507 {
508  m_cam = cam;
509 
510  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
511  ++it) {
512  (*it)->setCameraParameters(cam);
513  }
514 }
515 
517 {
518  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
519  ++it) {
520  (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
521  }
522 }
523 
525 {
526  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
527  ++it) {
528  (*it)->setDepthDenseFilteringMethod(method);
529  }
530 }
531 
533 {
534  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
535  ++it) {
536  (*it)->setDepthDenseFilteringMinDistance(minDistance);
537  }
538 }
539 
541 {
542  if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
543  std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
544  return;
545  }
546 
547  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
548  ++it) {
549  (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
550  }
551 }
552 
553 #ifdef VISP_HAVE_PCL
554 void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
555 {
557 
558 #if DEBUG_DISPLAY_DEPTH_DENSE
559  if (!m_debugDisp_depthDense->isInitialised()) {
560  m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width);
561  m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
562  }
563 
564  m_debugImage_depthDense = 0;
565  std::vector<std::vector<vpImagePoint> > roiPts_vec;
566 #endif
567 
568  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
569  ++it) {
570  vpMbtFaceDepthDense *face = *it;
571 
572  if (face->isVisible() && face->isTracked()) {
573 #if DEBUG_DISPLAY_DEPTH_DENSE
574  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
575 #endif
577 #if DEBUG_DISPLAY_DEPTH_DENSE
578  ,
579  m_debugImage_depthDense, roiPts_vec_
580 #endif
581  ,
582  m_mask)) {
583  m_depthDenseListOfActiveFaces.push_back(*it);
584 
585 #if DEBUG_DISPLAY_DEPTH_DENSE
586  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
587 #endif
588  }
589  }
590  }
591 
592 #if DEBUG_DISPLAY_DEPTH_DENSE
593  vpDisplay::display(m_debugImage_depthDense);
594 
595  for (size_t i = 0; i < roiPts_vec.size(); i++) {
596  if (roiPts_vec[i].empty())
597  continue;
598 
599  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
600  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
601  }
602  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
603  vpColor::red, 2);
604  }
605 
606  vpDisplay::flush(m_debugImage_depthDense);
607 #endif
608 }
609 #endif
610 
611 void vpMbDepthDenseTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width,
612  unsigned int height)
613 {
615 
616 #if DEBUG_DISPLAY_DEPTH_DENSE
617  if (!m_debugDisp_depthDense->isInitialised()) {
618  m_debugImage_depthDense.resize(height, width);
619  m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
620  }
621 
622  m_debugImage_depthDense = 0;
623  std::vector<std::vector<vpImagePoint> > roiPts_vec;
624 #endif
625 
626  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
627  ++it) {
628  vpMbtFaceDepthDense *face = *it;
629 
630  if (face->isVisible() && face->isTracked()) {
631 #if DEBUG_DISPLAY_DEPTH_DENSE
632  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
633 #endif
634  if (face->computeDesiredFeatures(m_cMo, width, height, point_cloud, m_depthDenseSamplingStepX,
636 #if DEBUG_DISPLAY_DEPTH_DENSE
637  ,
638  m_debugImage_depthDense, roiPts_vec_
639 #endif
640  ,
641  m_mask)) {
642  m_depthDenseListOfActiveFaces.push_back(*it);
643 
644 #if DEBUG_DISPLAY_DEPTH_DENSE
645  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
646 #endif
647  }
648  }
649  }
650 
651 #if DEBUG_DISPLAY_DEPTH_DENSE
652  vpDisplay::display(m_debugImage_depthDense);
653 
654  for (size_t i = 0; i < roiPts_vec.size(); i++) {
655  if (roiPts_vec[i].empty())
656  continue;
657 
658  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
659  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
660  }
661  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
662  vpColor::red, 2);
663  }
664 
665  vpDisplay::flush(m_debugImage_depthDense);
666 #endif
667 }
668 
669 
670 void vpMbDepthDenseTracker::segmentPointCloud(const vpMatrix &point_cloud, unsigned int width,
671  unsigned int height)
672 {
674 
675 #if DEBUG_DISPLAY_DEPTH_DENSE
676  if (!m_debugDisp_depthDense->isInitialised()) {
677  m_debugImage_depthDense.resize(height, width);
678  m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
679  }
680 
681  m_debugImage_depthDense = 0;
682  std::vector<std::vector<vpImagePoint> > roiPts_vec;
683 #endif
684 
685  for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
686  ++it) {
687  vpMbtFaceDepthDense *face = *it;
688 
689  if (face->isVisible() && face->isTracked()) {
690 #if DEBUG_DISPLAY_DEPTH_DENSE
691  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
692 #endif
693  if (face->computeDesiredFeatures(m_cMo, width, height, point_cloud, m_depthDenseSamplingStepX,
695 #if DEBUG_DISPLAY_DEPTH_DENSE
696  ,
697  m_debugImage_depthDense, roiPts_vec_
698 #endif
699  ,
700  m_mask)) {
701  m_depthDenseListOfActiveFaces.push_back(*it);
702 
703 #if DEBUG_DISPLAY_DEPTH_DENSE
704  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
705 #endif
706  }
707  }
708  }
709 
710 #if DEBUG_DISPLAY_DEPTH_DENSE
711  vpDisplay::display(m_debugImage_depthDense);
712 
713  for (size_t i = 0; i < roiPts_vec.size(); i++) {
714  if (roiPts_vec[i].empty())
715  continue;
716 
717  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
718  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
719  }
720  vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
721  vpColor::red, 2);
722  }
723 
724  vpDisplay::flush(m_debugImage_depthDense);
725 #endif
726 }
727 
729 {
731 #ifdef VISP_HAVE_OGRE
732  faces.getOgreContext()->setWindowName("MBT Depth Dense");
733 #endif
734 }
735 
737 {
738  m_cMo = cdMo;
739  init(I);
740 }
741 
743 {
744  m_cMo = cdMo;
745  vpImageConvert::convert(I_color, m_I);
746  init(m_I);
747 }
748 
749 #if defined(VISP_HAVE_PCL)
750 void vpMbDepthDenseTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
751  const vpHomogeneousMatrix &cdMo)
752 {
753  m_I.resize(point_cloud->height, point_cloud->width);
754  m_cMo = cdMo;
755  init(m_I);
756 }
757 #endif
758 
760 {
762 
763  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
764  ++it) {
765  (*it)->setScanLineVisibilityTest(v);
766  }
767 }
768 
769 void vpMbDepthDenseTracker::setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
770 {
771  for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin(); it != m_depthDenseFaces.end();
772  ++it) {
773  vpMbtFaceDepthDense *face = *it;
774  if (face->m_polygon->getName() == name) {
775  face->setTracked(useDepthDenseTracking);
776  }
777  }
778 }
779 
781 
783 {
784  throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
785 }
786 
788 {
789  throw vpException(vpException::fatalError, "Cannot track with a color image!");
790 }
791 
792 #ifdef VISP_HAVE_PCL
793 void vpMbDepthDenseTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
794 {
795  segmentPointCloud(point_cloud);
796 
797  computeVVS();
798 
799  computeVisibility(point_cloud->width, point_cloud->height);
800 }
801 #endif
802 
803 void vpMbDepthDenseTracker::track(const std::vector<vpColVector> &point_cloud, unsigned int width, unsigned int height)
804 {
805  segmentPointCloud(point_cloud, width, height);
806 
807  computeVVS();
808 
809  computeVisibility(width, height);
810 }
811 
812 void vpMbDepthDenseTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
813  double /*radius*/, int /*idFace*/, const std::string & /*name*/)
814 {
815  throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCircle() should not be called!");
816 }
817 
818 void vpMbDepthDenseTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, double /*radius*/,
819  int /*idFace*/, const std::string & /*name*/)
820 {
821  throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCylinder() should not be called!");
822 }
823 
825 
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int getRows() const
Definition: vpArray2D.h:284
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
static void display(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, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ ioError
I/O error.
Definition: vpException.h:79
@ fatalError
Fatal error.
Definition: vpException.h:84
const char * what() const
Definition: vpException.cpp:70
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:249
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:787
unsigned int getHeight() const
Definition: vpImage.h:184
static double rad(double deg)
Definition: vpMath.h:127
static double sqr(double x)
Definition: vpMath.h:201
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void eye()
Definition: vpMatrix.cpp:442
vpMatrix AtA() const
Definition: vpMatrix.cpp:633
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5713
virtual void init(const vpImage< unsigned char > &I) vp_override
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
vpMbtTukeyEstimator< double > m_robust_depthDense
Tukey M-Estimator.
virtual void setDepthDenseFilteringMaxDistance(double maxDistance)
virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override
void computeVisibility(unsigned int width, unsigned int height)
virtual void computeVVSInteractionMatrixAndResidu() vp_override
virtual void initFaceFromCorners(vpMbtPolygon &polygon) vp_override
virtual void setScanLineVisibilityTest(const bool &v) vp_override
virtual ~vpMbDepthDenseTracker() vp_override
virtual void setOgreVisibilityTest(const bool &v) vp_override
virtual void setDepthDenseFilteringMethod(int method)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="") vp_override
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false)
vpColVector m_weightedError_depthDense
Weighted error.
unsigned int m_depthDenseSamplingStepY
Sampling step in y-direction.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) vp_override
vpMbHiddenFaces< vpMbtPolygon > m_depthDenseHiddenFacesDisplay
Set of faces describing the object used only for display with scan line.
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) vp_override
virtual void setCameraParameters(const vpCameraParameters &camera) vp_override
virtual void resetTracker() vp_override
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
virtual void track(const vpImage< unsigned char > &) vp_override
void setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
unsigned int m_depthDenseSamplingStepX
Sampling step in x-direction.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) vp_override
vpColVector m_error_depthDense
(s - s*)
vpMatrix m_L_depthDense
Interaction matrix.
std::vector< vpMbtFaceDepthDense * > m_depthDenseListOfActiveFaces
List of current active (visible and features extracted) faces.
virtual void computeVVSInit() vp_override
void setDepthDenseSamplingStep(unsigned int stepX, unsigned int stepY)
void addFace(vpMbtPolygon &polygon, bool alreadyClose)
virtual void setDepthDenseFilteringMinDistance(double minDistance)
virtual void testTracking() vp_override
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="") vp_override
unsigned int m_denseDepthNbFeatures
Nb features.
vpColVector m_w_depthDense
Robust weights.
virtual void setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
std::vector< vpMbtFaceDepthDense * > m_depthDenseFaces
List of faces.
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void setOgreShowConfigDialog(bool showConfigDialog)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
bool modelInitialised
Definition: vpMbTracker.h:123
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
Definition: vpMbTracker.h:227
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
Definition: vpMbTracker.h:191
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:608
virtual void setOgreVisibilityTest(const bool &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)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
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=nullptr, vpColVector *const m_w_prev=nullptr)
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
double m_distFarClip
Distance for near clipping.
vpPlane m_planeObject
Plane equation described in the object frame.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
unsigned int getNbFeatures() const
void setTracked(bool tracked)
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
vpMbtPolygon * m_polygon
Polygon defining the face.
bool m_useScanLine
Scan line visibility.
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=nullptr)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
vpCameraParameters m_cam
Camera intrinsic parameters.
double m_distNearClip
Distance for near clipping.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, vpUniRand &rand_gen, int polygon=-1, std::string name="")
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:58
std::string getName() const
Definition: vpMbtPolygon.h:98
int getIndex() const
Definition: vpMbtPolygon.h:91
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void getCameraParameters(vpCameraParameters &cam) const
void setDepthDenseSamplingStepY(unsigned int stepY)
void setAngleDisappear(const double &adisappear)
void setDepthDenseSamplingStepX(unsigned int stepX)
void setAngleAppear(const double &aappear)
unsigned int getDepthDenseSamplingStepY() const
void parse(const std::string &filename)
unsigned int getDepthDenseSamplingStepX() const
void setCameraParameters(const vpCameraParameters &cam)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:54
@ object_frame
Definition: vpPlane.h:65
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:71
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:127
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:76
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)