YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
PointCloudUtils.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7#include <algorithm>
8#include <cstring>
9
10using namespace yarp::sig;
11
12namespace {
13YARP_LOG_COMPONENT(POINTCLOUDUTILS, "yarp.sig.PointCloudUtils")
14}
15
17 const yarp::sig::IntrinsicParams &intrinsic)
18{
19 yCAssert(POINTCLOUDUTILS, depth.width() != 0);
20 yCAssert(POINTCLOUDUTILS, depth.height() != 0);
21 size_t w = depth.width();
22 size_t h = depth.height();
23 PointCloud<DataXYZ> pointCloud;
24 pointCloud.resize(w, h);
25
26 for (size_t u = 0; u < w; ++u) {
27 for (size_t v = 0; v < h; ++v) {
28 // De-projection equation (pinhole model):
29 // x = (u - ppx)/ fx * z
30 // y = (v - ppy)/ fy * z
31 // z = z
32 double x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
33 double y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
34 double z = depth.pixel(u,v);
35 }
36 }
37 return pointCloud;
38}
39
41 const yarp::sig::IntrinsicParams& intrinsic,
42 const PCL_ROI& roi,
43 size_t step_x,
44 size_t step_y,
45 const std::string& output_order)
46{
47 yCAssert(POINTCLOUDUTILS, depth.width() != 0);
48 yCAssert(POINTCLOUDUTILS, depth.height() != 0);
49
50 size_t max_x = roi.max_x == 0 ? depth.width() : std::min(roi.max_x, depth.width());
51 size_t max_y = roi.max_y == 0 ? depth.height() : std::min(roi.max_y, depth.height());
52 size_t min_x = std::min(roi.min_x, max_x);
53 size_t min_y = std::min(roi.min_y, max_y);
54 // avoid step larger than ROI and division by zero
55 step_x = std::max<size_t>(std::min(step_x, max_x - min_x), 1);
56 step_y = std::max<size_t>(std::min(step_y, max_y - min_y), 1);
57
58 PointCloud<DataXYZ> pointCloud;
59 size_t size_x = (max_x - min_x) / step_x;
60 size_t size_y = (max_y - min_y) / step_y;
61 pointCloud.resize(size_x, size_y);
62
63 const char* mapping = output_order.c_str();
64 double values_xyz[3];
65 char axes[3] = {mapping[1], mapping[3], mapping[5]};
66 for (size_t i = 0, u = min_x; u < max_x; i++, u += step_x)
67 {
68 for (size_t j = 0, v = min_y; v < max_y; j++, v += step_y)
69 {
70 // De-projection equation (pinhole model):
71 // x = (u - ppx)/ fx * z
72 // y = (v - ppy)/ fy * z
73 // z = z
74 values_xyz[0] = (u - intrinsic.principalPointX) / intrinsic.focalLengthX * depth.pixel(u, v);
75 values_xyz[1] = (v - intrinsic.principalPointY) / intrinsic.focalLengthY * depth.pixel(u, v);
76 values_xyz[2] = depth.pixel(u, v);
77
78 pointCloud(i,j).x = (mapping[0] == '-') ? (-values_xyz[axes[0] - 'X']) : (values_xyz[axes[0] - 'X']);
79 pointCloud(i,j).y = (mapping[2] == '-') ? (-values_xyz[axes[1] - 'X']) : (values_xyz[axes[1] - 'X']);
80 pointCloud(i,j).z = (mapping[4] == '-') ? (-values_xyz[axes[2] - 'X']) : (values_xyz[axes[2] - 'X']);
81 }
82 }
83 return pointCloud;
84}
Typed image class.
Definition Image.h:603
The PointCloud class.
Definition PointCloud.h:21
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
#define yCAssert(component, x)
#define YARP_LOG_COMPONENT(name,...)
yarp::sig::PointCloud< yarp::sig::DataXYZ > depthToPC(const yarp::sig::ImageOf< yarp::sig::PixelFloat > &depth, const yarp::sig::IntrinsicParams &intrinsic)
depthToPC, compute the PointCloud given depth image and the intrinsic parameters of the camera.
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).