6#ifndef YARP_SIG_POINTCLOUDUTILS_INL_H
7#define YARP_SIG_POINTCLOUDUTILS_INL_H
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>
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;
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>
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;
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
61template<
typename T1,
typename T2>
68 const std::string& output_order)
74 size_t w =
depth.width();
75 size_t h =
depth.height();
79 pointCloud.
resize(w/step_x, h/step_y);
82 const char* mapping = output_order.c_str();
84 char axes[3] = {mapping[1], mapping[3], mapping[5]};
85 for (
size_t u = 0, cu = 0; u < w; u+=step_x, cu++)
87 for (
size_t v = 0, cv = 0; v < h; v+=step_y, cv++)
89 double dp =
depth.pixel(u, v);
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']);
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;
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;
T & pixel(size_t x, size_t y)
size_t width() const
Gets width of image in pixels.
size_t height() const
Gets height of image in pixels.
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).