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