search

roboticslab-uc3m / vision

Vision processing
https://robots.uc3m.es/vision/
11 stars 7 forks source link

Create new library of cloud-related YARP utilities #102

Closed PeterBowman closed 3 years ago

PeterBowman commented 3 years ago

Following on https://github.com/roboticslab-uc3m/vision/issues/101#issuecomment-729749286, I'd like to propose a new shared library of free functions focused on operations on point clouds. The point here is to interface with YARP classes, therefore I propose the name YarpCloudUtils. Functionalities envisioned so far include (temp names):

While YARP_pcl exposes two handy methods, savePCD and loadPCD, note the PCD format is (perhaps too) closely related to PCL. I'd rather work with PLY files as those can be probably read by a wider range of software, starting with Meshlab and Windows' Paint3D. Anyway, in case of need, use pcl_viewer and pcl_pcd2ply/pcl_pcl2pcd to perform the necessary conversions.

It is worth noting that we could still internally take advantage of PCL: pcl::io::savePLYFile. I'm prone to offer two alternative implementations of these functions: one that uses PCL if available, the other is basically hand-crafted file import/export in ASCII-based PLY. Of course, stuff such as meshFromCloud is only feasible with an underlying PCL implementation.

PeterBowman commented 3 years ago

I really want to provide a simple way to export clouds generated by sceneReconstruction (our new Kinect Fusion service). It makes little sense to create specific RPC commands for that purpose, chiefly because it would not store the data on the client's PC but rather on the server executing sceneReconstruction. So, a simple client app would do. It might also cover the cloud-to-mesh conversion case and saving clouds directly into .ply as polygonal meshes.

PeterBowman commented 3 years ago

I'm prone to offer two alternative implementations of these functions: (...), the other is basically hand-crafted file import/export in ASCII-based PLY.

To avoid reinventing the wheel, there are some valuable FOSS alternatives:

See also http://paulbourke.net/dataformats/ply/.

PeterBowman commented 3 years ago

The following program shows a sample usage of all supported specializations of savePLY and loadPLY regarding point clouds and meshes. Mind https://github.com/robotology/yarp/pull/2417 regarding the toString instructions for RGBA values to be printed correctly. It is also useful for illustrating how yarp::sig::PointCloud elements can be constructed and pushed into the cloud via aggregate initialization.

cmake_minimum_required(VERSION 3.12 FATAL_ERROR)
project(vision-cloud-utils LANGUAGES CXX)
find_package(ROBOTICSLAB_VISION REQUIRED)
add_executable(${PROJECT_NAME} main.cpp)
target_link_libraries(${PROJECT_NAME} ROBOTICSLAB::YarpCloudUtils)
#undef NDEBUG
#include <cassert>
#include <chrono>
#include <iostream>
#include <YarpCloudUtils.hpp>

#define ITER for (int i = 0; i < 5; i++)

int main()
{
    const yarp::sig::VectorOf<int> verts {5, 1, 3, 7, 2, 9, 4, 2, 3};

    yarp::sig::PointCloudXY pcXY;
    ITER pcXY.push_back({1.5f, 2.5f});
    roboticslab::YarpCloudUtils::savePLY("xy.ply", pcXY, verts, false);

    yarp::sig::PointCloudXYZ pcXYZ;
    ITER pcXYZ.push_back({1.5f, 2.5f, 3.5f});
    roboticslab::YarpCloudUtils::savePLY("xyz.ply", pcXYZ, verts, false);

    yarp::sig::PointCloudNormal pcNormal;
    ITER pcNormal.push_back({{1.5f, 2.5f, 3.5f}, 4.5f});
    roboticslab::YarpCloudUtils::savePLY("normal.ply", pcNormal, verts, false);

    yarp::sig::PointCloudXYZRGBA pcXYZRGBA;
    ITER pcXYZRGBA.push_back({{1.5f, 2.5f, 3.5f}, {10, 11, 12, 13}});
    roboticslab::YarpCloudUtils::savePLY("xyzrgba.ply", pcXYZRGBA, verts, true);

    yarp::sig::PointCloudXYZI pcXYZI;
    ITER pcXYZI.push_back({{1.5f, 2.5f, 3.5f}, 4.5f});
    roboticslab::YarpCloudUtils::savePLY("xyzi.ply", pcXYZI, verts, false);

    yarp::sig::PointCloudInterestPointXYZ pcXYZinterest;
    ITER pcXYZinterest.push_back({{1.5f, 2.5f, 3.5f}, 4.5f});
    roboticslab::YarpCloudUtils::savePLY("xyzint.ply", pcXYZinterest, verts, false);

    yarp::sig::PointCloudXYZNormal pcXYZNormal;
    ITER pcXYZNormal.push_back({{1.5f, 2.5f, 3.5f}, {4.5f, 5.5f, 6.5f}, 7.5f});
    roboticslab::YarpCloudUtils::savePLY("xyznormal.ply", pcXYZNormal, verts, false);

    yarp::sig::PointCloudXYZNormalRGBA pcXYZNormalRGBA;
    ITER pcXYZNormalRGBA.push_back({{1.5f, 2.5f, 3.5f}, {4.5f, 5.5f, 6.5f}, {10, 11, 12, 13, 7.5f}});
    roboticslab::YarpCloudUtils::savePLY("xyznormalrgba.ply", pcXYZNormalRGBA, verts, false);

    yarp::sig::PointCloudXY pcXY2;
    yarp::sig::VectorOf<int> vertsXY;
    assert(roboticslab::YarpCloudUtils::loadPLY("xy.ply", pcXY2, vertsXY));
    std::cout << "[1]" << pcXY2.toString(1) << std::endl << vertsXY.toString() << std::endl;

    yarp::sig::PointCloudXYZ pcXYZ2;
    yarp::sig::VectorOf<int> vertsXYZ;
    assert(roboticslab::YarpCloudUtils::loadPLY("xyz.ply", pcXYZ2, vertsXYZ));
    std::cout << "[2]" << pcXYZ2.toString(1) << std::endl << vertsXYZ.toString() << std::endl;

    yarp::sig::PointCloudNormal pcNormal2;
    yarp::sig::VectorOf<int> vertsNormal;
    assert(roboticslab::YarpCloudUtils::loadPLY("normal.ply", pcNormal2, vertsNormal));
    std::cout << "[3]" << pcNormal2.toString(1) << std::endl << vertsNormal.toString() << std::endl;

    yarp::sig::PointCloudXYZRGBA pcXYZRGBA2;
    yarp::sig::VectorOf<int> vertsXYZRGBA;
    assert(roboticslab::YarpCloudUtils::loadPLY("xyzrgba.ply", pcXYZRGBA2, vertsXYZRGBA));
    std::cout << "[4]" << pcXYZRGBA2.toString(1) << std::endl << vertsXYZRGBA.toString() << std::endl;

    yarp::sig::PointCloudXYZI pcXYZI2;
    yarp::sig::VectorOf<int> vertsXYZI;
    assert(roboticslab::YarpCloudUtils::loadPLY("xyzi.ply", pcXYZI2, vertsXYZI));
    std::cout << "[5]" << pcXYZI2.toString(1) << std::endl << vertsXYZI.toString() << std::endl;

    yarp::sig::PointCloudInterestPointXYZ pcXYZinterest2;
    yarp::sig::VectorOf<int> vertsXYZInterest;
    assert(roboticslab::YarpCloudUtils::loadPLY("xyzint.ply", pcXYZinterest2, vertsXYZInterest));
    std::cout << "[6]" << pcXYZinterest2.toString(1) << std::endl << vertsXYZInterest.toString() << std::endl;

    yarp::sig::PointCloudXYZNormal pcXYZNormal2;
    yarp::sig::VectorOf<int> vertsXYZNormal;
    assert(roboticslab::YarpCloudUtils::loadPLY("xyznormal.ply", pcXYZNormal2, vertsXYZNormal));
    std::cout << "[7]" << pcXYZNormal2.toString(1) << std::endl << vertsXYZNormal.toString() << std::endl;

    yarp::sig::PointCloudXYZNormalRGBA pcXYZNormalRGBA2;
    yarp::sig::VectorOf<int> vertsXYZNormalRGBA;
    assert(roboticslab::YarpCloudUtils::loadPLY("xyznormalrgba.ply", pcXYZNormalRGBA2, vertsXYZNormalRGBA));
    std::cout << "[8]" << pcXYZNormalRGBA2.toString(1) << std::endl << vertsXYZNormalRGBA.toString() << std::endl;

    return 0;
}
PeterBowman commented 3 years ago

Tinyply presents two major shortcomings (especially wrt. the PCL implementation I want to align with):

All of this concerns PLY data import. Exporting data to a .ply file is pretty easy and needs not to involve tinyply if we don't want to.

Edit: reported at https://github.com/ddiakopoulos/tinyply/issues/48.

PeterBowman commented 3 years ago

On my machine, reading an ASCII 31M .ply file of XYZ points takes around 1670 ms with PCL and 750 ms with tinyply AToW.

PeterBowman commented 3 years ago

Tinyply presents two major shortcomings...

On my machine, reading an ASCII 31M .ply file of XYZ points takes around 1670 ms with PCL and 750 ms with tinyply AToW.

Fixed at https://github.com/roboticslab-uc3m/vision/commit/5873b7c739003486dc9f0d534097f869b3583e5f and no perf drop observed. It's even less memory-consuming since no copies into intermediary buffers are involved.

PeterBowman commented 3 years ago

Note PCL ignores the "intensity" and "strength" properties for pcl::PointXYZI and pcl::InterestPoint, respectively, when saving meshes. Also, loading meshes with types pcl::PointXY or pcl::Normal is not possible.

PeterBowman commented 3 years ago

This table compares the performance of PCL versus tinyply, tested with an XYZ cloud of 1525437 points (using the sceneReconstruction sample client). Cell values represent average elapsed time in milliseconds. The table also includes a variation of the tinyply PLY reader using buffered streams, inspired by this tinyply example, which allegedly yields a 40% parsing speed improvement for most files smaller than 1 GB.

ASCII binary
write
 read write read
PCL 1220 2070 135 130
tinyply 1140 1010 80 150
tinyply
(buffered)		 1020 130

For reference, file sizes range from 44-51 MB (ASCII) to 18 MB (binary). Given the above results, I'm prone to drop the PCL implementation in favor of the buffered tinyply solution.

(PS generated with https://www.tablesgenerator.com/html_tables)

PeterBowman commented 3 years ago

Given the above results, I'm prone to drop the PCL implementation in favor of the buffered tinyply solution.

Done at https://github.com/roboticslab-uc3m/vision/commit/6d6cf427f4a0f21c0065e46c8c623824b61587f8. For the record, I'd like to link here to the deleted sample usage of type traits for future reference:

https://github.com/roboticslab-uc3m/vision/blob/d0d62eda6d4832a2e28ce81d15d101b6a6506238/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp#L14-L47

https://github.com/roboticslab-uc3m/vision/blob/d0d62eda6d4832a2e28ce81d15d101b6a6506238/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp#L95-L96

Also, a bunch of interesting decltypes:

https://github.com/roboticslab-uc3m/vision/blob/d0d62eda6d4832a2e28ce81d15d101b6a6506238/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp#L59-L61

PeterBowman commented 3 years ago

Cloud downsampling (pcl::Filter, pcl::FilterIndices, module filters):

Cloud filtering (pcl::Filter, pcl::FilterIndices, module filters):

Cloud surface processing (pcl::CloudSurfaceProcessing, module surface):

Normal estimation (pcl::Feature, module features):

Surface meshing techniques (module surface):

Mesh processing (pcl::MeshProcessing, module surface):

Removal of unused vertices from mesh: pcl::SimplificationRemoveUnusedVertices (header, impl).

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. In any case, if the downsampling step is enabled, the original cloud will be altered anyway.

BTW all VTK-based methods return dense vertex clouds.

PeterBowman commented 3 years ago

I'm introducing three new C++ example apps related to cloud generation and meshing:

The live sensor app requires https://github.com/robotology/yarp/pull/2425 for ROI/decimation of input frames (targetting YARP 3.4.2).

PeterBowman commented 3 years ago

Not sure if I'm ever going to consider this seriously, but instead of a one-pass omnibus free function I could wrap the whole pipeline in a class and, using the builder pattern for configuration, grant the user better control of the underlying pipeline:

yarp::os::Property cropOpts, downsampleOpts, smoothOpts, reconstructOpts, processOpts;
yarp::sig::PointCloud input, output;
yarp::sig::VectorOf<int> indices;

auto mesher = SurfaceMeshingBuilder::createInstance()
    .setInputCloud(input)
    .setOutputCloud(output) // optional for mesh construction, mandatory for surface reconstruction
    .addCropStep(cropOpts)
    .addDownsampleStep(downsampleOpts)
    .addSmoothStep(smoothOpts)
    .addReconstructStep(reconstructOpts) // required
    .addProcessStep(processOpts);

bool ret = mesher.doWork(indices);

// perhaps add individual steps, too, for better user control? e.g.:
// mesher.crop(...);
PeterBowman commented 3 years ago

I could wrap the whole pipeline in a class and, using the builder pattern for configuration, grant the user better control of the underlying pipeline

Done at https://github.com/roboticslab-uc3m/vision/commit/ba9ed6913bf79105dba1f945ab2f9f6808649e6a, but quite far from my original intention. I realized that the class that shall represent the result of the builder mechanism would only have one method. It's equivalent to the current status quo of having a one-pass free function that accepts a parameter for internal configuration, so I'm just sticking to it. The new SurfaceMeshingBuilder class encapsulates the configuration of each step of the internal pipeline and produces a yarp::os::Property, which can fit into meshFromCloud as the configuration parameter. Long story short, its usage is optional, it's just a convenient way of initializing the kinda large amount of parameters that can derive from said pipeline.

PeterBowman commented 3 years ago

At first, I was enforcing that input and output clouds (i.e. input point cloud and output vertex cloud) must share the same point type:

https://github.com/roboticslab-uc3m/vision/blob/18e3db6d3eb6d7c733ab9a26bf94407b462e6660/libraries/YarpCloudUtils/YarpCloudUtils.hpp#L46-L56

Then, I allowed the instantiation of distinct type arguments for yarp::sig::PointCloud<T> as input and output:

https://github.com/roboticslab-uc3m/vision/blob/a04274fe235b88ff05af0aaf81412978db274551/libraries/YarpCloudUtils/YarpCloudUtils.hpp#L46-L56

This is motivated by the fact that certain inputs, e.g. DataXIZ, are stripped from their additional fields throughout the pipeline. It is therefore of little use to populate the vertex cloud with so many null (zero) fields. It is expected that clients invoke this function template with at least the same input/output types or narrower ones (e.g. input is XYZI, but output is XYZ), thus avoiding those null fields and some warnings from the PCL side. It always depends on the selected algorithm, thus the proper decision must be taken on compile time attending to the expected behavior on run time. Of course, nothing prevents downstreams from requesting wider point types (e.g. input is XYZ, output is XYZRGBA), which is a waste of resources due to the consequent null fields. I'd rather grant better flexibility at the cost of potentially producing a less-optimal result if not taken proper care of. Also, https://github.com/roboticslab-uc3m/vision/commit/2835973ac89372e300c4b1a4f6599539d8f10d05 aims to signalize that at least the same point type is preferred.

The major drawback of this decision was the list of template instantiations exponentially growing on the number of allowed point types:

https://github.com/roboticslab-uc3m/vision/blob/a04274fe235b88ff05af0aaf81412978db274551/libraries/YarpCloudUtils/YarpCloudUtils-inst.hpp#L16-L86

Binaries have become only slightly bigger, I'm not worried about that.

PeterBowman commented 3 years ago

I've reworked the implementation (https://github.com/roboticslab-uc3m/vision/commit/8a8223c16a24faee474f29e226b062d1524dd6b5), thus gaining much more flexibility. Next, I allowed clients to configure their custom pipeline via vector of properties (each element is another step in said pipeline):

https://github.com/roboticslab-uc3m/vision/blob/4cb018b74ce6b976f388a324cc9c336f409cf46d/libraries/YarpCloudUtils/YarpCloudUtils.hpp#L45-L55

The new SurfaceMeshingBuilder class encapsulates the configuration of each step of the internal pipeline...

Removed at https://github.com/roboticslab-uc3m/vision/commit/e63e69ea25f95a30e6941a1c8dd2b5b3c413f519. I'm providing an overload intended for parsing section collections, especially via .ini config files:

https://github.com/roboticslab-uc3m/vision/blob/e63e69ea25f95a30e6941a1c8dd2b5b3c413f519/libraries/YarpCloudUtils/YarpCloudUtils.hpp#L56-L61

PeterBowman commented 3 years ago

I'm not adding bindings because YARP doesn't support translation of yarp::sig::PointCloud into Python. Also, stuff like VectorOf<Property> would need special treatment (ideally, list of dictionaries).

PeterBowman commented 3 years ago

Commit https://github.com/roboticslab-uc3m/vision/commit/68e6e60cb5dfbbccba2bbd3ee63cbef1afb12b2f added a volatile dummy variable to avoid a (corner?) case of aggressive compiler optimization. Why is that:

This is the reason why I'm using all available type instantiations in the unit tests:

https://github.com/roboticslab-uc3m/vision/blob/f0bbb9dc434b463a115d5da7ea064a81291c094d/tests/testYarpCloudUtils.cpp#L456-L548

PeterBowman commented 3 years ago

Done at https://github.com/roboticslab-uc3m/vision/commit/2c8786191bf8232694a3c831c9d216fbb803f771. In addition to cloud/mesh import/export and cloud2mesh, I also added plain cloud processing so that a new point cloud can be obtained through any of the 33 algorithms featured in this library (those that output another cloud, of course).

Documentation:

Examples:

PeterBowman commented 3 years ago

Follow-up: https://github.com/roboticslab-uc3m/vision/commit/a0ac04febce90546b1e02fb0aa030b5b7bdd0c3e introduces transformPointCloud and transformPointCloudWithNormals. These functions perform an affine transformation (translation+rotation) on a point cloud.

Ideally, I'd like to apply this step on the resulting surface mesh. Since these functions only accept pcl::PointCloud<T> instead of pcl::PolygonMesh, it would be possible to make meshFromCloudPCL return a pair of cloud+indices instead of this strange ROS-compatible structure, also sparing a few conversions between data representations. Sadly, all mesh processing algorithms (the VTK ones) plus pcl::surface::SimplificationRemoveUnusedVertices only accept pcl::PolygonMesh, therefore it could get even worse due to the need for converting back and forth between this creature and pcl::PointCloud<T>.

At the end, copying clouds might not be that inefficient in view of the time-consuming filter and reconstruction steps. On the other hand, applying a transformation to the input point cloud might not be either. My idea was to perform this step at the end of the pipeline considering that the resulting vertex cloud could be much smaller due to downsampling/decimation, hence a (possibly) substantial reduction of compute time as opposed to doing this right at the beginning on the input cloud. Again, premature optimization is the root of all evil.