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 pointCloud(u,v).x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
33 pointCloud(u,v).y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
34 pointCloud(u,v).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{
46 yCAssert(POINTCLOUDUTILS, depth.width() != 0);
47 yCAssert(POINTCLOUDUTILS, depth.height() != 0);
48
49 size_t max_x = roi.max_x == 0 ? depth.width() : std::min(roi.max_x, depth.width());
50 size_t max_y = roi.max_y == 0 ? depth.height() : std::min(roi.max_y, depth.height());
51 size_t min_x = std::min(roi.min_x, max_x);
52 size_t min_y = std::min(roi.min_y, max_y);
53 // avoid step larger than ROI and division by zero
54 step_x = std::max<size_t>(std::min(step_x, max_x - min_x), 1);
55 step_y = std::max<size_t>(std::min(step_y, max_y - min_y), 1);
56
57 PointCloud<DataXYZ> pointCloud;
58 size_t size_x = (max_x - min_x) / step_x;
59 size_t size_y = (max_y - min_y) / step_y;
60 pointCloud.resize(size_x, size_y);
61
62 for (size_t i = 0, u = min_x; u < max_x; i++, u += step_x) {
63 for (size_t j = 0, v = min_y; v < max_y; j++, v += step_y) {
64 // De-projection equation (pinhole model):
65 // x = (u - ppx)/ fx * z
66 // y = (v - ppy)/ fy * z
67 // z = z
68 pointCloud(i, j).x = (u - intrinsic.principalPointX) / intrinsic.focalLengthX * depth.pixel(u, v);
69 pointCloud(i, j).y = (v - intrinsic.principalPointY) / intrinsic.focalLengthY * depth.pixel(u, v);
70 pointCloud(i, j).z = depth.pixel(u, v);
71 }
72 }
73 return pointCloud;
74}
Typed image class.
Definition Image.h:616
T & pixel(size_t x, size_t y)
Definition Image.h:632
size_t width() const
Gets width of image in pixels.
Definition Image.h:163
size_t height() const
Gets height of image in pixels.
Definition Image.h:169
The PointCloud class.
Definition PointCloud.h:22
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
Definition PointCloud.h:59
#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).
double focalLengthY
Result of the product of the physical focal length(mm) and the size sy of the individual imager eleme...
double focalLengthX
Result of the product of the physical focal length(mm) and the size sx of the individual imager eleme...
double principalPointX
Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge.
double principalPointY
Vertical coordinate of the principal point of the image, as a pixel offset from the top edge.