Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added new functionalities to pointCloud library #3160

Merged
merged 1 commit into from
Feb 10, 2025
Merged

Added new functionalities to pointCloud library #3160

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions doc/release/master.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ New Features

* added new datatype `yarp::sig::LayeredImage`
* added `yarp::sig::utils::sum()` to transform `yarp::sig::LayeredImage` to `yarp::sig::Image`
* modified signature of method `yarp::sig::utils::depthToPC` and `yarp::sig::utils::depthRgbToPC`.
They now accept step_x and step_y parameters to perform pointcloud decimation.
They also accept a new parameter `output_order` which allows to swap the axis of the output point cloud (see code documentation)

#### `libYARP_dev`

Expand Down
62 changes: 41 additions & 21 deletions src/libYARP_sig/src/yarp/sig/PointCloudUtils-inl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,15 +8,13 @@

#include <type_traits>


namespace {

template<typename T1,
typename T2,
std::enable_if_t<std::is_same<T1, yarp::sig::DataXYZRGBA>::value &&
(std::is_same<T2, yarp::sig::PixelRgb>::value ||
std::is_same<T2, yarp::sig::PixelBgr>::value), int> = 0
>
std::is_same<T2, yarp::sig::PixelBgr>::value), int> = 0>
inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
const yarp::sig::ImageOf<T2>& color,
const size_t u,
Expand All @@ -31,8 +29,7 @@ template<typename T1,
typename T2,
std::enable_if_t<std::is_same<T1, yarp::sig::DataXYZRGBA>::value &&
(std::is_same<T2, yarp::sig::PixelRgba>::value ||
std::is_same<T2, yarp::sig::PixelBgra>::value), int> = 0
>
std::is_same<T2, yarp::sig::PixelBgra>::value), int> = 0>
inline void copyColorData(yarp::sig::PointCloud<T1>& pointCloud,
const yarp::sig::ImageOf<T2>& color,
const size_t u,
Expand Down Expand Up @@ -65,7 +62,10 @@ template<typename T1, typename T2>
yarp::sig::PointCloud<T1> yarp::sig::utils::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)
const yarp::sig::utils::OrganizationType organizationType,
size_t step_x,
size_t step_y,
const std::string& output_order)
{
yAssert(depth.width() != 0);
yAssert(depth.height() != 0);
Expand All @@ -74,28 +74,48 @@ yarp::sig::PointCloud<T1> yarp::sig::utils::depthRgbToPC(const yarp::sig::ImageO
size_t w = depth.width();
size_t h = depth.height();
yarp::sig::PointCloud<T1> pointCloud;
if (organizationType == yarp::sig::utils::OrganizationType::Organized){
pointCloud.resize(w, h);
if (organizationType == yarp::sig::utils::OrganizationType::Organized)
{
pointCloud.resize(w/step_x, h/step_y);
}
for (size_t u = 0; u < w; ++u) {
for (size_t v = 0; v < h; ++v) {
if (organizationType == yarp::sig::utils::OrganizationType::Organized){

const char* mapping = output_order.c_str();
double values_xyz[3];
char axes[3] = {mapping[1], mapping[3], mapping[5]};
for (size_t u = 0, cu = 0; u < w; u+=step_x, cu++)
{
for (size_t v = 0, cv = 0; v < h; v+=step_y, cv++)
{
double dp = depth.pixel(u, v);
if (organizationType == yarp::sig::utils::OrganizationType::Organized)
{
// Depth
// De-projection equation (pinhole model):
// x = (u - ppx)/ fx * z
// y = (v - ppy)/ fy * z
// z = z
pointCloud(u,v).x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
pointCloud(u,v).y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
pointCloud(u,v).z = depth.pixel(u,v);
copyColorData(pointCloud, color, u, v);

} else if (organizationType == yarp::sig::utils::OrganizationType::Unorganized) {
if (depth.pixel(u,v) > 0){
values_xyz[0] = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
values_xyz[1] = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
values_xyz[2] = dp;
pointCloud(cu, cv).x = (mapping[0] == '-') ? (-values_xyz[axes[0] - 'X']) : (values_xyz[axes[0] - 'X']);
pointCloud(cu, cv).y = (mapping[2] == '-') ? (-values_xyz[axes[1] - 'X']) : (values_xyz[axes[1] - 'X']);
pointCloud(cu, cv).z = (mapping[4] == '-') ? (-values_xyz[axes[2] - 'X']) : (values_xyz[axes[2] - 'X']);
//copyColorData(pointCloud, color, u, v);
pointCloud(cu,cv).r = color.pixel(u,v).r;
pointCloud(cu,cv).g = color.pixel(u,v).g;
pointCloud(cu,cv).b = color.pixel(u,v).b;
}
else if (organizationType == yarp::sig::utils::OrganizationType::Unorganized)
{
//if ( dp > 0) //why?
{
T1 point;
point.x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
point.y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
point.z = depth.pixel(u,v);
values_xyz[0] = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
values_xyz[1] = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
values_xyz[2] = dp;
point.x = (mapping[0] == '-') ? (-values_xyz[axes[0] - 'X']) : (values_xyz[axes[0] - 'X']);
point.y = (mapping[2] == '-') ? (-values_xyz[axes[1] - 'X']) : (values_xyz[axes[1] - 'X']);
point.z = (mapping[4] == '-') ? (-values_xyz[axes[2] - 'X']) : (values_xyz[axes[2] - 'X']);
point.r = color.pixel(u,v).r;
point.g = color.pixel(u,v).g;
point.b = color.pixel(u,v).b;
Expand Down
28 changes: 19 additions & 9 deletions src/libYARP_sig/src/yarp/sig/PointCloudUtils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,9 @@ PointCloud<DataXYZ> utils::depthToPC(const yarp::sig::ImageOf<PixelFloat> &depth
// x = (u - ppx)/ fx * z
// y = (v - ppy)/ fy * z
// z = z
pointCloud(u,v).x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
pointCloud(u,v).y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
pointCloud(u,v).z = depth.pixel(u,v);
double x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
double y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
double z = depth.pixel(u,v);
}
}
return pointCloud;
Expand All @@ -41,7 +41,8 @@ PointCloud<DataXYZ> utils::depthToPC(const yarp::sig::ImageOf<PixelFloat>& depth
const yarp::sig::IntrinsicParams& intrinsic,
const PCL_ROI& roi,
size_t step_x,
size_t step_y)
size_t step_y,
const std::string& output_order)
{
yCAssert(POINTCLOUDUTILS, depth.width() != 0);
yCAssert(POINTCLOUDUTILS, depth.height() != 0);
Expand All @@ -59,15 +60,24 @@ PointCloud<DataXYZ> utils::depthToPC(const yarp::sig::ImageOf<PixelFloat>& depth
size_t size_y = (max_y - min_y) / step_y;
pointCloud.resize(size_x, size_y);

for (size_t i = 0, u = min_x; u < max_x; i++, u += step_x) {
for (size_t j = 0, v = min_y; v < max_y; j++, v += step_y) {
const char* mapping = output_order.c_str();
double values_xyz[3];
char axes[3] = {mapping[1], mapping[3], mapping[5]};
for (size_t i = 0, u = min_x; u < max_x; i++, u += step_x)
{
for (size_t j = 0, v = min_y; v < max_y; j++, v += step_y)
{
// De-projection equation (pinhole model):
// x = (u - ppx)/ fx * z
// y = (v - ppy)/ fy * z
// z = z
pointCloud(i, j).x = (u - intrinsic.principalPointX) / intrinsic.focalLengthX * depth.pixel(u, v);
pointCloud(i, j).y = (v - intrinsic.principalPointY) / intrinsic.focalLengthY * depth.pixel(u, v);
pointCloud(i, j).z = depth.pixel(u, v);
values_xyz[0] = (u - intrinsic.principalPointX) / intrinsic.focalLengthX * depth.pixel(u, v);
values_xyz[1] = (v - intrinsic.principalPointY) / intrinsic.focalLengthY * depth.pixel(u, v);
values_xyz[2] = depth.pixel(u, v);

pointCloud(i,j).x = (mapping[0] == '-') ? (-values_xyz[axes[0] - 'X']) : (values_xyz[axes[0] - 'X']);
pointCloud(i,j).y = (mapping[2] == '-') ? (-values_xyz[axes[1] - 'X']) : (values_xyz[axes[1] - 'X']);
pointCloud(i,j).z = (mapping[4] == '-') ? (-values_xyz[axes[2] - 'X']) : (values_xyz[axes[2] - 'X']);
}
}
return pointCloud;
Expand Down
28 changes: 20 additions & 8 deletions src/libYARP_sig/src/yarp/sig/PointCloudUtils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ struct PCL_ROI
* @param[in] intrinsic, intrinsic parameter of the camera.
* @note the intrinsic parameters are the one of the depth sensor if the depth frame IS NOT aligned with the
* colored one. On the other hand use the intrinsic parameters of the RGB camera if the frames are aligned.
* @return the pointcloud obtained by the de-projection.
* @return the point cloud obtained by the de-projection.
*/
YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZ> depthToPC(const yarp::sig::ImageOf<yarp::sig::PixelFloat>& depth,
const yarp::sig::IntrinsicParams& intrinsic);
Expand All @@ -42,32 +42,44 @@ YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZ> depthToPC(const yarp::sig
* @param[in] intrinsic, intrinsic parameter of the camera.
* @param[in] roi, the Region Of Interest intrinsic of the depth image that we want to convert.
* @param[in] step_x, the depth image size can be decimated, by selecting a column every step_x;
* @param[in] step_t, the depth image size can be decimated, by selecting a row every step_y;
* @param[in] step_y, the depth image size can be decimated, by selecting a row every step_y;
* @param[in] output_order, the string will optionally rearrange data in any combination of positive and negative axes. e.g. "+Z-Y+X".
* Useful when dealing with not right-handed coordinate systems. Optimized for efficiency during point cloud creation with no additional computation.
* @note the intrinsic parameters are the one of the depth sensor if the depth frame IS NOT aligned with the
* colored one. On the other hand use the intrinsic parameters of the RGB camera if the frames are aligned.
* @return the pointcloud obtained by the de-projection.
* @return the point cloud obtained by the de-projection.
*/
YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZ> depthToPC(const yarp::sig::ImageOf<yarp::sig::PixelFloat>& depth,
const yarp::sig::IntrinsicParams& intrinsic,
const yarp::sig::utils::PCL_ROI& roi,
size_t step_x,
size_t step_y);
size_t step_x=1,
size_t step_y=1,
const std::string& output_order = "+X+Y+Z");

/**
* @brief depthRgbToPC, compute the colored PointCloud given depth image, color image and the intrinsic
* parameters of the camera.
* @param[in] depth, the input depth image.
* @param[in] color, the input color image.
* @param[in] intrinsic, intrinsic parameter of the camera.
* @note the intrinsic parameters are the one of the depth sensor if the depth frame IS NOT aligned with the
* @param[in] organizationType, if organized the point cloud has size (width, depth). if unorganized the point cloud
* has size (width*height, 1). Note that in this case the data are organized column-wise (i.e. y,x and not x,y)
* @param[in] step_x, the depth image size can be decimated, by selecting a column every step_x;
* @param[in] step_y, the depth image size can be decimated, by selecting a row every step_y;
* @param[in] output_order, the string will optionally rearrange data in any combination of positive and negative axes. e.g. "+Z-Y+X".
* Useful when dealing with not right-handed coordinate systems. Optimized for efficiency during point cloud creation with no additional computation.
* @note the intrinsic parameters are the one of the depth sensor if the depth frame IS NOT aligned with the
* colored one. On the other hand use the intrinsic parameters of the RGB camera if the frames are aligned.
* @return the pointcloud obtained by the de-projection.
* @return the point cloud obtained by the de-projection.
*/
template<typename T1, typename T2>
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);
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");
} // namespace yarp::sig::utils

#include <yarp/sig/PointCloudUtils-inl.h>
Expand Down
Loading
Loading