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
11namespace {
12
13template<typename T1,
14 typename T2,
15 std::enable_if_t<std::is_same<T1, yarp::sig::DataXYZRGBA>::value &&
16 (std::is_same<T2, yarp::sig::PixelRgb>::value ||
17 std::is_same<T2, yarp::sig::PixelBgr>::value), int> = 0>
18inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
19 const yarp::sig::ImageOf<T2>& color,
20 const size_t u,
21 const size_t v)
22{
23 pointCloud(u,v).r = color.pixel(u,v).r;
24 pointCloud(u,v).g = color.pixel(u,v).g;
25 pointCloud(u,v).b = color.pixel(u,v).b;
26}
27
28template<typename T1,
29 typename T2,
30 std::enable_if_t<std::is_same<T1, yarp::sig::DataXYZRGBA>::value &&
31 (std::is_same<T2, yarp::sig::PixelRgba>::value ||
32 std::is_same<T2, yarp::sig::PixelBgra>::value), int> = 0>
33inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
34 const yarp::sig::ImageOf<T2>& color,
35 const size_t u,
36 const size_t v)
37{
38 pointCloud(u,v).r = color.pixel(u,v).r;
39 pointCloud(u,v).g = color.pixel(u,v).g;
40 pointCloud(u,v).b = color.pixel(u,v).b;
41 pointCloud(u,v).a = color.pixel(u,v).a;
42}
43
44template<typename T1,
45 typename T2,
46 std::enable_if_t<!std::is_same<T1, yarp::sig::DataXYZRGBA>::value ||
47 (!std::is_same<T2, yarp::sig::PixelRgb>::value &&
48 !std::is_same<T2, yarp::sig::PixelBgr>::value &&
49 !std::is_same<T2, yarp::sig::PixelRgba>::value &&
50 !std::is_same<T2, yarp::sig::PixelBgra>::value), int> = 0
51>
52inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
53 const yarp::sig::ImageOf<T2>& color,
54 size_t u,
55 size_t v)
56{
57}
58
59} // namespace
60
61template<typename T1, typename T2>
63 const yarp::sig::ImageOf<T2>& color,
64 const yarp::sig::IntrinsicParams& intrinsic,
65 const yarp::sig::utils::OrganizationType organizationType,
66 size_t step_x,
67 size_t step_y,
68 const std::string& output_order)
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();
78 {
79 pointCloud.resize(w/step_x, h/step_y);
80 }
81
82 const char* mapping = output_order.c_str();
83 double values_xyz[3];
84 char axes[3] = {mapping[1], mapping[3], mapping[5]};
85 for (size_t u = 0, cu = 0; u < w; u+=step_x, cu++)
86 {
87 for (size_t v = 0, cv = 0; v < h; v+=step_y, cv++)
88 {
89 double dp = depth.pixel(u, v);
91 {
92 // Depth
93 // De-projection equation (pinhole model):
94 // x = (u - ppx)/ fx * z
95 // y = (v - ppy)/ fy * z
96 // z = z
97 values_xyz[0] = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
98 values_xyz[1] = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
99 values_xyz[2] = dp;
100 pointCloud(cu, cv).x = (mapping[0] == '-') ? (-values_xyz[axes[0] - 'X']) : (values_xyz[axes[0] - 'X']);
101 pointCloud(cu, cv).y = (mapping[2] == '-') ? (-values_xyz[axes[1] - 'X']) : (values_xyz[axes[1] - 'X']);
102 pointCloud(cu, cv).z = (mapping[4] == '-') ? (-values_xyz[axes[2] - 'X']) : (values_xyz[axes[2] - 'X']);
103 //copyColorData(pointCloud, color, u, v);
104 pointCloud(cu,cv).r = color.pixel(u,v).r;
105 pointCloud(cu,cv).g = color.pixel(u,v).g;
106 pointCloud(cu,cv).b = color.pixel(u,v).b;
107 }
108 else if (organizationType == yarp::sig::utils::OrganizationType::Unorganized)
109 {
110 //if ( dp > 0) //why?
111 {
112 T1 point;
113 values_xyz[0] = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
114 values_xyz[1] = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
115 values_xyz[2] = dp;
116 point.x = (mapping[0] == '-') ? (-values_xyz[axes[0] - 'X']) : (values_xyz[axes[0] - 'X']);
117 point.y = (mapping[2] == '-') ? (-values_xyz[axes[1] - 'X']) : (values_xyz[axes[1] - 'X']);
118 point.z = (mapping[4] == '-') ? (-values_xyz[axes[2] - 'X']) : (values_xyz[axes[2] - 'X']);
119 point.r = color.pixel(u,v).r;
120 point.g = color.pixel(u,v).g;
121 point.b = color.pixel(u,v).b;
122 pointCloud.push_back(point);
123 }
124 }
125 }
126 }
127 return pointCloud;
128}
129
130#endif // YARP_SIG_POINTCLOUDUTILS_INL_H
#define yAssert(x)
Definition Log.h:388
Typed image class.
Definition Image.h:603
T & pixel(size_t x, size_t y)
Definition Image.h:619
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
The PointCloud class.
Definition PointCloud.h:21
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
void push_back(const T &pt)
Insert a new point in the cloud, at the end of the container.
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, size_t step_x=1, size_t step_y=1, const std::string &output_order="+X+Y+Z")
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).