Visual Servoing Platform  version 3.6.1 under development (2024-12-07)
vpImageConvert_pcl.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Convert image types.
32  *
33 *****************************************************************************/
34 
40 #include <visp3/core/vpConfig.h>
41 
42 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
43 
44 #include <visp3/core/vpImageConvert.h>
45 
46 #if defined(_OPENMP)
47 #include <omp.h>
48 #endif
49 
50 BEGIN_VISP_NAMESPACE
73  int vpImageConvert::depthToPointCloud(const vpImage<uint16_t> &depth_raw, float depth_scale,
74  const vpCameraParameters &cam_depth,
75  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud, std::mutex *pointcloud_mutex,
76  const vpImage<unsigned char> *depth_mask, float Z_min, float Z_max)
77 {
78 
79  int size = static_cast<int>(depth_raw.getSize());
80  unsigned int width = depth_raw.getWidth();
81  unsigned int height = depth_raw.getHeight();
82  int pcl_size = 0;
83  const unsigned int index_0 = 0;
84  const unsigned int index_1 = 1;
85  const unsigned int index_2 = 2;
86 
87  if (depth_mask) {
88  if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) {
89  throw(vpImageException(vpImageException::notInitializedError, "Depth image and mask size differ"));
90  }
91  if (pointcloud_mutex) {
92  pointcloud_mutex->lock();
93  }
94  pointcloud->clear();
95 #if defined(_OPENMP)
96  std::mutex mutex;
97 #pragma omp parallel for
98 #endif
99  for (int p = 0; p < size; ++p) {
100  if (depth_mask->bitmap[p]) {
101  if (static_cast<int>(depth_raw.bitmap[p])) {
102  float Z = static_cast<float>(depth_raw.bitmap[p]) * depth_scale;
103  if (Z < Z_max) {
104  double x = 0;
105  double y = 0;
106  unsigned int j = p % width;
107  unsigned int i = (p - j) / width;
108  vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
109  vpColVector point_3D({ x * Z, y * Z, Z });
110  if (point_3D[index_2] > Z_min) {
111 #if defined(_OPENMP)
112  std::lock_guard<std::mutex> lock(mutex);
113 #endif
114  pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2]));
115  }
116  }
117  }
118  }
119  }
120  pcl_size = pointcloud->size();
121  if (pointcloud_mutex) {
122  pointcloud_mutex->unlock();
123  }
124  }
125  else {
126  if (pointcloud_mutex) {
127  pointcloud_mutex->lock();
128  }
129  pointcloud->clear();
130 #if defined(_OPENMP)
131  std::mutex mutex;
132 #pragma omp parallel for
133 #endif
134  for (int p = 0; p < size; ++p) {
135  if (static_cast<int>(depth_raw.bitmap[p])) {
136  float Z = static_cast<float>(depth_raw.bitmap[p]) * depth_scale;
137  if (Z < 2.5) {
138  double x = 0;
139  double y = 0;
140  unsigned int j = p % width;
141  unsigned int i = (p - j) / width;
142  vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
143  vpColVector point_3D({ x * Z, y * Z, Z, 1 });
144  if (point_3D[index_2] >= 0.1) {
145 #if defined(_OPENMP)
146  std::lock_guard<std::mutex> lock(mutex);
147 #endif
148  pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2]));
149  }
150  }
151  }
152  }
153  pcl_size = pointcloud->size();
154  if (pointcloud_mutex) {
155  pointcloud_mutex->unlock();
156  }
157  }
158 
159  return pcl_size;
160 }
161 
186  float depth_scale, const vpCameraParameters &cam_depth,
187  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud, std::mutex *pointcloud_mutex,
188  const vpImage<unsigned char> *depth_mask, float Z_min, float Z_max)
189 {
190  int size = static_cast<int>(depth_raw.getSize());
191  unsigned int width = depth_raw.getWidth();
192  unsigned int height = depth_raw.getHeight();
193  int pcl_size = 0;
194  const unsigned int index_0 = 0;
195  const unsigned int index_1 = 1;
196  const unsigned int index_2 = 2;
197 
198  if (depth_mask) {
199  if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) {
200  throw(vpImageException(vpImageException::notInitializedError, "Depth image and mask size differ"));
201  }
202  if (pointcloud_mutex) {
203  pointcloud_mutex->lock();
204  }
205  pointcloud->clear();
206 #if defined(_OPENMP)
207  std::mutex mutex;
208 #pragma omp parallel for
209 #endif
210  for (int p = 0; p < size; ++p) {
211  if (depth_mask->bitmap[p]) {
212  if (static_cast<int>(depth_raw.bitmap[p])) {
213  float Z = static_cast<float>(depth_raw.bitmap[p]) * depth_scale;
214  if (Z < Z_max) {
215  double x = 0;
216  double y = 0;
217  unsigned int j = p % width;
218  unsigned int i = (p - j) / width;
219  vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
220  vpColVector point_3D({ x * Z, y * Z, Z });
221  if (point_3D[index_2] > Z_min) {
222 #if defined(_OPENMP)
223  std::lock_guard<std::mutex> lock(mutex);
224 #endif
225 #if PCL_VERSION_COMPARE(>=,1,14,1)
226  pointcloud->push_back(pcl::PointXYZRGB(point_3D[index_0], point_3D[index_1], point_3D[index_2],
227  color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B));
228 #else
229  pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B);
230  pt.x = point_3D[index_0];
231  pt.y = point_3D[index_1];
232  pt.z = point_3D[index_2];
233  pointcloud->push_back(pcl::PointXYZRGB(pt));
234 #endif
235  }
236  }
237  }
238  }
239  }
240  pcl_size = pointcloud->size();
241  if (pointcloud_mutex) {
242  pointcloud_mutex->unlock();
243  }
244  }
245  else {
246  if (pointcloud_mutex) {
247  pointcloud_mutex->lock();
248  }
249  pointcloud->clear();
250 #if defined(_OPENMP)
251  std::mutex mutex;
252 #pragma omp parallel for
253 #endif
254  for (int p = 0; p < size; ++p) {
255  if (static_cast<int>(depth_raw.bitmap[p])) {
256  float Z = static_cast<float>(depth_raw.bitmap[p]) * depth_scale;
257  if (Z < 2.5) {
258  double x = 0;
259  double y = 0;
260  unsigned int j = p % width;
261  unsigned int i = (p - j) / width;
262  vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
263  vpColVector point_3D({ x * Z, y * Z, Z, 1 });
264  if (point_3D[index_2] >= 0.1) {
265 #if defined(_OPENMP)
266  std::lock_guard<std::mutex> lock(mutex);
267 #endif
268 #if PCL_VERSION_COMPARE(>=,1,14,1)
269  pointcloud->push_back(pcl::PointXYZRGB(point_3D[index_0], point_3D[index_1], point_3D[index_2],
270  color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B));
271 #else
272  pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B);
273  pt.x = point_3D[index_0];
274  pt.y = point_3D[index_1];
275  pt.z = point_3D[index_2];
276  pointcloud->push_back(pcl::PointXYZRGB(pt));
277 #endif
278  }
279  }
280  }
281  }
282  pcl_size = pointcloud->size();
283  if (pointcloud_mutex) {
284  pointcloud_mutex->unlock();
285  }
286  }
287 
288  return pcl_size;
289 }
290 END_VISP_NAMESPACE
291 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static int depthToPointCloud(const vpImage< uint16_t > &depth_raw, float depth_scale, const vpCameraParameters &cam_depth, pcl::PointCloud< pcl::PointXYZ >::Ptr pointcloud, std::mutex *pointcloud_mutex=nullptr, const vpImage< unsigned char > *mask=nullptr, float Z_min=0.2, float Z_max=2.5)
Error that can be emitted by the vpImage class and its derivatives.
@ notInitializedError
Image not initialized.
Definition of the vpImage class member functions.
Definition: vpImage.h:131
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getSize() const
Definition: vpImage.h:221
Type * bitmap
points toward the bitmap
Definition: vpImage.h:135
unsigned int getHeight() const
Definition: vpImage.h:181
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned char B
Blue component.
Definition: vpRGBa.h:169
unsigned char R
Red component.
Definition: vpRGBa.h:167
unsigned char G
Green component.
Definition: vpRGBa.h:168