YARP
Yet Another Robot Platform
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:

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.

// Open the ports
outPort.open("/test/pointcloud/out");
inPort.open("/test/pointcloud/in");
// Connect the ports
// Declare the point cloud to be sent
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';
}
// write the point cloud
outPort.write();
// read the point cloud
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):

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:

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::os::Property propIntrinsic;
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:

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::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:

...
// 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
result = yarp::pcl::loadPCD< pcl::PointXYZRGBA, yarp::sig::DataXYZRGBA >(filename, cloud2);

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

yarp::os::Port::open
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: Port.cpp:82
yError
#define yError(...)
Definition: Log.h:282
yarp::sig::ImageOf< yarp::sig::PixelFloat >
yarp::sig::PointCloud
The PointCloud class.
Definition: PointCloud.h:27
yarp::os::BufferedPort::prepare
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
Definition: BufferedPort-inl.h:114
yarp::os::Port
A mini-server for network communication.
Definition: Port.h:50
yarp::os::BufferedPort
A mini-server for performing network communication in the background.
Definition: BufferedPort.h:64
yarp::sig::PointCloud::resize
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
Definition: PointCloud.h:64
yarp::sig::utils::depthToPC
yarp::sig::PointCloud< yarp::sig::DataXYZ > depthToPC(const yarp::sig::ImageOf< yarp::sig::PixelFloat > &depth, const yarp::sig::IntrinsicParams &intrinsic)
depthToPC, compute the PointCloud given depth image and the intrinsic parameters of the camera.
Definition: PointCloudUtils.cpp:18
yarp::os::NetworkBase::connect
static bool connect(const std::string &src, const std::string &dest, const std::string &carrier="", bool quiet=true)
Request that an output port connect to an input port.
Definition: Network.cpp:685
yarp::os::BufferedPort::open
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: BufferedPort-inl.h:41
yarp::sig::DataXYZRGBA
Definition: PointCloudTypes.h:486
yarp::dev::IRGBDSensor
A generic interface for cameras that have both color camera as well as depth camera sensor,...
Definition: IRGBDSensor.h:56
yarp::os::Contactable::getName
virtual std::string getName() const
Get name of port.
Definition: Contactable.cpp:17
yarp::os::BufferedPort::getName
std::string getName() const override
Get name of port.
Definition: BufferedPort-inl.h:108
yarp::os::Port::read
bool read(PortReader &reader, bool willReply=false) override
Read an object from the port.
Definition: Port.cpp:475
Pcl.h
yarp::os::BufferedPort::write
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
Definition: BufferedPort-inl.h:126
yarp::sig::IntrinsicParams
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).
Definition: IntrinsicParams.h:44
yarp::os::Time::delay
void delay(double seconds)
Wait for a certain number of seconds.
Definition: Time.cpp:114
yarp::os::Property
A class for storing options and configuration information.
Definition: Property.h:37