PointCloud in YARP

cmake/YarpDescribe.cmake

@@ -43,6 +43,7 @@ foreach(lib ${YARP_LIBS})
      NOT "${lib}" STREQUAL "yarpmod" AND
      NOT "${lib}" STREQUAL "YARP_wire_rep_utils" AND
      NOT "${lib}" STREQUAL "YARP_manager" AND
+     NOT "${lib}" STREQUAL "YARP_pcl" AND
      NOT "${lib}" STREQUAL "YARP_profiler" AND
      NOT "${lib}" STREQUAL "YARP_logger" AND
      NOT "${lib}" STREQUAL "YARP_serversql" AND

doc/main.dox

@@ -108,6 +108,7 @@ periodically check it.
 \li \subpage portmonitor
 \li \subpage yarp_env_variables
 \li \subpage rtf_plugins
+\li \subpage yarp_pointcloud
 
 \section yarp_more_info More information
 \li \subpage what_is_yarp

doc/release/v3_0_0.md

@@ -31,6 +31,7 @@ Important Changes
 #### `YARP_sig`
 
 * The file `yarp/sig/IplImage.h` is deprecated, use opencv headers instead.
+* Added the `yarp::sig::PointCloud` class. See the documentation for details.
 
 #### `YARP_dev`
 

doc/tutorials.dox

@@ -49,6 +49,7 @@ Using YARP devices:
 \li \subpage yarp_config_files
 
 More software tutorials:
+\li \subpage yarp_pointcloud
 \li \subpage yarp_code_examples
 \li \subpage gpu_tutorial
 \li \subpage using_cmake

doc/yarp_pointcloud.dox

@@ -0,0 +1,153 @@
+/**
+  @page yarp_pointcloud YARP PointCloud
+
+  \author Nicolo' Genesio
+
+This tutorial covers how to use the template class yarp::sig::PointCloud with pcl::PointCloud and stand alone.
+
+\section what_is_a_pointcloud 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.
+
+\section read_and_write_point_cloud 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.
+
+\code {.cpp}
+        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);
+\endcode
+**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 `XYZ`fields will be filled
+- the `Normal` fields will be set to default
+- the`RGBA` will be ignored
+
+\section general_usage_point_cloud PointCloud initialization
+
+The flexibility on types has been implemented also for what concerns the 
+initialization of the point cloud (**copy constructor**
+and **assignment operator**):
+
+\code {.cpp}
+        yarp::sig::PointCloud<yarp::sig::DataXYZNormalRGBA> pc1;
+        yarp::sig::PointCloud<yarp::sig::DataXYZNormal> pc2(pc1);
+        yarp::sig::PointCloud<yarp::sig::DataXYZNormalRGBA> pc3 = pc2;
+\endcode
+
+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.
+
+\section pcl_compatibility 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
+
+\code {.cpp}
+    // 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);
+\endcode
+
+Note that these conversions (PCL -> YARP, YARP -> PCL) are made through a **copy**.
+
+\section yarp_pcd 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:
+
+\code {.cpp}
+    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);
+\endcode
+
+Note that these functions implicitly convert to and from PCL types.
+
+*/

src/CMakeLists.txt

@@ -19,6 +19,7 @@ add_subdirectory(libYARP_gsl)
 add_subdirectory(libYARP_math)
 add_subdirectory(libYARP_rosmsg)
 add_subdirectory(libYARP_dev)
+add_subdirectory(libYARP_pcl)
 
 # private libraries
 add_subdirectory(libYARP_name)

src/libYARP_pcl/CMakeLists.txt

@@ -0,0 +1,34 @@
+# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
+# All rights reserved.
+#
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license. See the accompanying LICENSE file for details.
+
+project(YARP_pcl)
+
+set(YARP_pcl_HDRS include/yarp/pcl/Pcl.h)
+
+source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}"
+             PREFIX "Header Files"
+             FILES ${YARP_pcl_HDRS})
+
+set_property(GLOBAL APPEND PROPERTY YARP_TREE_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/include")
+
+add_library(YARP_pcl INTERFACE)
+add_library(YARP::YARP_pcl ALIAS YARP_pcl)
+
+target_include_directories(YARP_pcl INTERFACE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+                                                  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+                                                  ${PCL_INCLUDE_DIR})
+target_link_libraries(YARP_pcl INTERFACE YARP_sig ${PCL_LIBRARIES})
+
+
+install(TARGETS YARP_pcl
+        EXPORT YARP
+        COMPONENT YARP_pcl
+        PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/yarp/pcl)
+
+install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/yarp/pcl
+        DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/yarp)
+
+set_property(GLOBAL APPEND PROPERTY YARP_LIBS YARP_pcl)

src/libYARP_pcl/include/yarp/pcl/Pcl.h

@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#ifndef YARP_PCL_PCL_H
+#define YARP_PCL_PCL_H
+
+#include <pcl/io/pcd_io.h>
+#include <yarp/sig/PointCloud.h>
+
+namespace yarp
+{
+namespace pcl
+{
+
+/**
+ * Convert a yarp::sig::PointCloud to a pcl::PointCloud object
+ * @param[in] yarpCloud yarp::sig::PointCloud input
+ * @param[out] pclCloud pcl::PointCloud filled with data contained in the yarp cloud
+ * @return true on success, false otherwise.
+ */
+template< class T1, class T2 >
+inline bool toPCL(const yarp::sig::PointCloud< T1 > &yarpCloud, ::pcl::PointCloud< T2 > &pclCloud)
+{
+    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::toPcl: T1 and T2 are incompatible");
+    pclCloud.points.resize(yarpCloud.size());
+    pclCloud.width  = yarpCloud.width();
+    pclCloud.height = yarpCloud.height();
+    memcpy((char*)& pclCloud.points.at(0), yarpCloud.getRawData(), yarpCloud.dataSizeBytes());
+    return true;
+}
+
+/**
+ * Convert a pcl::PointCloud to a yarp::sig::PointCloud object
+ * @param[in] pclCloud pcl::PointCloud input
+ * @param[out] yarpCloud yarp cloud filled with data contained in the pcl cloud.
+ * @return true on success, false otherwise.
+ */
+template< class T1, class T2 >
+inline bool fromPCL(const ::pcl::PointCloud< T1 > &pclCloud, yarp::sig::PointCloud< T2 > &yarpCloud)
+{
+    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::fromPCL: T1 and T2 are incompatible");
+    yarpCloud.fromExternalPC((char*) &pclCloud(0,0), yarpCloud.getPointType(), pclCloud.width, pclCloud.height, pclCloud.is_dense);
+    return true;
+}
+
+/**
+ * Save a yarp::sig::PointCloud to PCD file, ASCII format
+ * @param[in] file_name name of the file to be created wth the cloud
+ * @param[in] yarpCloud yarp::sig::PointCloud input
+ * @return result of the save operation(see pcl::io::savePCDFile documentation)
+ */
+template< class T1, class T2 >
+inline int savePCD(const std::string &file_name, const yarp::sig::PointCloud< T1 > &yarpCloud)
+{
+    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::savePCD: T1 and T2 are incompatible");
+    ::pcl::PointCloud<T2> pclCloud(yarpCloud.width(), yarpCloud.height());
+    yarp::pcl::toPCL< T1, T2 >(yarpCloud, pclCloud);
+    return ::pcl::io::savePCDFile(file_name, pclCloud);
+}
+
+/**
+ * Load a yarp::sig::PointCloud from a PCD file, ASCII format
+ * @param[in] file_name of the PCD file containing the cloud
+ * @param[out] yarpCloud yarp::sig::PointCloud obtained from the PCD file
+ * @return result of the load operation(see pcl::io::loadPCDFile documentation)
+ */
+template< class T1, class T2 >
+inline int loadPCD(const std::string &file_name, yarp::sig::PointCloud<T2> &yarpCloud)
+{
+    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::loadPCD: T1 and T2 are incompatible");
+    ::pcl::PointCloud<T1> pclCloud;
+    int ret = ::pcl::io::loadPCDFile(file_name, pclCloud);
+    yarp::pcl::fromPCL< T1, T2 >(pclCloud, yarpCloud);
+    return ret;
+}
+
+} // namespace pcl
+} // namespace yarp
+
+
+#endif

src/libYARP_sig/CMakeLists.txt

@@ -14,6 +14,10 @@ set(YARP_sig_HDRS include/yarp/sig/all.h
                   include/yarp/sig/Image.h
                   include/yarp/sig/ImageNetworkHeader.h
                   include/yarp/sig/Matrix.h
+                  include/yarp/sig/PointCloud.h
+                  include/yarp/sig/PointCloudBase.h
+                  include/yarp/sig/PointCloudNetworkHeader.h
+                  include/yarp/sig/PointCloudTypes.h
                   include/yarp/sig/SoundFile.h
                   include/yarp/sig/Sound.h
                   include/yarp/sig/Vector.h)
@@ -30,6 +34,7 @@ set(YARP_sig_SRCS src/ImageCopy.cpp
                   src/ImageFile.cpp
                   src/IplImage.cpp
                   src/Matrix.cpp
+                  src/PointCloudBase.cpp
                   src/Sound.cpp
                   src/SoundFile.cpp
                   src/Vector.cpp