Visual Servoing Platform  version 3.2.1 under development (2019-11-21)
vpMbtFaceDepthDense.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  * Manage depth dense features for a particular face.
33  *
34  *****************************************************************************/
35 
36 #include <visp3/core/vpCPUFeatures.h>
37 #include <visp3/mbt/vpMbtFaceDepthDense.h>
38 
39 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
40 #include <emmintrin.h>
41 #define VISP_HAVE_SSE2 1
42 #endif
43 
44 #define USE_SSE_CODE 1
45 #if VISP_HAVE_SSE2 && USE_SSE_CODE
46 #define USE_SSE 1
47 #else
48 #define USE_SSE 0
49 #endif
50 
52  : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(NULL),
53  m_planeObject(), m_polygon(NULL), m_useScanLine(false),
54  m_depthDenseFilteringMethod(DEPTH_OCCUPANCY_RATIO_FILTERING), m_depthDenseFilteringMaxDist(3.0),
55  m_depthDenseFilteringMinDist(0.8), m_depthDenseFilteringOccupancyRatio(0.3), m_isTrackedDepthDenseFace(true),
56  m_isVisible(false), m_listOfFaceLines(), m_planeCamera(), m_pointCloudFace(), m_polygonLines()
57 {
58 }
59 
61 {
62  for (size_t i = 0; i < m_listOfFaceLines.size(); i++) {
63  delete m_listOfFaceLines[i];
64  }
65 }
66 
81  std::string name)
82 {
83  // Build a PolygonLine to be able to easily display the lines model
84  PolygonLine polygon_line;
85 
86  // Add polygon
87  polygon_line.m_poly.setNbPoint(2);
88  polygon_line.m_poly.addPoint(0, P1);
89  polygon_line.m_poly.addPoint(1, P2);
90 
91  polygon_line.m_poly.setClipping(m_clippingFlag);
92  polygon_line.m_poly.setNearClippingDistance(m_distNearClip);
93  polygon_line.m_poly.setFarClippingDistance(m_distFarClip);
94 
95  polygon_line.m_p1 = &polygon_line.m_poly.p[0];
96  polygon_line.m_p2 = &polygon_line.m_poly.p[1];
97 
98  m_polygonLines.push_back(polygon_line);
99 
100  // suppress line already in the model
101  bool already_here = false;
103 
104  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
105  ++it) {
106  l = *it;
107  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
108  already_here = true;
109  l->addPolygon(polygon);
110  l->hiddenface = faces;
112  }
113  }
114 
115  if (!already_here) {
116  l = new vpMbtDistanceLine;
117 
119  l->buildFrom(P1, P2);
120  l->addPolygon(polygon);
121  l->hiddenface = faces;
123 
124  l->setIndex((unsigned int)m_listOfFaceLines.size());
125  l->setName(name);
126 
129 
130  if ((m_clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
132 
133  if ((m_clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
135 
136  m_listOfFaceLines.push_back(l);
137  }
138 }
139 
140 #ifdef VISP_HAVE_PCL
142  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
143  const unsigned int stepX, const unsigned int stepY
144 #if DEBUG_DISPLAY_DEPTH_DENSE
145  ,
146  vpImage<unsigned char> &debugImage,
147  std::vector<std::vector<vpImagePoint> > &roiPts_vec
148 #endif
149  , const vpImage<bool> *mask
150 )
151 {
152  unsigned int width = point_cloud->width, height = point_cloud->height;
153  m_pointCloudFace.clear();
154 
155  if (point_cloud->width == 0 || point_cloud->height == 0)
156  return false;
157 
158  std::vector<vpImagePoint> roiPts;
159  double distanceToFace;
160  computeROI(cMo, width, height, roiPts
161 #if DEBUG_DISPLAY_DEPTH_DENSE
162  ,
163  roiPts_vec
164 #endif
165  ,
166  distanceToFace);
167 
168  if (roiPts.size() <= 2) {
169 #ifndef NDEBUG
170  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
171 #endif
172  return false;
173  }
174 
177  return false;
178  }
179 
180  vpPolygon polygon_2d(roiPts);
181  vpRect bb = polygon_2d.getBoundingBox();
182 
183  unsigned int top = (unsigned int)std::max(0.0, bb.getTop());
184  unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom()));
185  unsigned int left = (unsigned int)std::max(0.0, bb.getLeft());
186  unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
187 
188  bb.setTop(top);
189  bb.setBottom(bottom);
190  bb.setLeft(left);
191  bb.setRight(right);
192 
193  if (bb.getHeight() < 0 || bb.getWidth() < 0) {
194  return false;
195  }
196 
197  m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
198 
199  bool checkSSE2 = vpCPUFeatures::checkSSE2();
200 #if !USE_SSE
201  checkSSE2 = false;
202 #else
203  bool push = false;
204  double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
205 #endif
206 
207  int totalTheoreticalPoints = 0, totalPoints = 0;
208  for (unsigned int i = top; i < bottom; i += stepY) {
209  for (unsigned int j = left; j < right; j += stepX) {
210  if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
211  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
212  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
213  : polygon_2d.isInside(vpImagePoint(i, j)))) {
214  totalTheoreticalPoints++;
215 
216  if (vpMeTracker::inMask(mask, i, j) && pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0) {
217  totalPoints++;
218 
219  if (checkSSE2) {
220 #if USE_SSE
221  if (!push) {
222  push = true;
223  prev_x = (*point_cloud)(j, i).x;
224  prev_y = (*point_cloud)(j, i).y;
225  prev_z = (*point_cloud)(j, i).z;
226  } else {
227  push = false;
228  m_pointCloudFace.push_back(prev_x);
229  m_pointCloudFace.push_back((*point_cloud)(j, i).x);
230 
231  m_pointCloudFace.push_back(prev_y);
232  m_pointCloudFace.push_back((*point_cloud)(j, i).y);
233 
234  m_pointCloudFace.push_back(prev_z);
235  m_pointCloudFace.push_back((*point_cloud)(j, i).z);
236  }
237 #endif
238  } else {
239  m_pointCloudFace.push_back((*point_cloud)(j, i).x);
240  m_pointCloudFace.push_back((*point_cloud)(j, i).y);
241  m_pointCloudFace.push_back((*point_cloud)(j, i).z);
242  }
243 
244 #if DEBUG_DISPLAY_DEPTH_DENSE
245  debugImage[i][j] = 255;
246 #endif
247  }
248  }
249  }
250  }
251 
252 #if USE_SSE
253  if (checkSSE2 && push) {
254  m_pointCloudFace.push_back(prev_x);
255  m_pointCloudFace.push_back(prev_y);
256  m_pointCloudFace.push_back(prev_z);
257  }
258 #endif
259 
260  if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) &&
261  totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
262  return false;
263  }
264 
265  return true;
266 }
267 #endif
268 
269 bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width,
270  const unsigned int height, const std::vector<vpColVector> &point_cloud,
271  const unsigned int stepX, const unsigned int stepY
272 #if DEBUG_DISPLAY_DEPTH_DENSE
273  ,
274  vpImage<unsigned char> &debugImage,
275  std::vector<std::vector<vpImagePoint> > &roiPts_vec
276 #endif
277  , const vpImage<bool> *mask
278 )
279 {
280  m_pointCloudFace.clear();
281 
282  if (width == 0 || height == 0)
283  return 0;
284 
285  std::vector<vpImagePoint> roiPts;
286  double distanceToFace;
287  computeROI(cMo, width, height, roiPts
288 #if DEBUG_DISPLAY_DEPTH_DENSE
289  ,
290  roiPts_vec
291 #endif
292  ,
293  distanceToFace);
294 
295  if (roiPts.size() <= 2) {
296 #ifndef NDEBUG
297  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
298 #endif
299  return false;
300  }
301 
304  return false;
305  }
306 
307  vpPolygon polygon_2d(roiPts);
308  vpRect bb = polygon_2d.getBoundingBox();
309 
310  unsigned int top = (unsigned int)std::max(0.0, bb.getTop());
311  unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom()));
312  unsigned int left = (unsigned int)std::max(0.0, bb.getLeft());
313  unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
314 
315  bb.setTop(top);
316  bb.setBottom(bottom);
317  bb.setLeft(left);
318  bb.setRight(right);
319 
320  m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
321 
322  bool checkSSE2 = vpCPUFeatures::checkSSE2();
323 #if !USE_SSE
324  checkSSE2 = false;
325 #else
326  bool push = false;
327  double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
328 #endif
329 
330  int totalTheoreticalPoints = 0, totalPoints = 0;
331  for (unsigned int i = top; i < bottom; i += stepY) {
332  for (unsigned int j = left; j < right; j += stepX) {
333  if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
334  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
335  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
336  : polygon_2d.isInside(vpImagePoint(i, j)))) {
337  totalTheoreticalPoints++;
338 
339  if (vpMeTracker::inMask(mask, i, j) && point_cloud[i * width + j][2] > 0) {
340  totalPoints++;
341 
342  if (checkSSE2) {
343 #if USE_SSE
344  if (!push) {
345  push = true;
346  prev_x = point_cloud[i * width + j][0];
347  prev_y = point_cloud[i * width + j][1];
348  prev_z = point_cloud[i * width + j][2];
349  } else {
350  push = false;
351  m_pointCloudFace.push_back(prev_x);
352  m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
353 
354  m_pointCloudFace.push_back(prev_y);
355  m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
356 
357  m_pointCloudFace.push_back(prev_z);
358  m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
359  }
360 #endif
361  } else {
362  m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
363  m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
364  m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
365  }
366 
367 #if DEBUG_DISPLAY_DEPTH_DENSE
368  debugImage[i][j] = 255;
369 #endif
370  }
371  }
372  }
373  }
374 
375 #if USE_SSE
376  if (checkSSE2 && push) {
377  m_pointCloudFace.push_back(prev_x);
378  m_pointCloudFace.push_back(prev_y);
379  m_pointCloudFace.push_back(prev_z);
380  }
381 #endif
382 
383  if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) &&
384  totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
385  return false;
386  }
387 
388  return true;
389 }
390 
392 
394 {
395  // Compute lines visibility, only for display
396  vpMbtDistanceLine *line;
397  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
398  ++it) {
399  line = *it;
400  bool isvisible = false;
401 
402  for (std::list<int>::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end();
403  ++itindex) {
404  int index = *itindex;
405  if (index == -1) {
406  isvisible = true;
407  } else {
408  if (line->hiddenface->isVisible((unsigned int)index)) {
409  isvisible = true;
410  }
411  }
412  }
413 
414  // Si la ligne n'appartient a aucune face elle est tout le temps visible
415  if (line->Lindex_polygon.empty())
416  isvisible = true; // Not sure that this can occur
417 
418  if (isvisible) {
419  line->setVisible(true);
420  } else {
421  line->setVisible(false);
422  }
423  }
424 }
425 
427  vpColVector &error)
428 {
429  if (m_pointCloudFace.empty()) {
430  L.resize(0, 0);
431  error.resize(0);
432  return;
433  }
434 
435  L.resize(getNbFeatures(), 6, false, false);
436  error.resize(getNbFeatures(), false);
437 
438  // Transform the plane equation for the current pose
441 
442  double nx = m_planeCamera.getA();
443  double ny = m_planeCamera.getB();
444  double nz = m_planeCamera.getC();
445  double D = m_planeCamera.getD();
446 
447  bool checkSSE2 = vpCPUFeatures::checkSSE2();
448 #if !USE_SSE
449  checkSSE2 = false;
450 #endif
451 
452  if (checkSSE2) {
453 #if USE_SSE
454  size_t cpt = 0;
455  if (getNbFeatures() >= 2) {
456  double *ptr_point_cloud = &m_pointCloudFace[0];
457  double *ptr_L = L.data;
458  double *ptr_error = error.data;
459 
460  const __m128d vnx = _mm_set1_pd(nx);
461  const __m128d vny = _mm_set1_pd(ny);
462  const __m128d vnz = _mm_set1_pd(nz);
463  const __m128d vd = _mm_set1_pd(D);
464 
465  double tmp_a1[2], tmp_a2[2], tmp_a3[2];
466 
467  for (; cpt <= m_pointCloudFace.size() - 6; cpt += 6, ptr_point_cloud += 6) {
468  const __m128d vx = _mm_loadu_pd(ptr_point_cloud);
469  const __m128d vy = _mm_loadu_pd(ptr_point_cloud + 2);
470  const __m128d vz = _mm_loadu_pd(ptr_point_cloud + 4);
471 
472  const __m128d va1 = _mm_sub_pd(_mm_mul_pd(vnz, vy), _mm_mul_pd(vny, vz));
473  const __m128d va2 = _mm_sub_pd(_mm_mul_pd(vnx, vz), _mm_mul_pd(vnz, vx));
474  const __m128d va3 = _mm_sub_pd(_mm_mul_pd(vny, vx), _mm_mul_pd(vnx, vy));
475 
476  _mm_storeu_pd(tmp_a1, va1);
477  _mm_storeu_pd(tmp_a2, va2);
478  _mm_storeu_pd(tmp_a3, va3);
479 
480  *ptr_L = nx;
481  ptr_L++;
482  *ptr_L = ny;
483  ptr_L++;
484  *ptr_L = nz;
485  ptr_L++;
486  *ptr_L = tmp_a1[0];
487  ptr_L++;
488  *ptr_L = tmp_a2[0];
489  ptr_L++;
490  *ptr_L = tmp_a3[0];
491  ptr_L++;
492 
493  *ptr_L = nx;
494  ptr_L++;
495  *ptr_L = ny;
496  ptr_L++;
497  *ptr_L = nz;
498  ptr_L++;
499  *ptr_L = tmp_a1[1];
500  ptr_L++;
501  *ptr_L = tmp_a2[1];
502  ptr_L++;
503  *ptr_L = tmp_a3[1];
504  ptr_L++;
505 
506  const __m128d verror =
507  _mm_add_pd(_mm_add_pd(vd, _mm_mul_pd(vnx, vx)), _mm_add_pd(_mm_mul_pd(vny, vy), _mm_mul_pd(vnz, vz)));
508  _mm_storeu_pd(ptr_error, verror);
509  ptr_error += 2;
510  }
511  }
512 
513  for (; cpt < m_pointCloudFace.size(); cpt += 3) {
514  double x = m_pointCloudFace[cpt];
515  double y = m_pointCloudFace[cpt + 1];
516  double z = m_pointCloudFace[cpt + 2];
517 
518  double _a1 = (nz * y) - (ny * z);
519  double _a2 = (nx * z) - (nz * x);
520  double _a3 = (ny * x) - (nx * y);
521 
522  // L
523  L[(unsigned int)(cpt / 3)][0] = nx;
524  L[(unsigned int)(cpt / 3)][1] = ny;
525  L[(unsigned int)(cpt / 3)][2] = nz;
526  L[(unsigned int)(cpt / 3)][3] = _a1;
527  L[(unsigned int)(cpt / 3)][4] = _a2;
528  L[(unsigned int)(cpt / 3)][5] = _a3;
529 
530  vpColVector normal(3);
531  normal[0] = nx;
532  normal[1] = ny;
533  normal[2] = nz;
534 
535  vpColVector pt(3);
536  pt[0] = x;
537  pt[1] = y;
538  pt[2] = z;
539 
540  // Error
541  error[(unsigned int)(cpt / 3)] = D + (normal.t() * pt);
542  }
543 #endif
544  } else {
545  vpColVector normal(3);
546  normal[0] = nx;
547  normal[1] = ny;
548  normal[2] = nz;
549  vpColVector pt(3);
550 
551  unsigned int idx = 0;
552  for (size_t i = 0; i < m_pointCloudFace.size(); i += 3, idx++) {
553  double x = m_pointCloudFace[i];
554  double y = m_pointCloudFace[i + 1];
555  double z = m_pointCloudFace[i + 2];
556 
557  double _a1 = (nz * y) - (ny * z);
558  double _a2 = (nx * z) - (nz * x);
559  double _a3 = (ny * x) - (nx * y);
560 
561  // L
562  L[idx][0] = nx;
563  L[idx][1] = ny;
564  L[idx][2] = nz;
565  L[idx][3] = _a1;
566  L[idx][4] = _a2;
567  L[idx][5] = _a3;
568 
569  pt[0] = x;
570  pt[1] = y;
571  pt[2] = z;
572  // Error
573  error[idx] = D + (normal.t() * pt);
574  }
575  }
576 }
577 
578 void vpMbtFaceDepthDense::computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width,
579  const unsigned int height, std::vector<vpImagePoint> &roiPts
580 #if DEBUG_DISPLAY_DEPTH_DENSE
581  ,
582  std::vector<std::vector<vpImagePoint> > &roiPts_vec
583 #endif
584  ,
585  double &distanceToFace)
586 {
587  if (m_useScanLine || m_clippingFlag > 2)
588  m_cam.computeFov(width, height);
589 
590  if (m_useScanLine) {
591  for (std::vector<PolygonLine>::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) {
592  it->m_p1->changeFrame(cMo);
593  it->m_p2->changeFrame(cMo);
594 
595  vpImagePoint ip1, ip2;
596 
597  it->m_poly.changeFrame(cMo);
598  it->m_poly.computePolygonClipped(m_cam);
599 
600  if (it->m_poly.polyClipped.size() == 2 &&
601  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
602  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
603  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
604  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
605  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
606  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
607 
608  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
609  m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst,
610  true);
611 
612  vpPoint faceCentroid;
613 
614  for (unsigned int i = 0; i < linesLst.size(); i++) {
615  linesLst[i].first.project();
616  linesLst[i].second.project();
617 
618  vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
619  vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
620 
621  it->m_imPt1 = ip1;
622  it->m_imPt2 = ip2;
623 
624  roiPts.push_back(ip1);
625  roiPts.push_back(ip2);
626 
627  faceCentroid.set_X(faceCentroid.get_X() + linesLst[i].first.get_X() + linesLst[i].second.get_X());
628  faceCentroid.set_Y(faceCentroid.get_Y() + linesLst[i].first.get_Y() + linesLst[i].second.get_Y());
629  faceCentroid.set_Z(faceCentroid.get_Z() + linesLst[i].first.get_Z() + linesLst[i].second.get_Z());
630 
631 #if DEBUG_DISPLAY_DEPTH_DENSE
632  std::vector<vpImagePoint> roiPts_;
633  roiPts_.push_back(ip1);
634  roiPts_.push_back(ip2);
635  roiPts_vec.push_back(roiPts_);
636 #endif
637  }
638 
639  if (linesLst.empty()) {
640  distanceToFace = std::numeric_limits<double>::max();
641  } else {
642  faceCentroid.set_X(faceCentroid.get_X() / (2 * linesLst.size()));
643  faceCentroid.set_Y(faceCentroid.get_Y() / (2 * linesLst.size()));
644  faceCentroid.set_Z(faceCentroid.get_Z() / (2 * linesLst.size()));
645 
646  distanceToFace =
647  sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() +
648  faceCentroid.get_Z() * faceCentroid.get_Z());
649  }
650  }
651  }
652  } else {
653  // Get polygon clipped
654  m_polygon->getRoiClipped(m_cam, roiPts, cMo);
655 
656  // Get 3D polygon clipped
657  std::vector<vpPoint> polygonsClipped;
658  m_polygon->getPolygonClipped(polygonsClipped);
659 
660  if (polygonsClipped.empty()) {
661  distanceToFace = std::numeric_limits<double>::max();
662  } else {
663  vpPoint faceCentroid;
664 
665  for (size_t i = 0; i < polygonsClipped.size(); i++) {
666  faceCentroid.set_X(faceCentroid.get_X() + polygonsClipped[i].get_X());
667  faceCentroid.set_Y(faceCentroid.get_Y() + polygonsClipped[i].get_Y());
668  faceCentroid.set_Z(faceCentroid.get_Z() + polygonsClipped[i].get_Z());
669  }
670 
671  faceCentroid.set_X(faceCentroid.get_X() / polygonsClipped.size());
672  faceCentroid.set_Y(faceCentroid.get_Y() / polygonsClipped.size());
673  faceCentroid.set_Z(faceCentroid.get_Z() / polygonsClipped.size());
674 
675  distanceToFace = sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() +
676  faceCentroid.get_Z() * faceCentroid.get_Z());
677  }
678 
679 #if DEBUG_DISPLAY_DEPTH_DENSE
680  roiPts_vec.push_back(roiPts);
681 #endif
682  }
683 }
684 
686  const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness,
687  const bool displayFullModel)
688 {
689  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
690 
691  for (size_t i = 0; i < models.size(); i++) {
692  vpImagePoint ip1(models[i][1], models[i][2]);
693  vpImagePoint ip2(models[i][3], models[i][4]);
694  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
695  }
696 }
697 
699  const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness,
700  const bool displayFullModel)
701 {
702  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
703 
704  for (size_t i = 0; i < models.size(); i++) {
705  vpImagePoint ip1(models[i][1], models[i][2]);
706  vpImagePoint ip2(models[i][3], models[i][4]);
707  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
708  }
709 }
710 
712  const vpCameraParameters & /*cam*/, const double /*scale*/,
713  const unsigned int /*thickness*/)
714 {
715 }
716 
718  const vpCameraParameters & /*cam*/, const double /*scale*/,
719  const unsigned int /*thickness*/)
720 {
721 }
722 
734 std::vector<std::vector<double> > vpMbtFaceDepthDense::getModelForDisplay(unsigned int width, unsigned int height,
735  const vpHomogeneousMatrix &cMo,
736  const vpCameraParameters &cam,
737  const bool displayFullModel)
738 {
739  std::vector<std::vector<double> > models;
740 
741  if ((m_polygon->isVisible() && m_isTrackedDepthDenseFace) || displayFullModel) {
743 
744  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
745  ++it) {
746  vpMbtDistanceLine *line = *it;
747  std::vector<std::vector<double> > lineModels = line->getModelForDisplay(width, height, cMo, cam, displayFullModel);
748  models.insert(models.end(), lineModels.begin(), lineModels.end());
749  }
750  }
751 
752  return models;
753 }
754 
764 bool vpMbtFaceDepthDense::samePoint(const vpPoint &P1, const vpPoint &P2) const
765 {
766  double dx = fabs(P1.get_oX() - P2.get_oX());
767  double dy = fabs(P1.get_oY() - P2.get_oY());
768  double dz = fabs(P1.get_oZ() - P2.get_oZ());
769 
770  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
771  dz <= std::numeric_limits<double>::epsilon())
772  return true;
773  else
774  return false;
775 }
776 
778 {
779  m_cam = camera;
780 
781  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
782  ++it) {
783  (*it)->setCameraParameters(camera);
784  }
785 }
786 
788 {
789  m_useScanLine = v;
790 
791  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
792  ++it) {
793  (*it)->useScanLine = v;
794  }
795 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
double m_distNearClip
Distance for near clipping.
double getTop() const
Definition: vpRect.h:181
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:424
Implements a 3D polygon with render functionnalities like clipping.
Definition: vpPolygon3D.h:59
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void setVisible(bool _isvisible)
int getIndex() const
Definition: vpMbtPolygon.h:101
std::vector< double > m_pointCloudFace
List of depth points inside the face.
vpCameraParameters m_cam
Camera intrinsic parameters.
Implementation of an homogeneous matrix and operations on such kind of matrices.
int m_depthDenseFilteringMethod
Method to use to consider or not the face.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setLeft(double pos)
Definition: vpRect.h:265
bool m_useScanLine
Scan line visibility.
vpPoint * p1
The first extremity.
vpMbScanLine & getMbScanLineRenderer()
Manage the line of a polygon used in the model-based tracker.
vpRowVector t() const
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
double m_depthDenseFilteringMinDist
Minimum distance threshold.
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:454
vpMbtPolygon & getPolygon()
bool m_isTrackedDepthDenseFace
Flag to define if the face should be tracked or not.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:422
double getRight() const
Definition: vpRect.h:168
static bool inMask(const vpImage< bool > *mask, const unsigned int i, const unsigned int j)
vpRect getBoundingBox() const
Definition: vpPolygon.h:177
Class that defines what is a point.
Definition: vpPoint.h:58
bool m_isVisible
Visibility flag.
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPlane m_planeObject
Plane equation described in the object frame.
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:458
void setTop(double pos)
Definition: vpRect.h:300
double getWidth() const
Definition: vpRect.h:203
double getD() const
Definition: vpPlane.h:108
unsigned int m_clippingFlag
Flags specifying which clipping to used.
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)
vpPoint * p2
The second extremity.
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
VISP_EXPORT bool checkSSE2()
double getB() const
Definition: vpPlane.h:104
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:317
double m_distFarClip
Distance for near clipping.
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
Generic class defining intrinsic camera parameters.
double m_depthDenseFilteringOccupancyRatio
Ratio between available depth points and theoretical number of points.
void setIndex(const unsigned int i)
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:456
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:426
void setScanLineVisibilityTest(const bool v)
double getLeft() const
Definition: vpRect.h:162
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
unsigned int getNbFeatures() const
double m_depthDenseFilteringMaxDist
Maximum distance threshold.
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
double getA() const
Definition: vpPlane.h:102
void setName(const std::string &line_name)
void setCameraParameters(const vpCameraParameters &camera)
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:413
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
unsigned int getHeight() const
Definition: vpImage.h:186
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setRight(double pos)
Definition: vpRect.h:291
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double getHeight() const
Definition: vpRect.h:155
bool isVisible(const unsigned int i)
double getC() const
Definition: vpPlane.h:106
std::vector< PolygonLine > m_polygonLines
Polygon lines used for scan-line visibility.
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
Defines a rectangle in the plane.
Definition: vpRect.h:78
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:417
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:415
unsigned int getWidth() const
Definition: vpImage.h:244
double getBottom() const
Definition: vpRect.h:100
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, std::vector< vpImagePoint > &roiPts, double &distanceToFace)
void setBottom(double pos)
Definition: vpRect.h:232
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
void computeFov(const unsigned int &w, const unsigned int &h)