41 #include <visp3/core/vpConfig.h>
43 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
45 #include <visp3/core/vpImageConvert.h>
73 int vpImageConvert::depthToPointCloud(
const vpImage<uint16_t> &depth_raw,
float depth_scale,
75 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud, std::mutex *pointcloud_mutex,
79 int size =
static_cast<int>(depth_raw.
getSize());
80 unsigned int width = depth_raw.
getWidth();
81 unsigned int height = depth_raw.
getHeight();
88 if (pointcloud_mutex) {
89 pointcloud_mutex->lock();
94 #pragma omp parallel for
96 for (
int p = 0; p < size; ++p) {
97 if (depth_mask->
bitmap[p]) {
98 if (
static_cast<int>(depth_raw.
bitmap[p])) {
99 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
103 unsigned int j = p % width;
104 unsigned int i = (p - j) / width;
107 if (point_3D[2] > Z_min) {
109 std::lock_guard<std::mutex> lock(mutex);
111 pointcloud->push_back(pcl::PointXYZ(point_3D[0], point_3D[1], point_3D[2]));
117 pcl_size = pointcloud->size();
118 if (pointcloud_mutex) {
119 pointcloud_mutex->unlock();
123 if (pointcloud_mutex) {
124 pointcloud_mutex->lock();
129 #pragma omp parallel for
131 for (
int p = 0; p < size; ++p) {
132 if (
static_cast<int>(depth_raw.
bitmap[p])) {
133 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
137 unsigned int j = p % width;
138 unsigned int i = (p - j) / width;
141 if (point_3D[2] >= 0.1) {
143 std::lock_guard<std::mutex> lock(mutex);
145 pointcloud->push_back(pcl::PointXYZ(point_3D[0], point_3D[1], point_3D[2]));
150 pcl_size = pointcloud->size();
151 if (pointcloud_mutex) {
152 pointcloud_mutex->unlock();
184 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud, std::mutex *pointcloud_mutex,
187 int size =
static_cast<int>(depth_raw.
getSize());
188 unsigned int width = depth_raw.
getWidth();
189 unsigned int height = depth_raw.
getHeight();
196 if (pointcloud_mutex) {
197 pointcloud_mutex->lock();
202 #pragma omp parallel for
204 for (
int p = 0; p < size; ++p) {
205 if (depth_mask->
bitmap[p]) {
206 if (
static_cast<int>(depth_raw.
bitmap[p])) {
207 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
211 unsigned int j = p % width;
212 unsigned int i = (p - j) / width;
215 if (point_3D[2] > Z_min) {
217 std::lock_guard<std::mutex> lock(mutex);
219 #if PCL_VERSION_COMPARE(>=,1,14,1)
220 pointcloud->push_back(pcl::PointXYZRGB(point_3D[0], point_3D[1], point_3D[2],
227 pointcloud->push_back(pcl::PointXYZRGB(pt));
234 pcl_size = pointcloud->size();
235 if (pointcloud_mutex) {
236 pointcloud_mutex->unlock();
240 if (pointcloud_mutex) {
241 pointcloud_mutex->lock();
246 #pragma omp parallel for
248 for (
int p = 0; p < size; ++p) {
249 if (
static_cast<int>(depth_raw.
bitmap[p])) {
250 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
254 unsigned int j = p % width;
255 unsigned int i = (p - j) / width;
258 if (point_3D[2] >= 0.1) {
260 std::lock_guard<std::mutex> lock(mutex);
262 #if PCL_VERSION_COMPARE(>=,1,14,1)
263 pointcloud->push_back(pcl::PointXYZRGB(point_3D[0], point_3D[1], point_3D[2],
270 pointcloud->push_back(pcl::PointXYZRGB(pt));
276 pcl_size = pointcloud->size();
277 if (pointcloud_mutex) {
278 pointcloud_mutex->unlock();
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Error that can be emitted by the vpImage class and its derivatives.
@ notInitializedError
Image not initialized.
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getSize() const
Type * bitmap
points toward the bitmap
unsigned int getHeight() const
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned char B
Blue component.
unsigned char R
Red component.
unsigned char G
Green component.