Skip to content

Commit

Permalink
Added new functionalities to pointCloud library
Browse files Browse the repository at this point in the history
  • Loading branch information
@randaz81
randaz81 committed Jan 20, 2025
1 parent ffb7a7e commit 9d7f5aa
Show file tree
Hide file tree
Showing 4 changed files with 153 additions and 34 deletions.
60 changes: 39 additions & 21 deletions src/libYARP_sig/src/yarp/sig/PointCloudUtils-inl.h
View file
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,9 @@ 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)
{
yAssert(depth.width() != 0);
yAssert(depth.height() != 0);
Expand All @@ -74,28 +73,47 @@ 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){

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){
double x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
double y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
double z = dp;
// manipulateCoords(x, y, z);
pointCloud(cu, cv).x = x;
pointCloud(cu, cv).y = y;
pointCloud(cu, cv).z = z;
//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);
double x = (u - intrinsic.principalPointX)/intrinsic.focalLengthX*depth.pixel(u,v);
double y = (v - intrinsic.principalPointY)/intrinsic.focalLengthY*depth.pixel(u,v);
double z = dp;
// manipulateCoords(x, y, z);
point.x = x;
point.y = y;
point.z = z;
point.r = color.pixel(u,v).r;
point.g = color.pixel(u,v).g;
point.b = color.pixel(u,v).b;
Expand Down
31 changes: 25 additions & 6 deletions src/libYARP_sig/src/yarp/sig/PointCloudUtils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,15 @@ namespace {
YARP_LOG_COMPONENT(POINTCLOUDUTILS, "yarp.sig.PointCloudUtils")
}

inline void manipulateCoords(double& x, double& y, double& z, const char axis [7] = "+x+y+z")
{
if (axis[0] == '-') {x=-x;}
if (axis[1] == 'x') {x=x;}

if (axis[2] == '-') {y=-y;}
if (axis[4] == '-') {z=-z;}
}

PointCloud<DataXYZ> utils::depthToPC(const yarp::sig::ImageOf<PixelFloat> &depth,
const yarp::sig::IntrinsicParams &intrinsic)
{
Expand All @@ -29,9 +38,14 @@ 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);
char ttt [7]= "+x+y+z";
manipulateCoords(x, y, z, ttt);
pointCloud(u, v).x = x;
pointCloud(u, v).y = y;
pointCloud(u, v).z = z;
}
}
return pointCloud;
Expand Down Expand Up @@ -65,9 +79,14 @@ PointCloud<DataXYZ> utils::depthToPC(const yarp::sig::ImageOf<PixelFloat>& depth
// 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);
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);
char ttt [7]= "+x+y+z";
manipulateCoords(x, y, z, ttt);
pointCloud(i, j).x = x;
pointCloud(i, j).y = y;
pointCloud(i, j).z = z;
}
}
return pointCloud;
Expand Down
20 changes: 13 additions & 7 deletions src/libYARP_sig/src/yarp/sig/PointCloudUtils.h
View file
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,38 @@ 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;
* @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);

/**
* @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.
* @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;
* @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);
} // namespace yarp::sig::utils

#include <yarp/sig/PointCloudUtils-inl.h>
Expand Down
76 changes: 76 additions & 0 deletions src/libYARP_sig/tests/PointCloudTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1080,5 +1080,81 @@ TEST_CASE("sig::PointCloudTest", "[yarp::sig]")
CHECK(testPC(2).z == 7.0);
}

SECTION("Testing depthRgbToPC with decimation (organized)")
{
ImageOf<PixelFloat> depth;
size_t width {40};
size_t height {30};
depth.resize(width, height);
IntrinsicParams intp;

auto pc = utils::depthToPC(depth, intp);
CHECK(pc.width() == depth.width()); // Checking PC width
CHECK(pc.height() == depth.height()); // Checking PC height

// create the depth
double distance = 0;
for (size_t y = 0; y < depth.height(); y++)
for (size_t x = 0; x < depth.width(); x++)
{
depth(x, y) = distance;
distance += 1.0f;
}

ImageOf<PixelRgba> color;
color.resize(width, height);
auto pcCol = utils::depthRgbToPC<DataXYZRGBA, PixelRgba>(depth, color, intp,yarp::sig::utils::OrganizationType::Organized, 2,2);

//check the contents
CHECK(pcCol.width() == depth.width()/2); // Checking PC width
CHECK(pcCol.height() == depth.height()/2); // Checking PC height
double distance_c = 0;
for (size_t y = 0; y < depth.height()/2; y++)
for (size_t x = 0; x < depth.width()/2; x++)
{
distance_c = (y * 2.0 * depth.width() + x * 2.0);
CHECK(pcCol(x, y).z == distance_c);
}
}

SECTION("Testing depthRgbToPC with decimation (not organized)")
{
ImageOf<PixelFloat> depth;
size_t width {6};
size_t height {4};
depth.resize(width, height);
IntrinsicParams intp;

auto pc = utils::depthToPC(depth, intp);
CHECK(pc.width() == depth.width()); // Checking PC width
CHECK(pc.height() == depth.height()); // Checking PC height

// create the depth
double distance = 0;
for (size_t y = 0; y < depth.height(); y++)
for (size_t x = 0; x < depth.width(); x++)
{
depth(x, y) = distance;
distance += 1.0f;
}

ImageOf<PixelRgba> color;
color.resize(width, height);
auto pcCol = utils::depthRgbToPC<DataXYZRGBA, PixelRgba>(depth, color, intp,yarp::sig::utils::OrganizationType::Unorganized, 2,2);

//check the contents
CHECK(pcCol.width() == width*height/4); // Checking PC width
CHECK(pcCol.height() == 1); // unorganized PC are one height only
double distance_c = 0;
for (size_t y = 0; y < 1; y++)
for (size_t x = 0; x < depth.width()/2*depth.height()/2; x++)
{
size_t cx = x % (depth.height()/2);
size_t cy = x / (depth.height()/2);
distance_c = (cy * 2.0 + cx * 2.0 * depth.width());
CHECK(pcCol(x, y).z == distance_c);
}
}

Network::setLocalMode(false);
}

0 comments on commit 9d7f5aa

Please sign in to comment.