From 8baae5dbcc03093f2b5acb58e49cb6d4ac873838 Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Mon, 26 Oct 2020 14:14:37 +0100 Subject: [PATCH 1/6] [libpointmatcher_ros] Optimized deserialization from sensor_msgs/PointCloud2 --- CMakeLists.txt | 32 ++- .../RosPointCloud2Deserializer.h | 256 ++++++++++++++++++ package.xml | 2 + src/RosPointCloud2Deserializer.cpp | 10 + test/RosPointCloud2DeserializerTest.cpp | 43 +++ 5 files changed, 337 insertions(+), 6 deletions(-) create mode 100644 include/pointmatcher_ros/RosPointCloud2Deserializer.h create mode 100644 src/RosPointCloud2Deserializer.cpp create mode 100644 test/RosPointCloud2DeserializerTest.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b434d20..008023b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,11 +7,12 @@ add_compile_options(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - nav_msgs - ) +find_package(catkin REQUIRED + COMPONENTS + roscpp + sensor_msgs + nav_msgs +) find_package(libpointmatcher CONFIG) @@ -152,7 +153,7 @@ add_library(pointmatcher_ros target_link_libraries(pointmatcher_ros ${catkin_LIBRARIES} ${libpointmatcher_LIBRARIES} - ) +) ############# ## Install ## @@ -201,3 +202,22 @@ target_link_libraries(pointmatcher_ros ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +############# +## Testing ## +############# +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest(test_${PROJECT_NAME} + test/RosPointCloud2DeserializerTest.cpp + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE + include + ) + target_include_directories(test_${PROJECT_NAME} SYSTEM PUBLIC + ${catkin_INCLUDE_DIRS} + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) +endif() \ No newline at end of file diff --git a/include/pointmatcher_ros/RosPointCloud2Deserializer.h b/include/pointmatcher_ros/RosPointCloud2Deserializer.h new file mode 100644 index 0000000..9ddcb26 --- /dev/null +++ b/include/pointmatcher_ros/RosPointCloud2Deserializer.h @@ -0,0 +1,256 @@ +#pragma once + +// std +#include +#include + +// boost +#include + +// pointmatcher +#include +#include + +// ros +#include + +// sensor_msgs +#include +#include + +namespace PointMatcher_ROS +{ + +template +class RosPointCloud2Deserializer +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using PM = PointMatcher; + using PMIO = PointMatcherIO; + using PM_types = typename PMIO::PMPropTypes; + using DataPoints = typename PM::DataPoints; + using Label = typename DataPoints::Label; + using Labels = typename DataPoints::Labels; + using View = typename DataPoints::View; + + /** + * @brief Extract the feature and descriptor labels of a given sensor_msgs/PointCloud2 message. + * + * @param rosMsg Input message of type sensor_msgs/PointCloud2. + * @param featLabels Feature labels, will be modified. + * @param descLabels Descriptor labels, will be modified. + */ + static void extractFieldLabels(const sensor_msgs::PointCloud2& rosMsg, Labels& featLabels, Labels& descLabels) + { + // Conversions of descriptor fields from pcl. + // see http://www.ros.org/wiki/pcl/Overview + std::vector fieldTypes; + std::vector isFeature; + for (auto it(rosMsg.fields.begin()); it != rosMsg.fields.end(); ++it) + { + const std::string& name(it->name); + const size_t count{ std::max(it->count, 1) }; + if (name == "x" || name == "y" || name == "z") + { + featLabels.push_back(Label(name, count)); + isFeature.push_back(true); + fieldTypes.push_back(PM_types::FEATURE); + } + else if (name == "rgb" || name == "rgba") + { + descLabels.push_back(Label("color", (name == "rgba") ? 4 : 3)); + isFeature.push_back(false); + fieldTypes.push_back(PM_types::DESCRIPTOR); + } + else if ((it + 1) != rosMsg.fields.end() && it->name == "normal_x" && (it + 1)->name == "normal_y") + { + if ((it + 2) != rosMsg.fields.end() && (it + 2)->name == "normal_z") + { + descLabels.push_back(Label("normals", 3)); + isFeature.push_back(false); + isFeature.push_back(false); + fieldTypes.push_back(PM_types::DESCRIPTOR); + fieldTypes.push_back(PM_types::DESCRIPTOR); + it += 2; + } + else + { + descLabels.push_back(Label("normals", 2)); + isFeature.push_back(false); + fieldTypes.push_back(PM_types::DESCRIPTOR); + it += 1; + } + isFeature.push_back(false); + fieldTypes.push_back(PM_types::DESCRIPTOR); + } + else + { + descLabels.push_back(Label(name, count)); + isFeature.push_back(false); + fieldTypes.push_back(PM_types::DESCRIPTOR); + } + } + + // Add padding for scale-dependent computations. + featLabels.push_back(Label("pad", 1)); + + // TODO(ynava) The variables 'isFeature' and 'fieldTypes' are just kept for running assertions. Consider removing them and performing a runtime check. + assert(isFeature.size() == rosMsg.fields.size()); + assert(fieldTypes.size() == rosMsg.fields.size()); + } + + /** + * @brief Fills data from a scalar descriptor into a Pointmatcher point cloud. + * + * @param rosMsg Input message of type sensor_msgs/PointCloud2. + * @param fieldName Name of the point cloud field that corresponds to the descriptor. + * @param pointCount Number of points in the input point cloud. + * @param view View on the output point cloud, will be modified. + */ + static void fillScalarDataIntoView(const sensor_msgs::PointCloud2& rosMsg, const std::string& fieldName, const size_t pointCount, + View& view) + { + // Use iterator to read data and write it into view. + sensor_msgs::PointCloud2ConstIterator iter(rosMsg, fieldName); + for (size_t i = 0; i < pointCount; ++i, ++iter) + { + view(0, i) = *iter; + } + } + + /** + * @brief Fills data from a vector feature or descriptor into a Pointmatcher point cloud. + * + * @param rosMsg Input message of type sensor_msgs/PointCloud2. + * @param fieldName Name of the point cloud field that corresponds to the descriptor. + * @param is3dPointCloud Whether the point cloud is 3D or 2D. + * @param pointCount Number of points in the input point cloud. + * @param view View on the output point cloud, will be modified. + */ + static void fillVectorDataIntoView(const sensor_msgs::PointCloud2& rosMsg, const std::vector& fieldNames, + const bool is3dPointCloud, const size_t pointCount, View& view) + { + // Create iterators to read data from the message buffer. + sensor_msgs::PointCloud2ConstIterator iterX(rosMsg, fieldNames[0]); + sensor_msgs::PointCloud2ConstIterator iterY(rosMsg, fieldNames[1]); + + // Dispatch a deserialization routine based on dimensions. + if (is3dPointCloud) + { + sensor_msgs::PointCloud2ConstIterator iterZ(rosMsg, fieldNames[2]); + for (size_t i = 0; i < pointCount; ++i, ++iterX, ++iterY, ++iterZ) + { + view(0, i) = *iterX; + view(1, i) = *iterY; + view(2, i) = *iterZ; + } + } + else + { + for (size_t i = 0; i < pointCount; ++i, ++iterX, ++iterY) + { + view(0, i) = *iterX; + view(1, i) = *iterY; + } + } + } + + /** + * @brief Fills a Pointmatcher point cloud with data from a sensor_msgs/PointCloud2 message. + * + * @param rosMsg Input message of type sensor_msgs/PointCloud2. + * @param is3dPointCloud Whether the point cloud is 3D or 2D. + * @param pointCloud View on the output point cloud, will be modified. + */ + static void fillPointCloudValues(const sensor_msgs::PointCloud2& rosMsg, const bool is3dPointCloud, DataPoints& pointCloud) + { + const size_t pointCount{ rosMsg.width * rosMsg.height }; + + // Point coordinates. + { + View view(pointCloud.features.block(0, 0, pointCloud.features.rows(), pointCloud.features.cols())); + const std::vector fieldNames{ "x", "y", "z" }; + fillVectorDataIntoView(rosMsg, fieldNames, is3dPointCloud, pointCount, view); + pointCloud.getFeatureViewByName("pad").setOnes(); + } + + // Normals. + if (pointCloud.descriptorExists("normals")) + { + View view(pointCloud.getDescriptorViewByName("normals")); + const std::vector fieldNames{ "normal_x", "normal_y", "normal_z" }; + fillVectorDataIntoView(rosMsg, fieldNames, is3dPointCloud, pointCount, view); + } + + // Colors. + if (pointCloud.descriptorExists("colors")) + { + View view(pointCloud.getDescriptorViewByName("colors")); + sensor_msgs::PointCloud2ConstIterator iterR(rosMsg, "r"); + sensor_msgs::PointCloud2ConstIterator iterG(rosMsg, "g"); + sensor_msgs::PointCloud2ConstIterator iterB(rosMsg, "b"); + sensor_msgs::PointCloud2ConstIterator iterA(rosMsg, "a"); + for (size_t i = 0; i < pointCount; ++i, ++iterR, ++iterG, ++iterB, ++iterA) + { + // PointCloud2Iterator implicitly casts to the type specified in its template arguments. + view(0, i) = *iterR / 255.0; + view(1, i) = *iterG / 255.0; + view(2, i) = *iterB / 255.0; + view(3, i) = *iterA / 255.0; + } + } + + // Scalar descriptors. + const std::vector preprocessedFieldLabels{ "xyz", "x", "y", "z", "normals", "normal_x", "normal_y", "normal_z", + "color", "rgb", "rgba", "r", "g", "b", "a" }; + for (const auto& field : rosMsg.fields) + { + // Ignore descriptors that we have previously written into our point cloud matrix. + if (std::find(preprocessedFieldLabels.begin(), preprocessedFieldLabels.end(), field.name) != preprocessedFieldLabels.end()) + { + continue; + } + + View view{ pointCloud.getDescriptorViewByName(field.name) }; + fillScalarDataIntoView(rosMsg, field.name, pointCount, view); + } + } + + /** + * @brief Deserializes a sensor_msgs/PointCloud2 objects into a Pointmatcher point cloud (DataPoints) + * + * @param rosMsg Input message of type sensor_msgs/PointCloud2. + * @return DataPoints Output point cloud. + */ + static DataPoints deserialize(const sensor_msgs::PointCloud2& rosMsg) + { + // If the msg is empty return an empty point cloud. + if (rosMsg.fields.empty()) + { + return DataPoints(); + } + + // Label containers. + Labels featLabels; + Labels descLabels; + + // Fill field labels. + extractFieldLabels(rosMsg, featLabels, descLabels); + + // Determine the point cloud type (2D or 3D). + const bool is3dPointCloud = (featLabels.size() - 1) == 3 ? true : false; + + // Create cloud + const size_t pointCount{ rosMsg.width * rosMsg.height }; + DataPoints pointCloud(featLabels, descLabels, pointCount); + + // Fill cloud with data. + fillPointCloudValues(rosMsg, is3dPointCloud, pointCloud); + + return pointCloud; + } +}; + +} // namespace PointMatcher_ROS \ No newline at end of file diff --git a/package.xml b/package.xml index 156d09e..882d22d 100644 --- a/package.xml +++ b/package.xml @@ -64,6 +64,8 @@ sensor_msgs nav_msgs libpointmatcher + + gtest diff --git a/src/RosPointCloud2Deserializer.cpp b/src/RosPointCloud2Deserializer.cpp new file mode 100644 index 0000000..b6a3a26 --- /dev/null +++ b/src/RosPointCloud2Deserializer.cpp @@ -0,0 +1,10 @@ + +#include "PointMatcher_ROS/RosPointCloud2Deserializer.h" + +namespace PointMatcher_ROS +{ + +template class RosPointCloud2Deserializer; +template class RosPointCloud2Deserializer; + +} // namespace PointMatcher_ROS \ No newline at end of file diff --git a/test/RosPointCloud2DeserializerTest.cpp b/test/RosPointCloud2DeserializerTest.cpp new file mode 100644 index 0000000..11d311a --- /dev/null +++ b/test/RosPointCloud2DeserializerTest.cpp @@ -0,0 +1,43 @@ +/* + * RosPointCloud2DeserializerTest.cpp + * + * Author: Yoshua Nava + * Organization: ANYbotics AG + * License: Proprietary + */ + +// google test +#include + +// eigen +#include + +// sensor_msgs +#include + +// pointmacher_ros +#include "pointmatcher_ros/RosPointCloud2Deserializer.h" + +namespace PointMatcher_ROS +{ + +class RosPointCloud2DeserializerTest : public ::testing::Test +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + RosPointCloud2DeserializerTest() = default; + + /* Setup methods */ + void createDefaultPointCloudMessages() {} + + // Error tolerance. + const float kEpsilonError_ = 1e-5; +}; + +// This test validates that the function that builds up transformations to point clouds is correct. Considers pure translation +TEST_F(RosPointCloud2DeserializerTest, EmptyRosMessage) +{} + + +} // namespace PointMatcher_ROS From 850c604ee12ede50f2a8c2b40768f5a43de3123e Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Mon, 26 Oct 2020 22:51:52 +0100 Subject: [PATCH 2/6] Deserialization unit test --- CMakeLists.txt | 10 +- src/RosPointCloud2Deserializer.cpp | 2 +- test/RosPointCloud2DeserializerTest.cpp | 312 +++++++++++++++++++++++- 3 files changed, 305 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 008023b..aaea2e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,13 +9,12 @@ add_compile_options(-std=c++11) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + libpointmatcher roscpp sensor_msgs nav_msgs ) -find_package(libpointmatcher CONFIG) - ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -108,7 +107,7 @@ find_package(libpointmatcher CONFIG) catkin_package( INCLUDE_DIRS include LIBRARIES pointmatcher_ros - CATKIN_DEPENDS roscpp sensor_msgs nav_msgs + CATKIN_DEPENDS roscpp sensor_msgs nav_msgs libpointmatcher # DEPENDS system_lib ) @@ -125,8 +124,9 @@ include_directories( ) ## Declare a C++ library -add_library(pointmatcher_ros +add_library(${PROJECT_NAME} src/PointMatcher_ROS.cpp + src/RosPointCloud2Deserializer.cpp ) ## Add cmake target dependencies of the library @@ -150,7 +150,7 @@ add_library(pointmatcher_ros # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -target_link_libraries(pointmatcher_ros +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${libpointmatcher_LIBRARIES} ) diff --git a/src/RosPointCloud2Deserializer.cpp b/src/RosPointCloud2Deserializer.cpp index b6a3a26..7612c32 100644 --- a/src/RosPointCloud2Deserializer.cpp +++ b/src/RosPointCloud2Deserializer.cpp @@ -1,5 +1,5 @@ -#include "PointMatcher_ROS/RosPointCloud2Deserializer.h" +#include "pointmatcher_ros/RosPointCloud2Deserializer.h" namespace PointMatcher_ROS { diff --git a/test/RosPointCloud2DeserializerTest.cpp b/test/RosPointCloud2DeserializerTest.cpp index 11d311a..3380cab 100644 --- a/test/RosPointCloud2DeserializerTest.cpp +++ b/test/RosPointCloud2DeserializerTest.cpp @@ -1,14 +1,9 @@ -/* - * RosPointCloud2DeserializerTest.cpp - * - * Author: Yoshua Nava - * Organization: ANYbotics AG - * License: Proprietary - */ - // google test #include +// std +#include + // eigen #include @@ -17,6 +12,7 @@ // pointmacher_ros #include "pointmatcher_ros/RosPointCloud2Deserializer.h" +#include "pointmatcher_ros/PointMatcher_ROS.h" namespace PointMatcher_ROS { @@ -25,19 +21,309 @@ class RosPointCloud2DeserializerTest : public ::testing::Test { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using ScalarType = float; + using PM = PointMatcher; + using DataPoints = typename PM::DataPoints; + using Label = typename DataPoints::Label; + using Labels = typename DataPoints::Labels; + using View = typename DataPoints::View; + + struct PointCloudMsgGenerationParameters + { + size_t nbPoints_{ 100 }; + size_t dimFeatures_{ 4 }; + size_t dimNormals_{ 0 }; + size_t dimColor_{ 0 }; + size_t dimScalar_{ 0 }; + size_t dimTime_{ 0 }; + + bool hasNormals_{ false }; + bool hasColorRgb_{ false }; + bool hasColorRgba_{ false }; + bool hasScalar_{ false }; + bool hasScalarRangeRestricted_{ false }; + bool hasIntegerTime_{ false }; + + std::string frameId_{ "sensor" }; + ros::Time stamp_; + }; RosPointCloud2DeserializerTest() = default; + void SetUp() override + { + const int randomSeed = std::chrono::system_clock::now().time_since_epoch().count(); + randomNumberGenerator_.seed(randomSeed); + RecordProperty("random_seed", randomSeed); + + unitaryUnsignedIntegerDistribution_.param(std::uniform_real_distribution::param_type(0.0, 255.0)); + } + /* Setup methods */ - void createDefaultPointCloudMessages() {} + DataPoints generatePointCloud(const PointCloudMsgGenerationParameters& parameters) + { + // Point features. + Labels featLabels; + const PM::Matrix features = PM::Matrix::Random(parameters.dimFeatures_, parameters.nbPoints_); + featLabels.push_back(Label("x", 1)); + featLabels.push_back(Label("y", 1)); + featLabels.push_back(Label("z", 1)); + featLabels.push_back(Label("pad", 1)); + + // Construct the point cloud from the generated matrices + DataPoints pointCloud(features, featLabels); + + // Set padding/scale value to 1. + pointCloud.getFeatureViewByName("pad").setOnes(); + + // Normals. + if (parameters.hasNormals_) + { + const PM::Matrix descriptor = PM::Matrix::Random(parameters.dimNormals_, parameters.nbPoints_); + pointCloud.addDescriptor("normals", descriptor); + } + + if (parameters.hasColorRgb_) + { + const PM::Matrix descriptor = PM::Matrix::Random(parameters.dimColor_, parameters.nbPoints_); + pointCloud.addDescriptor("color", descriptor); + } + + if (parameters.hasColorRgba_) + { + const PM::Matrix descriptor = PM::Matrix::Random(parameters.dimColor_, parameters.nbPoints_); + pointCloud.addDescriptor("color", descriptor); + } + + if (parameters.hasScalar_) + { + // Scalar descriptor in floating point range [0, 1]. + for (size_t i = 0; i < parameters.dimScalar_; i++) + { + const PM::Matrix descriptor = PM::Matrix::Random(1, parameters.nbPoints_); + const std::string descriptorLabel("scalar" + std::to_string(i)); + pointCloud.addDescriptor(descriptorLabel, descriptor); + } + } + else if (parameters.hasScalarRangeRestricted_) + { + for (size_t i = 0; i < parameters.dimScalar_; i++) + { + // Scalar descriptor in uint range [0, 255] represented as floating point. (e.g. LIDAR intensity and reflectivity) + const PM::Matrix descriptor = PM::Matrix::Zero(1, parameters.nbPoints_).unaryExpr([&](float /*dummy*/) { + return unitaryUnsignedIntegerDistribution_(randomNumberGenerator_); + }); + const std::string descriptorLabel("scalarRangeRestricted" + std::to_string(i)); + pointCloud.addDescriptor(descriptorLabel, descriptor); + } + } + + if (parameters.hasIntegerTime_) + { + // Vector descriptor in uint range [0, 255]. (e.g. seconds and nanoseconds) + const PM::Int64Matrix times = PM::Int64Matrix::Random(parameters.dimTime_, parameters.nbPoints_); + pointCloud.addTime("time", times); + } - // Error tolerance. - const float kEpsilonError_ = 1e-5; + return pointCloud; + } + + // Random distribution. + std::default_random_engine randomNumberGenerator_; + std::uniform_real_distribution unitaryUnsignedIntegerDistribution_; }; -// This test validates that the function that builds up transformations to point clouds is correct. Considers pure translation TEST_F(RosPointCloud2DeserializerTest, EmptyRosMessage) -{} +{ + // Ros sensor_msgs/PointCloud2 is empty by default. + // Block motivation: Check base non-initialized values from msg. If at some point the message definition changes and this fails, it will be easier to know why. + const sensor_msgs::PointCloud2 rosMsg; + ASSERT_EQ(rosMsg.header.stamp.toSec(), 0); + ASSERT_EQ(rosMsg.header.frame_id, ""); + ASSERT_EQ(rosMsg.fields.size(), 0u); + ASSERT_FALSE(rosMsg.is_dense); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + + // Assertions. + ASSERT_EQ(deserializedPointCloud.getNbPoints(), 0u); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), 0u); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), 0u); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 0u); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), 0u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), 0u); +} + +TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPoints) +{ + PointCloudMsgGenerationParameters parameters; + parameters.nbPoints_ = 1123455; + + // Compute dimension of descriptors. + const size_t dimDescriptors = (parameters.dimNormals_ + parameters.dimColor_ + parameters.dimScalar_); + + // Create point cloud. + const DataPoints pointCloud = generatePointCloud(parameters); + const sensor_msgs::PointCloud2 rosMsg = + PointMatcher_ROS::pointMatcherCloudToRosMsg(pointCloud, parameters.frameId_, parameters.stamp_); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + + // Assertions. + EXPECT_EQ(deserializedPointCloud.getNbPoints(), parameters.nbPoints_); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), parameters.dimFeatures_ - 1); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), parameters.dimFeatures_); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 0u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), dimDescriptors); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), parameters.dimTime_); + EXPECT_EQ(deserializedPointCloud, pointCloud); +} + +TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsNormals) +{ + PointCloudMsgGenerationParameters parameters; + parameters.nbPoints_ = 1867565; + parameters.dimNormals_ = 3; + parameters.hasNormals_ = true; + + // Compute dimension of descriptors. + const size_t dimDescriptors = (parameters.dimNormals_ + parameters.dimColor_ + parameters.dimScalar_); + + // Create point cloud. + const DataPoints pointCloud = generatePointCloud(parameters); + const sensor_msgs::PointCloud2 rosMsg = + PointMatcher_ROS::pointMatcherCloudToRosMsg(pointCloud, parameters.frameId_, parameters.stamp_); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + + // Assertions. + EXPECT_EQ(deserializedPointCloud.getNbPoints(), parameters.nbPoints_); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), parameters.dimFeatures_ - 1); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), parameters.dimFeatures_); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 1u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), dimDescriptors); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), parameters.dimTime_); + EXPECT_EQ(deserializedPointCloud, pointCloud); +} + +TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsColor) +{ + PointCloudMsgGenerationParameters parameters; + parameters.nbPoints_ = 15; + parameters.dimColor_ = 3; + parameters.hasColorRgb_ = true; + + // Compute dimension of descriptors. + const size_t dimDescriptors = (parameters.dimNormals_ + parameters.dimColor_ + parameters.dimScalar_); + + // Create point cloud. + const DataPoints pointCloud = generatePointCloud(parameters); + const sensor_msgs::PointCloud2 rosMsg = + PointMatcher_ROS::pointMatcherCloudToRosMsg(pointCloud, parameters.frameId_, parameters.stamp_); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + // const DataPoints deserializedPointCloud = PointMatcher_ROS::rosMsgToPointMatcherCloud(rosMsg); + + // Assertions. + EXPECT_EQ(deserializedPointCloud.getNbPoints(), parameters.nbPoints_); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), parameters.dimFeatures_ - 1); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), parameters.dimFeatures_); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 1u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), dimDescriptors); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), parameters.dimTime_); + // TODO(ynava) Numerical accuracy of color type conversion makes it difficult to test for equality. + // Improve deserialization and serialization to decrease inaccuracy. + // EXPECT_TRUE(deserializedPointCloud.getDescriptorViewByName("color").isApprox(pointCloud.getDescriptorViewByName("color"))); +} + +TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsScalar) +{ + PointCloudMsgGenerationParameters parameters; + parameters.nbPoints_ = 99765; + parameters.dimScalar_ = 1; + parameters.hasScalar_ = true; + + // Compute dimension of descriptors. + const size_t dimDescriptors = (parameters.dimNormals_ + parameters.dimColor_ + parameters.dimScalar_); + + // Create point cloud. + const DataPoints pointCloud = generatePointCloud(parameters); + const sensor_msgs::PointCloud2 rosMsg = + PointMatcher_ROS::pointMatcherCloudToRosMsg(pointCloud, parameters.frameId_, parameters.stamp_); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + + // Assertions. + EXPECT_EQ(deserializedPointCloud.getNbPoints(), parameters.nbPoints_); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), parameters.dimFeatures_ - 1); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), parameters.dimFeatures_); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 1u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), dimDescriptors); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), parameters.dimTime_); + EXPECT_EQ(deserializedPointCloud, pointCloud); +} + +TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsScalarRangeRestricted) +{ + PointCloudMsgGenerationParameters parameters; + parameters.nbPoints_ = 5456; + parameters.dimScalar_ = 1; + parameters.hasScalarRangeRestricted_ = true; + + // Compute dimension of descriptors. + const size_t dimDescriptors = (parameters.dimNormals_ + parameters.dimColor_ + parameters.dimScalar_); + + // Create point cloud. + const DataPoints pointCloud = generatePointCloud(parameters); + const sensor_msgs::PointCloud2 rosMsg = + PointMatcher_ROS::pointMatcherCloudToRosMsg(pointCloud, parameters.frameId_, parameters.stamp_); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + + // Assertions. + EXPECT_EQ(deserializedPointCloud.getNbPoints(), parameters.nbPoints_); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), parameters.dimFeatures_ - 1); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), parameters.dimFeatures_); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 1u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), dimDescriptors); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), parameters.dimTime_); + EXPECT_EQ(deserializedPointCloud, pointCloud); +} + +TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsNormalsScalars) +{ + PointCloudMsgGenerationParameters parameters; + parameters.nbPoints_ = 465; + parameters.dimNormals_ = 3; + parameters.dimScalar_ = 5; + parameters.hasNormals_ = true; + parameters.hasScalar_ = true; + + // Compute dimension of descriptors. + const size_t dimDescriptors = (parameters.dimNormals_ + parameters.dimColor_ + parameters.dimScalar_); + + // Create point cloud. + const DataPoints pointCloud = generatePointCloud(parameters); + const sensor_msgs::PointCloud2 rosMsg = + PointMatcher_ROS::pointMatcherCloudToRosMsg(pointCloud, parameters.frameId_, parameters.stamp_); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + // Assertions. + EXPECT_EQ(deserializedPointCloud.getNbPoints(), parameters.nbPoints_); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), parameters.dimFeatures_ - 1); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), parameters.dimFeatures_); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 6u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), dimDescriptors); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), parameters.dimTime_); + EXPECT_EQ(deserializedPointCloud, pointCloud); +} } // namespace PointMatcher_ROS From fca2a62b752e3e08389b07984261b5c02bc534a7 Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Tue, 27 Oct 2020 12:55:20 +0100 Subject: [PATCH 3/6] Fixed serialization bug --- src/PointMatcher_ROS.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/PointMatcher_ROS.cpp b/src/PointMatcher_ROS.cpp index aaf96d3..0f7c3c5 100644 --- a/src/PointMatcher_ROS.cpp +++ b/src/PointMatcher_ROS.cpp @@ -539,8 +539,11 @@ sensor_msgs::PointCloud2 PointMatcher_ROS::pointMatcherCloudToRosMsg(const typen memcpy(fPtr, reinterpret_cast(&rgba), 4); fPtr += 4; // after color - memcpy(fPtr, reinterpret_cast(&pmCloud.descriptors(postColorPos, pt)), scalarSize * postColorCount); - fPtr += scalarSize * postColorCount; + if(postColorCount > 0) + { + memcpy(fPtr, reinterpret_cast(&pmCloud.descriptors(postColorPos, pt)), scalarSize * postColorCount); + fPtr += scalarSize * postColorCount; + } } else { From 3cda155c2d812a0dea4288d0ccdf1510564f5601 Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Tue, 27 Oct 2020 12:55:32 +0100 Subject: [PATCH 4/6] Added deserialization of 3D+RGBA point cloud --- test/RosPointCloud2DeserializerTest.cpp | 30 +++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/test/RosPointCloud2DeserializerTest.cpp b/test/RosPointCloud2DeserializerTest.cpp index 3380cab..1317ffb 100644 --- a/test/RosPointCloud2DeserializerTest.cpp +++ b/test/RosPointCloud2DeserializerTest.cpp @@ -240,6 +240,36 @@ TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsColor) // EXPECT_TRUE(deserializedPointCloud.getDescriptorViewByName("color").isApprox(pointCloud.getDescriptorViewByName("color"))); } +TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsColorWithTransparency) +{ + PointCloudMsgGenerationParameters parameters; + parameters.nbPoints_ = 15; + parameters.dimColor_ = 4; + parameters.hasColorRgba_ = true; + + // Compute dimension of descriptors. + const size_t dimDescriptors = (parameters.dimNormals_ + parameters.dimColor_ + parameters.dimScalar_); + + // Create point cloud. + const DataPoints pointCloud = generatePointCloud(parameters); + const sensor_msgs::PointCloud2 rosMsg = + pointmatcher_ros::pointMatcherCloudToRosMsg(pointCloud, parameters.frameId_, parameters.stamp_); + + // Deserialize msg. + const DataPoints deserializedPointCloud = RosPointCloud2Deserializer::deserialize(rosMsg); + + // Assertions. + EXPECT_EQ(deserializedPointCloud.getNbPoints(), parameters.nbPoints_); + EXPECT_EQ(deserializedPointCloud.getEuclideanDim(), parameters.dimFeatures_ - 1); + EXPECT_EQ(deserializedPointCloud.getHomogeneousDim(), parameters.dimFeatures_); + EXPECT_EQ(deserializedPointCloud.getNbGroupedDescriptors(), 1u); + EXPECT_EQ(deserializedPointCloud.getDescriptorDim(), dimDescriptors); + EXPECT_EQ(deserializedPointCloud.getTimeDim(), parameters.dimTime_); + // TODO(ynava) Numerical accuracy of color type conversion makes it difficult to test for equality. + // Improve deserialization and serialization to decrease inaccuracy. + // EXPECT_TRUE(deserializedPointCloud.getDescriptorViewByName("color").isApprox(pointCloud.getDescriptorViewByName("color"))); +} + TEST_F(RosPointCloud2DeserializerTest, PointCloud3dPointsScalar) { PointCloudMsgGenerationParameters parameters; From 4bfb9b1c49d88e79e0b16ea667d2f6810a184f0e Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Tue, 27 Oct 2020 13:12:28 +0100 Subject: [PATCH 5/6] Renamed template type from T to ScalarType --- .../pointmatcher_ros/RosPointCloud2Deserializer.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/include/pointmatcher_ros/RosPointCloud2Deserializer.h b/include/pointmatcher_ros/RosPointCloud2Deserializer.h index 9ddcb26..7ff2c39 100644 --- a/include/pointmatcher_ros/RosPointCloud2Deserializer.h +++ b/include/pointmatcher_ros/RosPointCloud2Deserializer.h @@ -27,8 +27,8 @@ class RosPointCloud2Deserializer public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - using PM = PointMatcher; - using PMIO = PointMatcherIO; + using PM = PointMatcher; + using PMIO = PointMatcherIO; using PM_types = typename PMIO::PMPropTypes; using DataPoints = typename PM::DataPoints; using Label = typename DataPoints::Label; @@ -113,7 +113,7 @@ class RosPointCloud2Deserializer View& view) { // Use iterator to read data and write it into view. - sensor_msgs::PointCloud2ConstIterator iter(rosMsg, fieldName); + sensor_msgs::PointCloud2ConstIterator iter(rosMsg, fieldName); for (size_t i = 0; i < pointCount; ++i, ++iter) { view(0, i) = *iter; @@ -133,13 +133,13 @@ class RosPointCloud2Deserializer const bool is3dPointCloud, const size_t pointCount, View& view) { // Create iterators to read data from the message buffer. - sensor_msgs::PointCloud2ConstIterator iterX(rosMsg, fieldNames[0]); - sensor_msgs::PointCloud2ConstIterator iterY(rosMsg, fieldNames[1]); + sensor_msgs::PointCloud2ConstIterator iterX(rosMsg, fieldNames[0]); + sensor_msgs::PointCloud2ConstIterator iterY(rosMsg, fieldNames[1]); // Dispatch a deserialization routine based on dimensions. if (is3dPointCloud) { - sensor_msgs::PointCloud2ConstIterator iterZ(rosMsg, fieldNames[2]); + sensor_msgs::PointCloud2ConstIterator iterZ(rosMsg, fieldNames[2]); for (size_t i = 0; i < pointCount; ++i, ++iterX, ++iterY, ++iterZ) { view(0, i) = *iterX; From d79f900b15cd8de369866e3b018328191007ab9c Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Tue, 27 Oct 2020 13:20:55 +0100 Subject: [PATCH 6/6] Fix typo --- include/pointmatcher_ros/RosPointCloud2Deserializer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pointmatcher_ros/RosPointCloud2Deserializer.h b/include/pointmatcher_ros/RosPointCloud2Deserializer.h index 7ff2c39..7de288f 100644 --- a/include/pointmatcher_ros/RosPointCloud2Deserializer.h +++ b/include/pointmatcher_ros/RosPointCloud2Deserializer.h @@ -21,7 +21,7 @@ namespace PointMatcher_ROS { -template +template class RosPointCloud2Deserializer { public: