Visual Servoing Platform  version 3.6.1 under development (2024-10-09)
vpMbtFaceDepthDense.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * 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(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
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 // https://stackoverflow.com/a/40765925
49 #if !defined(__FMA__) && defined(__AVX2__)
50 #define __FMA__ 1
51 #endif
52 
53 #if defined _WIN32 && defined(_M_ARM64)
54 #define _ARM64_DISTINCT_NEON_TYPES
55 #include <Intrin.h>
56 #include <arm_neon.h>
57 #define VISP_HAVE_NEON 1
58 #elif (defined(__ARM_NEON__) || defined (__ARM_NEON)) && defined(__aarch64__)
59 #include <arm_neon.h>
60 #define VISP_HAVE_NEON 1
61 #endif
62 
63 #define USE_SIMD_CODE 1
64 
65 #if VISP_HAVE_SSE2 && USE_SIMD_CODE
66 #define USE_SSE 1
67 #else
68 #define USE_SSE 0
69 #endif
70 
71 #if VISP_HAVE_NEON && USE_SIMD_CODE
72 #define USE_NEON 1
73 #else
74 #define USE_NEON 0
75 #endif
76 
77 #if (VISP_HAVE_OPENCV_VERSION >= 0x040101 || (VISP_HAVE_OPENCV_VERSION < 0x040000 && VISP_HAVE_OPENCV_VERSION >= 0x030407)) && USE_SIMD_CODE
78 #define USE_OPENCV_HAL 1
79 #include <opencv2/core/simd_intrinsics.hpp>
80 #include <opencv2/core/hal/intrin.hpp>
81 #endif
82 
83 #if !USE_OPENCV_HAL && (USE_SSE || USE_NEON)
84 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
85 #include <cstdint>
86 #endif
87 
88 namespace
89 {
90 #if USE_SSE
91 inline void v_load_deinterleave(const uint64_t *ptr, __m128i &a, __m128i &b, __m128i &c)
92 {
93  __m128i t0 = _mm_loadu_si128((const __m128i *)ptr); // a0, b0
94  __m128i t1 = _mm_loadu_si128((const __m128i *)(ptr + 2)); // c0, a1
95  __m128i t2 = _mm_loadu_si128((const __m128i *)(ptr + 4)); // b1, c1
96 
97  t1 = _mm_shuffle_epi32(t1, 0x4e); // a1, c0
98 
99  a = _mm_unpacklo_epi64(t0, t1);
100  b = _mm_unpacklo_epi64(_mm_unpackhi_epi64(t0, t0), t2);
101  c = _mm_unpackhi_epi64(t1, t2);
102 }
103 
104 inline void v_load_deinterleave(const double *ptr, __m128d &a0, __m128d &b0, __m128d &c0)
105 {
106  __m128i a1, b1, c1;
107  v_load_deinterleave((const uint64_t *)ptr, a1, b1, c1);
108  a0 = _mm_castsi128_pd(a1);
109  b0 = _mm_castsi128_pd(b1);
110  c0 = _mm_castsi128_pd(c1);
111 }
112 
113 inline __m128d v_combine_low(const __m128d &a, const __m128d &b)
114 {
115  __m128i a1 = _mm_castpd_si128(a), b1 = _mm_castpd_si128(b);
116  return _mm_castsi128_pd(_mm_unpacklo_epi64(a1, b1));
117 }
118 
119 inline __m128d v_combine_high(const __m128d &a, const __m128d &b)
120 {
121  __m128i a1 = _mm_castpd_si128(a), b1 = _mm_castpd_si128(b);
122  return _mm_castsi128_pd(_mm_unpackhi_epi64(a1, b1));
123 }
124 
125 inline __m128d v_fma(const __m128d &a, const __m128d &b, const __m128d &c)
126 {
127 #if __FMA__
128  return _mm_fmadd_pd(a, b, c);
129 #else
130  return _mm_add_pd(_mm_mul_pd(a, b), c);
131 #endif
132 }
133 #else
134 inline void v_load_deinterleave(const double *ptr, float64x2_t &a0, float64x2_t &b0, float64x2_t &c0)
135 {
136  float64x2x3_t v = vld3q_f64(ptr);
137  a0 = v.val[0];
138  b0 = v.val[1];
139  c0 = v.val[2];
140 }
141 
142 inline float64x2_t v_combine_low(const float64x2_t &a, const float64x2_t &b)
143 {
144  return vcombine_f64(vget_low_f64(a), vget_low_f64(b));
145 }
146 
147 inline float64x2_t v_combine_high(const float64x2_t &a, const float64x2_t &b)
148 {
149  return vcombine_f64(vget_high_f64(a), vget_high_f64(b));
150 }
151 
152 inline float64x2_t v_fma(const float64x2_t &a, const float64x2_t &b, const float64x2_t &c)
153 {
154  return vfmaq_f64(c, a, b);
155 }
156 #endif
157 }
158 #endif // !USE_OPENCV_HAL && (USE_SSE || USE_NEON)
159 
160 BEGIN_VISP_NAMESPACE
162  : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(nullptr),
163  m_planeObject(), m_polygon(nullptr), m_useScanLine(false),
164  m_depthDenseFilteringMethod(DEPTH_OCCUPANCY_RATIO_FILTERING), m_depthDenseFilteringMaxDist(3.0),
165  m_depthDenseFilteringMinDist(0.8), m_depthDenseFilteringOccupancyRatio(0.3), m_isTrackedDepthDenseFace(true),
166  m_isVisible(false), m_listOfFaceLines(), m_planeCamera(), m_pointCloudFace(), m_polygonLines()
167 { }
168 
170 {
171  for (size_t i = 0; i < m_listOfFaceLines.size(); i++) {
172  delete m_listOfFaceLines[i];
173  }
174 }
175 
191  vpUniRand &rand_gen, int polygon, std::string name)
192 {
193  // Build a PolygonLine to be able to easily display the lines model
194  PolygonLine polygon_line;
195 
196  // Add polygon
197  polygon_line.m_poly.setNbPoint(2);
198  polygon_line.m_poly.addPoint(0, P1);
199  polygon_line.m_poly.addPoint(1, P2);
200 
201  polygon_line.m_poly.setClipping(m_clippingFlag);
202  polygon_line.m_poly.setNearClippingDistance(m_distNearClip);
203  polygon_line.m_poly.setFarClippingDistance(m_distFarClip);
204 
205  polygon_line.m_p1 = &polygon_line.m_poly.p[0];
206  polygon_line.m_p2 = &polygon_line.m_poly.p[1];
207 
208  m_polygonLines.push_back(polygon_line);
209 
210  // suppress line already in the model
211  bool already_here = false;
213 
214  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
215  ++it) {
216  l = *it;
217  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
218  already_here = true;
219  l->addPolygon(polygon);
220  l->hiddenface = faces;
222  }
223  }
224 
225  if (!already_here) {
226  l = new vpMbtDistanceLine;
227 
229  l->buildFrom(P1, P2, rand_gen);
230  l->addPolygon(polygon);
231  l->hiddenface = faces;
233 
234  l->setIndex((unsigned int)m_listOfFaceLines.size());
235  l->setName(name);
236 
239 
242 
245 
246  m_listOfFaceLines.push_back(l);
247  }
248 }
249 
250 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
252  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
253  unsigned int stepX, unsigned int stepY
254 #if DEBUG_DISPLAY_DEPTH_DENSE
255  ,
256  vpImage<unsigned char> &debugImage,
257  std::vector<std::vector<vpImagePoint> > &roiPts_vec
258 #endif
259  ,
260  const vpImage<bool> *mask)
261 {
262  unsigned int width = point_cloud->width, height = point_cloud->height;
263  m_pointCloudFace.clear();
264 
265  if (point_cloud->width == 0 || point_cloud->height == 0)
266  return false;
267 
268  std::vector<vpImagePoint> roiPts;
269  double distanceToFace;
270  computeROI(cMo, width, height, roiPts
271 #if DEBUG_DISPLAY_DEPTH_DENSE
272  ,
273  roiPts_vec
274 #endif
275  ,
276  distanceToFace);
277 
278  if (roiPts.size() <= 2) {
279 #ifndef NDEBUG
280  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
281 #endif
282  return false;
283  }
284 
287  return false;
288  }
289 
290  vpPolygon polygon_2d(roiPts);
291  vpRect bb = polygon_2d.getBoundingBox();
292 
293  unsigned int top = (unsigned int)std::max<double>(0.0, bb.getTop());
294  unsigned int bottom = (unsigned int)std::min<double>((double)height, std::max<double>(0.0, bb.getBottom()));
295  unsigned int left = (unsigned int)std::max<double>(0.0, bb.getLeft());
296  unsigned int right = (unsigned int)std::min<double>((double)width, std::max<double>(0.0, bb.getRight()));
297 
298  bb.setTop(top);
299  bb.setBottom(bottom);
300  bb.setLeft(left);
301  bb.setRight(right);
302 
303  if (bb.getHeight() < 0 || bb.getWidth() < 0) {
304  return false;
305  }
306 
307  m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
308 
309  int totalTheoreticalPoints = 0, totalPoints = 0;
310  for (unsigned int i = top; i < bottom; i += stepY) {
311  for (unsigned int j = left; j < right; j += stepX) {
312  if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
313  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
314  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
315  : polygon_2d.isInside(vpImagePoint(i, j)))) {
316  totalTheoreticalPoints++;
317 
318  if (vpMeTracker::inRoiMask(mask, i, j) && pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0) {
319  totalPoints++;
320 
321  m_pointCloudFace.push_back((*point_cloud)(j, i).x);
322  m_pointCloudFace.push_back((*point_cloud)(j, i).y);
323  m_pointCloudFace.push_back((*point_cloud)(j, i).z);
324 
325 #if DEBUG_DISPLAY_DEPTH_DENSE
326  debugImage[i][j] = 255;
327 #endif
328  }
329  }
330  }
331  }
332 
333  if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) &&
334  totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
335  return false;
336  }
337 
338  return true;
339 }
340 #endif
341 
343  unsigned int height, const std::vector<vpColVector> &point_cloud,
344  unsigned int stepX, unsigned int stepY
345 #if DEBUG_DISPLAY_DEPTH_DENSE
346  ,
347  vpImage<unsigned char> &debugImage,
348  std::vector<std::vector<vpImagePoint> > &roiPts_vec
349 #endif
350  ,
351  const vpImage<bool> *mask)
352 {
353  m_pointCloudFace.clear();
354 
355  if (width == 0 || height == 0)
356  return 0;
357 
358  std::vector<vpImagePoint> roiPts;
359  double distanceToFace;
360  computeROI(cMo, width, height, roiPts
361 #if DEBUG_DISPLAY_DEPTH_DENSE
362  ,
363  roiPts_vec
364 #endif
365  ,
366  distanceToFace);
367 
368  if (roiPts.size() <= 2) {
369 #ifndef NDEBUG
370  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
371 #endif
372  return false;
373  }
374 
377  return false;
378  }
379 
380  vpPolygon polygon_2d(roiPts);
381  vpRect bb = polygon_2d.getBoundingBox();
382 
383  unsigned int top = (unsigned int)std::max<double>(0.0, bb.getTop());
384  unsigned int bottom = (unsigned int)std::min<double>((double)height, std::max<double>(0.0, bb.getBottom()));
385  unsigned int left = (unsigned int)std::max<double>(0.0, bb.getLeft());
386  unsigned int right = (unsigned int)std::min<double>((double)width, std::max<double>(0.0, bb.getRight()));
387 
388  bb.setTop(top);
389  bb.setBottom(bottom);
390  bb.setLeft(left);
391  bb.setRight(right);
392 
393  m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
394 
395  int totalTheoreticalPoints = 0, totalPoints = 0;
396  for (unsigned int i = top; i < bottom; i += stepY) {
397  for (unsigned int j = left; j < right; j += stepX) {
398  if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
399  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
400  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
401  : polygon_2d.isInside(vpImagePoint(i, j)))) {
402  totalTheoreticalPoints++;
403 
404  if (vpMeTracker::inRoiMask(mask, i, j) && point_cloud[i * width + j][2] > 0) {
405  totalPoints++;
406 
407  m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
408  m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
409  m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
410 
411 #if DEBUG_DISPLAY_DEPTH_DENSE
412  debugImage[i][j] = 255;
413 #endif
414  }
415  }
416  }
417  }
418 
419  if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) &&
420  totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
421  return false;
422  }
423 
424  return true;
425 }
426 
428  unsigned int height, const vpMatrix &point_cloud,
429  unsigned int stepX, unsigned int stepY
430 #if DEBUG_DISPLAY_DEPTH_DENSE
431  ,
432  vpImage<unsigned char> &debugImage,
433  std::vector<std::vector<vpImagePoint> > &roiPts_vec
434 #endif
435  ,
436  const vpImage<bool> *mask)
437 {
438  m_pointCloudFace.clear();
439 
440  if (width == 0 || height == 0)
441  return 0;
442 
443  std::vector<vpImagePoint> roiPts;
444  double distanceToFace;
445  computeROI(cMo, width, height, roiPts
446 #if DEBUG_DISPLAY_DEPTH_DENSE
447  ,
448  roiPts_vec
449 #endif
450  ,
451  distanceToFace);
452 
453  if (roiPts.size() <= 2) {
454 #ifndef NDEBUG
455  std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
456 #endif
457  return false;
458  }
459 
462  return false;
463  }
464 
465  vpPolygon polygon_2d(roiPts);
466  vpRect bb = polygon_2d.getBoundingBox();
467 
468  unsigned int top = (unsigned int)std::max<double>(0.0, bb.getTop());
469  unsigned int bottom = (unsigned int)std::min<double>((double)height, std::max<double>(0.0, bb.getBottom()));
470  unsigned int left = (unsigned int)std::max<double>(0.0, bb.getLeft());
471  unsigned int right = (unsigned int)std::min<double>((double)width, std::max<double>(0.0, bb.getRight()));
472 
473  bb.setTop(top);
474  bb.setBottom(bottom);
475  bb.setLeft(left);
476  bb.setRight(right);
477 
478  m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
479 
480  int totalTheoreticalPoints = 0, totalPoints = 0;
481  for (unsigned int i = top; i < bottom; i += stepY) {
482  for (unsigned int j = left; j < right; j += stepX) {
483  if ((m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
484  j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
485  m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
486  : polygon_2d.isInside(vpImagePoint(i, j)))) {
487  totalTheoreticalPoints++;
488 
489  if (vpMeTracker::inRoiMask(mask, i, j) && point_cloud[i * width + j][2] > 0) {
490  totalPoints++;
491 
492  m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
493  m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
494  m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
495 
496 #if DEBUG_DISPLAY_DEPTH_DENSE
497  debugImage[i][j] = 255;
498 #endif
499  }
500  }
501  }
502  }
503 
504  if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) &&
505  totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
506  return false;
507  }
508 
509  return true;
510 }
511 
513 
515 {
516  // Compute lines visibility, only for display
517  vpMbtDistanceLine *line;
518  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
519  ++it) {
520  line = *it;
521  bool isvisible = false;
522 
523  for (std::list<int>::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end();
524  ++itindex) {
525  int index = *itindex;
526  if (index == -1) {
527  isvisible = true;
528  }
529  else {
530  if (line->hiddenface->isVisible((unsigned int)index)) {
531  isvisible = true;
532  }
533  }
534  }
535 
536  // Si la ligne n'appartient a aucune face elle est tout le temps visible
537  if (line->Lindex_polygon.empty())
538  isvisible = true; // Not sure that this can occur
539 
540  if (isvisible) {
541  line->setVisible(true);
542  }
543  else {
544  line->setVisible(false);
545  }
546  }
547 }
548 
550  vpColVector &error)
551 {
552  if (m_pointCloudFace.empty()) {
553  L.resize(0, 0);
554  error.resize(0);
555  return;
556  }
557 
558  L.resize(getNbFeatures(), 6, false, false);
559  error.resize(getNbFeatures(), false);
560 
561  // Transform the plane equation for the current pose
564 
565  double nx = m_planeCamera.getA();
566  double ny = m_planeCamera.getB();
567  double nz = m_planeCamera.getC();
568  double D = m_planeCamera.getD();
569 
570 #if defined(VISP_HAVE_SIMDLIB)
572 #else
573  bool useSIMD = vpCPUFeatures::checkSSE2();
574 #endif
575 #if USE_OPENCV_HAL
576  useSIMD = true;
577 #endif
578 #if !USE_SSE && !USE_NEON && !USE_OPENCV_HAL
579  useSIMD = false;
580 #endif
581 
582  if (useSIMD) {
583 #if USE_SSE || USE_NEON || USE_OPENCV_HAL
584  size_t cpt = 0;
585  if (getNbFeatures() >= 2) {
586  double *ptr_point_cloud = &m_pointCloudFace[0];
587  double *ptr_L = L.data;
588  double *ptr_error = error.data;
589 
590 #if USE_OPENCV_HAL
591  const cv::v_float64x2 vnx = cv::v_setall_f64(nx);
592  const cv::v_float64x2 vny = cv::v_setall_f64(ny);
593  const cv::v_float64x2 vnz = cv::v_setall_f64(nz);
594  const cv::v_float64x2 vd = cv::v_setall_f64(D);
595 #elif USE_SSE
596  const __m128d vnx = _mm_set1_pd(nx);
597  const __m128d vny = _mm_set1_pd(ny);
598  const __m128d vnz = _mm_set1_pd(nz);
599  const __m128d vd = _mm_set1_pd(D);
600 #else
601  const float64x2_t vnx = vdupq_n_f64(nx);
602  const float64x2_t vny = vdupq_n_f64(ny);
603  const float64x2_t vnz = vdupq_n_f64(nz);
604  const float64x2_t vd = vdupq_n_f64(D);
605 #endif
606 
607  for (; cpt <= m_pointCloudFace.size() - 6; cpt += 6, ptr_point_cloud += 6) {
608 #if USE_OPENCV_HAL
609  cv::v_float64x2 vx, vy, vz;
610  cv::v_load_deinterleave(ptr_point_cloud, vx, vy, vz);
611 
612 #if (VISP_HAVE_OPENCV_VERSION >= 0x040900)
613  cv::v_float64x2 va1 = cv::v_sub(cv::v_mul(vnz, vy), cv::v_mul(vny, vz)); // vnz*vy - vny*vz
614  cv::v_float64x2 va2 = cv::v_sub(cv::v_mul(vnx, vz), cv::v_mul(vnz, vx)); // vnx*vz - vnz*vx
615  cv::v_float64x2 va3 = cv::v_sub(cv::v_mul(vny, vx), cv::v_mul(vnx, vy)); // vny*vx - vnx*vy
616 #else
617  cv::v_float64x2 va1 = vnz*vy - vny*vz;
618  cv::v_float64x2 va2 = vnx*vz - vnz*vx;
619  cv::v_float64x2 va3 = vny*vx - vnx*vy;
620 #endif
621 
622  cv::v_float64x2 vnxy = cv::v_combine_low(vnx, vny);
623  cv::v_store(ptr_L, vnxy);
624  ptr_L += 2;
625  vnxy = cv::v_combine_low(vnz, va1);
626  cv::v_store(ptr_L, vnxy);
627  ptr_L += 2;
628  vnxy = cv::v_combine_low(va2, va3);
629  cv::v_store(ptr_L, vnxy);
630  ptr_L += 2;
631 
632  vnxy = cv::v_combine_high(vnx, vny);
633  cv::v_store(ptr_L, vnxy);
634  ptr_L += 2;
635  vnxy = cv::v_combine_high(vnz, va1);
636  cv::v_store(ptr_L, vnxy);
637  ptr_L += 2;
638  vnxy = cv::v_combine_high(va2, va3);
639  cv::v_store(ptr_L, vnxy);
640  ptr_L += 2;
641 
642 #if (VISP_HAVE_OPENCV_VERSION >= 0x040900)
643  cv::v_float64x2 verr = cv::v_add(vd, cv::v_muladd(vnx, vx, cv::v_muladd(vny, vy, cv::v_mul(vnz, vz))));
644 #else
645  cv::v_float64x2 verr = vd + cv::v_muladd(vnx, vx, cv::v_muladd(vny, vy, vnz*vz));
646 #endif
647 
648  cv::v_store(ptr_error, verr);
649  ptr_error += 2;
650 #elif USE_SSE
651  __m128d vx, vy, vz;
652  v_load_deinterleave(ptr_point_cloud, vx, vy, vz);
653 
654  __m128d va1 = _mm_sub_pd(_mm_mul_pd(vnz, vy), _mm_mul_pd(vny, vz));
655  __m128d va2 = _mm_sub_pd(_mm_mul_pd(vnx, vz), _mm_mul_pd(vnz, vx));
656  __m128d va3 = _mm_sub_pd(_mm_mul_pd(vny, vx), _mm_mul_pd(vnx, vy));
657 
658  __m128d vnxy = v_combine_low(vnx, vny);
659  _mm_storeu_pd(ptr_L, vnxy);
660  ptr_L += 2;
661  vnxy = v_combine_low(vnz, va1);
662  _mm_storeu_pd(ptr_L, vnxy);
663  ptr_L += 2;
664  vnxy = v_combine_low(va2, va3);
665  _mm_storeu_pd(ptr_L, vnxy);
666  ptr_L += 2;
667 
668  vnxy = v_combine_high(vnx, vny);
669  _mm_storeu_pd(ptr_L, vnxy);
670  ptr_L += 2;
671  vnxy = v_combine_high(vnz, va1);
672  _mm_storeu_pd(ptr_L, vnxy);
673  ptr_L += 2;
674  vnxy = v_combine_high(va2, va3);
675  _mm_storeu_pd(ptr_L, vnxy);
676  ptr_L += 2;
677 
678  const __m128d verror = _mm_add_pd(vd, v_fma(vnx, vx, v_fma(vny, vy, _mm_mul_pd(vnz, vz))));
679  _mm_storeu_pd(ptr_error, verror);
680  ptr_error += 2;
681 #else
682  float64x2_t vx, vy, vz;
683  v_load_deinterleave(ptr_point_cloud, vx, vy, vz);
684 
685  float64x2_t va1 = vsubq_f64(vmulq_f64(vnz, vy), vmulq_f64(vny, vz));
686  float64x2_t va2 = vsubq_f64(vmulq_f64(vnx, vz), vmulq_f64(vnz, vx));
687  float64x2_t va3 = vsubq_f64(vmulq_f64(vny, vx), vmulq_f64(vnx, vy));
688 
689  float64x2_t vnxy = v_combine_low(vnx, vny);
690  vst1q_f64(ptr_L, vnxy);
691  ptr_L += 2;
692  vnxy = v_combine_low(vnz, va1);
693  vst1q_f64(ptr_L, vnxy);
694  ptr_L += 2;
695  vnxy = v_combine_low(va2, va3);
696  vst1q_f64(ptr_L, vnxy);
697  ptr_L += 2;
698 
699  vnxy = v_combine_high(vnx, vny);
700  vst1q_f64(ptr_L, vnxy);
701  ptr_L += 2;
702  vnxy = v_combine_high(vnz, va1);
703  vst1q_f64(ptr_L, vnxy);
704  ptr_L += 2;
705  vnxy = v_combine_high(va2, va3);
706  vst1q_f64(ptr_L, vnxy);
707  ptr_L += 2;
708 
709  const float64x2_t verror = vaddq_f64(vd, v_fma(vnx, vx, v_fma(vny, vy, vmulq_f64(vnz, vz))));
710  vst1q_f64(ptr_error, verror);
711  ptr_error += 2;
712 #endif
713  }
714  }
715 
716  for (; cpt < m_pointCloudFace.size(); cpt += 3) {
717  double x = m_pointCloudFace[cpt];
718  double y = m_pointCloudFace[cpt + 1];
719  double z = m_pointCloudFace[cpt + 2];
720 
721  double _a1 = (nz * y) - (ny * z);
722  double _a2 = (nx * z) - (nz * x);
723  double _a3 = (ny * x) - (nx * y);
724 
725  // L
726  L[(unsigned int)(cpt / 3)][0] = nx;
727  L[(unsigned int)(cpt / 3)][1] = ny;
728  L[(unsigned int)(cpt / 3)][2] = nz;
729  L[(unsigned int)(cpt / 3)][3] = _a1;
730  L[(unsigned int)(cpt / 3)][4] = _a2;
731  L[(unsigned int)(cpt / 3)][5] = _a3;
732 
733  vpColVector normal(3);
734  normal[0] = nx;
735  normal[1] = ny;
736  normal[2] = nz;
737 
738  vpColVector pt(3);
739  pt[0] = x;
740  pt[1] = y;
741  pt[2] = z;
742 
743  // Error
744  error[(unsigned int)(cpt / 3)] = D + (normal.t() * pt);
745  }
746 #endif
747  }
748  else {
749  vpColVector normal(3);
750  normal[0] = nx;
751  normal[1] = ny;
752  normal[2] = nz;
753  vpColVector pt(3);
754 
755  unsigned int idx = 0;
756  for (size_t i = 0; i < m_pointCloudFace.size(); i += 3, idx++) {
757  double x = m_pointCloudFace[i];
758  double y = m_pointCloudFace[i + 1];
759  double z = m_pointCloudFace[i + 2];
760 
761  double _a1 = (nz * y) - (ny * z);
762  double _a2 = (nx * z) - (nz * x);
763  double _a3 = (ny * x) - (nx * y);
764 
765  // L
766  L[idx][0] = nx;
767  L[idx][1] = ny;
768  L[idx][2] = nz;
769  L[idx][3] = _a1;
770  L[idx][4] = _a2;
771  L[idx][5] = _a3;
772 
773  pt[0] = x;
774  pt[1] = y;
775  pt[2] = z;
776  // Error
777  error[idx] = D + (normal.t() * pt);
778  }
779  }
780 }
781 
782 void vpMbtFaceDepthDense::computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
783  std::vector<vpImagePoint> &roiPts
784 #if DEBUG_DISPLAY_DEPTH_DENSE
785  ,
786  std::vector<std::vector<vpImagePoint> > &roiPts_vec
787 #endif
788  ,
789  double &distanceToFace)
790 {
791  if (m_useScanLine || m_clippingFlag > 2)
792  m_cam.computeFov(width, height);
793 
794  if (m_useScanLine) {
795  for (std::vector<PolygonLine>::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) {
796  it->m_p1->changeFrame(cMo);
797  it->m_p2->changeFrame(cMo);
798 
799  vpImagePoint ip1, ip2;
800 
801  it->m_poly.changeFrame(cMo);
802  it->m_poly.computePolygonClipped(m_cam);
803 
804  if (it->m_poly.polyClipped.size() == 2 &&
805  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
806  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
807  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
808  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
809  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
810  ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
811 
812  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
813  m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst,
814  true);
815 
816  vpPoint faceCentroid;
817 
818  for (unsigned int i = 0; i < linesLst.size(); i++) {
819  linesLst[i].first.project();
820  linesLst[i].second.project();
821 
822  vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
823  vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
824 
825  it->m_imPt1 = ip1;
826  it->m_imPt2 = ip2;
827 
828  roiPts.push_back(ip1);
829  roiPts.push_back(ip2);
830 
831  faceCentroid.set_X(faceCentroid.get_X() + linesLst[i].first.get_X() + linesLst[i].second.get_X());
832  faceCentroid.set_Y(faceCentroid.get_Y() + linesLst[i].first.get_Y() + linesLst[i].second.get_Y());
833  faceCentroid.set_Z(faceCentroid.get_Z() + linesLst[i].first.get_Z() + linesLst[i].second.get_Z());
834 
835 #if DEBUG_DISPLAY_DEPTH_DENSE
836  std::vector<vpImagePoint> roiPts_;
837  roiPts_.push_back(ip1);
838  roiPts_.push_back(ip2);
839  roiPts_vec.push_back(roiPts_);
840 #endif
841  }
842 
843  if (linesLst.empty()) {
844  distanceToFace = std::numeric_limits<double>::max();
845  }
846  else {
847  faceCentroid.set_X(faceCentroid.get_X() / (2 * linesLst.size()));
848  faceCentroid.set_Y(faceCentroid.get_Y() / (2 * linesLst.size()));
849  faceCentroid.set_Z(faceCentroid.get_Z() / (2 * linesLst.size()));
850 
851  distanceToFace =
852  sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() +
853  faceCentroid.get_Z() * faceCentroid.get_Z());
854  }
855  }
856  }
857  }
858  else {
859  // Get polygon clipped
860  m_polygon->getRoiClipped(m_cam, roiPts, cMo);
861 
862  // Get 3D polygon clipped
863  std::vector<vpPoint> polygonsClipped;
864  m_polygon->getPolygonClipped(polygonsClipped);
865 
866  if (polygonsClipped.empty()) {
867  distanceToFace = std::numeric_limits<double>::max();
868  }
869  else {
870  vpPoint faceCentroid;
871 
872  for (size_t i = 0; i < polygonsClipped.size(); i++) {
873  faceCentroid.set_X(faceCentroid.get_X() + polygonsClipped[i].get_X());
874  faceCentroid.set_Y(faceCentroid.get_Y() + polygonsClipped[i].get_Y());
875  faceCentroid.set_Z(faceCentroid.get_Z() + polygonsClipped[i].get_Z());
876  }
877 
878  faceCentroid.set_X(faceCentroid.get_X() / polygonsClipped.size());
879  faceCentroid.set_Y(faceCentroid.get_Y() / polygonsClipped.size());
880  faceCentroid.set_Z(faceCentroid.get_Z() / polygonsClipped.size());
881 
882  distanceToFace = sqrt(faceCentroid.get_X() * faceCentroid.get_X() + faceCentroid.get_Y() * faceCentroid.get_Y() +
883  faceCentroid.get_Z() * faceCentroid.get_Z());
884  }
885 
886 #if DEBUG_DISPLAY_DEPTH_DENSE
887  roiPts_vec.push_back(roiPts);
888 #endif
889  }
890 }
891 
893  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
894  bool displayFullModel)
895 {
896  std::vector<std::vector<double> > models =
897  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
898 
899  for (size_t i = 0; i < models.size(); i++) {
900  vpImagePoint ip1(models[i][1], models[i][2]);
901  vpImagePoint ip2(models[i][3], models[i][4]);
902  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
903  }
904 }
905 
907  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
908  bool displayFullModel)
909 {
910  std::vector<std::vector<double> > models =
911  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
912 
913  for (size_t i = 0; i < models.size(); i++) {
914  vpImagePoint ip1(models[i][1], models[i][2]);
915  vpImagePoint ip2(models[i][3], models[i][4]);
916  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
917  }
918 }
919 
921  const vpCameraParameters & /*cam*/, const double /*scale*/,
922  const unsigned int /*thickness*/)
923 { }
924 
926  const vpCameraParameters & /*cam*/, const double /*scale*/,
927  const unsigned int /*thickness*/)
928 { }
929 
941 std::vector<std::vector<double> > vpMbtFaceDepthDense::getModelForDisplay(unsigned int width, unsigned int height,
942  const vpHomogeneousMatrix &cMo,
943  const vpCameraParameters &cam,
944  bool displayFullModel)
945 {
946  std::vector<std::vector<double> > models;
947 
948  if ((m_polygon->isVisible() && m_isTrackedDepthDenseFace) || displayFullModel) {
950 
951  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
952  ++it) {
953  vpMbtDistanceLine *line = *it;
954  std::vector<std::vector<double> > lineModels =
955  line->getModelForDisplay(width, height, cMo, cam, displayFullModel);
956  models.insert(models.end(), lineModels.begin(), lineModels.end());
957  }
958  }
959 
960  return models;
961 }
962 
972 bool vpMbtFaceDepthDense::samePoint(const vpPoint &P1, const vpPoint &P2) const
973 {
974  double dx = fabs(P1.get_oX() - P2.get_oX());
975  double dy = fabs(P1.get_oY() - P2.get_oY());
976  double dz = fabs(P1.get_oZ() - P2.get_oZ());
977 
978  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
979  dz <= std::numeric_limits<double>::epsilon())
980  return true;
981  else
982  return false;
983 }
984 
986 {
987  m_cam = camera;
988 
989  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
990  ++it) {
991  (*it)->setCameraParameters(camera);
992  }
993 }
994 
996 {
997  m_useScanLine = v;
998 
999  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
1000  ++it) {
1001  (*it)->useScanLine = v;
1002  }
1003 }
1004 END_VISP_NAMESPACE
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMbScanLine & getMbScanLineRenderer()
bool isVisible(unsigned int i)
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
Manage the line of a polygon used in the model-based tracker.
void setIndex(unsigned int i)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
vpMbtPolygon & getPolygon()
double m_depthDenseFilteringMinDist
Minimum distance threshold.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
bool m_isVisible
Visibility flag.
double m_distFarClip
Distance for near clipping.
std::vector< double > m_pointCloudFace
List of depth points inside the face.
vpPlane m_planeObject
Plane equation described in the object frame.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void setCameraParameters(const vpCameraParameters &camera)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, std::vector< vpImagePoint > &roiPts, double &distanceToFace)
unsigned int getNbFeatures() const
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error)
void setScanLineVisibilityTest(bool v)
vpMbtPolygon * m_polygon
Polygon defining the face.
bool m_useScanLine
Scan line visibility.
bool m_isTrackedDepthDenseFace
Flag to define if the face should be tracked or not.
double m_depthDenseFilteringMaxDist
Maximum distance threshold.
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=nullptr)
std::vector< PolygonLine > m_polygonLines
Polygon lines used for scan-line visibility.
int m_depthDenseFilteringMethod
Method to use to consider or not the face.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
vpCameraParameters m_cam
Camera intrinsic parameters.
double m_depthDenseFilteringOccupancyRatio
Ratio between available depth points and theoretical number of points.
double m_distNearClip
Distance for near clipping.
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="")
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
int getIndex() const
Definition: vpMbtPolygon.h:93
static bool inRoiMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:391
double getD() const
Definition: vpPlane.h:106
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
double getB() const
Definition: vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:411
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:404
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:415
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:446
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:448
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:406
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:450
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:413
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:402
Implements a 3D polygon with render functionalities like clipping.
Definition: vpPolygon3D.h:57
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:192
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:205
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:185
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
vpRect getBoundingBox() const
Definition: vpPolygon.h:164
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:400
Defines a rectangle in the plane.
Definition: vpRect.h:79
double getWidth() const
Definition: vpRect.h:227
void setTop(double pos)
Definition: vpRect.h:357
double getLeft() const
Definition: vpRect.h:173
void setLeft(double pos)
Definition: vpRect.h:321
void setRight(double pos)
Definition: vpRect.h:348
double getRight() const
Definition: vpRect.h:179
double getBottom() const
Definition: vpRect.h:97
double getHeight() const
Definition: vpRect.h:166
void setBottom(double pos)
Definition: vpRect.h:288
double getTop() const
Definition: vpRect.h:192
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:127
VISP_EXPORT bool checkSSE2()
VISP_EXPORT bool checkNeon()