YARP PointCloud

This tutorial covers how to use the template class yarp::sig::PointCloud with pcl::PointCloud and stand alone.

Collaboration diagram for YARP PointCloud:

This tutorial covers how to use the template class yarp::sig::PointCloud with pcl::PointCloud and stand alone.

What is a PointCloud?

A point cloud is a set of data points in a pre-defined coordinate system.

In a three-dimensional coordinate system, these points are usually defined by X, Y, and Z coordinates, and often are intended to represent the external surface of an object.

A point cloud can also contain other kinds of information such as the normals, the RGB, etc.

In YARP we support these point types:

• yarp::sig::DataXY
• yarp::sig::DataXYZ
• yarp::sig::DataNormal
• yarp::sig::DataXYZRGBA
• yarp::sig::DataXYZI
• yarp::sig::DataInterestPointXYZ
• yarp::sig::DataXYZNormal
• yarp::sig::DataXYZNormalRGBA

These structures have been created to be compatible with the PCL's ones.

Write and read PointCloud to/from ports

This code snippet shows how to send and then receive a yarp::sig::PointCloud through the YARP ports.

yarp::os::BufferedPort< yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> > outPort;
yarp::os::Port inPort;
// Open the ports
outPort.open("/test/pointcloud/out");
inPort.open("/test/pointcloud/in");
// Connect the ports
yarp::os::NetworkBase::connect(outPort.getName(), inPort.getName());
// Declare the point cloud to be sent
yarp::sig::PointCloud<yarp::sig::DataXYZRGBA>& testPC = outPort.prepare();
int width  = 100;
int height = 20;
testPC.resize(width, height);
// Fill the point cloud
for (int i=0; i<width*height; i++)
{
    testPC(i).x = i;
    testPC(i).y = i + 1;
    testPC(i).z = i + 2;
    testPC(i).r = '1';
    testPC(i).g = '2';
    testPC(i).b = '3';
    testPC(i).a = '4';
}
 
yarp::os::Time::delay(0.3);
// write the point cloud
outPort.write();
// read the point cloud
yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> inCloud;
inPort.read(inCloud);

Note:** in this example, the type of the testPC and inCloud is the same but it is NOT mandatory.

If the type of the input yarp::sig::PointCloud is different, only the fields in common with the output type will be actually read, the missing ones will be set to default values.

For example, if you write a yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> and you want to read through a yarp::sig::PointCloud<yarp::sig::DataXYZNormal>:

• the XYZfields will be filled
• the Normal fields will be set to default
• theRGBA will be ignored

PointCloud initialization

The flexibility on types has been implemented also for what concerns the initialization of the point cloud (copy constructor and assignment operator):

yarp::sig::PointCloud<yarp::sig::DataXYZNormalRGBA> pc1;
yarp::sig::PointCloud<yarp::sig::DataXYZNormal> pc2(pc1);
yarp::sig::PointCloud<yarp::sig::DataXYZNormalRGBA> pc3 = pc2;

In this case pc3 will have only the XYZ and Normal values of pc1 because the RGBA values have been lost in the construction of pc2 that not contains the RGBA field.

Compute PointClouds from depth images

YARP provides two functions that allow to compute yarp::sig::PointClouds given depth image and intrinsic parameters of the camera.

They are included in yarp/sig/PointCloudUtils.h and they are:

• yarp::sig::utils::depthToPC
• yarp::sig::utils::depthRgbToPC

Both functions assume that the images are already undistorted, no undistortion is performed.

yarp::sig::utilsdepthRgbToPC needs also the colored frame in order to generate a colored PointCloud. It assumes that the the two frames are aligned.

You have to use intrinsics parameters 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.

Here an example of usage:

yarp::sig::ImageOf<yarp::sig::PixelFloat> depth;
yarp::sig::ImageOf<yarp::sig::PixelRgb> color;
yarp::os::Property propIntrinsic;
yarp::sig::IntrinsicParams intrinsic;
bool aligned{false};
// check if the frames that are coming are aligned.
...
// get the intrinsic parameters from the RGBDClient
yarp::os::Property conf;
conf.put("device","RGBDSensorClient");
conf.put("localImagePort","/clientRgbPort:i");
conf.put("localDepthPort","/clientDepthPort:i");
conf.put("localRpcPort","/clientRpcPort");
conf.put("remoteImagePort","/depthCamera/rgbImage:o");
conf.put("remoteDepthPort","/depthCamera/depthImage:o");
conf.put("remoteRpcPort","/depthCamera/rpc:i");
 
if (!poly.open(conf))
{
    yError()<<"Unable to open RGBDClient polydriver";
    return false;
}
 
yarp::dev::IRGBDSensor* iRgbd{nullptr};
if (!poly.view(iRgbd))
{
    yError()<<"Unable to view IRGBD interface";
    return false;
}
 
bool ok{false};
if (aligned)
{
    ok = iRgbd->getRgbIntrinsicParam(propIntrinsics);
}
else
{
    ok = iRgbd->getDepthIntrinsicParam(propIntrinsics);
}
 
yarp::sig::IntrinsicParams intrinsics(propIntrinsics);
 
// read the images from ports
 
...
 
// Compute the PointCloud
 
yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> pc = yarp::sig::utils::depthRgbToPC<yarp::sig::DataXYZRGBA, yarp::sig::PixelRgb>(depth, color, intrinsics);

Or:

yarp::sig::PointCloud<yarp::sig::DataXYZ> pc = yarp::sig::utils::depthToPC(depth, intrinsics);

PCL compatibility

The yarp::sig::PointCloud class has been implemented to be compatible with Point Cloud Library (PCL).

libYARP_pcl makes yarp::sig::PointCloud compatible with PCL, and it is compiled if PCL is found during the YARP configuration.

This library has two methods to transform between the two point clouds toPcl<T,T1>(...) and fromPcl<T,T1>(...) that can be used as follow

// Include the compatibility library
#include <yarp/pcl/Pcl.h>
 
pcl::PointCloud<pcl::PointXYZRGBA> cloud;
cloud.resize(100);
for(int i=0; i<cloud.size(); i++)
{
    cloud.points.at(i).x = i;
    cloud.points.at(i).y = i+1;
    cloud.points.at(i).z = i+2;
    cloud.points.at(i).r = 'r';
    cloud.points.at(i).g = 'g';
    cloud.points.at(i).b = 'b';
}
// PCL -> YARP
yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> yarpCloud;
yarp::pcl::fromPCL<pcl::PointXYZRGBA, yarp::sig::DataXYZRGBA>(cloud, yarpCloud);
// YARP -> PCL
pcl::PointCloud<pcl::PointXYZRGBA> cloud2;
yarp::pcl::toPCL<yarp::sig::DataXYZRGBA, pcl::PointXYZRGBA>(yarpCloud, cloud2);

Note that these conversions (PCL -> YARP, YARP -> PCL) are made through a copy.

Saving/Loading PCDs

The YARP_pcl library has also methods to save/load yarp point clouds to PCD files. These functions savePCD<T,T1>(...) and loadPCD<T,T1>(...) can be used as follows:

yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> cloud1;
...
// save point cloud to file
const string filename("name.pcd");
int result = yarp::pcl::savePCD< yarp::sig::DataXYZRGBA, pcl::PointXYZRGBA>(filename, cloud1);
 
// load from file
yarp::sig::PointCloud<yarp::sig::XYZRGBADat> cloud2;
result = yarp::pcl::loadPCD< pcl::PointXYZRGBA, yarp::sig::DataXYZRGBA >(filename, cloud2);

Note that these functions implicitly convert to and from PCL types.