YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
PointCloudUtils-inl.h
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
6#ifndef YARP_SIG_POINTCLOUDUTILS_INL_H
7#define YARP_SIG_POINTCLOUDUTILS_INL_H
8
9#include <type_traits>
10
11
12namespace {
13
14template<typename T1,
15 typename T2,
16 std::enable_if_t<std::is_same<T1, yarp::sig::DataXYZRGBA>::value &&
17 (std::is_same<T2, yarp::sig::PixelRgb>::value ||
18 std::is_same<T2, yarp::sig::PixelBgr>::value), int> = 0
19>
20inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
21 const yarp::sig::ImageOf<T2>& color,
22 const size_t u,
23 const size_t v)
24{
25 pointCloud(u,v).r = color.pixel(u,v).r;
26 pointCloud(u,v).g = color.pixel(u,v).g;
27 pointCloud(u,v).b = color.pixel(u,v).b;
28}
29
30template<typename T1,
31 typename T2,
32 std::enable_if_t<std::is_same<T1, yarp::sig::DataXYZRGBA>::value &&
33 (std::is_same<T2, yarp::sig::PixelRgba>::value ||
34 std::is_same<T2, yarp::sig::PixelBgra>::value), int> = 0
35>
36inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
37 const yarp::sig::ImageOf<T2>& color,
38 const size_t u,
39 const size_t v)
40{
41 pointCloud(u,v).r = color.pixel(u,v).r;
42 pointCloud(u,v).g = color.pixel(u,v).g;
43 pointCloud(u,v).b = color.pixel(u,v).b;
44 pointCloud(u,v).a = color.pixel(u,v).a;
45}
46
47template<typename T1,
48 typename T2,
49 std::enable_if_t<!std::is_same<T1, yarp::sig::DataXYZRGBA>::value ||
50 (!std::is_same<T2, yarp::sig::PixelRgb>::value &&
51 !std::is_same<T2, yarp::sig::PixelBgr>::value &&
52 !std::is_same<T2, yarp::sig::PixelRgba>::value &&
53 !std::is_same<T2, yarp::sig::PixelBgra>::value), int> = 0
54>
55inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
56 const yarp::sig::ImageOf<T2>& color,
57 size_t u,
58 size_t v)
59{
60}
61
62} // namespace
63
64template<typename T1, typename T2>
66 const yarp::sig::ImageOf<T2>& color,
67 const yarp::sig::IntrinsicParams& intrinsic,
68 const yarp::sig::utils::OrganizationType organizationType)
69{
70 yAssert(depth.width() != 0);
71 yAssert(depth.height() != 0);
72 yAssert(depth.width() == color.width());
73 yAssert(depth.height() == color.height());
74 size_t w = depth.width();
75 size_t h = depth.height();
77 if (organizationType == yarp::sig::utils::OrganizationType::Organized){
78 pointCloud.resize(w, h);
79 }
80 for (size_t u = 0; u < w; ++u) {
81 for (size_t v = 0; v < h; ++v) {
82 if (organizationType == yarp::sig::utils::OrganizationType::Organized){
83 // Depth
84 // De-projection equation (pinhole model):
85 // x = (u - ppx)/ fx * z
86 // y = (v - ppy)/ fy * z
87 // z = z
88 pointCloud(u,v).x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
89 pointCloud(u,v).y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
90 pointCloud(u,v).z = depth.pixel(u,v);
91 copyColorData(pointCloud, color, u, v);
92
93 } else if (organizationType == yarp::sig::utils::OrganizationType::Unorganized) {
94 if (depth.pixel(u,v) > 0){
95 T1 point;
96 point.x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
97 point.y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
98 point.z = depth.pixel(u,v);
99 point.r = color.pixel(u,v).r;
100 point.g = color.pixel(u,v).g;
101 point.b = color.pixel(u,v).b;
102 pointCloud.push_back(point);
103 }
104 }
105 }
106 }
107 return pointCloud;
108}
109
110#endif // YARP_SIG_POINTCLOUDUTILS_INL_H
#define yAssert(x)
Definition Log.h:388
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
void push_back(const T &pt)
Insert a new point in the cloud, at the end of the container.
Definition PointCloud.h:206
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
Definition PointCloud.h:59
yarp::sig::PointCloud< T1 > depthRgbToPC(const yarp::sig::ImageOf< yarp::sig::PixelFloat > &depth, const yarp::sig::ImageOf< T2 > &color, const yarp::sig::IntrinsicParams &intrinsic, const yarp::sig::utils::OrganizationType organizationType=yarp::sig::utils::OrganizationType::Organized)
depthRgbToPC, compute the colored PointCloud given depth image, color image and the intrinsic paramet...
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.