Visual Servoing Platform  version 3.2.1 under development (2019-10-23) under development (2019-10-23)
vpMbDepthNormalTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Model-based tracker using depth normal features.
33  *
34  *****************************************************************************/
35 
36 #include <iostream>
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #ifdef VISP_HAVE_PCL
41 #include <pcl/point_cloud.h>
42 #endif
43 
44 #include <visp3/core/vpDisplay.h>
45 #include <visp3/core/vpExponentialMap.h>
46 #include <visp3/core/vpTrackingException.h>
47 #include <visp3/mbt/vpMbDepthNormalTracker.h>
48 #include <visp3/mbt/vpMbtXmlGenericParser.h>
49 
50 #if DEBUG_DISPLAY_DEPTH_NORMAL
51 #include <visp3/gui/vpDisplayGDI.h>
52 #include <visp3/gui/vpDisplayX.h>
53 #endif
54 
56  : m_depthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION),
57  m_depthNormalHiddenFacesDisplay(), m_depthNormalListOfActiveFaces(),
58  m_depthNormalListOfDesiredFeatures(), m_depthNormalFaces(), m_depthNormalPclPlaneEstimationMethod(2),
59  m_depthNormalPclPlaneEstimationRansacMaxIter(200), m_depthNormalPclPlaneEstimationRansacThreshold(0.001),
60  m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2), m_depthNormalUseRobust(false), m_error_depthNormal(),
61  m_featuresToBeDisplayedDepthNormal(), m_L_depthNormal(), m_robust_depthNormal(), m_w_depthNormal(), m_weightedError_depthNormal()
62 #if DEBUG_DISPLAY_DEPTH_NORMAL
63  ,
64  m_debugDisp_depthNormal(NULL), m_debugImage_depthNormal()
65 #endif
66 {
67 #ifdef VISP_HAVE_OGRE
68  faces.getOgreContext()->setWindowName("MBT Depth");
69 #endif
70 
71 #if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_NORMAL
72  m_debugDisp_depthNormal = new vpDisplayX;
73 #elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_NORMAL
74  m_debugDisp_depthNormal = new vpDisplayGDI;
75 #endif
76 }
77 
79 {
80  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
81  delete m_depthNormalFaces[i];
82  }
83 }
84 
85 void vpMbDepthNormalTracker::addFace(vpMbtPolygon &polygon, const bool alreadyClose)
86 {
87  if (polygon.nbpt < 3) {
88  return;
89  }
90 
91  // Copy hidden faces
93 
94  vpMbtFaceDepthNormal *normal_face = new vpMbtFaceDepthNormal;
95  normal_face->m_hiddenFace = &faces;
96  normal_face->m_polygon = &polygon;
97  normal_face->m_cam = cam;
98  normal_face->m_useScanLine = useScanLine;
99  normal_face->m_clippingFlag = clippingFlag;
100  normal_face->m_distNearClip = distNearClip;
101  normal_face->m_distFarClip = distFarClip;
106 
107  // Add lines that compose the face
108  unsigned int nbpt = polygon.getNbPoint();
109  if (nbpt > 0) {
110  for (unsigned int i = 0; i < nbpt - 1; i++) {
111  normal_face->addLine(polygon.p[i], polygon.p[i + 1], &m_depthNormalHiddenFacesDisplay, polygon.getIndex(),
112  polygon.getName());
113  }
114 
115  if (!alreadyClose) {
116  // Add last line that closes the face
117  normal_face->addLine(polygon.p[nbpt - 1], polygon.p[0], &m_depthNormalHiddenFacesDisplay, polygon.getIndex(),
118  polygon.getName());
119  }
120  }
121 
122  // Construct a vpPlane in object frame
123  vpPoint pts[3];
124  pts[0] = polygon.p[0];
125  pts[1] = polygon.p[1];
126  pts[2] = polygon.p[2];
127  normal_face->m_planeObject = vpPlane(pts[0], pts[1], pts[2], vpPlane::object_frame);
128 
129  m_depthNormalFaces.push_back(normal_face);
130 }
131 
132 void vpMbDepthNormalTracker::computeVisibility(const unsigned int width, const unsigned int height)
133 {
134  bool changed = false;
135  faces.setVisible(width, height, cam, cMo, angleAppears, angleDisappears, changed);
136 
137  if (useScanLine) {
138  // if (clippingFlag <= 2) {
139  // cam.computeFov(width, height);
140  // }
141 
143  faces.computeScanLineRender(cam, width, height);
144  }
145 
146  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
147  it != m_depthNormalFaces.end(); ++it) {
148  vpMbtFaceDepthNormal *face_normal = *it;
149  face_normal->computeVisibility();
150  }
151 }
152 
154 {
155  double normRes = 0;
156  double normRes_1 = -1;
157  unsigned int iter = 0;
158 
159  computeVVSInit();
160  unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size());
161 
162  vpColVector error_prev(nb_features);
163  vpMatrix LTL;
164  vpColVector LTR, v;
165 
166  double mu = m_initialMu;
167  vpHomogeneousMatrix cMo_prev;
168 
169  bool isoJoIdentity_ = true;
171  vpMatrix L_true, LVJ_true;
172 
173  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
175 
176  bool reStartFromLastIncrement = false;
177  computeVVSCheckLevenbergMarquardt(iter, m_error_depthNormal, error_prev, cMo_prev, mu, reStartFromLastIncrement);
178 
179  if (!reStartFromLastIncrement) {
182 
183  if (computeCovariance) {
184  L_true = m_L_depthNormal;
185  if (!isoJoIdentity_) {
187  cVo.buildFrom(cMo);
188  LVJ_true = (m_L_depthNormal * (cVo * oJo));
189  }
190  }
191 
192  // Compute DoF only once
193  if (iter == 0) {
194  isoJoIdentity_ = true;
195  oJo.eye();
196 
197  // If all the 6 dof should be estimated, we check if the interaction
198  // matrix is full rank. If not we remove automatically the dof that
199  // cannot be estimated This is particularly useful when consering
200  // circles (rank 5) and cylinders (rank 4)
201  if (isoJoIdentity_) {
202  cVo.buildFrom(cMo);
203 
204  vpMatrix K; // kernel
205  unsigned int rank = (m_L_depthNormal * cVo).kernel(K);
206  if (rank == 0) {
207  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
208  }
209 
210  if (rank != 6) {
211  vpMatrix I; // Identity
212  I.eye(6);
213  oJo = I - K.AtA();
214 
215  isoJoIdentity_ = false;
216  }
217  }
218  }
219 
220  double num = 0.0, den = 0.0;
221  for (unsigned int i = 0; i < m_L_depthNormal.getRows(); i++) {
222  // Compute weighted errors and stop criteria
224  num += m_w_depthNormal[i] * vpMath::sqr(m_error_depthNormal[i]);
225  den += m_w_depthNormal[i];
226 
227  // weight interaction matrix
228  for (unsigned int j = 0; j < 6; j++) {
229  m_L_depthNormal[i][j] *= m_w_depthNormal[i];
230  }
231  }
232 
234  m_error_depthNormal, error_prev, LTR, mu, v);
235 
236  cMo_prev = cMo;
238 
239  normRes_1 = normRes;
240  normRes = sqrt(num / den);
241  }
242 
243  iter++;
244  }
245 
246  computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthNormal, cMo_prev, L_true, LVJ_true, m_error_depthNormal);
247 }
248 
250 {
251  unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size());
252 
253  m_L_depthNormal.resize(nb_features, 6, false, false);
254  m_error_depthNormal.resize(nb_features, false);
255  m_weightedError_depthNormal.resize(nb_features, false);
256 
257  m_w_depthNormal.resize(nb_features, false);
258  m_w_depthNormal = 1;
259 
260  m_robust_depthNormal.resize(nb_features);
262 }
263 
265 {
266  unsigned int cpt = 0;
267  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalListOfActiveFaces.begin();
268  it != m_depthNormalListOfActiveFaces.end(); ++it) {
269  vpMatrix L_face;
270  vpColVector features_face;
271  (*it)->computeInteractionMatrix(cMo, L_face, features_face);
272 
273  vpColVector face_error = features_face - m_depthNormalListOfDesiredFeatures[(size_t)cpt];
274 
275  m_error_depthNormal.insert(cpt * 3, face_error);
276  m_L_depthNormal.insert(L_face, cpt * 3, 0);
277 
278  cpt++;
279  }
280 }
281 
283  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
284  const bool displayFullModel)
285 {
286  std::vector<std::vector<double> > models = vpMbDepthNormalTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, cam_, displayFullModel);
287 
288  for (size_t i = 0; i < models.size(); i++) {
289  if (vpMath::equal(models[i][0], 0)) {
290  vpImagePoint ip1(models[i][1], models[i][2]);
291  vpImagePoint ip2(models[i][3], models[i][4]);
292  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
293  }
294  }
295 
296  if (displayFeatures) {
297  std::vector<std::vector<double> > features = getFeaturesForDisplayDepthNormal();
298  for (size_t i = 0; i < features.size(); i++) {
299  vpImagePoint im_centroid(features[i][1], features[i][2]);
300  vpImagePoint im_extremity(features[i][3], features[i][4]);
301  bool desired = vpMath::equal(features[i][0], 2);
302  vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
303  }
304  }
305 }
306 
308  const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
309  const bool displayFullModel)
310 {
311  std::vector<std::vector<double> > models = vpMbDepthNormalTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, cam_, displayFullModel);
312 
313  for (size_t i = 0; i < models.size(); i++) {
314  if (vpMath::equal(models[i][0], 0)) {
315  vpImagePoint ip1(models[i][1], models[i][2]);
316  vpImagePoint ip2(models[i][3], models[i][4]);
317  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
318  }
319  }
320 
321  if (displayFeatures) {
322  std::vector<std::vector<double> > features = getFeaturesForDisplayDepthNormal();
323  for (size_t i = 0; i < features.size(); i++) {
324  vpImagePoint im_centroid(features[i][1], features[i][2]);
325  vpImagePoint im_extremity(features[i][3], features[i][4]);
326  bool desired = vpMath::equal(features[i][0], 2);
327  vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
328  }
329  }
330 }
331 
332 std::vector<std::vector<double> > vpMbDepthNormalTracker::getFeaturesForDisplayDepthNormal() {
333  std::vector<std::vector<double> > features;
334 
335  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
336  it != m_depthNormalFaces.end(); ++it) {
337  vpMbtFaceDepthNormal *face_normal = *it;
338  std::vector<std::vector<double> > currentFeatures = face_normal->getFeaturesForDisplay(cMo, cam);
339  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
340  }
341 
342  return features;
343 }
344 
358 std::vector<std::vector<double> > vpMbDepthNormalTracker::getModelForDisplay(unsigned int width, unsigned int height,
359  const vpHomogeneousMatrix &cMo_,
360  const vpCameraParameters &cam_,
361  const bool displayFullModel)
362 {
363  std::vector<std::vector<double> > models;
364 
365  vpCameraParameters c = cam_;
366 
367  bool changed = false;
368  m_depthNormalHiddenFacesDisplay.setVisible(width, height, c, cMo_, angleAppears, angleDisappears, changed);
369 
370  if (useScanLine) {
371  c.computeFov(width, height);
372 
375  }
376 
377  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
378  it != m_depthNormalFaces.end(); ++it) {
379  vpMbtFaceDepthNormal *face_normal = *it;
380  std::vector<std::vector<double> > modelLines = face_normal->getModelForDisplay(width, height, cMo_, cam_, displayFullModel);
381  models.insert(models.end(), modelLines.begin(), modelLines.end());
382  }
383 
384  return models;
385 }
386 
388 {
389  if (!modelInitialised) {
390  throw vpException(vpException::fatalError, "model not initialized");
391  }
392 
393  bool reInitialisation = false;
394  if (!useOgre) {
395  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
396  } else {
397 #ifdef VISP_HAVE_OGRE
398  if (!faces.isOgreInitialised()) {
401  faces.initOgre(cam);
402  // Turn off Ogre config dialog display for the next call to this
403  // function since settings are saved in the ogre.cfg file and used
404  // during the next call
405  ogreShowConfigDialog = false;
406  }
407 
408  faces.setVisibleOgre(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
409 #else
410  faces.setVisible(I.getWidth(), I.getHeight(), cam, cMo, angleAppears, angleDisappears, reInitialisation);
411 #endif
412  }
413 
414  if (useScanLine || clippingFlag > 3)
415  cam.computeFov(I.getWidth(), I.getHeight());
416 
418 }
419 
420 void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile)
421 {
422 #ifdef VISP_HAVE_PUGIXML
424 
425  xmlp.setCameraParameters(cam);
428 
435 
436  try {
437  std::cout << " *********** Parsing XML for Mb Depth Tracker ************ " << std::endl;
438  xmlp.parse(configFile);
439  } catch (const vpException &e) {
440  std::cerr << "Exception: " << e.what() << std::endl;
441  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
442  }
443 
444  vpCameraParameters camera;
445  xmlp.getCameraParameters(camera);
446  setCameraParameters(camera);
447 
450 
451  if (xmlp.hasNearClippingDistance())
453 
454  if (xmlp.hasFarClippingDistance())
456 
457  if (xmlp.getFovClipping())
459 
465 #else
466  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
467 #endif
468 }
469 
470 void vpMbDepthNormalTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
471  const vpHomogeneousMatrix &cMo_, const bool verbose)
472 {
473  cMo.eye();
474 
475  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
476  delete m_depthNormalFaces[i];
477  m_depthNormalFaces[i] = NULL;
478  }
479 
480  m_depthNormalFaces.clear();
481 
482  loadModel(cad_name, verbose);
483  initFromPose(I, cMo_);
484 }
485 
486 #if defined(VISP_HAVE_PCL)
487 void vpMbDepthNormalTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
488  const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
489  const bool verbose)
490 {
491  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
492  reInitModel(I_dummy, cad_name, cMo_, verbose);
493 }
494 
495 #endif
496 
498 {
499  cMo.eye();
500 
501  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
502  ++it) {
503  vpMbtFaceDepthNormal *normal_face = *it;
504  delete normal_face;
505  normal_face = NULL;
506  }
507 
508  m_depthNormalFaces.clear();
509 
510  m_computeInteraction = true;
511  computeCovariance = false;
512 
515 
517 
518  m_lambda = 1.0;
519 
520  faces.reset();
521 
523 
524  useScanLine = false;
525 
526 #ifdef VISP_HAVE_OGRE
527  useOgre = false;
528 #endif
529 
532 }
533 
535 {
537 #ifdef VISP_HAVE_OGRE
538  faces.getOgreContext()->setWindowName("MBT Depth");
539 #endif
540 }
541 
543 {
544  cMo = cdMo;
545  init(I);
546 }
547 
549 {
550  cMo = cdMo;
551  vpImageConvert::convert(I_color, m_I);
552  init(m_I);
553 }
554 
555 #if defined(VISP_HAVE_PCL)
556 void vpMbDepthNormalTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
557  const vpHomogeneousMatrix &cdMo)
558 {
559  vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
560  cMo = cdMo;
561  init(I_dummy);
562 }
563 #endif
564 
566 {
568 
569  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
570  it != m_depthNormalFaces.end(); ++it) {
571  (*it)->setScanLineVisibilityTest(v);
572  }
573 }
574 
575 void vpMbDepthNormalTracker::setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
576 {
577  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
578  it != m_depthNormalFaces.end(); ++it) {
579  vpMbtFaceDepthNormal *face = *it;
580  if (face->m_polygon->getName() == name) {
581  face->setTracked(useDepthNormalTracking);
582  }
583  }
584 }
585 
587 
588 #ifdef VISP_HAVE_PCL
589 void vpMbDepthNormalTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
590 {
593 
594 #if DEBUG_DISPLAY_DEPTH_NORMAL
595  if (!m_debugDisp_depthNormal->isInitialised()) {
596  m_debugImage_depthNormal.resize(point_cloud->height, point_cloud->width);
597  m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker");
598  }
599 
600  m_debugImage_depthNormal = 0;
601  std::vector<std::vector<vpImagePoint> > roiPts_vec;
602 #endif
603 
604  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
605  ++it) {
606  vpMbtFaceDepthNormal *face = *it;
607 
608  if (face->isVisible() && face->isTracked()) {
609  vpColVector desired_features;
610 
611 #if DEBUG_DISPLAY_DEPTH_NORMAL
612  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
613 #endif
614  if (face->computeDesiredFeatures(cMo, point_cloud->width, point_cloud->height, point_cloud, desired_features,
616 #if DEBUG_DISPLAY_DEPTH_NORMAL
617  ,
618  m_debugImage_depthNormal, roiPts_vec_
619 #endif
620  , m_mask
621  )) {
622  m_depthNormalListOfDesiredFeatures.push_back(desired_features);
623  m_depthNormalListOfActiveFaces.push_back(face);
624 
625 #if DEBUG_DISPLAY_DEPTH_NORMAL
626  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
627 #endif
628  }
629  }
630  }
631 
632 #if DEBUG_DISPLAY_DEPTH_NORMAL
633  vpDisplay::display(m_debugImage_depthNormal);
634 
635  for (size_t i = 0; i < roiPts_vec.size(); i++) {
636  if (roiPts_vec[i].empty())
637  continue;
638 
639  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
640  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
641  }
642  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
643  vpColor::red, 2);
644  }
645 
646  vpDisplay::flush(m_debugImage_depthNormal);
647 #endif
648 }
649 #endif
650 
651 void vpMbDepthNormalTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, const unsigned int width,
652  const unsigned int height)
653 {
656 
657 #if DEBUG_DISPLAY_DEPTH_NORMAL
658  if (!m_debugDisp_depthNormal->isInitialised()) {
659  m_debugImage_depthNormal.resize(height, width);
660  m_debugDisp_depthNormal->init(m_debugImage_depthNormal, 50, 0, "Debug display normal depth tracker");
661  }
662 
663  m_debugImage_depthNormal = 0;
664  std::vector<std::vector<vpImagePoint> > roiPts_vec;
665 #endif
666 
667  for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
668  ++it) {
669  vpMbtFaceDepthNormal *face = *it;
670 
671  if (face->isVisible() && face->isTracked()) {
672  vpColVector desired_features;
673 
674 #if DEBUG_DISPLAY_DEPTH_NORMAL
675  std::vector<std::vector<vpImagePoint> > roiPts_vec_;
676 #endif
677 
678  if (face->computeDesiredFeatures(cMo, width, height, point_cloud, desired_features, m_depthNormalSamplingStepX,
680 #if DEBUG_DISPLAY_DEPTH_NORMAL
681  ,
682  m_debugImage_depthNormal, roiPts_vec_
683 #endif
684  , m_mask
685  )) {
686  m_depthNormalListOfDesiredFeatures.push_back(desired_features);
687  m_depthNormalListOfActiveFaces.push_back(face);
688 
689 #if DEBUG_DISPLAY_DEPTH_NORMAL
690  roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
691 #endif
692  }
693  }
694  }
695 
696 #if DEBUG_DISPLAY_DEPTH_NORMAL
697  vpDisplay::display(m_debugImage_depthNormal);
698 
699  for (size_t i = 0; i < roiPts_vec.size(); i++) {
700  if (roiPts_vec[i].empty())
701  continue;
702 
703  for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
704  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
705  }
706  vpDisplay::displayLine(m_debugImage_depthNormal, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
707  vpColor::red, 2);
708  }
709 
710  vpDisplay::flush(m_debugImage_depthNormal);
711 #endif
712 }
713 
715 {
716  this->cam = camera;
717 
718  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
719  it != m_depthNormalFaces.end(); ++it) {
720  (*it)->setCameraParameters(camera);
721  }
722 }
723 
725 {
726  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
727  it != m_depthNormalFaces.end(); ++it) {
728  (*it)->setFaceCentroidMethod(method);
729  }
730 }
731 
734 {
736 
737  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
738  it != m_depthNormalFaces.end(); ++it) {
739  (*it)->setFeatureEstimationMethod(method);
740  }
741 }
742 
744 {
746 
747  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
748  it != m_depthNormalFaces.end(); ++it) {
749  (*it)->setPclPlaneEstimationMethod(method);
750  }
751 }
752 
754 {
756 
757  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
758  it != m_depthNormalFaces.end(); ++it) {
759  (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
760  }
761 }
762 
764 {
766 
767  for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
768  it != m_depthNormalFaces.end(); ++it) {
769  (*it)->setPclPlaneEstimationRansacThreshold(thresold);
770  }
771 }
772 
773 void vpMbDepthNormalTracker::setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
774 {
775  if (stepX == 0 || stepY == 0) {
776  std::cerr << "stepX and stepY must be greater than zero!" << std::endl;
777  return;
778  }
779 
782 }
783 
784 // void vpMbDepthNormalTracker::setDepthNormalUseRobust(const bool use) {
785 // m_depthNormalUseRobust = use;
786 //}
787 
789 {
790  throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
791 }
792 
794 {
795  throw vpException(vpException::fatalError, "Cannot track with a color image!");
796 }
797 
798 #ifdef VISP_HAVE_PCL
799 void vpMbDepthNormalTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
800 {
801  segmentPointCloud(point_cloud);
802 
803  computeVVS();
804 
805  computeVisibility(point_cloud->width, point_cloud->height);
806 }
807 #endif
808 
809 void vpMbDepthNormalTracker::track(const std::vector<vpColVector> &point_cloud, const unsigned int width,
810  const unsigned int height)
811 {
812  segmentPointCloud(point_cloud, width, height);
813 
814  computeVVS();
815 
816  computeVisibility(width, height);
817 }
818 
819 void vpMbDepthNormalTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
820  const double /*radius*/, const int /*idFace*/, const std::string & /*name*/)
821 {
822  throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCircle() should not be called!");
823 }
824 
825 void vpMbDepthNormalTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
826  const int /*idFace*/, const std::string & /*name*/)
827 {
828  throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCylinder() should not be called!");
829 }
830 
832 
bool m_computeInteraction
Definition: vpMbTracker.h:185
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
unsigned int getDepthNormalSamplingStepY() const
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod
Method to estimate the desired features.
bool m_depthNormalUseRobust
If true, use Tukey robust M-Estimator.
vpColVector m_w_depthNormal
Robust weights.
int getIndex() const
Definition: vpMbtPolygon.h:101
int getDepthNormalPclPlaneEstimationMethod() const
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
void setTracked(const bool tracked)
void getCameraParameters(vpCameraParameters &cam) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
std::string getName() const
Definition: vpMbtPolygon.h:108
int m_depthNormalPclPlaneEstimationRansacMaxIter
PCL RANSAC maximum number of iterations.
vpMatrix AtA() const
Definition: vpMatrix.cpp:693
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 setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpColVector m_error_depthNormal
(s - s*)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
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
virtual std::vector< std::vector< double > > getFeaturesForDisplayDepthNormal()
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< vpColVector > m_depthNormalListOfDesiredFeatures
List of desired features.
virtual void track(const vpImage< unsigned char > &)
double getDepthNormalPclPlaneEstimationRansacThreshold() const
void setOgreShowConfigDialog(const bool showConfigDialog)
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
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:289
vpHomogeneousMatrix inverse() const
virtual void setDepthNormalPclPlaneEstimationMethod(const int method)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
double m_distNearClip
Distance for near clipping.
virtual void init(const vpImage< unsigned char > &I)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
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
static void flush(const vpImage< unsigned char > &I)
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)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool m_useScanLine
Scan line visibility.
vpCameraParameters m_cam
Camera intrinsic parameters.
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)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false)
void setDepthNormalSamplingStepX(const unsigned int stepX)
vpColVector m_weightedError_depthNormal
Weighted error.
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
void setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
void setPclPlaneEstimationRansacThreshold(const double threshold)
vpMbtPolygon * m_polygon
Polygon defining the face.
vpAROgre * getOgreContext()
virtual void setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
void segmentPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud)
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4909
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
unsigned int m_depthNormalSamplingStepX
Sampling step in x-direction.
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
virtual void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
void setAngleDisappear(const double &adisappear)
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
double m_depthNormalPclPlaneEstimationRansacThreshold
PCL RANSAC threshold.
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
std::vector< std::vector< double > > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05)
static void display(const vpImage< unsigned char > &I)
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
Generic class defining intrinsic camera parameters.
virtual void setOgreVisibilityTest(const bool &v)
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
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
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
unsigned int getDepthNormalSamplingStepX() const
unsigned int m_depthNormalSamplingStepY
Sampling step in y-direction.
void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
double m_distFarClip
Distance for near clipping.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
virtual void setScanLineVisibilityTest(const bool &v)
void setAngleAppear(const double &aappear)
const char * what() const
static double rad(double deg)
Definition: vpMath.h:108
virtual void computeVVSInteractionMatrixAndResidu()
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
void insert(unsigned int i, const vpColVector &v)
void addFace(vpMbtPolygon &polygon, const bool alreadyClose)
vpMatrix m_L_depthNormal
Interaction matrix.
vpRobust m_robust_depthNormal
Robust.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
virtual void setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold)
unsigned int nbpt
Number of points used to define the polygon.
Definition: vpPolygon3D.h:76
virtual void loadConfigFile(const std::string &configFile)
double m_stopCriteriaEpsilon
Epsilon threshold to stop the VVS optimization loop.
Definition: vpMbTracker.h:191
static double deg(double rad)
Definition: vpMath.h:101
virtual void setOgreVisibilityTest(const bool &v)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
unsigned int getHeight() const
Definition: vpImage.h:186
std::vector< vpMbtFaceDepthNormal * > m_depthNormalFaces
List of faces.
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
void setPclPlaneEstimationMethod(const int method)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
int m_depthNormalPclPlaneEstimationMethod
PCL plane estimation method.
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)
std::vector< vpMbtFaceDepthNormal * > m_depthNormalListOfActiveFaces
List of current active (visible and with features extracted) faces.
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:147
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
int getDepthNormalPclPlaneEstimationRansacMaxIter() const
void setPclPlaneEstimationRansacMaxIter(const int maxIter)
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:597
virtual void setClipping(const unsigned int &flags)
void computeVisibility(const unsigned int width, const unsigned int height)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
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
virtual void setDepthNormalFeatureEstimationMethod(const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
vpPlane m_planeObject
Plane equation described in the object frame.
vpMbHiddenFaces< vpMbtPolygon > m_depthNormalHiddenFacesDisplay
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="")
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
void setDepthNormalPclPlaneEstimationMethod(const int method)
virtual void setFarClippingDistance(const double &dist)
void setDepthNormalPclPlaneEstimationRansacThreshold(const double threshold)
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
void setCameraParameters(const vpCameraParameters &cam)
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:244
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void setDepthNormalSamplingStepY(const unsigned int stepY)
vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
void eye()
Definition: vpMatrix.cpp:492
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)
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
static const vpColor blue
Definition: vpColor.h:186
virtual void setCameraParameters(const vpCameraParameters &camera)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)