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
vpMbDepthDenseTracker()55 vpMbDepthDenseTracker::vpMbDepthDenseTracker()
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
~vpMbDepthDenseTracker()75 vpMbDepthDenseTracker::~vpMbDepthDenseTracker()
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
addFace(vpMbtPolygon & polygon,bool alreadyClose)86 void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, bool alreadyClose)
87 {
88 if (polygon.nbpt < 3) {
89 return;
90 }
91
92 // Copy hidden faces
93 m_depthDenseHiddenFacesDisplay = 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, 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
computeVisibility(unsigned int width,unsigned int height)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
139 faces.computeClippedPolygons(m_cMo, m_cam);
140 faces.computeScanLineRender(m_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
computeVVS()150 void vpMbDepthDenseTracker::computeVVS()
151 {
152 double normRes = 0;
153 double normRes_1 = -1;
154 unsigned int iter = 0;
155
156 computeVVSInit();
157
158 vpColVector error_prev(m_denseDepthNbFeatures);
159 vpMatrix LTL;
160 vpColVector LTR, v;
161
162 double mu = m_initialMu;
163 vpHomogeneousMatrix cMo_prev;
164
165 bool isoJoIdentity_ = true;
166 vpVelocityTwistMatrix cVo;
167 vpMatrix L_true, LVJ_true;
168
169 while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
170 computeVVSInteractionMatrixAndResidu();
171
172 bool reStartFromLastIncrement = false;
173 computeVVSCheckLevenbergMarquardt(iter, m_error_depthDense, error_prev, cMo_prev, mu, reStartFromLastIncrement);
174
175 if (!reStartFromLastIncrement) {
176 computeVVSWeights();
177
178 if (computeCovariance) {
179 L_true = m_L_depthDense;
180 if (!isoJoIdentity_) {
181 cVo.buildFrom(m_cMo);
182 LVJ_true = (m_L_depthDense * cVo * oJo);
183 }
184 }
185
186 // Compute DoF only once
187 if (iter == 0) {
188 isoJoIdentity_ = true;
189 oJo.eye();
190
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 consering
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
217 m_weightedError_depthDense[i] = m_w_depthDense[i] * m_error_depthDense[i];
218 num += m_w_depthDense[i] * vpMath::sqr(m_error_depthDense[i]);
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
227 computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_depthDense, LTL, m_weightedError_depthDense,
228 m_error_depthDense, error_prev, LTR, mu, v);
229
230 cMo_prev = m_cMo;
231 m_cMo = vpExponentialMap::direct(v).inverse() * 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
computeVVSInit()243 void vpMbDepthDenseTracker::computeVVSInit()
244 {
245 m_denseDepthNbFeatures = 0;
246
247 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
248 it != m_depthDenseListOfActiveFaces.end(); ++it) {
249 vpMbtFaceDepthDense *face = *it;
250 m_denseDepthNbFeatures += face->getNbFeatures();
251 }
252
253 m_L_depthDense.resize(m_denseDepthNbFeatures, 6, false, false);
254 m_error_depthDense.resize(m_denseDepthNbFeatures, false);
255 m_weightedError_depthDense.resize(m_denseDepthNbFeatures, false);
256
257 m_w_depthDense.resize(m_denseDepthNbFeatures, false);
258 m_w_depthDense = 1;
259 }
260
computeVVSInteractionMatrixAndResidu()261 void vpMbDepthDenseTracker::computeVVSInteractionMatrixAndResidu()
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
computeVVSWeights()280 void vpMbDepthDenseTracker::computeVVSWeights()
281 {
282 m_robust_depthDense.MEstimator(m_error_depthDense, m_w_depthDense, 1e-3);
283 }
284
display(const vpImage<unsigned char> & I,const vpHomogeneousMatrix & cMo,const vpCameraParameters & cam,const vpColor & col,unsigned int thickness,bool displayFullModel)285 void vpMbDepthDenseTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
286 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
287 bool displayFullModel)
288 {
289 std::vector<std::vector<double> > models = vpMbDepthDenseTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
290
291 for (size_t i = 0; i < models.size(); i++) {
292 if (vpMath::equal(models[i][0], 0)) {
293 vpImagePoint ip1(models[i][1], models[i][2]);
294 vpImagePoint ip2(models[i][3], models[i][4]);
295 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
296 }
297 }
298 }
299
display(const vpImage<vpRGBa> & I,const vpHomogeneousMatrix & cMo,const vpCameraParameters & cam,const vpColor & col,unsigned int thickness,bool displayFullModel)300 void vpMbDepthDenseTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
301 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
302 bool displayFullModel)
303 {
304 std::vector<std::vector<double> > models = vpMbDepthDenseTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
305
306 for (size_t i = 0; i < models.size(); i++) {
307 if (vpMath::equal(models[i][0], 0)) {
308 vpImagePoint ip1(models[i][1], models[i][2]);
309 vpImagePoint ip2(models[i][3], models[i][4]);
310 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
311 }
312 }
313 }
314
315 /*!
316 Return a list of primitives parameters to display the model at a given pose and camera parameters.
317 - Line parameters are: `<primitive id (here 0 for line)>`, `<pt_start.i()>`, `<pt_start.j()>`,
318 `<pt_end.i()>`, `<pt_end.j()>`
319 - Ellipse parameters are: `<primitive id (here 1 for ellipse)>`, `<pt_center.i()>`, `<pt_center.j()>`,
320 `<n_20>`, `<n_11>`, `<n_02>` where `<n_ij>` are the second order centered moments of the ellipse
321 normalized by its area (i.e., such that \f$n_{ij} = \mu_{ij}/a\f$ where \f$\mu_{ij}\f$ are the centered moments and a the area).
322
323 \param width : Image width.
324 \param height : Image height.
325 \param cMo : Pose used to project the 3D model into the image.
326 \param cam : The camera parameters.
327 \param displayFullModel : If true, the line is displayed even if it is not
328 */
getModelForDisplay(unsigned int width,unsigned int height,const vpHomogeneousMatrix & cMo,const vpCameraParameters & cam,bool displayFullModel)329 std::vector<std::vector<double> > vpMbDepthDenseTracker::getModelForDisplay(unsigned int width, unsigned int height,
330 const vpHomogeneousMatrix &cMo,
331 const vpCameraParameters &cam,
332 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
344 m_depthDenseHiddenFacesDisplay.computeClippedPolygons(cMo, c);
345 m_depthDenseHiddenFacesDisplay.computeScanLineRender(c, width, height);
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
init(const vpImage<unsigned char> & I)358 void vpMbDepthDenseTracker::init(const vpImage<unsigned char> &I)
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(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
367 } else {
368 #ifdef VISP_HAVE_OGRE
369 if (!faces.isOgreInitialised()) {
370 faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
371 faces.setOgreShowConfigDialog(ogreShowConfigDialog);
372 faces.initOgre(m_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(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
380 #else
381 faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
382 #endif
383 }
384
385 if (useScanLine || clippingFlag > 3)
386 m_cam.computeFov(I.getWidth(), I.getHeight());
387
388 computeVisibility(I.getWidth(), I.getHeight());
389 }
390
loadConfigFile(const std::string & configFile,bool verbose)391 void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile, bool verbose)
392 {
393 vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::DEPTH_DENSE_PARSER);
394 xmlp.setVerbose(verbose);
395 xmlp.setCameraParameters(m_cam);
396 xmlp.setAngleAppear(vpMath::deg(angleAppears));
397 xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
398
399 xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
400 xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
401
402 try {
403 if (verbose) {
404 std::cout << " *********** Parsing XML for Mb Depth Dense Tracker ************ " << std::endl;
405 }
406 xmlp.parse(configFile);
407 } catch (const vpException &e) {
408 std::cerr << "Exception: " << e.what() << std::endl;
409 throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
410 }
411
412 vpCameraParameters camera;
413 xmlp.getCameraParameters(camera);
414 setCameraParameters(camera);
415
416 angleAppears = vpMath::rad(xmlp.getAngleAppear());
417 angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
418
419 if (xmlp.hasNearClippingDistance())
420 setNearClippingDistance(xmlp.getNearClippingDistance());
421
422 if (xmlp.hasFarClippingDistance())
423 setFarClippingDistance(xmlp.getFarClippingDistance());
424
425 if (xmlp.getFovClipping())
426 setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING);
427
428 setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
429 }
430
reInitModel(const vpImage<unsigned char> & I,const std::string & cad_name,const vpHomogeneousMatrix & cMo,bool verbose)431 void vpMbDepthDenseTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
432 const vpHomogeneousMatrix &cMo, bool verbose)
433 {
434 m_cMo.eye();
435
436 for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
437 delete m_depthDenseFaces[i];
438 m_depthDenseFaces[i] = NULL;
439 }
440
441 m_depthDenseFaces.clear();
442
443 loadModel(cad_name, verbose);
444 initFromPose(I, cMo);
445 }
446
447 #if defined(VISP_HAVE_PCL)
reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & point_cloud,const std::string & cad_name,const vpHomogeneousMatrix & cMo,bool verbose)448 void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
449 const std::string &cad_name, const vpHomogeneousMatrix &cMo,
450 bool verbose)
451 {
452 vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
453 reInitModel(I_dummy, cad_name, cMo, verbose);
454 }
455
456 #endif
457
resetTracker()458 void vpMbDepthDenseTracker::resetTracker()
459 {
460 m_cMo.eye();
461
462 for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin();
463 it != m_depthDenseFaces.end(); ++it) {
464 vpMbtFaceDepthDense *normal_face = *it;
465 delete normal_face;
466 normal_face = NULL;
467 }
468
469 m_depthDenseFaces.clear();
470
471 m_computeInteraction = true;
472 computeCovariance = false;
473
474 angleAppears = vpMath::rad(89);
475 angleDisappears = vpMath::rad(89);
476
477 clippingFlag = vpPolygon3D::NO_CLIPPING;
478
479 m_lambda = 1.0;
480 m_maxIter = 30;
481
482 faces.reset();
483
484 m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
485
486 useScanLine = false;
487
488 #ifdef VISP_HAVE_OGRE
489 useOgre = false;
490 #endif
491
492 m_depthDenseListOfActiveFaces.clear();
493 }
494
setCameraParameters(const vpCameraParameters & cam)495 void vpMbDepthDenseTracker::setCameraParameters(const vpCameraParameters &cam)
496 {
497 m_cam = cam;
498
499 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
500 it != m_depthDenseFaces.end(); ++it) {
501 (*it)->setCameraParameters(cam);
502 }
503 }
504
setDepthDenseFilteringMaxDistance(double maxDistance)505 void vpMbDepthDenseTracker::setDepthDenseFilteringMaxDistance(double maxDistance)
506 {
507 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
508 it != m_depthDenseFaces.end(); ++it) {
509 (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
510 }
511 }
512
setDepthDenseFilteringMethod(int method)513 void vpMbDepthDenseTracker::setDepthDenseFilteringMethod(int method)
514 {
515 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
516 it != m_depthDenseFaces.end(); ++it) {
517 (*it)->setDepthDenseFilteringMethod(method);
518 }
519 }
520
setDepthDenseFilteringMinDistance(double minDistance)521 void vpMbDepthDenseTracker::setDepthDenseFilteringMinDistance(double minDistance)
522 {
523 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
524 it != m_depthDenseFaces.end(); ++it) {
525 (*it)->setDepthDenseFilteringMinDistance(minDistance);
526 }
527 }
528
setDepthDenseFilteringOccupancyRatio(double occupancyRatio)529 void vpMbDepthDenseTracker::setDepthDenseFilteringOccupancyRatio(double occupancyRatio)
530 {
531 if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
532 std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
533 return;
534 }
535
536 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
537 it != m_depthDenseFaces.end(); ++it) {
538 (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
539 }
540 }
541
542 #ifdef VISP_HAVE_PCL
segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & point_cloud)543 void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
544 {
545 m_depthDenseListOfActiveFaces.clear();
546
547 #if DEBUG_DISPLAY_DEPTH_DENSE
548 if (!m_debugDisp_depthDense->isInitialised()) {
549 m_debugImage_depthDense.resize(point_cloud->height, point_cloud->width);
550 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
551 }
552
553 m_debugImage_depthDense = 0;
554 std::vector<std::vector<vpImagePoint> > roiPts_vec;
555 #endif
556
557 for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin();
558 it != m_depthDenseFaces.end(); ++it) {
559 vpMbtFaceDepthDense *face = *it;
560
561 if (face->isVisible() && face->isTracked()) {
562 #if DEBUG_DISPLAY_DEPTH_DENSE
563 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
564 #endif
565 if (face->computeDesiredFeatures(m_cMo, point_cloud, m_depthDenseSamplingStepX, m_depthDenseSamplingStepY
566 #if DEBUG_DISPLAY_DEPTH_DENSE
567 ,
568 m_debugImage_depthDense, roiPts_vec_
569 #endif
570 , m_mask
571 )) {
572 m_depthDenseListOfActiveFaces.push_back(*it);
573
574 #if DEBUG_DISPLAY_DEPTH_DENSE
575 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
576 #endif
577 }
578 }
579 }
580
581 #if DEBUG_DISPLAY_DEPTH_DENSE
582 vpDisplay::display(m_debugImage_depthDense);
583
584 for (size_t i = 0; i < roiPts_vec.size(); i++) {
585 if (roiPts_vec[i].empty())
586 continue;
587
588 for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
589 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
590 }
591 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
592 vpColor::red, 2);
593 }
594
595 vpDisplay::flush(m_debugImage_depthDense);
596 #endif
597 }
598 #endif
599
segmentPointCloud(const std::vector<vpColVector> & point_cloud,unsigned int width,unsigned int height)600 void vpMbDepthDenseTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, unsigned int width,
601 unsigned int height)
602 {
603 m_depthDenseListOfActiveFaces.clear();
604
605 #if DEBUG_DISPLAY_DEPTH_DENSE
606 if (!m_debugDisp_depthDense->isInitialised()) {
607 m_debugImage_depthDense.resize(height, width);
608 m_debugDisp_depthDense->init(m_debugImage_depthDense, 50, 0, "Debug display dense depth tracker");
609 }
610
611 m_debugImage_depthDense = 0;
612 std::vector<std::vector<vpImagePoint> > roiPts_vec;
613 #endif
614
615 for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseFaces.begin();
616 it != m_depthDenseFaces.end(); ++it) {
617 vpMbtFaceDepthDense *face = *it;
618
619 if (face->isVisible() && face->isTracked()) {
620 #if DEBUG_DISPLAY_DEPTH_DENSE
621 std::vector<std::vector<vpImagePoint> > roiPts_vec_;
622 #endif
623 if (face->computeDesiredFeatures(m_cMo, width, height, point_cloud, m_depthDenseSamplingStepX,
624 m_depthDenseSamplingStepY
625 #if DEBUG_DISPLAY_DEPTH_DENSE
626 ,
627 m_debugImage_depthDense, roiPts_vec_
628 #endif
629 , m_mask
630 )) {
631 m_depthDenseListOfActiveFaces.push_back(*it);
632
633 #if DEBUG_DISPLAY_DEPTH_DENSE
634 roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
635 #endif
636 }
637 }
638 }
639
640 #if DEBUG_DISPLAY_DEPTH_DENSE
641 vpDisplay::display(m_debugImage_depthDense);
642
643 for (size_t i = 0; i < roiPts_vec.size(); i++) {
644 if (roiPts_vec[i].empty())
645 continue;
646
647 for (size_t j = 0; j < roiPts_vec[i].size() - 1; j++) {
648 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][j], roiPts_vec[i][j + 1], vpColor::red, 2);
649 }
650 vpDisplay::displayLine(m_debugImage_depthDense, roiPts_vec[i][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
651 vpColor::red, 2);
652 }
653
654 vpDisplay::flush(m_debugImage_depthDense);
655 #endif
656 }
657
setOgreVisibilityTest(const bool & v)658 void vpMbDepthDenseTracker::setOgreVisibilityTest(const bool &v)
659 {
660 vpMbTracker::setOgreVisibilityTest(v);
661 #ifdef VISP_HAVE_OGRE
662 faces.getOgreContext()->setWindowName("MBT Depth Dense");
663 #endif
664 }
665
setPose(const vpImage<unsigned char> & I,const vpHomogeneousMatrix & cdMo)666 void vpMbDepthDenseTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
667 {
668 m_cMo = cdMo;
669 init(I);
670 }
671
setPose(const vpImage<vpRGBa> & I_color,const vpHomogeneousMatrix & cdMo)672 void vpMbDepthDenseTracker::setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo)
673 {
674 m_cMo = cdMo;
675 vpImageConvert::convert(I_color, m_I);
676 init(m_I);
677 }
678
679 #if defined(VISP_HAVE_PCL)
setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & point_cloud,const vpHomogeneousMatrix & cdMo)680 void vpMbDepthDenseTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
681 const vpHomogeneousMatrix &cdMo)
682 {
683 m_I.resize(point_cloud->height, point_cloud->width);
684 m_cMo = cdMo;
685 init(m_I);
686 }
687 #endif
688
setScanLineVisibilityTest(const bool & v)689 void vpMbDepthDenseTracker::setScanLineVisibilityTest(const bool &v)
690 {
691 vpMbTracker::setScanLineVisibilityTest(v);
692
693 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
694 it != m_depthDenseFaces.end(); ++it) {
695 (*it)->setScanLineVisibilityTest(v);
696 }
697 }
698
setUseDepthDenseTracking(const std::string & name,const bool & useDepthDenseTracking)699 void vpMbDepthDenseTracker::setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
700 {
701 for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseFaces.begin();
702 it != m_depthDenseFaces.end(); ++it) {
703 vpMbtFaceDepthDense *face = *it;
704 if (face->m_polygon->getName() == name) {
705 face->setTracked(useDepthDenseTracking);
706 }
707 }
708 }
709
testTracking()710 void vpMbDepthDenseTracker::testTracking() {}
711
track(const vpImage<unsigned char> &)712 void vpMbDepthDenseTracker::track(const vpImage<unsigned char> &)
713 {
714 throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
715 }
716
track(const vpImage<vpRGBa> &)717 void vpMbDepthDenseTracker::track(const vpImage<vpRGBa> &)
718 {
719 throw vpException(vpException::fatalError, "Cannot track with a color image!");
720 }
721
722 #ifdef VISP_HAVE_PCL
track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & point_cloud)723 void vpMbDepthDenseTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
724 {
725 segmentPointCloud(point_cloud);
726
727 computeVVS();
728
729 computeVisibility(point_cloud->width, point_cloud->height);
730 }
731 #endif
732
track(const std::vector<vpColVector> & point_cloud,unsigned int width,unsigned int height)733 void vpMbDepthDenseTracker::track(const std::vector<vpColVector> &point_cloud, unsigned int width,
734 unsigned int height)
735 {
736 segmentPointCloud(point_cloud, width, height);
737
738 computeVVS();
739
740 computeVisibility(width, height);
741 }
742
initCircle(const vpPoint &,const vpPoint &,const vpPoint &,double,int,const std::string &)743 void vpMbDepthDenseTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
744 double /*radius*/, int /*idFace*/, const std::string & /*name*/)
745 {
746 throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCircle() should not be called!");
747 }
748
initCylinder(const vpPoint &,const vpPoint &,double,int,const std::string &)749 void vpMbDepthDenseTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, double /*radius*/,
750 int /*idFace*/, const std::string & /*name*/)
751 {
752 throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCylinder() should not be called!");
753 }
754
initFaceFromCorners(vpMbtPolygon & polygon)755 void vpMbDepthDenseTracker::initFaceFromCorners(vpMbtPolygon &polygon) { addFace(polygon, false); }
756
initFaceFromLines(vpMbtPolygon & polygon)757 void vpMbDepthDenseTracker::initFaceFromLines(vpMbtPolygon &polygon) { addFace(polygon, true); }
758