vision
YarpCloudUtils: point cloud utilities for YARP

This library contains a collection of free functions that aim to provide support for the family of yarp::sig::PointCloud<T> template specializations. For the time being, roboticslab::YarpCloudUtils allows users to:

  • save a point cloud or triangular polygon mesh to .ply file,
  • read a point cloud or triangular polygon mesh from .ply file,
  • construct a triangular polygon mesh from a point cloud, or
  • process a point cloud to obtain another cloud.

Supported data types are:

  • yarp::sig::DataXY (only for data import/export via .ply)
  • yarp::sig::DataXYZ
  • yarp::sig::DataNormal (only for data import/export via .ply)
  • yarp::sig::DataXYZRGBA
  • yarp::sig::DataXYZI
  • yarp::sig::DataInterestPointXYZ
  • yarp::sig::DataXYZNormal
  • yarp::sig::DataXYZNormalRGBA

XY and plain-normal types are not available for surface meshing and cloud processing.

Supported algorithms

Both roboticslab::YarpCloudUtils::meshFromCloud and roboticslab::YarpCloudUtils::processCloud implement a set of Point Cloud Library (PCL) algorithms. A variety of PCL classes are exposed in a highly configurable pipeline:

PCL class usage point types notes
pcl::transformPointCloud affine transformation any translation (meters) and rotation (scaled axis-angle, radians)
pcl::transformPointCloudWithNormals affine transformation any normal type see above
pcl::ApproximateVoxelGrid cloud resampling any
pcl::BilateralFilter cloud filtering XYZI(Normal)
pcl::BilateralUpsampling cloud processing XYZRGBA organized clouds only
pcl::ConcaveHull mesh construction any
pcl::ConvexHull mesh construction any
pcl::CropBox cloud filtering any
pcl::FastBilateralFilter cloud filtering XYZ(RGBA) organized clouds only
pcl::FastBilateralFilterOMP cloud filtering XYZ(RGBA) organized clouds only
pcl::GreedyProjectionTriangulation mesh construction XYZ/XYZI/XYZRGBA + Normal
pcl::GridMinimum cloud resampling any
pcl::GridProjection surface reconstruction XYZ/XYZI/XYZRGBA + Normal
pcl::LocalMaximum cloud resampling any
pcl::MarchingCubesHoppe surface reconstruction XYZ/XYZI/XYZRGBA + Normal
pcl::MarchingCubesRBF surface reconstruction XYZ/XYZI/XYZRGBA + Normal
pcl::MedianFilter cloud filtering any organized clouds only
pcl::MeshQuadricDecimationVTK mesh processing (mesh)
pcl::MeshSmoothingLaplacianVTK mesh processing (mesh)
pcl::MeshSmoothingWindowedSincVTK mesh processing (mesh)
pcl::MeshSubdivisionVTK mesh processing (mesh)
pcl::MovingLeastSquares cloud processing any requires PCL 1.9+
pcl::NormalEstimation normal estimation any
pcl::NormalEstimationOMP normal estimation any
pcl::OrganizedFastMesh mesh construction XYZ(RGBA) organized clouds only
pcl::PassThrough cloud filtering any
pcl::Poisson surface reconstruction XYZ/XYZI/XYZRGBA + Normal threads requires PCL 1.12+
pcl::RadiusOutlierRemoval cloud filtering any
pcl::RandomSample cloud resampling any
pcl::SamplingSurfaceNormal cloud resampling XYZ/XYZI/XYZRGBA + Normal
pcl::ShadowPoints cloud filtering any normal type only normal types
pcl::SimplificationRemoveUnusedVertices mesh simplification (mesh)
pcl::StatisticalOutlierRemoval cloud filtering any
pcl::UniformSampling cloud resampling any
pcl::VoxelGrid cloud resampling any

Mesh construction methods preserve the original point cloud as the surface vertices and simply construct the mesh on top, while surface reconstruction changes the topology of the input cloud.

Configuration

YARP mechanisms are taken advantage of to set up the pipeline, especially in terms of the dictionary of key-values provided by yarp::os::Property. Depending on the selected function overload, each step of the pipeline corresponds either to an element of a vector of such dictionaries, or a group/dictionary within a named section collection context.

As a rule of thumb, there will always be a key named algorithm which corresponds to one of the supported PCL classes, i.e. the paired value must be equal to the class name of the selected algorithm.

Remaining keys of the configured step, if any, correspond to configuration setters pertaining to the algorithm, always following the camelCase naming convention. For instance, in order to access the pcl::Poisson::setMinDepth setter, one would create a dictionary key named minDepth and assign it an integer value. In a similar manner, dictionary values are mapped to enumerations, e.g. pcl::OrganizedFastMesh::TriangulationType:QUAD_MESH can be requested by assigning quadMesh to key triangulationType.

In absence of key-value entries (beyond the mandatory algorithm one), the pipeline assumes default values as defined by each class constructor.

Note: mind input/output types for each step. If the output of step N is not compatible with the expected input of step N+1, an error will be reported.

Vector of dictionaries

The most straightforward way to configure a pipeline is using the VectorOf<Property> overload. Each element of the vector is one step of the pipeline, order is preserved:

yarp::sig::VectorOf<yarp::os::Property> options {
{
{"algorithm", yarp::os::Value("VoxelGrid")},
{"leafSize", yarp::os::Value(0.02f)}
},
{
{"algorithm", yarp::os::Value("NormalEstimationOMP")},
{"kSearch", yarp::os::Value(40)}
},
{
{"algorithm", yarp::os::Value("Poisson")}
}
};
bool ret = roboticslab::YarpCloudUtils::meshFromCloud(cloud, vertices, indices, options);
bool meshFromCloud(const yarp::sig::PointCloud< T1 > &cloud, yarp::sig::PointCloud< T2 > &meshPoints, yarp::sig::VectorOf< int > &meshIndices, const yarp::sig::VectorOf< yarp::os::Property > &options)
Constructs a triangular polygon mesh from a point cloud.
Definition: YarpCloudUtils-pcl.cpp:286

The above pipeline will first downsample the input cloud using the pcl::VoxelGrid algorithm with a leaf size of 2 cm. Since pcl::Poisson requires an input normal type, we estimate normals with an intermediate NormalEstimation step, and then perform surface reconstruction.

Configuration context

Handy overloads are provided for both meshFromCloud and processCloud to parse the pipeline from a configuration file or similar context (e.g. command line) using the YARP native configuration format (see YARP config files).

The same behavior shown in the previous section can be achieved with an .ini file such as:

[myPipeline downsample]
algorithm "VoxelGrid"
leafSize 0.02f
[myPipeline estimate]
algorithm "NormalEstimationOMP"
kSearch 40
[myPipeline reconstruct]
algorithm "Poisson"

Then, in C++ code:

yarp::os::Property config;
config.fromConfigFile("path/to/file.ini");
bool ret = roboticslab::YarpCloudUtils::meshFromCloud(cloud, vertices, indices, config, "myPipeline");

The myPipeline element is the name of the section collection which is common to all steps in this configuration (see YARP docs). We can express this intent also directly in C++ code:

yarp::os::Property config("(myPipeline downsample estimate reconstruct)");
config.addGroup("downsample") = {
{"algorithm", yarp::os::Value("VoxelGrid")},
{"leafSize", yarp::os::Value(0.02f)}
};
config.addGroup("estimate") = {
{"algorithm", yarp::os::Value("NormalEstimationOMP")},
{"kSearch", yarp::os::Value(40)}
};
config.addGroup("reconstruct") = {
{"algorithm", yarp::os::Value("Poisson")}
};
bool ret = roboticslab::YarpCloudUtils::meshFromCloud(cloud, vertices, indices, config, "myPipeline");

Or, via command line:

application --myPipeline downsample estimate reconstruct \
--downsample::algorithm VoxelGrid --downsample::leafSize 0.02f \
--estimate::algorithm NormalEstimationOMP --estimate::kSearch 40 \
--reconstruct::algorithm Poisson
yarp::os::Property config;
config.fromCommand(argc, argv);
bool ret = roboticslab::YarpCloudUtils::meshFromCloud(cloud, vertices, indices, config, "myPipeline");

In case you want to maintain a collection of pipeline configurations, each one placed within its own .ini file, then select the most appropriate for the task at hand, it is advised to use the include feature combined with simultaneous section collection (docs). Refer to YARP documentation for further examples and a similar functionality related to directory inclusion.

See also

Bartek Łukawski, Alberto Rodríguez-Sanz, Elisabeth Menendez, Juan G. Victores, and Carlos Balaguer. A user-friendly point cloud processing pipeline for interfacing PCL with YARP. In XLV Jornadas de Automática. Universidade da Coruña, 2024.

```bibtex @inproceedings{lukawski2024jjaa, author = {{\L}ukawski, Bartek and Rodríguez-Sanz, Alberto and Menendez, Elisabeth and Victores, Juan G. and Balaguer, Carlos}, title = {A user-friendly point cloud processing pipeline for interfacing {PCL} with {YARP}}, booktitle = {XLV Jornadas de Automática}, year = {2024}, publisher = {Universidade da Coruña}, doi = {10.17979/ja-cea.2024.45.10925} }