diff --git a/cmake/YarpDescribe.cmake b/cmake/YarpDescribe.cmake index e41f9f93677..5a90c453fee 100644 --- a/cmake/YarpDescribe.cmake +++ b/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 diff --git a/doc/main.dox b/doc/main.dox index 996036595e8..5db6c0af64d 100644 --- a/doc/main.dox +++ b/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 diff --git a/doc/release/v3_0_0.md b/doc/release/v3_0_0.md index 6fb10111726..2d91b719daa 100644 --- a/doc/release/v3_0_0.md +++ b/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` diff --git a/doc/tutorials.dox b/doc/tutorials.dox index 2d54bb9fba5..615f0950db7 100644 --- a/doc/tutorials.dox +++ b/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 diff --git a/doc/yarp_pointcloud.dox b/doc/yarp_pointcloud.dox new file mode 100644 index 00000000000..73a47b3daf3 --- /dev/null +++ b/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 > 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& testPC = outPort.prepare(); + int width = 100; + int height = 20; + testPC.resize(width, height); + // Fill the point cloud + for (int i=0; i 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` and you +want to read through a `yarp::sig::PointCloud`: +- 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 pc1; + yarp::sig::PointCloud pc2(pc1); + yarp::sig::PointCloud 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(...)` and `fromPcl(...)` +that can be used as follow + +\code {.cpp} + // Include the compatibility library + #include + + pcl::PointCloud cloud; + cloud.resize(100); + for(int i=0; i YARP + yarp::sig::PointCloud yarpCloud; + yarp::pcl::fromPCL(cloud, yarpCloud); + // YARP -> PCL + pcl::PointCloud cloud2; + yarp::pcl::toPCL(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(...)` and `loadPCD(...)` can be used as follows: + +\code {.cpp} + yarp::sig::PointCloud 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 cloud2; + result = yarp::pcl::loadPCD< pcl::PointXYZRGBA, yarp::sig::DataXYZRGBA >(filename, cloud2); +\endcode + +Note that these functions implicitly convert to and from PCL types. + +*/ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 29730cd9e2c..894b744062e 100644 --- a/src/CMakeLists.txt +++ b/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) diff --git a/src/libYARP_pcl/CMakeLists.txt b/src/libYARP_pcl/CMakeLists.txt new file mode 100644 index 00000000000..321274b11c2 --- /dev/null +++ b/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 $ + $ + ${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) diff --git a/src/libYARP_pcl/include/yarp/pcl/Pcl.h b/src/libYARP_pcl/include/yarp/pcl/Pcl.h new file mode 100644 index 00000000000..ecd6ae1cc02 --- /dev/null +++ b/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 +#include + +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 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 &yarpCloud) +{ + static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::loadPCD: T1 and T2 are incompatible"); + ::pcl::PointCloud pclCloud; + int ret = ::pcl::io::loadPCDFile(file_name, pclCloud); + yarp::pcl::fromPCL< T1, T2 >(pclCloud, yarpCloud); + return ret; +} + +} // namespace pcl +} // namespace yarp + + +#endif diff --git a/src/libYARP_sig/CMakeLists.txt b/src/libYARP_sig/CMakeLists.txt index 9b0f488fdb7..531a35ba8f1 100644 --- a/src/libYARP_sig/CMakeLists.txt +++ b/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 diff --git a/src/libYARP_sig/include/yarp/sig/PointCloud.h b/src/libYARP_sig/include/yarp/sig/PointCloud.h new file mode 100644 index 00000000000..6b54df90494 --- /dev/null +++ b/src/libYARP_sig/include/yarp/sig/PointCloud.h @@ -0,0 +1,489 @@ +/* + * 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_SIG_POINTCLOUD_H +#define YARP_SIG_POINTCLOUD_H + +#include +#include + + + +namespace yarp { +namespace sig { + +template +/** + * @brief The PointCloud class. + */ +class PointCloud : public PointCloudBase +{ + static_assert(std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value || + std::is_same::value, "yarp::sig::PointCloud: T chosen is not supported"); +public: + + /** + * @brief PointCloud, default constructor. + */ + PointCloud() + { + data.clear(); + setPointType(); + } + + /** + * @brief PointCloud, copy constructor. + * Clones the content of another point cloud. + * @param alt the point cloud to clone. + */ + template + PointCloud(const PointCloud& alt) + { + setPointType(); + copy(alt); + } + + /** + * @brief Resize the PointCloud. + * @param width. + * @param height. + */ + virtual void resize(size_t width, size_t height) + { + header.width = width; + header.height = height; + data.resize(width * height); + } + + /** + * @brief Resize the PointCloud. + * @note This function resizes the point cloud and has the side effect + * of reducing the height to 1. Thus, by definition, the point cloud + * becomes unorganized. + * @param width. + */ + virtual void resize(size_t width) + { + header.width = width; + header.height = 1; + data.resize(width); + } + + virtual const char* getRawData() const override + { + return data.getMemoryBlock(); + } + + /** + * @brief Get the size of the data + the header in terms of + * number of bytes. + * @return the size of the data sent through the network + */ + virtual size_t wireSizeBytes() const override + { + return sizeof(header) + dataSizeBytes(); + } + + /** + * @brief Get the size of the data in terms of + * number of bytes. + * @return the size of the data + */ + virtual size_t dataSizeBytes() const override + { + return header.width * header.height * (sizeof(T)); + } + + virtual size_t size() const override + { + return data.size(); + } + + /** + * @brief Obtain the point given by the (column, row) coordinates. + * Only works on organized clouds (those that have height != 1). + * @param u, column coordinate + * @param v, row coordinate + */ + inline T& operator()(size_t u, size_t v) + { + yAssert(isOrganized()); + if (u > width() || v > height()) { + return nulldata; + } + return data[u + v * width()]; + } + + /** + * @brief Obtain the point given by the index. + * @param i, index + */ + inline T& operator()(size_t i) + { + if (i > data.size()) { + return nulldata; + } + return data[i]; + } + + template + /** + * Assignment operator. + * @brief Clones the content of another image. + * @param alt the image to clone + */ + const PointCloud& operator=(const PointCloud& alt) + { + copy(alt); + return *this; + } + + + /** + * @brief Concatenate a point cloud to the current cloud. + * @param rhs the cloud to add to the current cloud + * @return the new cloud as a concatenation of the current cloud and the new given cloud + */ + inline PointCloud& + operator+=(const PointCloud& rhs) + { + + yAssert(getPointType() == rhs.getPointType()); + + size_t nr_points = data.size(); + data.resize(nr_points + rhs.size()); + for (size_t i = nr_points; i < data.size(); ++i) { + data[i] = rhs.data[i - nr_points]; + } + + header.width = data.size(); + header.height = 1; + if (rhs.isDense() && isDense()) { + header.isDense = 1; + } else { + header.isDense = 0; + } + return (*this); + } + + /** + * @brief Concatenate a point cloud to another cloud. + * @param rhs the cloud to add to the current cloud + * @return the new cloud as a concatenation of the current cloud and the new given cloud + */ + inline const PointCloud + operator+(const PointCloud& rhs) + { + return (PointCloud(*this) += rhs); + } + + /** + * @brief Insert a new point in the cloud, at the end of the container. + * @note This breaks the organized structure of the cloud by setting the height to 1. + * @param[in] pt the point to insert. + */ + inline void push_back(const T& pt) + { + data.push_back(pt); + header.width = data.size(); + header.height = 1; + } + + /** + * @brief Clear the data. + */ + virtual inline void clear() + { + data.clear(); + header.width = 0; + header.height = 0; + } + + /** + * @brief Copy the content of an external PointCloud. + * @param source, pointer to the source data. + * @param type, enum representing the type of the source cloud. + * @param width, width of the source cloud. + * @param height, height of the source cloud. + * @param isDense + */ + virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true) + { + yAssert(source); + header.isDense = isDense; + resize(width, height); + if (this->getPointType() == type) { + memcpy(const_cast(getRawData()), source, dataSizeBytes()); + } else { + std::vector recipe = getComposition(type); + copyFromRawData(getRawData(), source, recipe); + } + } + + + template + /** + * Copy operator + * @brief clones the content of another point cloud + * @param alt the point cloud to clone + */ + void copy(const PointCloud& alt) + { + resize(alt.width(), alt.height()); + if (std::is_same::value) { + yAssert(dataSizeBytes() == alt.dataSizeBytes()); + memcpy(const_cast(getRawData()), alt.getRawData(), dataSizeBytes()); + } else { + std::vector recipe = getComposition(alt.getPointType()); + copyFromRawData(getRawData(), alt.getRawData(), recipe); + } + } + + virtual bool read(yarp::os::ConnectionReader& connection) override + { + connection.convertTextMode(); + yarp::sig::PointCloudNetworkHeader _header; + bool ok = connection.expectBlock((char*)&_header, sizeof(_header)); + if (!ok) { + return false; + } + + data.resize(_header.height * _header.width); + std::memset((void*)data.getFirst(), 0, data.size() * sizeof(T)); + + header.height = _header.height; + header.width = _header.width; + header.isDense = _header.isDense; + + if (header.pointType == _header.pointType) { + return data.read(connection); + } + + T* tmp = data.getFirst(); + + yAssert(tmp != nullptr); + + // Skip the vector header.... + connection.expectInt(); + connection.expectInt(); + + std::vector recipe = getComposition(_header.pointType); + + yarp::os::ManagedBytes dummy; + for (size_t i = 0; i < data.size(); i++) { + for (size_t j = 0; j < recipe.size(); j++) { + size_t sizeToRead = pointType2Size(recipe[j]); + if ((header.pointType & recipe[j])) { + size_t offset = getOffset(header.pointType, recipe[j]); + connection.expectBlock((char*)&tmp[i] + offset, sizeToRead); + } else { + dummy.allocateOnNeed(sizeToRead, sizeToRead); + connection.expectBlock(dummy.bytes().get(), sizeToRead); + } + } + } + + connection.convertTextMode(); + return true; + } + + virtual bool write(yarp::os::ConnectionWriter& writer) override + { + writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader)); + return data.write(writer); + } + + virtual yarp::os::ConstString toString(int precision = -1, int width = -1) + { + yarp::os::ConstString ret; + if (isOrganized()) { + for (size_t r = 0; r < this->width(); r++) { + for (size_t c = 0; c < this->height(); c++) { + ret += (*this)(r, c).toString(precision, width); + } + if (r < this->width() - 1) // if it is not the last row + { + ret += "\n"; + } + } + + } else { + for (size_t i = 0; i < this->size(); i++) { + ret += (*this)(i).toString(precision, width); + } + } + return ret; + } + + /** + * @brief Generate a yarp::os::Bottle filled with the PointCloud data. + * @return the yarp::os::Bottle generated + */ + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addInt(width()); + ret.addInt(height()); + ret.addInt(getPointType()); + ret.addInt(isDense()); + + for (size_t i = 0; i < this->size(); i++) { + ret.addList().append((*this)(i).toBottle()); + } + return ret; + } + + /** + * @brief Populate the PointCloud from a yarp::os::Bottle + * @param[in] bt, the yarp::os::Bottle to read from. It has to be formatted in + * the same way it is generated by the toBottle() method. + * @return true for success, false otherwise + */ + + bool fromBottle(const yarp::os::Bottle& bt) + { + if (bt.isNull()) { + return false; + } + + if (this->getPointType() != bt.get(2).asInt()) { + return false; + } + + this->resize(bt.get(0).asInt(), bt.get(1).asInt()); + this->header.isDense = bt.get(3).asInt(); + + if ((size_t)bt.size() != 4 + width() * height()) { + return false; + } + + for (size_t i = 0; i < this->size(); i++) { + (*this)(i).fromBottle(bt, i + 4); + } + + return true; + } + + virtual int getBottleTag() const override + { + return BottleTagMap(); + } + +private: + yarp::sig::VectorOf data; + T nulldata; + + void setPointType() + { + if (std::is_same::value) { + header.pointType = PCL_POINT2D_XY; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_NORMAL; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ_RGBA; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ_I; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_INTEREST_POINT_XYZ; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ_NORMAL; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ_NORMAL_RGBA; + return; + } + +// DataRGBA has sense to implement them? +// intensity has sense to implement them? +// DataViewpoint has sense to implement them? + + header.pointType = 0; + } +}; + +} // namespace sig +} // namespace yarp + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + +template <> +inline int BottleTagMap() +{ + return BOTTLE_TAG_DOUBLE; +} + + +#endif // YARP_SIG_POINTCLOUD_H diff --git a/src/libYARP_sig/include/yarp/sig/PointCloudBase.h b/src/libYARP_sig/include/yarp/sig/PointCloudBase.h new file mode 100644 index 00000000000..c4810675a18 --- /dev/null +++ b/src/libYARP_sig/include/yarp/sig/PointCloudBase.h @@ -0,0 +1,113 @@ +/* + * 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_SIG_POINTCLOUDBASE_H +#define YARP_SIG_POINTCLOUDBASE_H + +#include + +#include +#include +#include + +namespace yarp { +namespace sig { + +/** + * @brief The PointCloudBase class + * + * A Base class for a yarp::sig::PointCloud, provide default implementation for + * some methods and hides some internal implementation to the user. + * It can't be instantiated, only inherited. + */ +class YARP_sig_API PointCloudBase : public yarp::os::Portable +{ +public: + virtual ~PointCloudBase() = default; + + /** + * @brief Get the size of the data + the header in terms of + * number of bytes. + * @return the size of the data sent through the network + */ + virtual size_t wireSizeBytes() const = 0; + + /** + * @brief Get the size of the data in terms of + * number of bytes. + * @return the size of the data + */ + virtual size_t dataSizeBytes() const = 0; + + /** + * @brief + * @return the number of points of the PointCloud + */ + virtual size_t size() const = 0; + + /** + * @brief Get the pointer to the data. + * @return the pointer to the data. + */ + virtual const char* getRawData() const = 0; + + virtual bool read(yarp::os::ConnectionReader& connection) override = 0; + + virtual bool write(yarp::os::ConnectionWriter& writer) override = 0; + + virtual int getBottleTag() const = 0; + + /** + * @return the height specified in the yarp::sig::PointCloudNetworkHeader. + */ + virtual size_t height() const; + + /** + * @return the width specified int the yarp::sig::PointCloudNetworkHeader. + */ + virtual size_t width() const; + + /** + * @return the enum representing the point type. + */ + virtual int getPointType() const; + + virtual yarp::os::Type getType() override; + + /** + * @return true if the point cloud is organized in an image-like structure + */ + virtual bool isOrganized() const; + + /** + * @return true if the point cloud doesn't contain NaN or Inf values + */ + virtual inline bool isDense() const + { + return header.isDense != 0; + } + +protected: + PointCloudBase() = default; + + virtual void copyFromRawData(const char* dst, const char* source, std::vector& recipe); + + virtual std::vector getComposition(int type_composite) const; + + virtual size_t pointType2Size(int type) const; + + virtual size_t getOffset(int type_composite, int type_basic) const; + + yarp::sig::PointCloudNetworkHeader header; +}; + + +} // namespace sig +} // namespace yarp + +#endif diff --git a/src/libYARP_sig/include/yarp/sig/PointCloudNetworkHeader.h b/src/libYARP_sig/include/yarp/sig/PointCloudNetworkHeader.h new file mode 100644 index 00000000000..b5f676079f5 --- /dev/null +++ b/src/libYARP_sig/include/yarp/sig/PointCloudNetworkHeader.h @@ -0,0 +1,47 @@ +/* + * 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_SIG_POINTCLOUDNETWORKHEADER_H +#define YARP_SIG_POINTCLOUDNETWORKHEADER_H + +#include + +#include + +#include + +namespace yarp { +namespace sig { + +YARP_BEGIN_PACK +/** + * @brief The yarp::sig::PointCloudNetworkHeader class + */ +class YARP_sig_API PointCloudNetworkHeader +{ +public: + PointCloudNetworkHeader() : width(0), + height(0), + pointType(0), + isDense(1) + {} + + // PCL like fields + yarp::os::NetInt32 width; + yarp::os::NetInt32 height; + yarp::os::NetInt32 pointType; // bitwise of all possible informations -> could also be int64 or just an enum, but I thin bitwise gives more freedom about all possible combinations + YARP_INT8 isDense; // the point cloud is dense if not contains NaN or Inf values + +}; +YARP_END_PACK + +} // namespace sig +} // namespace yarp + + +#endif // YARP_SIG_POINTCLOUDNETWORKHEADER_H diff --git a/src/libYARP_sig/include/yarp/sig/PointCloudTypes.h b/src/libYARP_sig/include/yarp/sig/PointCloudTypes.h new file mode 100644 index 00000000000..f24af98a263 --- /dev/null +++ b/src/libYARP_sig/include/yarp/sig/PointCloudTypes.h @@ -0,0 +1,899 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2010 Willow Garage, Inc. + * Copyright (C) 2012 Open Perception, Inc. + * 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_SIG_POINTCLOUDTYPES_H +#define YARP_SIG_POINTCLOUDTYPES_H + +#include + +#include +#include +#include + +namespace yarp { +namespace sig { + + +// Plain xy point is not supported for now ... does it make sense to have it? +// How to let the user to add his own type? + +// Define a bit for each piece of information we want to carry +// Is enum better?? Define some helper to get a string from number +/** + * @brief The PointCloudBasicTypes enum + */ +enum PointCloudBasicType : YARP_INT32 { + PC_XY_DATA = (1 << 0) , + PC_XYZ_DATA = (1 << 1) , + PC_RGBA_DATA = (1 << 2) , + PC_INTENSITY_DATA = (1 << 3) , + PC_INTEREST_DATA = (1 << 4) , // in PCL this field is also called strenght + PC_NORMAL_DATA = (1 << 5) , + PC_CURVATURE_DATA = (1 << 6) , + PC_RANGE_DATA = (1 << 7) , + PC_VIEWPOINT_DATA = (1 << 8) , + PC_MOMENT_INV_DATA = (1 << 9) , + PC_RADII_RSD_DATA = (1 << 10), + PC_BOUNDARY_DATA = (1 << 11), + PC_PRINCIPAL_CURVATURE_DATA = (1 << 12), + PC_PFH_SIGNAT_125_DATA = (1 << 13), + PC_FPFH_SIGNAT_33_DATA = (1 << 14), + PC_VFH_SIGNAT_308_DATA = (1 << 15), + PC_NARF_36_DATA = (1 << 16), + PC_BORDER_DATA = (1 << 17), + PC_INTENSITY_GRAD_DATA = (1 << 18), + PC_HISTOGRAM_DATA = (1 << 19), + PC_SCALE_DATA = (1 << 20), + PC_CONFIDENCE_DATA = (1 << 21), + PC_RADIUS_DATA = (1 << 22), + PC_USER_DEFINED = (1 << 23), + PC_PADDING2 = (1 << 24), + PC_PADDING3 = (1 << 25) +}; +/** + * @brief The PointCloudCompositeType enum + */ +enum PointCloudCompositeType : YARP_INT32 { +// Shortcuts names for matching PCL predefined types + PCL_POINT2D_XY = (PC_XY_DATA), + PCL_POINT_XYZ = (PC_XYZ_DATA), + PCL_POINT_XYZ_RGBA = (PC_XYZ_DATA | PC_RGBA_DATA | PC_PADDING3), + PCL_POINT_XYZ_I = (PC_XYZ_DATA | PC_INTENSITY_DATA), + PCL_INTEREST_POINT_XYZ = (PC_XYZ_DATA | PC_INTEREST_DATA), + PCL_NORMAL = (PC_NORMAL_DATA | PC_CURVATURE_DATA | PC_PADDING3), + PCL_POINT_XYZ_NORMAL = (PC_XYZ_DATA | PC_NORMAL_DATA | PC_CURVATURE_DATA | PC_PADDING3), + PCL_POINT_XYZ_NORMAL_RGBA = (PC_XYZ_DATA | PC_RGBA_DATA | PC_NORMAL_DATA | PC_CURVATURE_DATA | PC_PADDING2), // Actually PCL has PointXYZRGBNormal, not RGBA, but downgrade from rgba to rgb can be done while casting + PCL_POINT_XYZ_I_NORMAL = (PC_XYZ_DATA | PC_INTENSITY_DATA | PC_NORMAL_DATA | PC_CURVATURE_DATA), + PCL_POINT_XYZ_RANGE = (PC_XYZ_DATA | PC_RANGE_DATA), + PCL_POINT_XYZ_VIEWPOINT = (PC_XYZ_DATA | PC_VIEWPOINT_DATA), + PCL_MOMENT_INVARIANTS = (PC_MOMENT_INV_DATA), + PCL_PRINCIPAL_RADII_RSD = (PC_RADII_RSD_DATA), + PCL_BOUNDARY = (PC_BOUNDARY_DATA), + PCL_PRINCIPAL_CURVATURES = (PC_PRINCIPAL_CURVATURE_DATA), + PCL_PFH_SIGNAT_125 = (PC_PFH_SIGNAT_125_DATA), + PCL_FPFH_SIGNAT_33 = (PC_FPFH_SIGNAT_33_DATA), + PCL_VFH_SIGNAT_308 = (PC_VFH_SIGNAT_308_DATA), + PCL_NARF_36 = (PC_NARF_36_DATA), + PCL_POINT2D_BORDER = (PC_XY_DATA | PC_BORDER_DATA), + PCL_INTENSITY_GRADIENT = (PC_INTENSITY_GRAD_DATA), + PCL_PC_HISTOGRAM_N = (PC_HISTOGRAM_DATA), + PCL_POINT_XYZ_SCALE = (PC_XYZ_DATA | PC_SCALE_DATA), + PCL_POINT_XYZ_SURFEL = (PC_XYZ_DATA | PC_RGBA_DATA | PC_NORMAL_DATA | PC_RADIUS_DATA | PC_CONFIDENCE_DATA) +}; + +// Defined as in PCL pointTypes.h file for better compatibility +enum PointCloudBorderTrait : YARP_INT32 +{ + BORDER_TRAIT__OBSTACLE_BORDER, + BORDER_TRAIT__SHADOW_BORDER, + BORDER_TRAIT__VEIL_POINT, + BORDER_TRAIT__SHADOW_BORDER_TOP, + BORDER_TRAIT__SHADOW_BORDER_RIGHT, + BORDER_TRAIT__SHADOW_BORDER_BOTTOM, + BORDER_TRAIT__SHADOW_BORDER_LEFT, + BORDER_TRAIT__OBSTACLE_BORDER_TOP, + BORDER_TRAIT__OBSTACLE_BORDER_RIGHT, + BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM, + BORDER_TRAIT__OBSTACLE_BORDER_LEFT, + BORDER_TRAIT__VEIL_POINT_TOP, + BORDER_TRAIT__VEIL_POINT_RIGHT, + BORDER_TRAIT__VEIL_POINT_BOTTOM, + BORDER_TRAIT__VEIL_POINT_LEFT, +}; + +// Definition of single fields data structures +YARP_BEGIN_PACK +struct DataXY +{ + union + { + float _xy[2]; + struct + { + float x; + float y; + }; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf\t", precision, x, + precision, y); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf", width, precision, x, + width, precision, y); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(x); + ret.addDouble(y); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + x = static_cast(intBt->get(0).asDouble()); + y = static_cast(intBt->get(1).asDouble()); + return; + } +}; +YARP_END_PACK + +// xyz +YARP_BEGIN_PACK +struct DataXYZ +{ + union + { + float _xyz[4]; + struct + { + float x; + float y; + float z; + }; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf\t", precision, x, + precision, y, + precision, z); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf", width, precision, x, + width, precision, y, + width, precision, z); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(x); + ret.addDouble(y); + ret.addDouble(z); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + x = static_cast(intBt->get(0).asDouble()); + y = static_cast(intBt->get(1).asDouble()); + z = static_cast(intBt->get(2).asDouble()); + return; + } +}; +YARP_END_PACK + +// RGBA fields - quite useless alone +YARP_BEGIN_PACK +struct DataRGBA +{ + union + { + struct + { + char b; + char g; + char r; + char a; + }; + yarp::os::NetInt32 rgba; +// float data_c[4]; + }; + yarp::os::ConstString toString(int precision, int width) + { + YARP_UNUSED(precision); + YARP_UNUSED(width); + yarp::os::ConstString ret = ""; + char tmp[10]; + snprintf(tmp, 128, "%c %c %c %c\t", r, g, b, a); + ret += tmp; + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addInt(r); + ret.addInt(g); + ret.addInt(b); + ret.addInt(a); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + r = intBt->get(0).asInt(); + g = intBt->get(1).asInt(); + b = intBt->get(2).asInt(); + a = intBt->get(3).asInt(); + return; + } +}; +YARP_END_PACK + +// Normal +YARP_BEGIN_PACK +struct DataNormal +{ + union + { + float filler_n[4]; + float normal[3]; + struct + { + float normal_x; + float normal_y; + float normal_z; + }; + }; + union + { + struct + { + float curvature; + }; + float data_c[4]; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf % .*lf\t", precision, normal_x, + precision, normal_y, + precision, normal_z, + precision, curvature); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf % *.*lf", width, precision, normal_x, + width, precision, normal_y, + width, precision, normal_z, + width, precision, curvature); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(normal_x); + ret.addDouble(normal_y); + ret.addDouble(normal_z); + ret.addDouble(curvature); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + normal_x = static_cast(intBt->get(0).asDouble()); + normal_y = static_cast(intBt->get(1).asDouble()); + normal_z = static_cast(intBt->get(2).asDouble()); + curvature = static_cast(intBt->get(3).asDouble()); + return; + } +}; +YARP_END_PACK + +YARP_BEGIN_PACK +struct DataNormalNoCurvature +{ + union + { + float filler_n[4]; + float normal[3]; + struct + { + float normal_x; + float normal_y; + float normal_z; + }; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf\t", precision, normal_x, + precision, normal_y, + precision, normal_z); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf", width, precision, normal_x, + width, precision, normal_y, + width, precision, normal_z); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(normal_x); + ret.addDouble(normal_y); + ret.addDouble(normal_z); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + normal_x = static_cast(intBt->get(0).asDouble()); + normal_y = static_cast(intBt->get(1).asDouble()); + normal_z = static_cast(intBt->get(2).asDouble()); + return; + } +}; +YARP_END_PACK + +// curvature +YARP_BEGIN_PACK +struct DataCurvature +{ + union + { + struct + { + float curvature; + }; + }; +}; +YARP_END_PACK +// Range +typedef float Range; + +// viewPoint +YARP_BEGIN_PACK +struct DataViewpoint +{ + union + { + float _xyz[4]; + struct + { + float vp_x; + float vp_y; + float vp_z; + }; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf\t", precision, vp_x, + precision, vp_y, + precision, vp_z); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf", width, precision, vp_x, + width, precision, vp_y, + width, precision, vp_z); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(vp_x); + ret.addDouble(vp_y); + ret.addDouble(vp_z); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + vp_x = static_cast(intBt->get(0).asDouble()); + vp_y = static_cast(intBt->get(1).asDouble()); + vp_z = static_cast(intBt->get(2).asDouble()); + return; + } +}; +YARP_END_PACK + +// TBD: many others ... + + +// +// Definition of packed types - PCL style +// + +// xyz + rgba - most common type +YARP_BEGIN_PACK +struct DataXYZRGBA +{ + union + { + float _xyz[4]; + struct + { + float x; + float y; + float z; + float xyz_padding; + }; + }; + + union + { + struct + { + char b; + char g; + char r; + char a; + }; + yarp::os::NetInt32 rgba; + float rgba_padding[4]; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf ", precision, x, + precision, y, + precision, z); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf ", width, precision, x, + width, precision, y, + width, precision, z); + ret += tmp; + } + snprintf(tmp, 128, "%c %c %c %c\t", r, g, b, a); + ret += tmp; + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(x); + ret.addDouble(y); + ret.addDouble(z); + ret.addInt(r); + ret.addInt(g); + ret.addInt(b); + ret.addInt(a); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + x = static_cast(intBt->get(0).asDouble()); + y = static_cast(intBt->get(1).asDouble()); + z = static_cast(intBt->get(2).asDouble()); + r = intBt->get(3).asInt(); + g = intBt->get(4).asInt(); + b = intBt->get(5).asInt(); + a = intBt->get(6).asInt(); + return; + } +}; +YARP_END_PACK + +// xyz + intensity +YARP_BEGIN_PACK +struct DataXYZI +{ + union + { + float _xyz[4]; + struct + { + float x; + float y; + float z; + }; + }; + + union + { + struct + { + float intensity; + }; + float intensity_padding[4]; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf % .*lf\t", precision, x, + precision, y, + precision, z, + precision, intensity); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf % *.*lf", width, precision, x, + width, precision, y, + width, precision, z, + width, precision, intensity); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(x); + ret.addDouble(y); + ret.addDouble(z); + ret.addDouble(intensity); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + x = static_cast(intBt->get(0).asDouble()); + y = static_cast(intBt->get(1).asDouble()); + z = static_cast(intBt->get(2).asDouble()); + intensity = static_cast(intBt->get(3).asDouble()); + return; + } +}; +YARP_END_PACK + +// interest point -> xyz + strength +YARP_BEGIN_PACK +struct DataInterestPointXYZ +{ + union + { + float _xyz[4]; + struct + { + float x; + float y; + float z; + }; + }; + + union + { + struct + { + float strength; + }; + float strength_padding[4]; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf % .*lf\t", precision, x, + precision, y, + precision, z, + precision, strength); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf % *.*lf", width, precision, x, + width, precision, y, + width, precision, z, + width, precision, strength); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(x); + ret.addDouble(y); + ret.addDouble(z); + ret.addDouble(strength); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + x = static_cast(intBt->get(0).asDouble()); + y = static_cast(intBt->get(1).asDouble()); + z = static_cast(intBt->get(2).asDouble()); + strength = static_cast(intBt->get(3).asDouble()); + return; + } +}; +YARP_END_PACK + + +// point xyz + normals +YARP_BEGIN_PACK +struct DataXYZNormal +{ + union + { + float data[4]; + struct + { + float x; + float y; + float z; + }; + }; + union + { + float filler_n[4]; + float normal[3]; + struct + { + float normal_x; + float normal_y; + float normal_z; + }; + }; + union + { + struct + { + float curvature; + }; + float filler_c[4]; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf ", precision, x, + precision, y, + precision, z); + ret += tmp; + snprintf(tmp, 128, "% .*lf % .*lf % .*lf % .*lf\t", precision, normal_x, + precision, normal_y, + precision, normal_z, + precision, curvature); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf ", width, precision, x, + width, precision, y, + width, precision, z); + ret += tmp; + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf % *.*lf", width, precision, normal_x, + width, precision, normal_y, + width, precision, normal_z, + width, precision, curvature); + ret += tmp; + } + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(x); + ret.addDouble(y); + ret.addDouble(z); + ret.addDouble(normal_x); + ret.addDouble(normal_y); + ret.addDouble(normal_z); + ret.addDouble(curvature); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + x = static_cast(intBt->get(0).asDouble()); + y = static_cast(intBt->get(1).asDouble()); + z = static_cast(intBt->get(2).asDouble()); + normal_x = static_cast(intBt->get(3).asDouble()); + normal_y = static_cast(intBt->get(4).asDouble()); + normal_z = static_cast(intBt->get(5).asDouble()); + curvature = static_cast(intBt->get(6).asDouble()); + return; + } +}; +YARP_END_PACK + +// point xyz + normals + RGBA +YARP_BEGIN_PACK +struct DataXYZNormalRGBA +{ + union + { + float data[4]; + struct + { + float x; + float y; + float z; + }; + }; + union + { + float filler_n[4]; + float normal[3]; + struct + { + float normal_x; + float normal_y; + float normal_z; + }; + }; + union + { + struct + { + // PCL here uses float rgb, probably for ROS compatibility as stated for PointXYZRGB + // Check compatibility, it should be ok if rgb component are in the right place and we drop 'a' + union + { + struct + { + char b; + char g; + char r; + char a; + }; + yarp::os::NetInt32 rgba; + }; + float curvature; + }; + float filler_others[4]; + }; + yarp::os::ConstString toString(int precision, int width) + { + yarp::os::ConstString ret = ""; + char tmp[128]; + if (width < 0) { + snprintf(tmp, 128, "% .*lf % .*lf % .*lf ", precision, x, + precision, y, + precision, z); + ret += tmp; + snprintf(tmp, 128, "% .*lf % .*lf % .*lf % .*lf ", precision, normal_x, + precision, normal_y, + precision, normal_z, + precision, curvature); + ret += tmp; + + } else { + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf ", width, precision, x, + width, precision, y, + width, precision, z); + ret += tmp; + snprintf(tmp, 128, "% *.*lf % *.*lf % *.*lf % *.*lf ", width, precision, normal_x, + width, precision, normal_y, + width, precision, normal_z, + width, precision, curvature); + ret += tmp; + } + snprintf(tmp, 128, "%c %c %c %c\t", r, g, b, a); + ret += tmp; + return ret; + } + yarp::os::Bottle toBottle() + { + yarp::os::Bottle ret; + ret.addDouble(x); + ret.addDouble(y); + ret.addDouble(z); + ret.addDouble(normal_x); + ret.addDouble(normal_y); + ret.addDouble(normal_z); + ret.addDouble(curvature); + ret.addInt(r); + ret.addInt(g); + ret.addInt(b); + ret.addInt(a); + return ret; + } + void fromBottle(const yarp::os::Bottle& bt, size_t i) + { + yarp::os::Bottle* intBt = bt.get(i).asList(); + + if (!intBt) { + return; + } + + x = static_cast(intBt->get(0).asDouble()); + y = static_cast(intBt->get(1).asDouble()); + z = static_cast(intBt->get(2).asDouble()); + normal_x = static_cast(intBt->get(3).asDouble()); + normal_y = static_cast(intBt->get(4).asDouble()); + normal_z = static_cast(intBt->get(5).asDouble()); + curvature = static_cast(intBt->get(6).asDouble()); + r = intBt->get(7).asInt(); + g = intBt->get(8).asInt(); + b = intBt->get(9).asInt(); + a = intBt->get(10).asInt(); + return; + } +}; +YARP_END_PACK + +// TBD: many others ... + +} // namespace sig +} // namespace yarp + + +#endif // YARP_SIG_POINTCLOUDTYPES_H diff --git a/src/libYARP_sig/include/yarp/sig/all.h b/src/libYARP_sig/include/yarp/sig/all.h index f83b37b7a02..0e3447088ff 100644 --- a/src/libYARP_sig/include/yarp/sig/all.h +++ b/src/libYARP_sig/include/yarp/sig/all.h @@ -19,5 +19,6 @@ #include #include #include +#include #endif // YARP_SIG_ALL_H diff --git a/src/libYARP_sig/src/PointCloudBase.cpp b/src/libYARP_sig/src/PointCloudBase.cpp new file mode 100644 index 00000000000..4a13d41092d --- /dev/null +++ b/src/libYARP_sig/src/PointCloudBase.cpp @@ -0,0 +1,170 @@ +/* + * 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. + */ + +#include + +using namespace yarp::sig; + +// Map that contains the offset of the basic types respect the origin of the struct +// representing the composite types. +const std::map, int> offsetMap = { + // PCL_NORMAL + { std::make_pair(PCL_NORMAL, PC_CURVATURE_DATA), sizeof(yarp::sig::DataNormalNoCurvature) }, + + // PCL_POINT_XYZ_RGBA + { std::make_pair(PCL_POINT_XYZ_RGBA, PC_RGBA_DATA), sizeof(yarp::sig::DataXYZ) }, + + // PCL_POINT_XYZ_I + { std::make_pair(PCL_POINT_XYZ_I, PC_INTENSITY_DATA), sizeof(yarp::sig::DataXYZ) }, + + // PCL_INTEREST_POINT_XYZ + { std::make_pair(PCL_INTEREST_POINT_XYZ, PC_INTEREST_DATA), sizeof(yarp::sig::DataXYZ) }, + + // PCL_POINT_XYZ_NORMAL + { std::make_pair(PCL_POINT_XYZ_NORMAL, PC_NORMAL_DATA), sizeof(yarp::sig::DataXYZ) }, + { std::make_pair(PCL_POINT_XYZ_NORMAL, PC_CURVATURE_DATA), sizeof(yarp::sig::DataXYZ) + sizeof(yarp::sig::DataNormalNoCurvature) }, + + // PCL_XYZ_NORMAL_RGBA + { std::make_pair(PCL_POINT_XYZ_NORMAL_RGBA, PC_NORMAL_DATA), sizeof(yarp::sig::DataXYZ) }, + { std::make_pair(PCL_POINT_XYZ_NORMAL_RGBA, PC_RGBA_DATA), sizeof(yarp::sig::DataXYZ) + sizeof(yarp::sig::DataNormalNoCurvature) }, + { std::make_pair(PCL_POINT_XYZ_NORMAL_RGBA, PC_CURVATURE_DATA), sizeof(yarp::sig::DataXYZ) + sizeof(yarp::sig::DataNormalNoCurvature) + sizeof(yarp::sig::DataRGBA) }, + + // PCL_XYZ_I_NORMAL TBD +}; +// Map that contains the information about the basic types that form +// the composite ones and in which order +const std::map > compositionMap = { + // recipe for basic data + { PC_XY_DATA, std::vector {PC_XY_DATA} }, + { PC_XYZ_DATA, std::vector {PC_XYZ_DATA} }, + { PC_RGBA_DATA, std::vector {PC_RGBA_DATA} }, + { PC_INTENSITY_DATA, std::vector {PC_INTENSITY_DATA} }, + { PC_INTEREST_DATA, std::vector {PC_INTEREST_DATA} }, + { PCL_NORMAL, std::vector {PC_NORMAL_DATA, PC_CURVATURE_DATA, PC_PADDING3} }, + { PC_NORMAL_DATA, std::vector {PC_NORMAL_DATA} }, + { PC_CURVATURE_DATA, std::vector {PC_CURVATURE_DATA} }, + { PC_RANGE_DATA, std::vector {PC_RANGE_DATA} }, + { PC_VIEWPOINT_DATA, std::vector {PC_VIEWPOINT_DATA} }, + // PCL_POINT_XYZ_RGBA + { PCL_POINT_XYZ_RGBA, std::vector {PC_XYZ_DATA, PC_RGBA_DATA, PC_PADDING3} }, + // PCL_POINT_XYZ_I + { PCL_POINT_XYZ_I, std::vector {PC_XYZ_DATA, PC_INTENSITY_DATA} }, + // PCL_INTEREST_POINT_XYZ + { PCL_INTEREST_POINT_XYZ, std::vector {PC_XYZ_DATA, PC_INTEREST_DATA} }, + // PCL_POINT_XYZ_NORMAL + { PCL_POINT_XYZ_NORMAL, std::vector {PC_XYZ_DATA, PC_NORMAL_DATA, PC_CURVATURE_DATA, PC_PADDING3} }, + // PCL_POINT_XYZ_NORMAL_RGBA + { PCL_POINT_XYZ_NORMAL_RGBA, std::vector {PC_XYZ_DATA, PC_NORMAL_DATA, PC_RGBA_DATA, PC_CURVATURE_DATA, PC_PADDING2} } +}; + +// Map that contains the size of the struct given the enum representing the type +const std::map sizeMap = { + { PC_PADDING3, 3*sizeof(float) }, + { PC_PADDING2, 2*sizeof(float) }, + { PC_XY_DATA, sizeof(yarp::sig::DataXY) }, + { PC_XYZ_DATA, sizeof(yarp::sig::DataXYZ) }, + { PC_RGBA_DATA, sizeof(yarp::sig::DataRGBA) }, + { PC_INTENSITY_DATA, sizeof(float) }, + { PC_INTEREST_DATA, sizeof(float) }, + { PC_NORMAL_DATA, sizeof(yarp::sig::DataNormalNoCurvature) }, + { PCL_NORMAL, sizeof(yarp::sig::DataNormal) }, + { PC_CURVATURE_DATA, sizeof(yarp::sig::DataCurvature) }, + { PC_RANGE_DATA, sizeof(yarp::sig::Range) }, + { PC_VIEWPOINT_DATA, sizeof(yarp::sig::DataViewpoint) }, + { PCL_POINT_XYZ_RGBA, sizeof(yarp::sig::DataXYZRGBA) }, + { PCL_POINT_XYZ_I, sizeof(yarp::sig::DataXYZI) }, + { PCL_INTEREST_POINT_XYZ, sizeof(yarp::sig::DataInterestPointXYZ) }, + { PCL_POINT_XYZ_NORMAL, sizeof(yarp::sig::DataXYZNormal) }, + { PCL_POINT_XYZ_NORMAL_RGBA, sizeof(yarp::sig::DataXYZNormalRGBA) } +}; + + +size_t PointCloudBase::height() const +{ + return header.height; +} + +size_t PointCloudBase::width() const +{ + return header.width; +} + +int PointCloudBase::getPointType() const +{ + return header.pointType; +} + +yarp::os::Type PointCloudBase::getType() +{ + return yarp::os::Type::byName("yarp/pointCloud"); +} + +bool PointCloudBase::isOrganized() const +{ + return height() > 1; +} + +void PointCloudBase::copyFromRawData(const char* dst, const char* source, std::vector& recipe) +{ + char* tmpSrc = const_cast(source); + char* tmpDst = const_cast(dst); + if (recipe.empty()) { + return; + } + yAssert(tmpSrc && tmpDst); + + size_t sizeDst = pointType2Size(getPointType()); + const size_t numPts = height() * width(); + for (size_t i = 0; i < numPts; i++) { + for (size_t j = 0; j < recipe.size(); j++) { + size_t sizeToRead = pointType2Size(recipe[j]); + if ((header.pointType & recipe[j])) { + size_t offset = getOffset(header.pointType, recipe[j]); + std::memcpy(tmpDst + i * sizeDst + offset, tmpSrc, sizeToRead); + } + + // increment anyways, if the field is missing in the destination, simply skip it + tmpSrc += sizeToRead; + } + } +} + + +std::vector PointCloudBase::getComposition(int type_composite) const +{ + //todo probably + std::vector ret; + auto it = compositionMap.find(type_composite); + if (it != compositionMap.end()) { + ret = it->second; + } + return ret; +} + + +size_t PointCloudBase::pointType2Size(int type) const +{ + size_t size = 0; + + auto it = sizeMap.find(type); + if (it != sizeMap.end()) { + size = it->second; + } + + return size; +} + +size_t PointCloudBase::getOffset(int type_composite, int type_basic) const +{ + size_t offset = 0; + auto it = offsetMap.find(std::make_pair(type_composite, type_basic)); + if (it != offsetMap.end()) { + offset = it->second; + } + return offset; +} diff --git a/tests/libYARP_sig/PointCloudTest.cpp b/tests/libYARP_sig/PointCloudTest.cpp new file mode 100644 index 00000000000..e0b8a1fd112 --- /dev/null +++ b/tests/libYARP_sig/PointCloudTest.cpp @@ -0,0 +1,787 @@ +/* + * 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. + */ + + +/** + * + * Tests for point cloud type + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "TestList.h" + +using namespace yarp::sig; +using namespace yarp::os; + +float acceptedDiff = 1e-6; + +class PointCloudTest : public yarp::os::impl::UnitTest +{ +public: + virtual ConstString getName() override + { return "PointCloudTest"; } + + void readWriteMatchTest() + { + Network::setLocalMode(true); + report(0, "Checking DataXYZRGBA sending - Type match"); + BufferedPort< PointCloud > outPort; + Port inPort; + checkTrue(outPort.open("/test/pointcloud/out"),"Opening output port"); + checkTrue(inPort.open("/test/pointcloud/in"),"Opening input port"); + checkTrue(NetworkBase::connect(outPort.getName(), inPort.getName()),"Checking connection"); + PointCloud& testPC = outPort.prepare(); + int width = 100; + int height = 20; + testPC.resize(width, height); + + for (int i=0; i inCloud; + inPort.read(inCloud); + + checkTrue(inCloud.dataSizeBytes() == testPC.dataSizeBytes(), "Checking size consistency"); + + bool ok = true; + for (int i=0; i > outPort; + Port inPort; + checkTrue(outPort.open("/test/pointcloud/out"),"Opening output port"); + checkTrue(inPort.open("/test/pointcloud/in"),"Opening input port"); + checkTrue(NetworkBase::connect(outPort.getName(), inPort.getName()),"Checking connection"); + PointCloud& testPC = outPort.prepare(); + int width = 200; + int height = 20; + testPC.resize(width, height); + + for (int i=0; i inCloud; + inPort.read(inCloud); + + checkFalse(inCloud.dataSizeBytes() == testPC.dataSizeBytes(), "Checking size, correctly different"); + + bool ok = true; + for (int i=0; i > outPort; + Port inPort; + checkTrue(outPort.open("/test/pointcloud/out"),"Opening output port"); + checkTrue(inPort.open("/test/pointcloud/in"),"Opening input port"); + checkTrue(NetworkBase::connect(outPort.getName(), inPort.getName()),"Checking connection"); + PointCloud& testPC = outPort.prepare(); + int width = 200; + int height = 42; + testPC.resize(width, height); + + for (int i=0; i inCloud; + inPort.read(inCloud); + + checkTrue(inCloud.dataSizeBytes() == testPC.dataSizeBytes(), "Checking size, equals for the padding"); + + bool ok = true; + for (int i=0; i testPC; + int width = 5; + int height = 5; + testPC.resize(width, height); + + for (int i=0; i testPC2(testPC); + + checkTrue(testPC2.dataSizeBytes() == testPC.dataSizeBytes(), "Checking size"); + + bool ok = true; + for (int i=0; i testPC3(testPC); + + checkFalse(testPC3.dataSizeBytes() == testPC.dataSizeBytes(), "Checking size, correctly different"); + checkTrue(testPC3.height() == testPC.height(), "Checking height"); + checkTrue(testPC3.width() == testPC.width(), "Checking width"); + + ok = true; + for (int i=0; i testPC4(testPC3); + + checkFalse(testPC4.dataSizeBytes() == testPC3.dataSizeBytes(), "Checking size, correctly different"); + checkTrue(testPC4.height() == testPC3.height(), "Checking height"); + checkTrue(testPC4.width() == testPC3.width(), "Checking width"); + + ok = true; + for (int i=0; i testPC5 = testPC4; + + + checkTrue(testPC5.dataSizeBytes() == testPC4.dataSizeBytes(), "Checking size"); + checkTrue(testPC5.height() == testPC4.height(), "Checking height"); + checkTrue(testPC5.width() == testPC4.width(), "Checking width"); + + ok = true; + for (int i=0; i testPC6; + testPC6.resize(width, height); + + for (int i=0; i testPC7(testPC6); + ok = true; + for (int i=0; i testPC8 = testPC6; + + + checkFalse(testPC8.dataSizeBytes() == testPC6.dataSizeBytes(), "Checking size, correctly different"); + checkTrue(testPC8.height() == testPC6.height(), "Checking height"); + checkTrue(testPC8.width() == testPC6.width(), "Checking width"); + + ok = true; + for (int i=0; i testPC9 = testPC7; + + + checkTrue(testPC9.dataSizeBytes() == testPC7.dataSizeBytes(), "Checking size"); + checkTrue(testPC9.height() == testPC7.height(), "Checking height"); + checkTrue(testPC9.width() == testPC7.width(), "Checking width"); + + ok = true; + for (int i=0; i testPC; + int width = 32; + int height = 25; + testPC.resize(width, height); + + for (int i=0; i testPC2; + testPC2.fromExternalPC(testPC.getRawData(), PCL_POINT_XYZ_RGBA, width, height); + + checkTrue(testPC2.dataSizeBytes() == testPC.dataSizeBytes(), "Checking size"); + + bool ok = true; + for (int i=0; i testPC3; + + testPC3.fromExternalPC(testPC2.getRawData(), PCL_POINT_XYZ_RGBA, width, height); + + checkFalse(testPC3.dataSizeBytes() == testPC2.dataSizeBytes(), "Checking size, correctly different"); + checkTrue(testPC3.height() == testPC2.height(), "Checking height"); + checkTrue(testPC3.width() == testPC2.width(), "Checking width"); + + ok = true; + for (int i=0; i testPC4(testPC3); + + testPC4.fromExternalPC(testPC3.getRawData(), PCL_POINT_XYZ, width, height); + + checkFalse(testPC4.dataSizeBytes() == testPC3.dataSizeBytes(), "Checking size, correctly different"); + checkTrue(testPC4.height() == testPC3.height(), "Checking height"); + checkTrue(testPC4.width() == testPC3.width(), "Checking width"); + + ok = true; + for (int i=0; i testPC; + PointCloud testPC2; + int width = 35; + int height = 62; + testPC.resize(width, height); + testPC2.resize(width, height); + + for (int i=0; i sumPC; + sumPC = testPC + testPC2; + + checkTrue(sumPC.size() == (size_t) (width*height*2), "Checking the size"); + + bool ok = true; + for (int i=0; i testPC; + size_t width = 21; size_t height = 32; + testPC.resize(width, height); + for (size_t i=0; i testPC2; + Bottle bt = testPC.toBottle(); + testPC2.fromBottle(bt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i testPCfail; + checkFalse(testPCfail.fromBottle(bt),"from bottle correctly failing... type mismatch"); + } + + { + report(0,"Testing fromBottle(toBottle) XYZ_NORMAL"); + PointCloud testPC; + size_t width = 21; size_t height = 32; + testPC.resize(width, height); + for (size_t i=0; i testPC2; + Bottle bt = testPC.toBottle(); + testPC2.fromBottle(bt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i testPC; + size_t width = 21; size_t height = 32; + testPC.resize(width, height); + for (size_t i=0; i testPC2; + Bottle bt = testPC.toBottle(); + testPC2.fromBottle(bt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i testPC; + size_t width = 21; size_t height = 32; + testPC.resize(width, height); + for (size_t i=0; i testPC2; + Bottle bt = testPC.toBottle(); + testPC2.fromBottle(bt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i testPC; + size_t width = 3; size_t height = 3; + testPC.resize(width, height); + for (size_t i=0; i testPC2; + Bottle bt = testPC.toBottle(); + testPC2.fromBottle(bt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i