From d96bab0b1fa1a3b64b51aac948a3d395e6603664 Mon Sep 17 00:00:00 2001 From: pvela2017 Date: Sat, 7 Sep 2024 15:02:54 +0800 Subject: [PATCH 01/77] Add pointcloud API contract and ROS interface Added a ROS point cloud interface for float data type and it respective testcase Signed-off-by: Pablo Vela --- .../beluga/sensor/data/point_cloud.hpp | 20 +- beluga_ros/include/beluga_ros/beluga_ros.hpp | 1 + beluga_ros/include/beluga_ros/messages.hpp | 19 +- beluga_ros/include/beluga_ros/point_cloud.hpp | 69 +- beluga_ros/test/cmake/ament.cmake | 5 + beluga_ros/test/cmake/catkin.cmake | 7 + beluga_ros/test/test_point_cloud.cpp | 865 +----------------- 7 files changed, 75 insertions(+), 911 deletions(-) diff --git a/beluga/include/beluga/sensor/data/point_cloud.hpp b/beluga/include/beluga/sensor/data/point_cloud.hpp index 295d4de09..a7bc285b0 100644 --- a/beluga/include/beluga/sensor/data/point_cloud.hpp +++ b/beluga/include/beluga/sensor/data/point_cloud.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Ekumen, Inc. +// Copyright 2023 Ekumen, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,14 +21,10 @@ #include + /** * \file - * \brief Implementation of a point cloud interface with memory-aligned data. - * - * \details - * Assumes data is aligned as X, Y, Z, other datafields . - * All datafields are the same type (float or double). - * The result of the data stide division by the type of data must be an integer. + * \brief Implementation of a point cloud interface. */ namespace beluga { @@ -36,11 +32,11 @@ namespace beluga { /** * \page PointCloudPage Beluga named requirements: PointCloud * - * A type `P` satisfies `PointCloud` requirements if given `p` a possibly - * const instance of `P`: - * - `P::Scalar` is defined and is a scalar type to be used for x, y and z coordinates values, - * - p.points()` returns a view to the unorganized 3D point collection of `Eigen::Map` type, - * - `p.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local + * A type `L` satisfies `PointCloud` requirements if given `l` a possibly + * const instance of `L`: + * - `L::Scalar` is defined and is a scalar type to be used for x, y and z coordinates values, + * - l.points()` returns a view of a point cloud, + * - `l.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local * frame in the sensor space; */ diff --git a/beluga_ros/include/beluga_ros/beluga_ros.hpp b/beluga_ros/include/beluga_ros/beluga_ros.hpp index 8f993ba81..02033d545 100644 --- a/beluga_ros/include/beluga_ros/beluga_ros.hpp +++ b/beluga_ros/include/beluga_ros/beluga_ros.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp index e718c24c7..6be06cfb1 100644 --- a/beluga_ros/include/beluga_ros/messages.hpp +++ b/beluga_ros/include/beluga_ros/messages.hpp @@ -24,9 +24,7 @@ #include #include #include -#include -#include -#include +#include #include #include #include @@ -41,9 +39,7 @@ #include #include #include -#include -#include -#include +#include #include #include #include @@ -65,12 +61,11 @@ using LaserScan = sensor_msgs::msg::LaserScan; using LaserScanConstSharedPtr = LaserScan::ConstSharedPtr; using PointCloud2 = sensor_msgs::msg::PointCloud2; using PointCloud2ConstSharedPtr = PointCloud2::ConstSharedPtr; -template +template using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator; -template +template using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator; using PointCloud2Modifier = sensor_msgs::PointCloud2Modifier; -using PointField = sensor_msgs::msg::PointField; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; using OccupancyGrid = nav_msgs::msg::OccupancyGrid; @@ -88,12 +83,8 @@ using LaserScan = sensor_msgs::LaserScan; using LaserScanConstSharedPtr = LaserScan::ConstPtr; using PointCloud2 = sensor_msgs::PointCloud2; using PointCloud2ConstSharedPtr = PointCloud2::ConstPtr; -template +template using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator; -template -using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator; -using PointCloud2Modifier = sensor_msgs::PointCloud2Modifier; -using PointField = sensor_msgs::PointField; using Marker = visualization_msgs::Marker; using MarkerArray = visualization_msgs::MarkerArray; using OccupancyGrid = nav_msgs::OccupancyGrid; diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index e810729d1..e1f65a2fe 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Ekumen, Inc. +// Copyright 2023 Ekumen, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,80 +23,49 @@ #include -#include - -#include "beluga/eigen_compatibility.hpp" - /** * \file - * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages with memory-aligned strides. - * - * \details - * The stride calculation ensures that the memory layout of the point cloud data is aligned with the size of the data - * type used for iteration (`iteratorType`). To maintain proper memory alignment, the stride must satisfy the condition: - * - * `cloud_->point_step % sizeof(iteratorType) == 0` - * - * This condition guarantees that `point_step` is a multiple of the size of the `iteratorType`, ensuring efficient - * access to each point in the cloud. + * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type. */ namespace beluga_ros { /// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. -template -class PointCloud3 : public beluga::BasePointCloud> { +class PointCloud2 : public beluga::BasePointCloud { public: - /// PointCloud type - using Scalar = T; - - /// Check type is float or double - static_assert( - std::is_same_v || std::is_same_v, - "Pointcloud3 only supports float or double datatype"); + /// Range type. + using Scalar = double; /// Constructor. /// /// \param cloud Point cloud message. /// \param origin Point cloud frame origin in the filter frame. - explicit PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d()) - : cloud_(std::move(cloud)), origin_(std::move(origin)) { + explicit PointCloud2( + beluga_ros::msg::PointCloud2ConstSharedPtr cloud, + Sophus::SE3d origin = Sophus::SE3d()) + : cloud_(std::move(cloud)), + origin_(std::move(origin)), + iter_points_(*cloud_, "x") { assert(cloud_ != nullptr); - constexpr uint8_t fieldType = sensor_msgs::typeAsPointFieldType::value; - // Check if point cloud is 3D - if (cloud_->fields.size() < 3) { - throw std::invalid_argument("PointCloud is not 3D"); - } - // Check point cloud is XYZ... type - if (cloud_->fields.at(0).name != "x" || cloud_->fields.at(1).name != "y" || cloud_->fields.at(2).name != "z") { - throw std::invalid_argument("PointCloud not XYZ..."); - } - // Check XYZ datatype is the same - if (cloud_->fields.at(0).datatype != fieldType || cloud_->fields.at(1).datatype != fieldType || - cloud_->fields.at(2).datatype != fieldType) { - throw std::invalid_argument("XYZ datatype are not same"); - } - // Check stride is divisible - if (cloud_->point_step % sizeof(Scalar) != 0) { - throw std::invalid_argument("Data is not memory-aligned"); - } - stride_ = static_cast(cloud_->point_step / sizeof(Scalar)); } /// Get the point cloud frame origin in the filter frame. [[nodiscard]] const auto& origin() const { return origin_; } - /// Get the unorganized 3D point collection as an Eigen Map. + /// Get point cloud view as a tuple. [[nodiscard]] auto points() const { - const beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); - return Eigen::Map, 0, Eigen::OuterStride<>>( - &iter_points[0], 3, cloud_->width * cloud_->height, stride_); + return ranges::views::iota(0, static_cast(cloud_->width * cloud_->height)) | + ranges::views::transform([this](int i) { + return std::make_tuple(static_cast(iter_points_[3 * i + 0]), + static_cast(iter_points_[3 * i + 1]), + static_cast(iter_points_[3 * i + 2])); + }); } private: beluga_ros::msg::PointCloud2ConstSharedPtr cloud_; - int stride_; Sophus::SE3d origin_; + beluga_ros::msg::PointCloud2ConstIterator iter_points_; }; } // namespace beluga_ros diff --git a/beluga_ros/test/cmake/ament.cmake b/beluga_ros/test/cmake/ament.cmake index a210e15cf..804e3ada1 100644 --- a/beluga_ros/test/cmake/ament.cmake +++ b/beluga_ros/test/cmake/ament.cmake @@ -14,6 +14,7 @@ find_package(ament_cmake_gmock REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) ament_add_gmock(test_amcl test_amcl.cpp) target_compile_options(test_amcl PRIVATE -Wno-deprecated-copy) @@ -42,3 +43,7 @@ target_link_libraries(test_particle_cloud beluga_ros) ament_add_gmock(test_point_cloud test_point_cloud.cpp) target_compile_options(test_point_cloud PRIVATE -Wno-deprecated-copy) target_link_libraries(test_point_cloud beluga_ros) + +ament_add_gmock(test_point_cloud test_point_cloud.cpp) +target_compile_options(test_point_cloud PRIVATE -Wno-deprecated-copy) +target_link_libraries(test_point_cloud beluga_ros) \ No newline at end of file diff --git a/beluga_ros/test/cmake/catkin.cmake b/beluga_ros/test/cmake/catkin.cmake index aec116941..ce3500e4e 100644 --- a/beluga_ros/test/cmake/catkin.cmake +++ b/beluga_ros/test/cmake/catkin.cmake @@ -62,3 +62,10 @@ target_link_libraries( ${PROJECT_NAME} ${catkin_LIBRARIES} gmock_main) + +catkin_add_gmock(test_point_cloud test_point_cloud.cpp) +target_link_libraries( + test_point_cloud + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gmock_main) \ No newline at end of file diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index ef81c25d6..f50682eff 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Ekumen, Inc. +// Copyright 2023 Ekumen, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,16 +19,8 @@ #include -#if BELUGA_ROS_VERSION == 1 -#include -#endif - -#include - -#include "beluga/eigen_compatibility.hpp" -#include "beluga_ros/messages.hpp" #include "beluga_ros/point_cloud.hpp" -#include "beluga_ros/sparse_point_cloud.hpp" +#include "beluga_ros/messages.hpp" namespace { @@ -40,835 +32,38 @@ auto make_message() { #endif } -template -auto make_pointcloud( - const typename std::vector::size_type& fields, - const unsigned int width, - const unsigned int height, - const std::vector>& point_data = {}, - const bool empty = false) { - if (point_data.size() < static_cast(width * height) && !empty) - throw std::invalid_argument("Not enough points"); - +TEST(TestPointCloud, PointsFromUnorderedPointCloudMessage) { auto message = make_message(); - - message->width = width; - message->height = height; - message->fields.clear(); - message->fields.reserve(fields); - - const std::vector intensity_data = {static_cast(1.1), static_cast(2.2), static_cast(3.3), - static_cast(4.4), static_cast(5.5), static_cast(6.6), - static_cast(7.7), static_cast(8.8), static_cast(9.9)}; - - // XY pointclouds - if (fields == 2) { - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, U, offset); - offset = addPointField(*message, "y", 1, U, offset); - - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - - // Return empty pointcloud - if (empty) - return message; - - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - - // Fill the PointCloud2 message - for (const auto& point : point_data) { - *iter_x = point.x(); - *iter_y = point.y(); - - ++iter_x; - ++iter_y; - } - } - - // XYZ pointclouds - else if (fields == 3) { - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, U, offset); - offset = addPointField(*message, "y", 1, U, offset); - offset = addPointField(*message, "z", 1, U, offset); - - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - - // Return empty pointcloud - if (empty) - return message; - - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - - // Fill the PointCloud2 message - for (const auto& point : point_data) { - *iter_x = point.x(); - *iter_y = point.y(); - *iter_z = point.z(); - - ++iter_x; - ++iter_y; - ++iter_z; - } - } - - // XYZI pointclouds - else if (fields == 4) { - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, U, offset); - offset = addPointField(*message, "y", 1, U, offset); - offset = addPointField(*message, "z", 1, U, offset); - offset = addPointField(*message, "intensity", 1, U, offset); - - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - - // Return empty pointcloud - if (empty) - return message; - - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); - - // Fill the PointCloud2 message - typename std::vector::size_type i = 0; - for (const auto& point : point_data) { - *iter_x = point.x(); - *iter_y = point.y(); - *iter_z = point.z(); - *iter_intensity = intensity_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - ++i; - } - } - - // Error - else { - throw std::invalid_argument("Number of fields error"); - } - return message; -} - -TEST(TestPointCloud, XYZPointsUnorderedPC) { const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 3; - const unsigned int width = 1; - const unsigned int height = 5; + message->width = 1; // Unordered point cloud + message->height = 4; // Number of points + // Set the point fields to x, y, z, and rgb + beluga_ros::msg::PointCloud2Modifier modifier(*message); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(message->width * message->height); // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - - // Check aligned pointcloud - const auto cloud = beluga_ros::PointCloud3(message, origin); - const auto map = cloud.points(); - // Check assert - for (unsigned i = 0; i < map.cols(); ++i) { - ASSERT_EQ(point_data.at(i).x(), map(0, i)); - ASSERT_EQ(point_data.at(i).y(), map(1, i)); - ASSERT_EQ(point_data.at(i).z(), map(2, i)); - } - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } -} - -TEST(TestPointCloud, XYZPointsOrderedPC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 3; - const unsigned int width = 3; - const unsigned int height = 3; - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F), - Eigen::Vector3f(16.0F, 17.0F, 18.0F), - Eigen::Vector3f(19.0F, 20.0F, 21.0F), - Eigen::Vector3f(22.0F, 23.0F, 24.0F), - Eigen::Vector3f(25.0F, 26.0F, 27.0F) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - - // Check aligned pointcloud - const auto cloud = beluga_ros::PointCloud3(message, origin); - const auto map = cloud.points(); - // Check assert - for (unsigned i = 0; i < map.cols(); ++i) { - ASSERT_EQ(point_data.at(i).x(), map(0, i)); - ASSERT_EQ(point_data.at(i).y(), map(1, i)); - ASSERT_EQ(point_data.at(i).z(), map(2, i)); - } - - // Check sparse pointcloud - auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } -} - -TEST(TestPointCloud, XYZIPointsUnorderedPC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 4; - const unsigned int width = 1; - const unsigned int height = 5; - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - - // Check aligned pointcloud - const auto cloud = beluga_ros::PointCloud3(message, origin); - const auto map = cloud.points(); - // Check assert - for (unsigned i = 0; i < map.cols(); ++i) { - ASSERT_EQ(point_data.at(i).x(), map(0, i)); - ASSERT_EQ(point_data.at(i).y(), map(1, i)); - ASSERT_EQ(point_data.at(i).z(), map(2, i)); - } - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } -} - -TEST(TestPointCloud, XYZIPointsOrderedPC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 4; - const unsigned int width = 3; - const unsigned int height = 3; - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F), - Eigen::Vector3f(16.0F, 17.0F, 18.0F), - Eigen::Vector3f(19.0F, 20.0F, 21.0F), - Eigen::Vector3f(22.0F, 23.0F, 24.0F), - Eigen::Vector3f(25.0F, 26.0F, 27.0F) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - - // Check aligned pointcloud - const auto cloud = beluga_ros::PointCloud3(message, origin); - const auto map = cloud.points(); - // Check assert - for (unsigned i = 0; i < map.cols(); ++i) { - ASSERT_EQ(point_data.at(i).x(), map(0, i)); - ASSERT_EQ(point_data.at(i).y(), map(1, i)); - ASSERT_EQ(point_data.at(i).z(), map(2, i)); - } - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } -} - -TEST(TestPointCloud, XYZIDoublePC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 4; - const unsigned int width = 1; - const unsigned int height = 5; - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3d(1.0, 2.0, 3.0), - Eigen::Vector3d(4.0, 5.0, 6.0), - Eigen::Vector3d(7.0, 8.0, 9.0), - Eigen::Vector3d(10.0, 11.0, 12.0), - Eigen::Vector3d(13.0, 14.0, 15.0) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - - // Check aligned pointcloud - const auto cloud = beluga_ros::PointCloud3(message, origin); - const auto map = cloud.points(); - // Check assert - for (unsigned i = 0; i < map.cols(); ++i) { - ASSERT_EQ(point_data.at(i).x(), map(0, i)); - ASSERT_EQ(point_data.at(i).y(), map(1, i)); - ASSERT_EQ(point_data.at(i).z(), map(2, i)); - } - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } -} - -TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 3; - const unsigned int width = 1; - const unsigned int height = 5; - const std::vector& point_data = {}; - const bool empty = true; - // Create point cloud message - const auto message = - make_pointcloud(fields, width, height, point_data, empty); - - // Check aligned pointcloud - const auto cloud = beluga_ros::PointCloud3(message, origin); - const auto map = cloud.points(); - // Check assert - for (unsigned i = 0; i < map.cols(); ++i) { - ASSERT_EQ(message->data.at(3 * i + 0), map(0, i)); - ASSERT_EQ(message->data.at(3 * i + 1), map(1, i)); - ASSERT_EQ(message->data.at(3 * i + 2), map(2, i)); - } - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(message->data.at(3 * i + 0), vector.at(static_cast(i)).x()); - ASSERT_EQ(message->data.at(3 * i + 1), vector.at(static_cast(i)).y()); - ASSERT_EQ(message->data.at(3 * i + 2), vector.at(static_cast(i)).z()); - } -} - -TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 4; - const unsigned int width = 1; - const unsigned int height = 5; - const std::vector& point_data = {}; - const bool empty = true; - // Create point cloud message - const auto message = - make_pointcloud(fields, width, height, point_data, empty); - - // Check aligned pointcloud - const auto cloud = beluga_ros::PointCloud3(message, origin); - const auto map = cloud.points(); - // Check assert - for (unsigned i = 0; i < map.cols(); ++i) { - ASSERT_EQ(message->data.at(3 * i + 0), map(0, i)); - ASSERT_EQ(message->data.at(3 * i + 1), map(1, i)); - ASSERT_EQ(message->data.at(3 * i + 2), map(2, i)); - } - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(message->data.at(3 * i + 0), vector.at(static_cast(i)).x()); - ASSERT_EQ(message->data.at(3 * i + 1), vector.at(static_cast(i)).y()); - ASSERT_EQ(message->data.at(3 * i + 2), vector.at(static_cast(i)).z()); - } -} - -TEST(TestPointCloud, 2DUnorderedPC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 2; - const unsigned int width = 1; - const unsigned int height = 5; - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, 2DOrderedPC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 2; - const unsigned int width = 2; - const unsigned int height = 2; - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, EmptyFieldsPC) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 4; // Number of points - // Set the point fields to x, y, z and intensity - const std::vector::size_type fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - message->is_dense = static_castis_dense)>(true); - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, WrongTypePC) { - const auto origin = Sophus::SE3d{}; - // Define pointcloud params - const std::vector::size_type fields = 4; - const unsigned int width = 1; - const unsigned int height = 5; - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3d(1.0, 2.0, 3.0), - Eigen::Vector3d(4.0, 5.0, 6.0), - Eigen::Vector3d(7.0, 8.0, 9.0), - Eigen::Vector3d(10.0, 11.0, 12.0), - Eigen::Vector3d(13.0, 14.0, 15.0) - }; - // clang-format on - // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, VoidPC) { - const auto origin = Sophus::SE3d{}; - auto message = make_message(); - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, IXYZPC) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y, z and intensity - const std::vector::size_type fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; - // Fill the PointCloud2 message - for (size_t i = 0; i < point_data.size(); ++i) { - *iter_x = point_data.at(i).x(); - *iter_y = point_data.at(i).y(); - *iter_z = point_data.at(i).z(); - *iter_intensity = intensity_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - } - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, ZXYIPC) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y, z and intensity - const std::vector::size_type fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; + std::vector point_data = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0}; + // Initialize iterators for x, y, z and rgb fields + beluga_ros::msg::PointCloud2Iterator iter_points(*message, "x"); // Fill the PointCloud2 message - for (size_t i = 0; i < point_data.size(); ++i) { - *iter_x = point_data.at(i).x(); - *iter_y = point_data.at(i).y(); - *iter_z = point_data.at(i).z(); - *iter_intensity = intensity_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - } - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, Velodyne) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields as velodyne - const std::vector::size_type fields = 6; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointField::UINT16, offset); - offset = addPointField(*message, "time", 1, beluga_ros::msg::PointField::FLOAT32, offset); - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); - beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); - beluga_ros::msg::PointCloud2Iterator iter_time(*message, "time"); - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; - const std::vector ring_data = {1, 2, 3, 4, 5}; - const std::vector time_data = {0.1F, 0.2F, 0.3F, 0.4F, 0.5F}; - // Fill the PointCloud2 message - for (size_t i = 0; i < point_data.size(); ++i) { - *iter_x = point_data.at(i).x(); - *iter_y = point_data.at(i).y(); - *iter_z = point_data.at(i).z(); - *iter_intensity = intensity_data.at(i); - *iter_ring = ring_data.at(i); - *iter_time = time_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - ++iter_ring; - ++iter_time; - } - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } -} - -TEST(TestPointCloud, Robosense) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields as robosense - const std::vector::size_type fields = 6; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointField::UINT16, offset); - offset = addPointField(*message, "time", 1, beluga_ros::msg::PointField::FLOAT64, offset); - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); - beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); - beluga_ros::msg::PointCloud2Iterator iter_time(*message, "time"); - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; - const std::vector ring_data = {1, 2, 3, 4, 5}; - const std::vector time_data = {0.1, 0.2, 0.3, 0.4, 0.5}; - // Fill the PointCloud2 message - for (size_t i = 0; i < point_data.size(); ++i) { - *iter_x = point_data.at(i).x(); - *iter_y = point_data.at(i).y(); - *iter_z = point_data.at(i).z(); - *iter_intensity = intensity_data.at(i); - *iter_ring = ring_data.at(i); - *iter_time = time_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - ++iter_ring; - ++iter_time; - } - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } -} - -TEST(TestPointCloud, Ouster) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields as ouster - const std::vector::size_type fields = 9; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset += 4; - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); - offset = addPointField(*message, "t", 1, beluga_ros::msg::PointField::UINT32, offset); - offset = addPointField(*message, "reflectivity", 1, beluga_ros::msg::PointField::UINT16, offset); - offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointField::UINT8, offset); - offset = addPointField(*message, "ambient", 1, beluga_ros::msg::PointField::UINT16, offset); - offset = addPointField(*message, "range", 1, beluga_ros::msg::PointField::UINT32, offset); - // Set message params - message->point_step = static_cast(offset); - message->row_step = message->width * message->point_step; - message->is_dense = static_castis_dense)>(true); - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); - beluga_ros::msg::PointCloud2Iterator iter_time(*message, "t"); - beluga_ros::msg::PointCloud2Iterator iter_reflectivity(*message, "reflectivity"); - beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); - beluga_ros::msg::PointCloud2Iterator iter_ambient(*message, "ambient"); - beluga_ros::msg::PointCloud2Iterator iter_range(*message, "range"); - // Create some raw data for the points - // clang-format off - const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), - Eigen::Vector3f(4.0F, 5.0F, 6.0F), - Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), - Eigen::Vector3f(13.0F, 14.0F, 15.0F) - }; - // clang-format on - const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; - const std::vector time_data = {1, 2, 3, 4, 5}; - const std::vector reflectivity_data = {10, 9, 8, 7, 6}; - const std::vector ring_data = {11, 12, 13, 14, 15}; - const std::vector ambient_data = {20, 19, 18, 17, 16}; - const std::vector range_data = {21, 22, 23, 24, 25}; - - // Fill the PointCloud2 message - for (size_t i = 0; i < point_data.size(); ++i) { - *iter_x = point_data.at(i).x(); - *iter_y = point_data.at(i).y(); - *iter_z = point_data.at(i).z(); - *iter_intensity = intensity_data.at(i); - *iter_time = time_data.at(i); - *iter_reflectivity = reflectivity_data.at(i); - *iter_ring = ring_data.at(i); - *iter_ambient = ambient_data.at(i); - *iter_range = range_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - ++iter_time; - ++iter_reflectivity; - ++iter_ring; - ++iter_ambient; - ++iter_range; - } - // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); - - // Check sparse pointcloud - const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); - auto vector = cloud_sparse.points(); - // Check assert - for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); - } + for (unsigned i = 0; i < point_data.size()/3; ++i) + { + iter_points[3 * i + 0] = point_data.at(3 * i + 0); + iter_points[3 * i + 1] = point_data.at(3 * i + 1); + iter_points[3 * i + 2] = point_data.at(3 * i + 2); + } + auto cloud = beluga_ros::PointCloud2(message, origin); + int i = 0; + for (const auto& point : cloud.points()) + { + std::apply([&](auto x, auto y, auto z){ + ASSERT_EQ(x, point_data[3 * i + 0]); + ASSERT_EQ(y, point_data[3 * i + 1]); + ASSERT_EQ(z, point_data[3 * i + 2]); + }, point); + i++; + } + } } // namespace From 251c03b909b9e45e81f596e5ed70383a227a2302 Mon Sep 17 00:00:00 2001 From: pvela2017 Date: Tue, 10 Sep 2024 10:48:01 +0800 Subject: [PATCH 02/77] Change PointCloud description for better clarification Changed description names to be more natural and specified pointcloud requirements Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor/data/point_cloud.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/beluga/include/beluga/sensor/data/point_cloud.hpp b/beluga/include/beluga/sensor/data/point_cloud.hpp index a7bc285b0..910933933 100644 --- a/beluga/include/beluga/sensor/data/point_cloud.hpp +++ b/beluga/include/beluga/sensor/data/point_cloud.hpp @@ -32,11 +32,11 @@ namespace beluga { /** * \page PointCloudPage Beluga named requirements: PointCloud * - * A type `L` satisfies `PointCloud` requirements if given `l` a possibly - * const instance of `L`: - * - `L::Scalar` is defined and is a scalar type to be used for x, y and z coordinates values, - * - l.points()` returns a view of a point cloud, - * - `l.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local + * A type `P` satisfies `PointCloud` requirements if given `p` a possibly + * const instance of `P`: + * - `P::Scalar` is defined and is a scalar type to be used for x, y and z coordinates values, + * - p.points()` returns a view to the unorganized 3D point collection, + * - `p.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local * frame in the sensor space; */ From 370a6fa664a2dfb655f559f231e1c964dd4e5b25 Mon Sep 17 00:00:00 2001 From: pvela2017 Date: Tue, 10 Sep 2024 10:49:19 +0800 Subject: [PATCH 03/77] Changed copyright year Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor/data/point_cloud.hpp | 2 +- beluga_ros/include/beluga_ros/point_cloud.hpp | 2 +- beluga_ros/test/test_point_cloud.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/beluga/include/beluga/sensor/data/point_cloud.hpp b/beluga/include/beluga/sensor/data/point_cloud.hpp index 910933933..400b6a369 100644 --- a/beluga/include/beluga/sensor/data/point_cloud.hpp +++ b/beluga/include/beluga/sensor/data/point_cloud.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Ekumen, Inc. +// Copyright 2024 Ekumen, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index e1f65a2fe..0407f14a5 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Ekumen, Inc. +// Copyright 2024 Ekumen, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index f50682eff..3969f2687 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Ekumen, Inc. +// Copyright 2024 Ekumen, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 54c44171c9af474ca928993659e1a653dd85c5ef Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 16 Sep 2024 16:59:48 +0800 Subject: [PATCH 04/77] Add newlines Signed-off-by: Pablo Vela --- beluga_ros/test/cmake/ament.cmake | 3 ++- beluga_ros/test/cmake/catkin.cmake | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/beluga_ros/test/cmake/ament.cmake b/beluga_ros/test/cmake/ament.cmake index 804e3ada1..0e94b819a 100644 --- a/beluga_ros/test/cmake/ament.cmake +++ b/beluga_ros/test/cmake/ament.cmake @@ -46,4 +46,5 @@ target_link_libraries(test_point_cloud beluga_ros) ament_add_gmock(test_point_cloud test_point_cloud.cpp) target_compile_options(test_point_cloud PRIVATE -Wno-deprecated-copy) -target_link_libraries(test_point_cloud beluga_ros) \ No newline at end of file +target_link_libraries(test_point_cloud beluga_ros) + diff --git a/beluga_ros/test/cmake/catkin.cmake b/beluga_ros/test/cmake/catkin.cmake index ce3500e4e..3e0efc290 100644 --- a/beluga_ros/test/cmake/catkin.cmake +++ b/beluga_ros/test/cmake/catkin.cmake @@ -68,4 +68,4 @@ target_link_libraries( test_point_cloud ${PROJECT_NAME} ${catkin_LIBRARIES} - gmock_main) \ No newline at end of file + gmock_main) From 74815433ad7c8737fcca42642801e3828069a480 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 16 Sep 2024 17:06:02 +0800 Subject: [PATCH 05/77] Expand testcase scenarios Added new testcases to cover all the cases Signed-off-by: Pablo Vela --- beluga_ros/test/test_point_cloud.cpp | 762 ++++++++++++++++++++++++++- 1 file changed, 733 insertions(+), 29 deletions(-) diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 3969f2687..d51284396 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -19,8 +19,8 @@ #include -#include "beluga_ros/point_cloud.hpp" #include "beluga_ros/messages.hpp" +#include "beluga_ros/point_cloud.hpp" namespace { @@ -32,38 +32,742 @@ auto make_message() { #endif } -TEST(TestPointCloud, PointsFromUnorderedPointCloudMessage) { +TEST(TestPointCloud, XYZPointsUnorderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y and z + int fields = 3; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, + 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + + ++iter_x; + ++iter_y; + ++iter_z; + } + auto cloud = beluga_ros::PointCloud3(message, origin); + auto map = cloud.points(); + // Check assert + for (int i = 0; i < map.cols(); ++i) { + for (int j = 0; j < map.rows(); ++j) { + ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); + } + } +} + +TEST(TestPointCloud, XYZPointsOrderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 3; // Unordered point cloud + message->height = 3; // Number of points + // Set the point fields to x, y and z + int fields = 3; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, + 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, + 19.0f, 20.0f, 21.0f, 22.0f, 23.0f, 24.0f, 25.0f, 26.0f, 27.0f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + + ++iter_x; + ++iter_y; + ++iter_z; + } + auto cloud = beluga_ros::PointCloud3(message, origin); + auto map = cloud.points(); + // Check assert + for (int i = 0; i < map.cols(); ++i) { + for (int j = 0; j < map.rows(); ++j) { + ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); + } + } +} + +TEST(TestPointCloud, XYZIPointsUnorderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, + 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; + const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + } + auto cloud = beluga_ros::PointCloud3(message, origin); + auto map = cloud.points(); + // Check assert + for (int i = 0; i < map.cols(); ++i) { + for (int j = 0; j < map.rows(); ++j) { + ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); + } + } +} + +TEST(TestPointCloud, XYZIPointsOrderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 3; // Ordered point cloud + message->height = 3; + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, + 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, + 19.0f, 20.0f, 21.0f, 22.0f, 23.0f, 24.0f, 25.0f, 26.0f, 27.0f}; + const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f, 6.6f, 7.7f, 8.8f, 9.9f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + } + auto cloud = beluga_ros::PointCloud3(message, origin); + auto map = cloud.points(); + // Check assert + for (int i = 0; i < map.cols(); ++i) { + for (int j = 0; j < map.rows(); ++j) { + ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); + } + } +} + +TEST(TestPointCloud, XYZIDoublePC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF64, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF64, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF64, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF64, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + // Create some raw data for the points + const std::vector point_data = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, + 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0}; + const std::vector intensity_data = {1.1, 2.2, 3.3, 4.4, 5.5}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + } + auto cloud = beluga_ros::PointCloud3(message, origin); + auto map = cloud.points(); + // Check assert + for (int i = 0; i < map.cols(); ++i) { + for (int j = 0; j < map.rows(); ++j) { + ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); + } + } +} + +TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y and z + int fields = 3; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + + auto cloud = beluga_ros::PointCloud3(message, origin); + auto map = cloud.points(); + // Check assert + for (int i = 0; i < map.cols(); ++i) { + for (int j = 0; j < map.rows(); ++j) { + ASSERT_EQ(map(j, i), message->data.at(3 * i + j)); + } + } +} + +TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 4; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + + auto cloud = beluga_ros::PointCloud3(message, origin); + auto map = cloud.points(); + // Check assert + for (int i = 0; i < map.cols(); ++i) { + for (int j = 0; j < map.rows(); ++j) { + ASSERT_EQ(map(j, i), message->data.at(3 * i + j)); + } + } +} + +TEST(TestPointCloud, IXYZPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, + 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; + const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + } + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, ZXYIPC) { auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 4; // Number of points - // Set the point fields to x, y, z, and rgb - beluga_ros::msg::PointCloud2Modifier modifier(*message); - modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(message->width * message->height); + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); // Create some raw data for the points - std::vector point_data = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0}; - // Initialize iterators for x, y, z and rgb fields - beluga_ros::msg::PointCloud2Iterator iter_points(*message, "x"); + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, + 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; + const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size()/3; ++i) - { - iter_points[3 * i + 0] = point_data.at(3 * i + 0); - iter_points[3 * i + 1] = point_data.at(3 * i + 1); - iter_points[3 * i + 2] = point_data.at(3 * i + 2); - } - auto cloud = beluga_ros::PointCloud2(message, origin); - int i = 0; - for (const auto& point : cloud.points()) - { - std::apply([&](auto x, auto y, auto z){ - ASSERT_EQ(x, point_data[3 * i + 0]); - ASSERT_EQ(y, point_data[3 * i + 1]); - ASSERT_EQ(z, point_data[3 * i + 2]); - }, point); - i++; - } - + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + } + /// Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, 2DUnorderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x and y + int fields = 2; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 4.0f, 5.0f, 7.0f, 8.0f, 10.0f, 11.0f, 12.0f, 13.0f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 2; ++i) { + *iter_x = point_data.at(2 * i + 0); + *iter_y = point_data.at(2 * i + 1); + + ++iter_x; + ++iter_y; + } + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, 2DOrderedPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 2; // Ordered point cloud + message->height = 2; + // Set the point fields to x and y + int fields = 2; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 4.0f, 5.0f, 7.0f, 8.0f, 10.0f, 11.0f, 12.0f, 13.0f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 2; ++i) { + *iter_x = point_data.at(2 * i + 0); + *iter_y = point_data.at(2 * i + 1); + + ++iter_x; + ++iter_y; + } + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, EmptyFieldsPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 4; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + message->is_dense = true; + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, NotDensePC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 4; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = false; + message->data.resize(message->point_step * message->width * message->height); + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, WrongTypePC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF64, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF64, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF64, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF64, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + // Create some raw data for the points + const std::vector point_data = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, + 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0}; + const std::vector intensity_data = {1.1, 2.2, 3.3, 4.4, 5.5}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + } + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, VoidPC) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, Velodyne) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields as velodyne + int fields = 6; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU16, offset); + offset = addPointField(*message, "time", 1, beluga_ros::msg::PointFieldF32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); + beluga_ros::msg::PointCloud2Iterator iter_time(*message, "time"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, + 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; + const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + const std::vector ring_data = {1, 2, 3, 4, 5}; + const std::vector time_data = {0.1f, 0.2f, 0.3f, 0.4f, 0.5f}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + *iter_ring = ring_data.at(i); + *iter_time = time_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + ++iter_ring; + ++iter_time; + } + // Check assert + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, Robosense) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields as robosense + int fields = 6; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU16, offset); + offset = addPointField(*message, "time", 1, beluga_ros::msg::PointFieldF64, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); + beluga_ros::msg::PointCloud2Iterator iter_time(*message, "time"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, + 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; + const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + const std::vector ring_data = {1, 2, 3, 4, 5}; + const std::vector time_data = {0.1, 0.2, 0.3, 0.4, 0.5}; + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + *iter_ring = ring_data.at(i); + *iter_time = time_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + ++iter_ring; + ++iter_time; + } + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); +} + +TEST(TestPointCloud, Ouster) { + auto message = make_message(); + const auto origin = Sophus::SE3d{}; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields as ouster + int fields = 9; + message->fields.clear(); + message->fields.reserve(fields); + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset += 4; + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "t", 1, beluga_ros::msg::PointFieldU32, offset); + offset = addPointField(*message, "reflectivity", 1, beluga_ros::msg::PointFieldU16, offset); + offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU8, offset); + offset = addPointField(*message, "ambient", 1, beluga_ros::msg::PointFieldU16, offset); + offset = addPointField(*message, "range", 1, beluga_ros::msg::PointFieldU32, offset); + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + beluga_ros::msg::PointCloud2Iterator iter_time(*message, "t"); + beluga_ros::msg::PointCloud2Iterator iter_reflectivity(*message, "reflectivity"); + beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); + beluga_ros::msg::PointCloud2Iterator iter_ambient(*message, "ambient"); + beluga_ros::msg::PointCloud2Iterator iter_range(*message, "range"); + // Create some raw data for the points + const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, + 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; + const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + const std::vector time_data = {1, 2, 3, 4, 5}; + const std::vector reflectivity_data = {10, 9, 8, 7, 6}; + const std::vector ring_data = {11, 12, 13, 14, 15}; + const std::vector ambient_data = {20, 19, 18, 17, 16}; + const std::vector range_data = {21, 22, 23, 24, 25}; + + // Fill the PointCloud2 message + for (unsigned i = 0; i < point_data.size() / 3; ++i) { + *iter_x = point_data.at(3 * i + 0); + *iter_y = point_data.at(3 * i + 1); + *iter_z = point_data.at(3 * i + 2); + *iter_intensity = intensity_data.at(i); + *iter_time = time_data.at(i); + *iter_reflectivity = reflectivity_data.at(i); + *iter_ring = ring_data.at(i); + *iter_ambient = ambient_data.at(i); + *iter_range = range_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_intensity; + ++iter_time; + ++iter_reflectivity; + ++iter_ring; + ++iter_ambient; + ++iter_range; + } + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); } } // namespace From 104131bfafca9f3ff99ddacf23719e3aef9de1a8 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 16 Sep 2024 17:09:59 +0800 Subject: [PATCH 06/77] Clarify documentation Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor/data/point_cloud.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/beluga/include/beluga/sensor/data/point_cloud.hpp b/beluga/include/beluga/sensor/data/point_cloud.hpp index 400b6a369..020200642 100644 --- a/beluga/include/beluga/sensor/data/point_cloud.hpp +++ b/beluga/include/beluga/sensor/data/point_cloud.hpp @@ -21,7 +21,6 @@ #include - /** * \file * \brief Implementation of a point cloud interface. @@ -35,7 +34,7 @@ namespace beluga { * A type `P` satisfies `PointCloud` requirements if given `p` a possibly * const instance of `P`: * - `P::Scalar` is defined and is a scalar type to be used for x, y and z coordinates values, - * - p.points()` returns a view to the unorganized 3D point collection, + * - p.points()` returns a view to the unorganized 3D point collection of `Eigen::Map` type, * - `p.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local * frame in the sensor space; */ From b39afcd0e1e6c55796086d4c03f3ecd64541571b Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 16 Sep 2024 17:11:43 +0800 Subject: [PATCH 07/77] Change to return Eigen::Map Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/beluga_ros.hpp | 2 - beluga_ros/include/beluga_ros/messages.hpp | 22 ++++-- beluga_ros/include/beluga_ros/point_cloud.hpp | 69 ++++++++++++++----- 3 files changed, 69 insertions(+), 24 deletions(-) diff --git a/beluga_ros/include/beluga_ros/beluga_ros.hpp b/beluga_ros/include/beluga_ros/beluga_ros.hpp index 02033d545..2e233d4aa 100644 --- a/beluga_ros/include/beluga_ros/beluga_ros.hpp +++ b/beluga_ros/include/beluga_ros/beluga_ros.hpp @@ -27,11 +27,9 @@ #include #include -#include #include #include #include -#include #include #include diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp index 6be06cfb1..b1c47ec7d 100644 --- a/beluga_ros/include/beluga_ros/messages.hpp +++ b/beluga_ros/include/beluga_ros/messages.hpp @@ -24,7 +24,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -39,7 +40,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -61,11 +63,16 @@ using LaserScan = sensor_msgs::msg::LaserScan; using LaserScanConstSharedPtr = LaserScan::ConstSharedPtr; using PointCloud2 = sensor_msgs::msg::PointCloud2; using PointCloud2ConstSharedPtr = PointCloud2::ConstSharedPtr; -template +template using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator; -template +template using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator; using PointCloud2Modifier = sensor_msgs::PointCloud2Modifier; +constexpr uint8_t PointFieldU8 = sensor_msgs::msg::PointField::UINT8; +constexpr uint8_t PointFieldU16 = sensor_msgs::msg::PointField::UINT16; +constexpr uint8_t PointFieldU32 = sensor_msgs::msg::PointField::UINT32; +constexpr uint8_t PointFieldF32 = sensor_msgs::msg::PointField::FLOAT32; +constexpr uint8_t PointFieldF64 = sensor_msgs::msg::PointField::FLOAT64; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; using OccupancyGrid = nav_msgs::msg::OccupancyGrid; @@ -83,8 +90,13 @@ using LaserScan = sensor_msgs::LaserScan; using LaserScanConstSharedPtr = LaserScan::ConstPtr; using PointCloud2 = sensor_msgs::PointCloud2; using PointCloud2ConstSharedPtr = PointCloud2::ConstPtr; -template +template using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator; +constexpr uint8_t PointFieldU8 = sensor_msgs::PointField::UINT8; +constexpr uint8_t PointFieldU16 = sensor_msgs::PointField::UINT16; +constexpr uint8_t PointFieldU32 = sensor_msgs::PointField::UINT32; +constexpr uint8_t PointFieldF32 = sensor_msgs::PointField::FLOAT32; +constexpr uint8_t PointFieldF64 = sensor_msgs::PointField::FLOAT64; using Marker = visualization_msgs::Marker; using MarkerArray = visualization_msgs::MarkerArray; using OccupancyGrid = nav_msgs::OccupancyGrid; diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index 0407f14a5..433845566 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -23,6 +23,8 @@ #include +#include + /** * \file * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type. @@ -30,42 +32,75 @@ namespace beluga_ros { +template +struct DataType; + +template <> +struct DataType { + using iteratorType = float; + using eigenType = const Eigen::Matrix3Xf; +}; + +template <> +struct DataType { + using iteratorType = double; + using eigenType = const Eigen::Matrix3Xd; +}; + /// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. -class PointCloud2 : public beluga::BasePointCloud { +/// Assumes an XYZ... type message. +/// Each field must have the same datatype. +/// The point cloud can't have invalid values, i.e., it must be dense. +template +class PointCloud3 : public beluga::BasePointCloud> { public: - /// Range type. - using Scalar = double; + /// PointCloud data fields type + using iteratorType = typename DataType::iteratorType; + using eigenType = typename DataType::eigenType; + + /// Check type is float or double + static_assert( + std::is_same::value || std::is_same::value, + "Pointcloud3 only supports float or double datatype"); /// Constructor. /// /// \param cloud Point cloud message. /// \param origin Point cloud frame origin in the filter frame. - explicit PointCloud2( - beluga_ros::msg::PointCloud2ConstSharedPtr cloud, - Sophus::SE3d origin = Sophus::SE3d()) - : cloud_(std::move(cloud)), - origin_(std::move(origin)), - iter_points_(*cloud_, "x") { + explicit PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d()) + : cloud_(std::move(cloud)), origin_(std::move(origin)) { + // Check there are not invalid values + if (!cloud_->is_dense) + throw std::invalid_argument("PointCloud is not dense"); + // Check if point cloud is 3D + if (cloud_->fields.size() < 3) + throw std::invalid_argument("PointCloud is not 3D"); + // Check point cloud is XYZ... type + if (cloud_->fields.at(0).name != "x" && cloud_->fields.at(1).name != "y" && cloud_->fields.at(2).name != "z") + throw std::invalid_argument("PointCloud not XYZ..."); + // Check all datatype is the same + if (!std::all_of( + cloud_->fields.begin(), cloud_->fields.end(), [&](const auto& field) { return field.datatype == T; })) { + throw std::invalid_argument("Fields do not match pointcloud datatype"); + } assert(cloud_ != nullptr); } /// Get the point cloud frame origin in the filter frame. [[nodiscard]] const auto& origin() const { return origin_; } - /// Get point cloud view as a tuple. + /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { - return ranges::views::iota(0, static_cast(cloud_->width * cloud_->height)) | - ranges::views::transform([this](int i) { - return std::make_tuple(static_cast(iter_points_[3 * i + 0]), - static_cast(iter_points_[3 * i + 1]), - static_cast(iter_points_[3 * i + 2])); - }); + beluga_ros::msg::PointCloud2ConstIterator iterPoints(*cloud_, "x"); + int stride_step = static_cast((cloud_->point_step + sizeof(iteratorType) - 1) / sizeof(iteratorType)); + Eigen::Map> map( + &iterPoints[0], 3, cloud_->width * cloud_->height, Eigen::OuterStride<>(stride_step)); + return map; } private: beluga_ros::msg::PointCloud2ConstSharedPtr cloud_; Sophus::SE3d origin_; - beluga_ros::msg::PointCloud2ConstIterator iter_points_; }; } // namespace beluga_ros From 18cbff5f530860b4a969f040b2280a9032eb2736 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 19 Sep 2024 15:06:33 +0800 Subject: [PATCH 08/77] Add make pointcloud message function Signed-off-by: Pablo Vela --- beluga_ros/test/test_point_cloud.cpp | 964 +++++++++++++++------------ 1 file changed, 530 insertions(+), 434 deletions(-) diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index d51284396..92b3188d9 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -21,6 +21,7 @@ #include "beluga_ros/messages.hpp" #include "beluga_ros/point_cloud.hpp" +#include "beluga_ros/point_cloud_sparse.hpp" namespace { @@ -32,452 +33,513 @@ auto make_message() { #endif } -TEST(TestPointCloud, XYZPointsUnorderedPC) { +template +auto make_pointcloud( + const int& fields, + const int& width, + const int& height, + const std::vector>& point_data = {}, + const bool empty = false) { + if (point_data.size() < static_cast(width * height) && !empty) + throw std::invalid_argument("Not enough points"); + auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y and z - int fields = 3; + + message->width = width; + message->height = height; message->fields.clear(); message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, - 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); - ++iter_x; - ++iter_y; - ++iter_z; + const std::vector intensity_data = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + + // XY pointclouds + if (fields == 2) { + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, U, offset); + offset = addPointField(*message, "y", 1, U, offset); + + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + + // Return empty pointcloud + if (empty) + return message; + + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + + // Fill the PointCloud2 message + for (const auto& point : point_data) { + *iter_x = point.x(); + *iter_y = point.y(); + + ++iter_x; + ++iter_y; + } + } + + // XYZ pointclouds + else if (fields == 3) { + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, U, offset); + offset = addPointField(*message, "y", 1, U, offset); + offset = addPointField(*message, "z", 1, U, offset); + + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + + // Return empty pointcloud + if (empty) + return message; + + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + + // Fill the PointCloud2 message + for (const auto& point : point_data) { + *iter_x = point.x(); + *iter_y = point.y(); + *iter_z = point.z(); + + ++iter_x; + ++iter_y; + ++iter_z; + } + } + + // XYZI pointclouds + else if (fields == 4) { + // Set offset + int offset = 0; + offset = addPointField(*message, "x", 1, U, offset); + offset = addPointField(*message, "y", 1, U, offset); + offset = addPointField(*message, "z", 1, U, offset); + offset = addPointField(*message, "intensity", 1, U, offset); + + // Set message params + message->point_step = offset; + message->row_step = message->width * message->point_step; + message->is_dense = true; + message->data.resize(message->point_step * message->width * message->height); + + // Return empty pointcloud + if (empty) + return message; + + // Create data iterators + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + + // Fill the PointCloud2 message + int i = 0; + for (const auto& point : point_data) { + *iter_x = point.x(); + *iter_y = point.y(); + *iter_z = point.z(); + *iter_intensity = intensity_data.at(i); + + ++iter_x; + ++iter_y; + ++iter_z; + ++i; + } + } + + // Error + else { + throw std::invalid_argument("Number of fields error"); } + return message; +} + +TEST(TestPointCloud, XYZPointsUnorderedPC) { + const auto origin = Sophus::SE3d{}; + // Define pointcloud params + int fields = 3; + int width = 1; + int height = 5; + // Create some raw data for the points + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + + // Check aligned pointcloud auto cloud = beluga_ros::PointCloud3(message, origin); auto map = cloud.points(); // Check assert - for (int i = 0; i < map.cols(); ++i) { - for (int j = 0; j < map.rows(); ++j) { - ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); - } + for (unsigned i = 0; i < map.cols(); ++i) { + ASSERT_EQ(point_data.at(i).x(), map(0, i)); + ASSERT_EQ(point_data.at(i).y(), map(1, i)); + ASSERT_EQ(point_data.at(i).z(), map(2, i)); + } + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); } } TEST(TestPointCloud, XYZPointsOrderedPC) { - auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 3; // Unordered point cloud - message->height = 3; // Number of points - // Set the point fields to x, y and z + // Define pointcloud params int fields = 3; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + int width = 3; + int height = 3; // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, - 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, - 19.0f, 20.0f, 21.0f, 22.0f, 23.0f, 24.0f, 25.0f, 26.0f, 27.0f}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); - - ++iter_x; - ++iter_y; - ++iter_z; - } + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), Eigen::Vector3f(4.0F, 5.0F, 6.0F), Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), Eigen::Vector3f(13.0F, 14.0F, 15.0F), Eigen::Vector3f(16.0F, 17.0F, 18.0F), + Eigen::Vector3f(19.0F, 20.0F, 21.0F), Eigen::Vector3f(22.0F, 23.0F, 24.0F), Eigen::Vector3f(25.0F, 26.0F, 27.0F)}; + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + + // Check aligned pointcloud auto cloud = beluga_ros::PointCloud3(message, origin); auto map = cloud.points(); // Check assert - for (int i = 0; i < map.cols(); ++i) { - for (int j = 0; j < map.rows(); ++j) { - ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); - } + for (unsigned i = 0; i < map.cols(); ++i) { + ASSERT_EQ(point_data.at(i).x(), map(0, i)); + ASSERT_EQ(point_data.at(i).y(), map(1, i)); + ASSERT_EQ(point_data.at(i).z(), map(2, i)); + } + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); } } TEST(TestPointCloud, XYZIPointsUnorderedPC) { - auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y, z and intensity + // Define pointcloud params int fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + int width = 1; + int height = 5; // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, - 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; - const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); - *iter_intensity = intensity_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - } + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + + // Check aligned pointcloud auto cloud = beluga_ros::PointCloud3(message, origin); auto map = cloud.points(); // Check assert - for (int i = 0; i < map.cols(); ++i) { - for (int j = 0; j < map.rows(); ++j) { - ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); - } + for (unsigned i = 0; i < map.cols(); ++i) { + ASSERT_EQ(point_data.at(i).x(), map(0, i)); + ASSERT_EQ(point_data.at(i).y(), map(1, i)); + ASSERT_EQ(point_data.at(i).z(), map(2, i)); + } + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); } } TEST(TestPointCloud, XYZIPointsOrderedPC) { - auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 3; // Ordered point cloud - message->height = 3; - // Set the point fields to x, y, z and intensity + // Define pointcloud params int fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + int width = 3; + int height = 3; // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, - 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, - 19.0f, 20.0f, 21.0f, 22.0f, 23.0f, 24.0f, 25.0f, 26.0f, 27.0f}; - const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f, 6.6f, 7.7f, 8.8f, 9.9f}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); - *iter_intensity = intensity_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - } + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F), + Eigen::Vector3f(16.0F, 17.0F, 18.0F), + Eigen::Vector3f(19.0F, 20.0F, 21.0F), + Eigen::Vector3f(22.0F, 23.0F, 24.0F), + Eigen::Vector3f(25.0F, 26.0F, 27.0F) + }; + // clang-format on + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + + // Check aligned pointcloud auto cloud = beluga_ros::PointCloud3(message, origin); auto map = cloud.points(); // Check assert - for (int i = 0; i < map.cols(); ++i) { - for (int j = 0; j < map.rows(); ++j) { - ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); - } + for (unsigned i = 0; i < map.cols(); ++i) { + ASSERT_EQ(point_data.at(i).x(), map(0, i)); + ASSERT_EQ(point_data.at(i).y(), map(1, i)); + ASSERT_EQ(point_data.at(i).z(), map(2, i)); + } + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); } } TEST(TestPointCloud, XYZIDoublePC) { - auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y, z and intensity + // Define pointcloud params int fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF64, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF64, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF64, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF64, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + int width = 1; + int height = 5; // Create some raw data for the points - const std::vector point_data = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, - 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0}; - const std::vector intensity_data = {1.1, 2.2, 3.3, 4.4, 5.5}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); - *iter_intensity = intensity_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - } + // clang-format off + const std::vector point_data = { + Eigen::Vector3d(1.0, 2.0, 3.0), + Eigen::Vector3d(4.0, 5.0, 6.0), + Eigen::Vector3d(7.0, 8.0, 9.0), + Eigen::Vector3d(10.0, 11.0, 12.0), + Eigen::Vector3d(13.0, 14.0, 15.0) + }; + // clang-format on + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + + // Check aligned pointcloud auto cloud = beluga_ros::PointCloud3(message, origin); auto map = cloud.points(); // Check assert - for (int i = 0; i < map.cols(); ++i) { - for (int j = 0; j < map.rows(); ++j) { - ASSERT_EQ(map(j, i), point_data.at(3 * i + j)); - } + for (unsigned i = 0; i < map.cols(); ++i) { + ASSERT_EQ(point_data.at(i).x(), map(0, i)); + ASSERT_EQ(point_data.at(i).y(), map(1, i)); + ASSERT_EQ(point_data.at(i).z(), map(2, i)); + } + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); } } TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { - auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y and z + // Define pointcloud params int fields = 3; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - + int width = 1; + int height = 5; + const std::vector& point_data = {}; + bool empty = true; + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data, empty); + + // Check aligned pointcloud auto cloud = beluga_ros::PointCloud3(message, origin); auto map = cloud.points(); // Check assert - for (int i = 0; i < map.cols(); ++i) { - for (int j = 0; j < map.rows(); ++j) { - ASSERT_EQ(map(j, i), message->data.at(3 * i + j)); - } + for (unsigned i = 0; i < map.cols(); ++i) { + ASSERT_EQ(message->data.at(3 * i + 0), map(0, i)); + ASSERT_EQ(message->data.at(3 * i + 1), map(1, i)); + ASSERT_EQ(message->data.at(3 * i + 2), map(2, i)); + } + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(message->data.at(3 * i + 0), vector.at(i).x()); + ASSERT_EQ(message->data.at(3 * i + 1), vector.at(i).y()); + ASSERT_EQ(message->data.at(3 * i + 2), vector.at(i).z()); } } TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { - auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 4; // Number of points - // Set the point fields to x, y, z and intensity + // Define pointcloud params int fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - + int width = 1; + int height = 5; + const std::vector& point_data = {}; + bool empty = true; + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data, empty); + + // Check aligned pointcloud auto cloud = beluga_ros::PointCloud3(message, origin); auto map = cloud.points(); // Check assert - for (int i = 0; i < map.cols(); ++i) { - for (int j = 0; j < map.rows(); ++j) { - ASSERT_EQ(map(j, i), message->data.at(3 * i + j)); - } + for (unsigned i = 0; i < map.cols(); ++i) { + ASSERT_EQ(message->data.at(3 * i + 0), map(0, i)); + ASSERT_EQ(message->data.at(3 * i + 1), map(1, i)); + ASSERT_EQ(message->data.at(3 * i + 2), map(2, i)); + } + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(message->data.at(3 * i + 0), vector.at(i).x()); + ASSERT_EQ(message->data.at(3 * i + 1), vector.at(i).y()); + ASSERT_EQ(message->data.at(3 * i + 2), vector.at(i).z()); } } -TEST(TestPointCloud, IXYZPC) { - auto message = make_message(); +TEST(TestPointCloud, 2DUnorderedPC) { const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y, z and intensity - int fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + // Define pointcloud params + int fields = 2; + int width = 1; + int height = 5; // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, - 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; - const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); - *iter_intensity = intensity_data.at(i); + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + // Check assert aligned pointcloud + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + // Check assert sparse pointcloud + ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); +} - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - } - // Check assert +TEST(TestPointCloud, 2DOrderedPC) { + const auto origin = Sophus::SE3d{}; + // Define pointcloud params + int fields = 2; + int width = 2; + int height = 2; + // Create some raw data for the points + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + // Check assert sparse pointcloud + ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); } -TEST(TestPointCloud, ZXYIPC) { +TEST(TestPointCloud, EmptyFieldsPC) { auto message = make_message(); const auto origin = Sophus::SE3d{}; message->width = 1; // Unordered point cloud - message->height = 5; // Number of points + message->height = 4; // Number of points // Set the point fields to x, y, z and intensity int fields = 4; message->fields.clear(); message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); - // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, - 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; - const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); - *iter_intensity = intensity_data.at(i); - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_intensity; - } - /// Check assert + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + // Check assert sparse pointcloud + ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); } -TEST(TestPointCloud, 2DUnorderedPC) { - auto message = make_message(); +TEST(TestPointCloud, WrongTypePC) { const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x and y - int fields = 2; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; - message->is_dense = true; - message->data.resize(message->point_step * message->width * message->height); - // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + // Define pointcloud params + int fields = 4; + int width = 1; + int height = 5; // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 4.0f, 5.0f, 7.0f, 8.0f, 10.0f, 11.0f, 12.0f, 13.0f}; - // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 2; ++i) { - *iter_x = point_data.at(2 * i + 0); - *iter_y = point_data.at(2 * i + 1); + // clang-format off + const std::vector point_data = { + Eigen::Vector3d(1.0, 2.0, 3.0), + Eigen::Vector3d(4.0, 5.0, 6.0), + Eigen::Vector3d(7.0, 8.0, 9.0), + Eigen::Vector3d(10.0, 11.0, 12.0), + Eigen::Vector3d(13.0, 14.0, 15.0) + }; + // clang-format on + // Create point cloud message + const auto message = make_pointcloud(fields, width, height, point_data); + // Check assert aligned pointcloud + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + // Check assert sparse pointcloud + ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); +} - ++iter_x; - ++iter_y; - } - // Check assert +TEST(TestPointCloud, VoidPC) { + const auto origin = Sophus::SE3d{}; + auto message = make_message(); + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + // Check assert sparse pointcloud + ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); } -TEST(TestPointCloud, 2DOrderedPC) { +TEST(TestPointCloud, IXYZPC) { auto message = make_message(); const auto origin = Sophus::SE3d{}; - message->width = 2; // Ordered point cloud - message->height = 2; - // Set the point fields to x and y - int fields = 2; + message->width = 1; // Unordered point cloud + message->height = 5; // Number of points + // Set the point fields to x, y, z and intensity + int fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset int offset = 0; + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); // Set message params message->point_step = offset; message->row_step = message->width * message->point_step; @@ -486,92 +548,78 @@ TEST(TestPointCloud, 2DOrderedPC) { // Create data iterators beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 4.0f, 5.0f, 7.0f, 8.0f, 10.0f, 11.0f, 12.0f, 13.0f}; + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 2; ++i) { - *iter_x = point_data.at(2 * i + 0); - *iter_y = point_data.at(2 * i + 1); + for (unsigned i = 0; i < point_data.size(); ++i) { + *iter_x = point_data.at(i).x(); + *iter_y = point_data.at(i).y(); + *iter_z = point_data.at(i).z(); + *iter_intensity = intensity_data.at(i); ++iter_x; ++iter_y; + ++iter_z; + ++iter_intensity; } - // Check assert - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, EmptyFieldsPC) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 4; // Number of points - // Set the point fields to x, y, z and intensity - int fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - message->is_dense = true; - // Check assert + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + // Check assert sparse pointcloud + ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); } -TEST(TestPointCloud, NotDensePC) { +TEST(TestPointCloud, ZXYIPC) { auto message = make_message(); const auto origin = Sophus::SE3d{}; message->width = 1; // Unordered point cloud - message->height = 4; // Number of points + message->height = 5; // Number of points // Set the point fields to x, y, z and intensity int fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset int offset = 0; + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); // Set message params message->point_step = offset; message->row_step = message->width * message->point_step; - message->is_dense = false; - message->data.resize(message->point_step * message->width * message->height); - // Check assert - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, WrongTypePC) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - message->width = 1; // Unordered point cloud - message->height = 5; // Number of points - // Set the point fields to x, y, z and intensity - int fields = 4; - message->fields.clear(); - message->fields.reserve(fields); - // Set offset - int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF64, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF64, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF64, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF64, offset); - // Set message params - message->point_step = offset; - message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); // Create data iterators - beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); - beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); - beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); - beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); + beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); + beluga_ros::msg::PointCloud2Iterator iter_y(*message, "y"); + beluga_ros::msg::PointCloud2Iterator iter_z(*message, "z"); + beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); // Create some raw data for the points - const std::vector point_data = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, - 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0}; - const std::vector intensity_data = {1.1, 2.2, 3.3, 4.4, 5.5}; + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); + for (unsigned i = 0; i < point_data.size(); ++i) { + *iter_x = point_data.at(i).x(); + *iter_y = point_data.at(i).y(); + *iter_z = point_data.at(i).z(); *iter_intensity = intensity_data.at(i); ++iter_x; @@ -579,15 +627,10 @@ TEST(TestPointCloud, WrongTypePC) { ++iter_z; ++iter_intensity; } - // Check assert - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); -} - -TEST(TestPointCloud, VoidPC) { - auto message = make_message(); - const auto origin = Sophus::SE3d{}; - // Check assert + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + // Check assert sparse pointcloud + ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); } TEST(TestPointCloud, Velodyne) { @@ -620,16 +663,23 @@ TEST(TestPointCloud, Velodyne) { beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); beluga_ros::msg::PointCloud2Iterator iter_time(*message, "time"); // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, - 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; - const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; const std::vector ring_data = {1, 2, 3, 4, 5}; - const std::vector time_data = {0.1f, 0.2f, 0.3f, 0.4f, 0.5f}; + const std::vector time_data = {0.1F, 0.2F, 0.3F, 0.4F, 0.5F}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); + for (unsigned i = 0; i < point_data.size(); ++i) { + *iter_x = point_data.at(i).x(); + *iter_y = point_data.at(i).y(); + *iter_z = point_data.at(i).z(); *iter_intensity = intensity_data.at(i); *iter_ring = ring_data.at(i); *iter_time = time_data.at(i); @@ -641,8 +691,18 @@ TEST(TestPointCloud, Velodyne) { ++iter_ring; ++iter_time; } - // Check assert + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + } } TEST(TestPointCloud, Robosense) { @@ -675,16 +735,23 @@ TEST(TestPointCloud, Robosense) { beluga_ros::msg::PointCloud2Iterator iter_ring(*message, "ring"); beluga_ros::msg::PointCloud2Iterator iter_time(*message, "time"); // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, - 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; - const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; const std::vector ring_data = {1, 2, 3, 4, 5}; const std::vector time_data = {0.1, 0.2, 0.3, 0.4, 0.5}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); + for (unsigned i = 0; i < point_data.size(); ++i) { + *iter_x = point_data.at(i).x(); + *iter_y = point_data.at(i).y(); + *iter_z = point_data.at(i).z(); *iter_intensity = intensity_data.at(i); *iter_ring = ring_data.at(i); *iter_time = time_data.at(i); @@ -696,7 +763,18 @@ TEST(TestPointCloud, Robosense) { ++iter_ring; ++iter_time; } + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + } } TEST(TestPointCloud, Ouster) { @@ -736,9 +814,16 @@ TEST(TestPointCloud, Ouster) { beluga_ros::msg::PointCloud2Iterator iter_ambient(*message, "ambient"); beluga_ros::msg::PointCloud2Iterator iter_range(*message, "range"); // Create some raw data for the points - const std::vector point_data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, - 9.0f, 10.0f, 11.0f, 12.0f, 13.0f, 14.0f, 15.0f}; - const std::vector intensity_data = {1.1f, 2.2f, 3.3f, 4.4f, 5.5f}; + // clang-format off + const std::vector point_data = { + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F) + }; + // clang-format on + const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; const std::vector time_data = {1, 2, 3, 4, 5}; const std::vector reflectivity_data = {10, 9, 8, 7, 6}; const std::vector ring_data = {11, 12, 13, 14, 15}; @@ -746,10 +831,10 @@ TEST(TestPointCloud, Ouster) { const std::vector range_data = {21, 22, 23, 24, 25}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size() / 3; ++i) { - *iter_x = point_data.at(3 * i + 0); - *iter_y = point_data.at(3 * i + 1); - *iter_z = point_data.at(3 * i + 2); + for (unsigned i = 0; i < point_data.size(); ++i) { + *iter_x = point_data.at(i).x(); + *iter_y = point_data.at(i).y(); + *iter_z = point_data.at(i).z(); *iter_intensity = intensity_data.at(i); *iter_time = time_data.at(i); *iter_reflectivity = reflectivity_data.at(i); @@ -767,7 +852,18 @@ TEST(TestPointCloud, Ouster) { ++iter_ambient; ++iter_range; } + // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + + // Check sparse pointcloud + auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto vector = cloud_sparse.points(); + // Check assert + for (unsigned i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); + ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); + ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + } } } // namespace From 11fc5065d95d5cdf559633ab62da7cb944123d74 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 19 Sep 2024 15:07:58 +0800 Subject: [PATCH 09/77] Add non-aligned memory pointcloud wrapper Signed-off-by: Pablo Vela --- .../include/beluga_ros/point_cloud_sparse.hpp | 92 +++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 beluga_ros/include/beluga_ros/point_cloud_sparse.hpp diff --git a/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp b/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp new file mode 100644 index 000000000..be1a73dc4 --- /dev/null +++ b/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp @@ -0,0 +1,92 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ROS_POINT_CLOUD_SPARSE_HPP +#define BELUGA_ROS_POINT_CLOUD_SPARSE_HPP + +#include + +#include +#include +#include + +#include + +#include + +/** + * \file + * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages without alignment constraints on the + * stride. + * + * \details + * Since alignment is not enforced, this allows for more flexible messages layouts, though it may lead to less efficient + * access patterns in certain cases. + */ + +namespace beluga_ros { + +/// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. +/// Assumes an XYZ... type message. +/// XYZ datafields must be the same type (float or double). +/// Other datafields can be different types. +template +class PointCloudSparse3 : public beluga::BasePointCloud> { + public: + /// PointCloud data fields type + using iteratorType = typename DataType::iteratorType; + + /// Check type is float or double + static_assert( + std::is_same::value || std::is_same::value, + "PointcloudSparse3 only supports float or double datatype"); + + /// Constructor. + /// + /// \param cloud Point cloud message. + /// \param origin Point cloud frame origin in the filter frame. + explicit PointCloudSparse3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d()) + : cloud_(std::move(cloud)), origin_(std::move(origin)) { + assert(cloud_ != nullptr); + // Check if point cloud is 3D + if (cloud_->fields.size() < 3) + throw std::invalid_argument("PointCloud is not 3D"); + // Check point cloud is XYZ... type + if (cloud_->fields.at(0).name != "x" && cloud_->fields.at(1).name != "y" && cloud_->fields.at(2).name != "z") + throw std::invalid_argument("PointCloud not XYZ..."); + // Check XYZ datatype is the same + if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || cloud_->fields.at(2).datatype != T) + throw std::invalid_argument("XYZ datatype are not same"); + } + + /// Get the point cloud frame origin in the filter frame. + [[nodiscard]] const auto& origin() const { return origin_; } + + /// Get the unorganized 3D point collection as an Eigen Map. + [[nodiscard]] auto points() const { + beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); + return ranges::views::iota(0, static_cast(cloud_->width * cloud_->height)) | + ranges::views::transform([iter_points](int i) mutable { + return Eigen::Map>(&(iter_points + i)[0]); + }); + } + + private: + beluga_ros::msg::PointCloud2ConstSharedPtr cloud_; + Sophus::SE3d origin_; +}; + +} // namespace beluga_ros + +#endif // BELUGA_ROS_POINT_CLOUD_SPARSE_HPP From 31117514de38849467c44d55711c6dca5cf2d4bb Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 19 Sep 2024 15:09:43 +0800 Subject: [PATCH 10/77] Add non-aligned memory pointcloud wrapper Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/beluga_ros.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/beluga_ros/include/beluga_ros/beluga_ros.hpp b/beluga_ros/include/beluga_ros/beluga_ros.hpp index 2e233d4aa..360a8b2c1 100644 --- a/beluga_ros/include/beluga_ros/beluga_ros.hpp +++ b/beluga_ros/include/beluga_ros/beluga_ros.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include From d53e2d453c886ee5da39568d0bce95da161252cc Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 19 Sep 2024 15:11:11 +0800 Subject: [PATCH 11/77] Add more restrictions to check Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/point_cloud.hpp | 41 +++++++++++-------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index 433845566..090f9b448 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -27,7 +27,16 @@ /** * \file - * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type. + * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages with with memory-aligned strides. + * + * \details + * The stride calculation ensures that the memory layout of the point cloud data is aligned with the size of the data + * type used for iteration (`iteratorType`). To maintain proper memory alignment, the stride must satisfy the condition: + * + * `cloud_->point_step % sizeof(iteratorType) == 0` + * + * This condition guarantees that `point_step` is a multiple of the size of the `iteratorType`, ensuring efficient + * access to each point in the cloud. */ namespace beluga_ros { @@ -49,8 +58,7 @@ struct DataType { /// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. /// Assumes an XYZ... type message. -/// Each field must have the same datatype. -/// The point cloud can't have invalid values, i.e., it must be dense. +/// All datafields must be the same type (float or double). template class PointCloud3 : public beluga::BasePointCloud> { public: @@ -69,37 +77,36 @@ class PointCloud3 : public beluga::BasePointCloud> { /// \param origin Point cloud frame origin in the filter frame. explicit PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d()) : cloud_(std::move(cloud)), origin_(std::move(origin)) { - // Check there are not invalid values - if (!cloud_->is_dense) - throw std::invalid_argument("PointCloud is not dense"); + assert(cloud_ != nullptr); // Check if point cloud is 3D if (cloud_->fields.size() < 3) throw std::invalid_argument("PointCloud is not 3D"); // Check point cloud is XYZ... type - if (cloud_->fields.at(0).name != "x" && cloud_->fields.at(1).name != "y" && cloud_->fields.at(2).name != "z") + if (cloud_->fields.at(0).name != "x" || cloud_->fields.at(1).name != "y" || cloud_->fields.at(2).name != "z") throw std::invalid_argument("PointCloud not XYZ..."); - // Check all datatype is the same - if (!std::all_of( - cloud_->fields.begin(), cloud_->fields.end(), [&](const auto& field) { return field.datatype == T; })) { - throw std::invalid_argument("Fields do not match pointcloud datatype"); - } - assert(cloud_ != nullptr); + // Check XYZ datatype is the same + if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || cloud_->fields.at(2).datatype != T) + throw std::invalid_argument("XYZ datatype are not same"); + // verificar q el stride sea entero => throw + if (cloud_->point_step % sizeof(iteratorType) != 0) + throw std::invalid_argument("Data is not memory-aligned"); + stride_ = static_cast(cloud_->point_step / sizeof(iteratorType)); } /// Get the point cloud frame origin in the filter frame. [[nodiscard]] const auto& origin() const { return origin_; } - /// Get the unorganized 3D point collection as an Eigen Map. + /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { - beluga_ros::msg::PointCloud2ConstIterator iterPoints(*cloud_, "x"); - int stride_step = static_cast((cloud_->point_step + sizeof(iteratorType) - 1) / sizeof(iteratorType)); + beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); Eigen::Map> map( - &iterPoints[0], 3, cloud_->width * cloud_->height, Eigen::OuterStride<>(stride_step)); + &iter_points[0], 3, cloud_->width * cloud_->height, Eigen::OuterStride<>(stride_)); return map; } private: beluga_ros::msg::PointCloud2ConstSharedPtr cloud_; + int stride_; Sophus::SE3d origin_; }; From 73df8e273a233931d71e2ad7a56445f7d1fcf61a Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 19 Sep 2024 18:10:24 +0800 Subject: [PATCH 12/77] Change to pointfieldtypeastype templatization Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/messages.hpp | 2 ++ beluga_ros/include/beluga_ros/point_cloud.hpp | 30 +++++-------------- .../include/beluga_ros/point_cloud_sparse.hpp | 10 +++---- 3 files changed, 14 insertions(+), 28 deletions(-) diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp index b1c47ec7d..14ed66a41 100644 --- a/beluga_ros/include/beluga_ros/messages.hpp +++ b/beluga_ros/include/beluga_ros/messages.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,7 @@ #include #include #include +#include #include #include #include diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index 090f9b448..3c934ceb1 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -41,34 +41,18 @@ namespace beluga_ros { -template -struct DataType; - -template <> -struct DataType { - using iteratorType = float; - using eigenType = const Eigen::Matrix3Xf; -}; - -template <> -struct DataType { - using iteratorType = double; - using eigenType = const Eigen::Matrix3Xd; -}; - /// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. /// Assumes an XYZ... type message. /// All datafields must be the same type (float or double). -template +template class PointCloud3 : public beluga::BasePointCloud> { public: /// PointCloud data fields type - using iteratorType = typename DataType::iteratorType; - using eigenType = typename DataType::eigenType; + using Scalar = typename sensor_msgs::pointFieldTypeAsType::type; /// Check type is float or double static_assert( - std::is_same::value || std::is_same::value, + std::is_same::value || std::is_same::value, "Pointcloud3 only supports float or double datatype"); /// Constructor. @@ -88,9 +72,9 @@ class PointCloud3 : public beluga::BasePointCloud> { if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || cloud_->fields.at(2).datatype != T) throw std::invalid_argument("XYZ datatype are not same"); // verificar q el stride sea entero => throw - if (cloud_->point_step % sizeof(iteratorType) != 0) + if (cloud_->point_step % sizeof(Scalar) != 0) throw std::invalid_argument("Data is not memory-aligned"); - stride_ = static_cast(cloud_->point_step / sizeof(iteratorType)); + stride_ = static_cast(cloud_->point_step / sizeof(Scalar)); } /// Get the point cloud frame origin in the filter frame. @@ -98,8 +82,8 @@ class PointCloud3 : public beluga::BasePointCloud> { /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { - beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); - Eigen::Map> map( + beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); + Eigen::Map, 0, Eigen::OuterStride<>> map( &iter_points[0], 3, cloud_->width * cloud_->height, Eigen::OuterStride<>(stride_)); return map; } diff --git a/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp b/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp index be1a73dc4..1148a9417 100644 --- a/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp @@ -41,15 +41,15 @@ namespace beluga_ros { /// Assumes an XYZ... type message. /// XYZ datafields must be the same type (float or double). /// Other datafields can be different types. -template +template class PointCloudSparse3 : public beluga::BasePointCloud> { public: /// PointCloud data fields type - using iteratorType = typename DataType::iteratorType; + using Scalar = typename sensor_msgs::pointFieldTypeAsType::type; /// Check type is float or double static_assert( - std::is_same::value || std::is_same::value, + std::is_same::value || std::is_same::value, "PointcloudSparse3 only supports float or double datatype"); /// Constructor. @@ -75,10 +75,10 @@ class PointCloudSparse3 : public beluga::BasePointCloud> { /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { - beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); + beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); return ranges::views::iota(0, static_cast(cloud_->width * cloud_->height)) | ranges::views::transform([iter_points](int i) mutable { - return Eigen::Map>(&(iter_points + i)[0]); + return Eigen::Map>(&(iter_points + i)[0]); }); } From 674f7c41c724eb7de6667206f2f616a88f51cc10 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 19 Sep 2024 19:58:34 +0800 Subject: [PATCH 13/77] Fix ROS1 Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/messages.hpp | 3 + beluga_ros/test/test_point_cloud.cpp | 100 +++++++++++---------- 2 files changed, 56 insertions(+), 47 deletions(-) diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp index 14ed66a41..bb5b190f9 100644 --- a/beluga_ros/include/beluga_ros/messages.hpp +++ b/beluga_ros/include/beluga_ros/messages.hpp @@ -94,6 +94,9 @@ using PointCloud2 = sensor_msgs::PointCloud2; using PointCloud2ConstSharedPtr = PointCloud2::ConstPtr; template using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator; +template +using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator; +using PointCloud2Modifier = sensor_msgs::PointCloud2Modifier; constexpr uint8_t PointFieldU8 = sensor_msgs::PointField::UINT8; constexpr uint8_t PointFieldU16 = sensor_msgs::PointField::UINT16; constexpr uint8_t PointFieldU32 = sensor_msgs::PointField::UINT32; diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 92b3188d9..8c3cf3f12 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -19,6 +19,10 @@ #include +#if BELUGA_ROS_VERSION == 1 +#include +#endif + #include "beluga_ros/messages.hpp" #include "beluga_ros/point_cloud.hpp" #include "beluga_ros/point_cloud_sparse.hpp" @@ -35,9 +39,9 @@ auto make_message() { template auto make_pointcloud( - const int& fields, - const int& width, - const int& height, + const unsigned int& fields, + const unsigned int& width, + const unsigned int& height, const std::vector>& point_data = {}, const bool empty = false) { if (point_data.size() < static_cast(width * height) && !empty) @@ -50,12 +54,14 @@ auto make_pointcloud( message->fields.clear(); message->fields.reserve(fields); - const std::vector intensity_data = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9}; + const std::vector intensity_data = {static_cast(1.1), static_cast(2.2), static_cast(3.3), + static_cast(4.4), static_cast(5.5), static_cast(6.6), + static_cast(7.7), static_cast(8.8), static_cast(9.9)}; // XY pointclouds if (fields == 2) { // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "x", 1, U, offset); offset = addPointField(*message, "y", 1, U, offset); @@ -86,7 +92,7 @@ auto make_pointcloud( // XYZ pointclouds else if (fields == 3) { // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "x", 1, U, offset); offset = addPointField(*message, "y", 1, U, offset); offset = addPointField(*message, "z", 1, U, offset); @@ -121,7 +127,7 @@ auto make_pointcloud( // XYZI pointclouds else if (fields == 4) { // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "x", 1, U, offset); offset = addPointField(*message, "y", 1, U, offset); offset = addPointField(*message, "z", 1, U, offset); @@ -168,9 +174,9 @@ auto make_pointcloud( TEST(TestPointCloud, XYZPointsUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 3; - int width = 1; - int height = 5; + unsigned int fields = 3; + unsigned int width = 1; + unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -208,9 +214,9 @@ TEST(TestPointCloud, XYZPointsUnorderedPC) { TEST(TestPointCloud, XYZPointsOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 3; - int width = 3; - int height = 3; + unsigned int fields = 3; + unsigned int width = 3; + unsigned int height = 3; // Create some raw data for the points const std::vector point_data = { Eigen::Vector3f(1.0F, 2.0F, 3.0F), Eigen::Vector3f(4.0F, 5.0F, 6.0F), Eigen::Vector3f(7.0F, 8.0F, 9.0F), @@ -243,9 +249,9 @@ TEST(TestPointCloud, XYZPointsOrderedPC) { TEST(TestPointCloud, XYZIPointsUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 4; - int width = 1; - int height = 5; + unsigned int fields = 4; + unsigned int width = 1; + unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -283,9 +289,9 @@ TEST(TestPointCloud, XYZIPointsUnorderedPC) { TEST(TestPointCloud, XYZIPointsOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 4; - int width = 3; - int height = 3; + unsigned int fields = 4; + unsigned int width = 3; + unsigned int height = 3; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -327,9 +333,9 @@ TEST(TestPointCloud, XYZIPointsOrderedPC) { TEST(TestPointCloud, XYZIDoublePC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 4; - int width = 1; - int height = 5; + unsigned int fields = 4; + unsigned int width = 1; + unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -367,9 +373,9 @@ TEST(TestPointCloud, XYZIDoublePC) { TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 3; - int width = 1; - int height = 5; + unsigned int fields = 3; + unsigned int width = 1; + unsigned int height = 5; const std::vector& point_data = {}; bool empty = true; // Create point cloud message @@ -399,9 +405,9 @@ TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 4; - int width = 1; - int height = 5; + unsigned int fields = 4; + unsigned int width = 1; + unsigned int height = 5; const std::vector& point_data = {}; bool empty = true; // Create point cloud message @@ -431,9 +437,9 @@ TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { TEST(TestPointCloud, 2DUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 2; - int width = 1; - int height = 5; + unsigned int fields = 2; + unsigned int width = 1; + unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -455,9 +461,9 @@ TEST(TestPointCloud, 2DUnorderedPC) { TEST(TestPointCloud, 2DOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 2; - int width = 2; - int height = 2; + unsigned int fields = 2; + unsigned int width = 2; + unsigned int height = 2; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -495,9 +501,9 @@ TEST(TestPointCloud, EmptyFieldsPC) { TEST(TestPointCloud, WrongTypePC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - int fields = 4; - int width = 1; - int height = 5; + unsigned int fields = 4; + unsigned int width = 1; + unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -531,11 +537,11 @@ TEST(TestPointCloud, IXYZPC) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields to x, y, z and intensity - int fields = 4; + unsigned int fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); @@ -585,11 +591,11 @@ TEST(TestPointCloud, ZXYIPC) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields to x, y, z and intensity - int fields = 4; + unsigned int fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); @@ -639,11 +645,11 @@ TEST(TestPointCloud, Velodyne) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as velodyne - int fields = 6; + unsigned int fields = 6; message->fields.clear(); message->fields.reserve(fields); // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); @@ -711,11 +717,11 @@ TEST(TestPointCloud, Robosense) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as robosense - int fields = 6; + unsigned int fields = 6; message->fields.clear(); message->fields.reserve(fields); // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); @@ -783,11 +789,11 @@ TEST(TestPointCloud, Ouster) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as ouster - int fields = 9; + unsigned int fields = 9; message->fields.clear(); message->fields.reserve(fields); // Set offset - int offset = 0; + unsigned int offset = 0; offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); From d4e5767f56315e4529f349a6e1a953c82195e6ba Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 20 Sep 2024 00:30:14 +0800 Subject: [PATCH 14/77] Add eigen lib to test Signed-off-by: Pablo Vela --- beluga_ros/test/test_point_cloud.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 8c3cf3f12..16137b6d9 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -23,6 +23,8 @@ #include #endif +#include + #include "beluga_ros/messages.hpp" #include "beluga_ros/point_cloud.hpp" #include "beluga_ros/point_cloud_sparse.hpp" From 3f5ec82cd61bd95a27657a7239ec3f2c1f6d459d Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 20 Sep 2024 22:03:03 +0800 Subject: [PATCH 15/77] Fix ROS1 eigen compatibility Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/point_cloud.hpp | 4 +++- .../include/beluga_ros/point_cloud_sparse.hpp | 2 ++ beluga_ros/test/test_point_cloud.cpp | 16 +++++++++++++--- 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index 3c934ceb1..0860f9996 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -25,6 +25,8 @@ #include +#include "beluga/eigen_compatibility.hpp" + /** * \file * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages with with memory-aligned strides. @@ -71,7 +73,7 @@ class PointCloud3 : public beluga::BasePointCloud> { // Check XYZ datatype is the same if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || cloud_->fields.at(2).datatype != T) throw std::invalid_argument("XYZ datatype are not same"); - // verificar q el stride sea entero => throw + // Check stride is divisible if (cloud_->point_step % sizeof(Scalar) != 0) throw std::invalid_argument("Data is not memory-aligned"); stride_ = static_cast(cloud_->point_step / sizeof(Scalar)); diff --git a/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp b/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp index 1148a9417..5c03d776b 100644 --- a/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp @@ -25,6 +25,8 @@ #include +#include "beluga/eigen_compatibility.hpp" + /** * \file * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages without alignment constraints on the diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 16137b6d9..a8139ca4d 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -25,6 +25,7 @@ #include +#include "beluga/eigen_compatibility.hpp" #include "beluga_ros/messages.hpp" #include "beluga_ros/point_cloud.hpp" #include "beluga_ros/point_cloud_sparse.hpp" @@ -220,10 +221,19 @@ TEST(TestPointCloud, XYZPointsOrderedPC) { unsigned int width = 3; unsigned int height = 3; // Create some raw data for the points + // clang-format off const std::vector point_data = { - Eigen::Vector3f(1.0F, 2.0F, 3.0F), Eigen::Vector3f(4.0F, 5.0F, 6.0F), Eigen::Vector3f(7.0F, 8.0F, 9.0F), - Eigen::Vector3f(10.0F, 11.0F, 12.0F), Eigen::Vector3f(13.0F, 14.0F, 15.0F), Eigen::Vector3f(16.0F, 17.0F, 18.0F), - Eigen::Vector3f(19.0F, 20.0F, 21.0F), Eigen::Vector3f(22.0F, 23.0F, 24.0F), Eigen::Vector3f(25.0F, 26.0F, 27.0F)}; + Eigen::Vector3f(1.0F, 2.0F, 3.0F), + Eigen::Vector3f(4.0F, 5.0F, 6.0F), + Eigen::Vector3f(7.0F, 8.0F, 9.0F), + Eigen::Vector3f(10.0F, 11.0F, 12.0F), + Eigen::Vector3f(13.0F, 14.0F, 15.0F), + Eigen::Vector3f(16.0F, 17.0F, 18.0F), + Eigen::Vector3f(19.0F, 20.0F, 21.0F), + Eigen::Vector3f(22.0F, 23.0F, 24.0F), + Eigen::Vector3f(25.0F, 26.0F, 27.0F) + }; + // clang-format on // Create point cloud message const auto message = make_pointcloud(fields, width, height, point_data); From cff6c8fc150d5067b85c8d648be0831855e038ec Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 21 Sep 2024 01:47:18 +0800 Subject: [PATCH 16/77] Fix clang tidy Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/point_cloud.hpp | 3 +- beluga_ros/test/test_point_cloud.cpp | 68 +++++++++---------- 2 files changed, 36 insertions(+), 35 deletions(-) diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index 0860f9996..c80bc66a6 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -76,6 +76,7 @@ class PointCloud3 : public beluga::BasePointCloud> { // Check stride is divisible if (cloud_->point_step % sizeof(Scalar) != 0) throw std::invalid_argument("Data is not memory-aligned"); + // stride_ = static_cast(cloud_->point_step / sizeof(Scalar)); stride_ = static_cast(cloud_->point_step / sizeof(Scalar)); } @@ -86,7 +87,7 @@ class PointCloud3 : public beluga::BasePointCloud> { [[nodiscard]] auto points() const { beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); Eigen::Map, 0, Eigen::OuterStride<>> map( - &iter_points[0], 3, cloud_->width * cloud_->height, Eigen::OuterStride<>(stride_)); + &iter_points[0], 3, cloud_->width * cloud_->height, stride_); return map; } diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index a8139ca4d..454c4dd84 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -42,7 +42,7 @@ auto make_message() { template auto make_pointcloud( - const unsigned int& fields, + typename std::vector::size_type& fields, const unsigned int& width, const unsigned int& height, const std::vector>& point_data = {}, @@ -64,12 +64,12 @@ auto make_pointcloud( // XY pointclouds if (fields == 2) { // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "x", 1, U, offset); offset = addPointField(*message, "y", 1, U, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); @@ -95,13 +95,13 @@ auto make_pointcloud( // XYZ pointclouds else if (fields == 3) { // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "x", 1, U, offset); offset = addPointField(*message, "y", 1, U, offset); offset = addPointField(*message, "z", 1, U, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); @@ -130,14 +130,14 @@ auto make_pointcloud( // XYZI pointclouds else if (fields == 4) { // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "x", 1, U, offset); offset = addPointField(*message, "y", 1, U, offset); offset = addPointField(*message, "z", 1, U, offset); offset = addPointField(*message, "intensity", 1, U, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); @@ -153,7 +153,7 @@ auto make_pointcloud( beluga_ros::msg::PointCloud2Iterator iter_intensity(*message, "intensity"); // Fill the PointCloud2 message - int i = 0; + typename std::vector::size_type i = 0; for (const auto& point : point_data) { *iter_x = point.x(); *iter_y = point.y(); @@ -177,7 +177,7 @@ auto make_pointcloud( TEST(TestPointCloud, XYZPointsUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 3; + std::vector::size_type fields = 3; unsigned int width = 1; unsigned int height = 5; // Create some raw data for the points @@ -217,7 +217,7 @@ TEST(TestPointCloud, XYZPointsUnorderedPC) { TEST(TestPointCloud, XYZPointsOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 3; + std::vector::size_type fields = 3; unsigned int width = 3; unsigned int height = 3; // Create some raw data for the points @@ -261,7 +261,7 @@ TEST(TestPointCloud, XYZPointsOrderedPC) { TEST(TestPointCloud, XYZIPointsUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 4; + std::vector::size_type fields = 4; unsigned int width = 1; unsigned int height = 5; // Create some raw data for the points @@ -301,7 +301,7 @@ TEST(TestPointCloud, XYZIPointsUnorderedPC) { TEST(TestPointCloud, XYZIPointsOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 4; + std::vector::size_type fields = 4; unsigned int width = 3; unsigned int height = 3; // Create some raw data for the points @@ -345,7 +345,7 @@ TEST(TestPointCloud, XYZIPointsOrderedPC) { TEST(TestPointCloud, XYZIDoublePC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 4; + std::vector::size_type fields = 4; unsigned int width = 1; unsigned int height = 5; // Create some raw data for the points @@ -385,7 +385,7 @@ TEST(TestPointCloud, XYZIDoublePC) { TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 3; + std::vector::size_type fields = 3; unsigned int width = 1; unsigned int height = 5; const std::vector& point_data = {}; @@ -417,7 +417,7 @@ TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 4; + std::vector::size_type fields = 4; unsigned int width = 1; unsigned int height = 5; const std::vector& point_data = {}; @@ -449,7 +449,7 @@ TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { TEST(TestPointCloud, 2DUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 2; + std::vector::size_type fields = 2; unsigned int width = 1; unsigned int height = 5; // Create some raw data for the points @@ -473,7 +473,7 @@ TEST(TestPointCloud, 2DUnorderedPC) { TEST(TestPointCloud, 2DOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 2; + std::vector::size_type fields = 2; unsigned int width = 2; unsigned int height = 2; // Create some raw data for the points @@ -500,7 +500,7 @@ TEST(TestPointCloud, EmptyFieldsPC) { message->width = 1; // Unordered point cloud message->height = 4; // Number of points // Set the point fields to x, y, z and intensity - int fields = 4; + std::vector::size_type fields = 4; message->fields.clear(); message->fields.reserve(fields); message->is_dense = true; @@ -513,7 +513,7 @@ TEST(TestPointCloud, EmptyFieldsPC) { TEST(TestPointCloud, WrongTypePC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - unsigned int fields = 4; + std::vector::size_type fields = 4; unsigned int width = 1; unsigned int height = 5; // Create some raw data for the points @@ -549,17 +549,17 @@ TEST(TestPointCloud, IXYZPC) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields to x, y, z and intensity - unsigned int fields = 4; + std::vector::size_type fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); @@ -603,17 +603,17 @@ TEST(TestPointCloud, ZXYIPC) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields to x, y, z and intensity - unsigned int fields = 4; + std::vector::size_type fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); @@ -657,11 +657,11 @@ TEST(TestPointCloud, Velodyne) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as velodyne - unsigned int fields = 6; + std::vector::size_type fields = 6; message->fields.clear(); message->fields.reserve(fields); // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); @@ -669,7 +669,7 @@ TEST(TestPointCloud, Velodyne) { offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU16, offset); offset = addPointField(*message, "time", 1, beluga_ros::msg::PointFieldF32, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); @@ -729,11 +729,11 @@ TEST(TestPointCloud, Robosense) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as robosense - unsigned int fields = 6; + std::vector::size_type fields = 6; message->fields.clear(); message->fields.reserve(fields); // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); @@ -741,7 +741,7 @@ TEST(TestPointCloud, Robosense) { offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU16, offset); offset = addPointField(*message, "time", 1, beluga_ros::msg::PointFieldF64, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); @@ -801,11 +801,11 @@ TEST(TestPointCloud, Ouster) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as ouster - unsigned int fields = 9; + std::vector::size_type fields = 9; message->fields.clear(); message->fields.reserve(fields); // Set offset - unsigned int offset = 0; + int offset = 0; offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); @@ -817,7 +817,7 @@ TEST(TestPointCloud, Ouster) { offset = addPointField(*message, "ambient", 1, beluga_ros::msg::PointFieldU16, offset); offset = addPointField(*message, "range", 1, beluga_ros::msg::PointFieldU32, offset); // Set message params - message->point_step = offset; + message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; message->is_dense = true; message->data.resize(message->point_step * message->width * message->height); From 8f2718ae19ac026e06db8e3db22b87a501ecc07e Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 23 Sep 2024 16:36:23 +0800 Subject: [PATCH 17/77] Rename sparse pointcloud Signed-off-by: Pablo Vela --- .../include/beluga_ros/point_cloud_sparse.hpp | 94 ------------------- .../include/beluga_ros/sparse_point_cloud.hpp | 23 +++-- 2 files changed, 11 insertions(+), 106 deletions(-) delete mode 100644 beluga_ros/include/beluga_ros/point_cloud_sparse.hpp diff --git a/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp b/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp deleted file mode 100644 index 5c03d776b..000000000 --- a/beluga_ros/include/beluga_ros/point_cloud_sparse.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2024 Ekumen, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BELUGA_ROS_POINT_CLOUD_SPARSE_HPP -#define BELUGA_ROS_POINT_CLOUD_SPARSE_HPP - -#include - -#include -#include -#include - -#include - -#include - -#include "beluga/eigen_compatibility.hpp" - -/** - * \file - * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages without alignment constraints on the - * stride. - * - * \details - * Since alignment is not enforced, this allows for more flexible messages layouts, though it may lead to less efficient - * access patterns in certain cases. - */ - -namespace beluga_ros { - -/// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. -/// Assumes an XYZ... type message. -/// XYZ datafields must be the same type (float or double). -/// Other datafields can be different types. -template -class PointCloudSparse3 : public beluga::BasePointCloud> { - public: - /// PointCloud data fields type - using Scalar = typename sensor_msgs::pointFieldTypeAsType::type; - - /// Check type is float or double - static_assert( - std::is_same::value || std::is_same::value, - "PointcloudSparse3 only supports float or double datatype"); - - /// Constructor. - /// - /// \param cloud Point cloud message. - /// \param origin Point cloud frame origin in the filter frame. - explicit PointCloudSparse3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d()) - : cloud_(std::move(cloud)), origin_(std::move(origin)) { - assert(cloud_ != nullptr); - // Check if point cloud is 3D - if (cloud_->fields.size() < 3) - throw std::invalid_argument("PointCloud is not 3D"); - // Check point cloud is XYZ... type - if (cloud_->fields.at(0).name != "x" && cloud_->fields.at(1).name != "y" && cloud_->fields.at(2).name != "z") - throw std::invalid_argument("PointCloud not XYZ..."); - // Check XYZ datatype is the same - if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || cloud_->fields.at(2).datatype != T) - throw std::invalid_argument("XYZ datatype are not same"); - } - - /// Get the point cloud frame origin in the filter frame. - [[nodiscard]] const auto& origin() const { return origin_; } - - /// Get the unorganized 3D point collection as an Eigen Map. - [[nodiscard]] auto points() const { - beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); - return ranges::views::iota(0, static_cast(cloud_->width * cloud_->height)) | - ranges::views::transform([iter_points](int i) mutable { - return Eigen::Map>(&(iter_points + i)[0]); - }); - } - - private: - beluga_ros::msg::PointCloud2ConstSharedPtr cloud_; - Sophus::SE3d origin_; -}; - -} // namespace beluga_ros - -#endif // BELUGA_ROS_POINT_CLOUD_SPARSE_HPP diff --git a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp index 96d255a06..480c65593 100644 --- a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_ROS_SPARSE_POINT_CLOUD_HPP -#define BELUGA_ROS_SPARSE_POINT_CLOUD_HPP +#ifndef BELUGA_ROS_POINT_CLOUD_SPARSE_HPP +#define BELUGA_ROS_POINT_CLOUD_SPARSE_HPP #include -#include +#include #include #include @@ -43,15 +43,15 @@ namespace beluga_ros { /// Assumes an XYZ... type message. /// XYZ datafields must be the same type (float or double). /// Other datafields can be different types. -template -class SparsePointCloud3 : public beluga::BaseSparsePointCloud> { +template +class SparsePointCloud3 : public beluga::BasePointCloud> { public: - /// PointCloud type - using Scalar = T; + /// PointCloud data fields type + using Scalar = typename sensor_msgs::pointFieldTypeAsType::type; /// Check type is float or double static_assert( - std::is_same_v || std::is_same_v, + std::is_same::value || std::is_same::value, "PointcloudSparse3 only supports float or double datatype"); /// Constructor. @@ -61,7 +61,6 @@ class SparsePointCloud3 : public beluga::BaseSparsePointCloud::value; // Check if point cloud is 3D if (cloud_->fields.size() < 3) { throw std::invalid_argument("PointCloud is not 3D"); @@ -71,8 +70,8 @@ class SparsePointCloud3 : public beluga::BaseSparsePointCloudfields.at(0).datatype != fieldType || cloud_->fields.at(1).datatype != fieldType || - cloud_->fields.at(2).datatype != fieldType) { + if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || + cloud_->fields.at(2).datatype != T) { throw std::invalid_argument("XYZ datatype are not same"); } } @@ -96,4 +95,4 @@ class SparsePointCloud3 : public beluga::BaseSparsePointCloud Date: Mon, 23 Sep 2024 16:41:03 +0800 Subject: [PATCH 18/77] Change using Pointfield Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/messages.hpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp index bb5b190f9..e718c24c7 100644 --- a/beluga_ros/include/beluga_ros/messages.hpp +++ b/beluga_ros/include/beluga_ros/messages.hpp @@ -70,11 +70,7 @@ using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator; template using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator; using PointCloud2Modifier = sensor_msgs::PointCloud2Modifier; -constexpr uint8_t PointFieldU8 = sensor_msgs::msg::PointField::UINT8; -constexpr uint8_t PointFieldU16 = sensor_msgs::msg::PointField::UINT16; -constexpr uint8_t PointFieldU32 = sensor_msgs::msg::PointField::UINT32; -constexpr uint8_t PointFieldF32 = sensor_msgs::msg::PointField::FLOAT32; -constexpr uint8_t PointFieldF64 = sensor_msgs::msg::PointField::FLOAT64; +using PointField = sensor_msgs::msg::PointField; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; using OccupancyGrid = nav_msgs::msg::OccupancyGrid; @@ -97,11 +93,7 @@ using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator; template using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator; using PointCloud2Modifier = sensor_msgs::PointCloud2Modifier; -constexpr uint8_t PointFieldU8 = sensor_msgs::PointField::UINT8; -constexpr uint8_t PointFieldU16 = sensor_msgs::PointField::UINT16; -constexpr uint8_t PointFieldU32 = sensor_msgs::PointField::UINT32; -constexpr uint8_t PointFieldF32 = sensor_msgs::PointField::FLOAT32; -constexpr uint8_t PointFieldF64 = sensor_msgs::PointField::FLOAT64; +using PointField = sensor_msgs::PointField; using Marker = visualization_msgs::Marker; using MarkerArray = visualization_msgs::MarkerArray; using OccupancyGrid = nav_msgs::OccupancyGrid; From c703610cac300a8400726a8d69998b669a988d96 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 23 Sep 2024 16:42:37 +0800 Subject: [PATCH 19/77] Change details Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/beluga_ros.hpp | 2 +- beluga_ros/include/beluga_ros/point_cloud.hpp | 17 ++++++++++------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/beluga_ros/include/beluga_ros/beluga_ros.hpp b/beluga_ros/include/beluga_ros/beluga_ros.hpp index 360a8b2c1..8f993ba81 100644 --- a/beluga_ros/include/beluga_ros/beluga_ros.hpp +++ b/beluga_ros/include/beluga_ros/beluga_ros.hpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index c80bc66a6..896cfc1c6 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -65,18 +65,22 @@ class PointCloud3 : public beluga::BasePointCloud> { : cloud_(std::move(cloud)), origin_(std::move(origin)) { assert(cloud_ != nullptr); // Check if point cloud is 3D - if (cloud_->fields.size() < 3) + if (cloud_->fields.size() < 3) { throw std::invalid_argument("PointCloud is not 3D"); + } // Check point cloud is XYZ... type - if (cloud_->fields.at(0).name != "x" || cloud_->fields.at(1).name != "y" || cloud_->fields.at(2).name != "z") + if (cloud_->fields.at(0).name != "x" || cloud_->fields.at(1).name != "y" || cloud_->fields.at(2).name != "z") { throw std::invalid_argument("PointCloud not XYZ..."); + } // Check XYZ datatype is the same - if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || cloud_->fields.at(2).datatype != T) + if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || + cloud_->fields.at(2).datatype != T) { throw std::invalid_argument("XYZ datatype are not same"); + } // Check stride is divisible - if (cloud_->point_step % sizeof(Scalar) != 0) + if (cloud_->point_step % sizeof(Scalar) != 0) { throw std::invalid_argument("Data is not memory-aligned"); - // stride_ = static_cast(cloud_->point_step / sizeof(Scalar)); + } stride_ = static_cast(cloud_->point_step / sizeof(Scalar)); } @@ -86,9 +90,8 @@ class PointCloud3 : public beluga::BasePointCloud> { /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); - Eigen::Map, 0, Eigen::OuterStride<>> map( + return Eigen::Map, 0, Eigen::OuterStride<>>( &iter_points[0], 3, cloud_->width * cloud_->height, stride_); - return map; } private: From 91606d5784a5f8b1d9a82dbe8900b9f606061d17 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 23 Sep 2024 17:08:06 +0800 Subject: [PATCH 20/77] Change to true types Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/point_cloud.hpp | 11 +- .../include/beluga_ros/sparse_point_cloud.hpp | 11 +- beluga_ros/test/test_point_cloud.cpp | 230 ++++++++---------- 3 files changed, 120 insertions(+), 132 deletions(-) diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index 896cfc1c6..85ffd3fc3 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -46,11 +46,11 @@ namespace beluga_ros { /// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. /// Assumes an XYZ... type message. /// All datafields must be the same type (float or double). -template +template class PointCloud3 : public beluga::BasePointCloud> { public: - /// PointCloud data fields type - using Scalar = typename sensor_msgs::pointFieldTypeAsType::type; + /// PointCloud type + using Scalar = T; /// Check type is float or double static_assert( @@ -64,6 +64,7 @@ class PointCloud3 : public beluga::BasePointCloud> { explicit PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d()) : cloud_(std::move(cloud)), origin_(std::move(origin)) { assert(cloud_ != nullptr); + constexpr uint8_t fieldType = sensor_msgs::typeAsPointFieldType::value; // Check if point cloud is 3D if (cloud_->fields.size() < 3) { throw std::invalid_argument("PointCloud is not 3D"); @@ -73,8 +74,8 @@ class PointCloud3 : public beluga::BasePointCloud> { throw std::invalid_argument("PointCloud not XYZ..."); } // Check XYZ datatype is the same - if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || - cloud_->fields.at(2).datatype != T) { + if (cloud_->fields.at(0).datatype != fieldType || cloud_->fields.at(1).datatype != fieldType || + cloud_->fields.at(2).datatype != fieldType) { throw std::invalid_argument("XYZ datatype are not same"); } // Check stride is divisible diff --git a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp index 480c65593..a81551262 100644 --- a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp @@ -43,11 +43,11 @@ namespace beluga_ros { /// Assumes an XYZ... type message. /// XYZ datafields must be the same type (float or double). /// Other datafields can be different types. -template +template class SparsePointCloud3 : public beluga::BasePointCloud> { public: - /// PointCloud data fields type - using Scalar = typename sensor_msgs::pointFieldTypeAsType::type; + /// PointCloud type + using Scalar = T; /// Check type is float or double static_assert( @@ -61,6 +61,7 @@ class SparsePointCloud3 : public beluga::BasePointCloud> { explicit SparsePointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d()) : cloud_(std::move(cloud)), origin_(std::move(origin)) { assert(cloud_ != nullptr); + constexpr uint8_t fieldType = sensor_msgs::typeAsPointFieldType::value; // Check if point cloud is 3D if (cloud_->fields.size() < 3) { throw std::invalid_argument("PointCloud is not 3D"); @@ -70,8 +71,8 @@ class SparsePointCloud3 : public beluga::BasePointCloud> { throw std::invalid_argument("PointCloud not XYZ..."); } // Check XYZ datatype is the same - if (cloud_->fields.at(0).datatype != T || cloud_->fields.at(1).datatype != T || - cloud_->fields.at(2).datatype != T) { + if (cloud_->fields.at(0).datatype != fieldType || cloud_->fields.at(1).datatype != fieldType || + cloud_->fields.at(2).datatype != fieldType) { throw std::invalid_argument("XYZ datatype are not same"); } } diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 454c4dd84..4f811db58 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -28,7 +28,7 @@ #include "beluga/eigen_compatibility.hpp" #include "beluga_ros/messages.hpp" #include "beluga_ros/point_cloud.hpp" -#include "beluga_ros/point_cloud_sparse.hpp" +#include "beluga_ros/sparse_point_cloud.hpp" namespace { @@ -43,8 +43,8 @@ auto make_message() { template auto make_pointcloud( typename std::vector::size_type& fields, - const unsigned int& width, - const unsigned int& height, + const unsigned int width, + const unsigned int height, const std::vector>& point_data = {}, const bool empty = false) { if (point_data.size() < static_cast(width * height) && !empty) @@ -191,11 +191,11 @@ TEST(TestPointCloud, XYZPointsUnorderedPC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check aligned pointcloud - auto cloud = beluga_ros::PointCloud3(message, origin); - auto map = cloud.points(); + const auto cloud = beluga_ros::PointCloud3(message, origin); + const auto map = cloud.points(); // Check assert for (unsigned i = 0; i < map.cols(); ++i) { ASSERT_EQ(point_data.at(i).x(), map(0, i)); @@ -204,13 +204,11 @@ TEST(TestPointCloud, XYZPointsUnorderedPC) { } // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } @@ -235,11 +233,11 @@ TEST(TestPointCloud, XYZPointsOrderedPC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check aligned pointcloud - auto cloud = beluga_ros::PointCloud3(message, origin); - auto map = cloud.points(); + const auto cloud = beluga_ros::PointCloud3(message, origin); + const auto map = cloud.points(); // Check assert for (unsigned i = 0; i < map.cols(); ++i) { ASSERT_EQ(point_data.at(i).x(), map(0, i)); @@ -248,13 +246,11 @@ TEST(TestPointCloud, XYZPointsOrderedPC) { } // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } @@ -275,11 +271,11 @@ TEST(TestPointCloud, XYZIPointsUnorderedPC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check aligned pointcloud - auto cloud = beluga_ros::PointCloud3(message, origin); - auto map = cloud.points(); + const auto cloud = beluga_ros::PointCloud3(message, origin); + const auto map = cloud.points(); // Check assert for (unsigned i = 0; i < map.cols(); ++i) { ASSERT_EQ(point_data.at(i).x(), map(0, i)); @@ -288,13 +284,11 @@ TEST(TestPointCloud, XYZIPointsUnorderedPC) { } // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } @@ -319,11 +313,11 @@ TEST(TestPointCloud, XYZIPointsOrderedPC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check aligned pointcloud - auto cloud = beluga_ros::PointCloud3(message, origin); - auto map = cloud.points(); + const auto cloud = beluga_ros::PointCloud3(message, origin); + const auto map = cloud.points(); // Check assert for (unsigned i = 0; i < map.cols(); ++i) { ASSERT_EQ(point_data.at(i).x(), map(0, i)); @@ -332,13 +326,11 @@ TEST(TestPointCloud, XYZIPointsOrderedPC) { } // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } @@ -359,11 +351,11 @@ TEST(TestPointCloud, XYZIDoublePC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check aligned pointcloud - auto cloud = beluga_ros::PointCloud3(message, origin); - auto map = cloud.points(); + const auto cloud = beluga_ros::PointCloud3(message, origin); + const auto map = cloud.points(); // Check assert for (unsigned i = 0; i < map.cols(); ++i) { ASSERT_EQ(point_data.at(i).x(), map(0, i)); @@ -372,13 +364,11 @@ TEST(TestPointCloud, XYZIDoublePC) { } // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } @@ -391,11 +381,12 @@ TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { const std::vector& point_data = {}; bool empty = true; // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data, empty); + const auto message = + make_pointcloud(fields, width, height, point_data, empty); // Check aligned pointcloud - auto cloud = beluga_ros::PointCloud3(message, origin); - auto map = cloud.points(); + const auto cloud = beluga_ros::PointCloud3(message, origin); + const auto map = cloud.points(); // Check assert for (unsigned i = 0; i < map.cols(); ++i) { ASSERT_EQ(message->data.at(3 * i + 0), map(0, i)); @@ -404,10 +395,10 @@ TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { } // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { + for (size_t i = 0; i < vector.size(); ++i) { ASSERT_EQ(message->data.at(3 * i + 0), vector.at(i).x()); ASSERT_EQ(message->data.at(3 * i + 1), vector.at(i).y()); ASSERT_EQ(message->data.at(3 * i + 2), vector.at(i).z()); @@ -423,11 +414,12 @@ TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { const std::vector& point_data = {}; bool empty = true; // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data, empty); + const auto message = + make_pointcloud(fields, width, height, point_data, empty); // Check aligned pointcloud - auto cloud = beluga_ros::PointCloud3(message, origin); - auto map = cloud.points(); + const auto cloud = beluga_ros::PointCloud3(message, origin); + const auto map = cloud.points(); // Check assert for (unsigned i = 0; i < map.cols(); ++i) { ASSERT_EQ(message->data.at(3 * i + 0), map(0, i)); @@ -436,10 +428,10 @@ TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { } // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { + for (size_t i = 0; i < vector.size(); ++i) { ASSERT_EQ(message->data.at(3 * i + 0), vector.at(i).x()); ASSERT_EQ(message->data.at(3 * i + 1), vector.at(i).y()); ASSERT_EQ(message->data.at(3 * i + 2), vector.at(i).z()); @@ -463,11 +455,11 @@ TEST(TestPointCloud, 2DUnorderedPC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); } TEST(TestPointCloud, 2DOrderedPC) { @@ -487,11 +479,11 @@ TEST(TestPointCloud, 2DOrderedPC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); } TEST(TestPointCloud, EmptyFieldsPC) { @@ -505,9 +497,9 @@ TEST(TestPointCloud, EmptyFieldsPC) { message->fields.reserve(fields); message->is_dense = true; // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); } TEST(TestPointCloud, WrongTypePC) { @@ -527,20 +519,20 @@ TEST(TestPointCloud, WrongTypePC) { }; // clang-format on // Create point cloud message - const auto message = make_pointcloud(fields, width, height, point_data); + const auto message = make_pointcloud(fields, width, height, point_data); // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); } TEST(TestPointCloud, VoidPC) { const auto origin = Sophus::SE3d{}; auto message = make_message(); // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); } TEST(TestPointCloud, IXYZPC) { @@ -554,10 +546,10 @@ TEST(TestPointCloud, IXYZPC) { message->fields.reserve(fields); // Set offset int offset = 0; - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; @@ -580,7 +572,7 @@ TEST(TestPointCloud, IXYZPC) { // clang-format on const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size(); ++i) { + for (size_t i = 0; i < point_data.size(); ++i) { *iter_x = point_data.at(i).x(); *iter_y = point_data.at(i).y(); *iter_z = point_data.at(i).z(); @@ -592,9 +584,9 @@ TEST(TestPointCloud, IXYZPC) { ++iter_intensity; } // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); } TEST(TestPointCloud, ZXYIPC) { @@ -608,10 +600,10 @@ TEST(TestPointCloud, ZXYIPC) { message->fields.reserve(fields); // Set offset int offset = 0; - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; @@ -634,7 +626,7 @@ TEST(TestPointCloud, ZXYIPC) { // clang-format on const std::vector intensity_data = {1.1F, 2.2F, 3.3F, 4.4F, 5.5F}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size(); ++i) { + for (size_t i = 0; i < point_data.size(); ++i) { *iter_x = point_data.at(i).x(); *iter_y = point_data.at(i).y(); *iter_z = point_data.at(i).z(); @@ -646,9 +638,9 @@ TEST(TestPointCloud, ZXYIPC) { ++iter_intensity; } // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud - ASSERT_THROW(beluga_ros::PointCloudSparse3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::SparsePointCloud3(message, origin), std::invalid_argument); } TEST(TestPointCloud, Velodyne) { @@ -662,12 +654,12 @@ TEST(TestPointCloud, Velodyne) { message->fields.reserve(fields); // Set offset int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU16, offset); - offset = addPointField(*message, "time", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointField::UINT16, offset); + offset = addPointField(*message, "time", 1, beluga_ros::msg::PointField::FLOAT32, offset); // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; @@ -694,7 +686,7 @@ TEST(TestPointCloud, Velodyne) { const std::vector ring_data = {1, 2, 3, 4, 5}; const std::vector time_data = {0.1F, 0.2F, 0.3F, 0.4F, 0.5F}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size(); ++i) { + for (size_t i = 0; i < point_data.size(); ++i) { *iter_x = point_data.at(i).x(); *iter_y = point_data.at(i).y(); *iter_z = point_data.at(i).z(); @@ -710,16 +702,14 @@ TEST(TestPointCloud, Velodyne) { ++iter_time; } // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } @@ -734,12 +724,12 @@ TEST(TestPointCloud, Robosense) { message->fields.reserve(fields); // Set offset int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU16, offset); - offset = addPointField(*message, "time", 1, beluga_ros::msg::PointFieldF64, offset); + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointField::UINT16, offset); + offset = addPointField(*message, "time", 1, beluga_ros::msg::PointField::FLOAT64, offset); // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; @@ -766,7 +756,7 @@ TEST(TestPointCloud, Robosense) { const std::vector ring_data = {1, 2, 3, 4, 5}; const std::vector time_data = {0.1, 0.2, 0.3, 0.4, 0.5}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size(); ++i) { + for (size_t i = 0; i < point_data.size(); ++i) { *iter_x = point_data.at(i).x(); *iter_y = point_data.at(i).y(); *iter_z = point_data.at(i).z(); @@ -782,16 +772,14 @@ TEST(TestPointCloud, Robosense) { ++iter_time; } // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } @@ -806,16 +794,16 @@ TEST(TestPointCloud, Ouster) { message->fields.reserve(fields); // Set offset int offset = 0; - offset = addPointField(*message, "x", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "y", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "z", 1, beluga_ros::msg::PointFieldF32, offset); + offset = addPointField(*message, "x", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "y", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "z", 1, beluga_ros::msg::PointField::FLOAT32, offset); offset += 4; - offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointFieldF32, offset); - offset = addPointField(*message, "t", 1, beluga_ros::msg::PointFieldU32, offset); - offset = addPointField(*message, "reflectivity", 1, beluga_ros::msg::PointFieldU16, offset); - offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointFieldU8, offset); - offset = addPointField(*message, "ambient", 1, beluga_ros::msg::PointFieldU16, offset); - offset = addPointField(*message, "range", 1, beluga_ros::msg::PointFieldU32, offset); + offset = addPointField(*message, "intensity", 1, beluga_ros::msg::PointField::FLOAT32, offset); + offset = addPointField(*message, "t", 1, beluga_ros::msg::PointField::UINT32, offset); + offset = addPointField(*message, "reflectivity", 1, beluga_ros::msg::PointField::UINT16, offset); + offset = addPointField(*message, "ring", 1, beluga_ros::msg::PointField::UINT8, offset); + offset = addPointField(*message, "ambient", 1, beluga_ros::msg::PointField::UINT16, offset); + offset = addPointField(*message, "range", 1, beluga_ros::msg::PointField::UINT32, offset); // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; @@ -849,7 +837,7 @@ TEST(TestPointCloud, Ouster) { const std::vector range_data = {21, 22, 23, 24, 25}; // Fill the PointCloud2 message - for (unsigned i = 0; i < point_data.size(); ++i) { + for (size_t i = 0; i < point_data.size(); ++i) { *iter_x = point_data.at(i).x(); *iter_y = point_data.at(i).y(); *iter_z = point_data.at(i).z(); @@ -871,16 +859,14 @@ TEST(TestPointCloud, Ouster) { ++iter_range; } // Check assert aligned pointcloud - ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); + ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check sparse pointcloud - auto cloud_sparse = beluga_ros::PointCloudSparse3(message, origin); + const auto cloud_sparse = beluga_ros::SparsePointCloud3(message, origin); auto vector = cloud_sparse.points(); // Check assert - for (unsigned i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i).x(), vector.at(i).x()); - ASSERT_EQ(point_data.at(i).y(), vector.at(i).y()); - ASSERT_EQ(point_data.at(i).z(), vector.at(i).z()); + for (size_t i = 0; i < vector.size(); ++i) { + ASSERT_EQ(point_data.at(i), vector.at(i)); } } From 96eab5da7228968e7a690375b4881a56ac784ded Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 25 Sep 2024 17:17:54 +0800 Subject: [PATCH 21/77] Add sparse pointcloud API contract Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor/data/point_cloud.hpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/beluga/include/beluga/sensor/data/point_cloud.hpp b/beluga/include/beluga/sensor/data/point_cloud.hpp index 020200642..295d4de09 100644 --- a/beluga/include/beluga/sensor/data/point_cloud.hpp +++ b/beluga/include/beluga/sensor/data/point_cloud.hpp @@ -23,7 +23,12 @@ /** * \file - * \brief Implementation of a point cloud interface. + * \brief Implementation of a point cloud interface with memory-aligned data. + * + * \details + * Assumes data is aligned as X, Y, Z, other datafields . + * All datafields are the same type (float or double). + * The result of the data stide division by the type of data must be an integer. */ namespace beluga { @@ -34,7 +39,7 @@ namespace beluga { * A type `P` satisfies `PointCloud` requirements if given `p` a possibly * const instance of `P`: * - `P::Scalar` is defined and is a scalar type to be used for x, y and z coordinates values, - * - p.points()` returns a view to the unorganized 3D point collection of `Eigen::Map` type, + * - p.points()` returns a view to the unorganized 3D point collection of `Eigen::Map` type, * - `p.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local * frame in the sensor space; */ From 3688a6a9ee64eaecf5e7d349eb0e9338da4489db Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 25 Sep 2024 17:20:20 +0800 Subject: [PATCH 22/77] Improve documentation Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/point_cloud.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index 85ffd3fc3..aa385880f 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -29,7 +29,7 @@ /** * \file - * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages with with memory-aligned strides. + * \brief Implementation of `sensor_msgs/PointCloud2` wrapper type for messages with memory-aligned strides. * * \details * The stride calculation ensures that the memory layout of the point cloud data is aligned with the size of the data @@ -44,8 +44,6 @@ namespace beluga_ros { /// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages. -/// Assumes an XYZ... type message. -/// All datafields must be the same type (float or double). template class PointCloud3 : public beluga::BasePointCloud> { public: From 51ba6478a990562eb2e6d14f2b0ba0950a1b9f49 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 25 Sep 2024 17:21:00 +0800 Subject: [PATCH 23/77] Change name Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/sparse_point_cloud.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp index a81551262..bcb2de842 100644 --- a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_ROS_POINT_CLOUD_SPARSE_HPP -#define BELUGA_ROS_POINT_CLOUD_SPARSE_HPP +#ifndef BELUGA_ROS_SPARSE_POINT_CLOUD_HPP +#define BELUGA_ROS_SPARSE_POINT_CLOUD_HPP #include -#include +#include #include #include @@ -44,7 +44,7 @@ namespace beluga_ros { /// XYZ datafields must be the same type (float or double). /// Other datafields can be different types. template -class SparsePointCloud3 : public beluga::BasePointCloud> { +class SparsePointCloud3 : public beluga::BaseSparsePointCloud> { public: /// PointCloud type using Scalar = T; @@ -96,4 +96,4 @@ class SparsePointCloud3 : public beluga::BasePointCloud> { } // namespace beluga_ros -#endif // BELUGA_ROS_POINT_CLOUD_SPARSE_HPP +#endif // BELUGA_ROS_SPARSE_POINT_CLOUD_HPP From 29e838ad5991865a0a85cd40c92f892d9a80f15e Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 25 Sep 2024 17:57:17 +0800 Subject: [PATCH 24/77] Fix clang Signed-off-by: Pablo Vela --- beluga_ros/test/test_point_cloud.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 4f811db58..6313a7fc1 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -208,7 +208,7 @@ TEST(TestPointCloud, XYZPointsUnorderedPC) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } @@ -250,7 +250,7 @@ TEST(TestPointCloud, XYZPointsOrderedPC) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } @@ -288,7 +288,7 @@ TEST(TestPointCloud, XYZIPointsUnorderedPC) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } @@ -330,7 +330,7 @@ TEST(TestPointCloud, XYZIPointsOrderedPC) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } @@ -368,7 +368,7 @@ TEST(TestPointCloud, XYZIDoublePC) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } @@ -399,9 +399,9 @@ TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(message->data.at(3 * i + 0), vector.at(i).x()); - ASSERT_EQ(message->data.at(3 * i + 1), vector.at(i).y()); - ASSERT_EQ(message->data.at(3 * i + 2), vector.at(i).z()); + ASSERT_EQ(message->data.at(3 * i + 0), vector.at(static_cast(i)).x()); + ASSERT_EQ(message->data.at(3 * i + 1), vector.at(static_cast(i)).y()); + ASSERT_EQ(message->data.at(3 * i + 2), vector.at(static_cast(i)).z()); } } @@ -432,9 +432,9 @@ TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(message->data.at(3 * i + 0), vector.at(i).x()); - ASSERT_EQ(message->data.at(3 * i + 1), vector.at(i).y()); - ASSERT_EQ(message->data.at(3 * i + 2), vector.at(i).z()); + ASSERT_EQ(message->data.at(3 * i + 0), vector.at(static_cast(i)).x()); + ASSERT_EQ(message->data.at(3 * i + 1), vector.at(static_cast(i)).y()); + ASSERT_EQ(message->data.at(3 * i + 2), vector.at(static_cast(i)).z()); } } @@ -709,7 +709,7 @@ TEST(TestPointCloud, Velodyne) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } @@ -779,7 +779,7 @@ TEST(TestPointCloud, Robosense) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } @@ -866,7 +866,7 @@ TEST(TestPointCloud, Ouster) { auto vector = cloud_sparse.points(); // Check assert for (size_t i = 0; i < vector.size(); ++i) { - ASSERT_EQ(point_data.at(i), vector.at(i)); + ASSERT_EQ(point_data.at(i), vector.at(static_cast(i))); } } From 8229dcfb552d5f6884e6bcf12ceaf005e0f72b34 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 25 Sep 2024 19:50:03 +0800 Subject: [PATCH 25/77] Fix is_dense bool noetic Signed-off-by: Pablo Vela --- beluga_ros/test/test_point_cloud.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 6313a7fc1..737f3257d 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -71,7 +71,7 @@ auto make_pointcloud( // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Return empty pointcloud @@ -103,7 +103,7 @@ auto make_pointcloud( // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Return empty pointcloud @@ -139,7 +139,7 @@ auto make_pointcloud( // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Return empty pointcloud @@ -495,7 +495,7 @@ TEST(TestPointCloud, EmptyFieldsPC) { std::vector::size_type fields = 4; message->fields.clear(); message->fields.reserve(fields); - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); // Check assert aligned pointcloud ASSERT_THROW(beluga_ros::PointCloud3(message, origin), std::invalid_argument); // Check assert sparse pointcloud @@ -553,7 +553,7 @@ TEST(TestPointCloud, IXYZPC) { // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Create data iterators beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); @@ -607,7 +607,7 @@ TEST(TestPointCloud, ZXYIPC) { // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Create data iterators beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); @@ -663,7 +663,7 @@ TEST(TestPointCloud, Velodyne) { // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Create data iterators beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); @@ -733,7 +733,7 @@ TEST(TestPointCloud, Robosense) { // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Create data iterators beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); @@ -807,7 +807,7 @@ TEST(TestPointCloud, Ouster) { // Set message params message->point_step = static_cast(offset); message->row_step = message->width * message->point_step; - message->is_dense = true; + message->is_dense = static_castis_dense)>(true); message->data.resize(message->point_step * message->width * message->height); // Create data iterators beluga_ros::msg::PointCloud2Iterator iter_x(*message, "x"); From b7c9c6686ee181550795bba6f5bb74d81f868107 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 26 Sep 2024 10:27:01 +0800 Subject: [PATCH 26/77] Fix clang jazzy Signed-off-by: Pablo Vela --- beluga_ros/include/beluga_ros/point_cloud.hpp | 4 +- .../include/beluga_ros/sparse_point_cloud.hpp | 2 +- beluga_ros/test/test_point_cloud.cpp | 79 ++++++++++--------- 3 files changed, 43 insertions(+), 42 deletions(-) diff --git a/beluga_ros/include/beluga_ros/point_cloud.hpp b/beluga_ros/include/beluga_ros/point_cloud.hpp index aa385880f..e810729d1 100644 --- a/beluga_ros/include/beluga_ros/point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/point_cloud.hpp @@ -52,7 +52,7 @@ class PointCloud3 : public beluga::BasePointCloud> { /// Check type is float or double static_assert( - std::is_same::value || std::is_same::value, + std::is_same_v || std::is_same_v, "Pointcloud3 only supports float or double datatype"); /// Constructor. @@ -88,7 +88,7 @@ class PointCloud3 : public beluga::BasePointCloud> { /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { - beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); + const beluga_ros::msg::PointCloud2ConstIterator iter_points(*cloud_, "x"); return Eigen::Map, 0, Eigen::OuterStride<>>( &iter_points[0], 3, cloud_->width * cloud_->height, stride_); } diff --git a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp index bcb2de842..96d255a06 100644 --- a/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp +++ b/beluga_ros/include/beluga_ros/sparse_point_cloud.hpp @@ -51,7 +51,7 @@ class SparsePointCloud3 : public beluga::BaseSparsePointCloud::value || std::is_same::value, + std::is_same_v || std::is_same_v, "PointcloudSparse3 only supports float or double datatype"); /// Constructor. diff --git a/beluga_ros/test/test_point_cloud.cpp b/beluga_ros/test/test_point_cloud.cpp index 737f3257d..ef81c25d6 100644 --- a/beluga_ros/test/test_point_cloud.cpp +++ b/beluga_ros/test/test_point_cloud.cpp @@ -42,7 +42,7 @@ auto make_message() { template auto make_pointcloud( - typename std::vector::size_type& fields, + const typename std::vector::size_type& fields, const unsigned int width, const unsigned int height, const std::vector>& point_data = {}, @@ -163,6 +163,7 @@ auto make_pointcloud( ++iter_x; ++iter_y; ++iter_z; + ++iter_intensity; ++i; } } @@ -177,9 +178,9 @@ auto make_pointcloud( TEST(TestPointCloud, XYZPointsUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 3; - unsigned int width = 1; - unsigned int height = 5; + const std::vector::size_type fields = 3; + const unsigned int width = 1; + const unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -215,9 +216,9 @@ TEST(TestPointCloud, XYZPointsUnorderedPC) { TEST(TestPointCloud, XYZPointsOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 3; - unsigned int width = 3; - unsigned int height = 3; + const std::vector::size_type fields = 3; + const unsigned int width = 3; + const unsigned int height = 3; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -257,9 +258,9 @@ TEST(TestPointCloud, XYZPointsOrderedPC) { TEST(TestPointCloud, XYZIPointsUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 4; - unsigned int width = 1; - unsigned int height = 5; + const std::vector::size_type fields = 4; + const unsigned int width = 1; + const unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -295,9 +296,9 @@ TEST(TestPointCloud, XYZIPointsUnorderedPC) { TEST(TestPointCloud, XYZIPointsOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 4; - unsigned int width = 3; - unsigned int height = 3; + const std::vector::size_type fields = 4; + const unsigned int width = 3; + const unsigned int height = 3; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -337,9 +338,9 @@ TEST(TestPointCloud, XYZIPointsOrderedPC) { TEST(TestPointCloud, XYZIDoublePC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 4; - unsigned int width = 1; - unsigned int height = 5; + const std::vector::size_type fields = 4; + const unsigned int width = 1; + const unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -375,11 +376,11 @@ TEST(TestPointCloud, XYZIDoublePC) { TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 3; - unsigned int width = 1; - unsigned int height = 5; + const std::vector::size_type fields = 3; + const unsigned int width = 1; + const unsigned int height = 5; const std::vector& point_data = {}; - bool empty = true; + const bool empty = true; // Create point cloud message const auto message = make_pointcloud(fields, width, height, point_data, empty); @@ -408,11 +409,11 @@ TEST(TestPointCloud, XYZPointsEmptyUnorderedPC) { TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 4; - unsigned int width = 1; - unsigned int height = 5; + const std::vector::size_type fields = 4; + const unsigned int width = 1; + const unsigned int height = 5; const std::vector& point_data = {}; - bool empty = true; + const bool empty = true; // Create point cloud message const auto message = make_pointcloud(fields, width, height, point_data, empty); @@ -441,9 +442,9 @@ TEST(TestPointCloud, XYZIPointsEmptyUnorderedPC) { TEST(TestPointCloud, 2DUnorderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 2; - unsigned int width = 1; - unsigned int height = 5; + const std::vector::size_type fields = 2; + const unsigned int width = 1; + const unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -465,9 +466,9 @@ TEST(TestPointCloud, 2DUnorderedPC) { TEST(TestPointCloud, 2DOrderedPC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 2; - unsigned int width = 2; - unsigned int height = 2; + const std::vector::size_type fields = 2; + const unsigned int width = 2; + const unsigned int height = 2; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -492,7 +493,7 @@ TEST(TestPointCloud, EmptyFieldsPC) { message->width = 1; // Unordered point cloud message->height = 4; // Number of points // Set the point fields to x, y, z and intensity - std::vector::size_type fields = 4; + const std::vector::size_type fields = 4; message->fields.clear(); message->fields.reserve(fields); message->is_dense = static_castis_dense)>(true); @@ -505,9 +506,9 @@ TEST(TestPointCloud, EmptyFieldsPC) { TEST(TestPointCloud, WrongTypePC) { const auto origin = Sophus::SE3d{}; // Define pointcloud params - std::vector::size_type fields = 4; - unsigned int width = 1; - unsigned int height = 5; + const std::vector::size_type fields = 4; + const unsigned int width = 1; + const unsigned int height = 5; // Create some raw data for the points // clang-format off const std::vector point_data = { @@ -541,7 +542,7 @@ TEST(TestPointCloud, IXYZPC) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields to x, y, z and intensity - std::vector::size_type fields = 4; + const std::vector::size_type fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset @@ -595,7 +596,7 @@ TEST(TestPointCloud, ZXYIPC) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields to x, y, z and intensity - std::vector::size_type fields = 4; + const std::vector::size_type fields = 4; message->fields.clear(); message->fields.reserve(fields); // Set offset @@ -649,7 +650,7 @@ TEST(TestPointCloud, Velodyne) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as velodyne - std::vector::size_type fields = 6; + const std::vector::size_type fields = 6; message->fields.clear(); message->fields.reserve(fields); // Set offset @@ -719,7 +720,7 @@ TEST(TestPointCloud, Robosense) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as robosense - std::vector::size_type fields = 6; + const std::vector::size_type fields = 6; message->fields.clear(); message->fields.reserve(fields); // Set offset @@ -789,7 +790,7 @@ TEST(TestPointCloud, Ouster) { message->width = 1; // Unordered point cloud message->height = 5; // Number of points // Set the point fields as ouster - std::vector::size_type fields = 9; + const std::vector::size_type fields = 9; message->fields.clear(); message->fields.reserve(fields); // Set offset From 4870924b543aa7f1cbd199ad6e86a7f29391b5be Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 26 Sep 2024 23:05:35 +0800 Subject: [PATCH 27/77] Add openvdb dependencies Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 4 ++++ beluga/cmake/Config.cmake.in | 3 +++ beluga/package.xml | 1 + docker/images/humble/Dockerfile | 1 + docker/images/iron/Dockerfile | 1 + docker/images/jazzy/Dockerfile | 1 + docker/images/noetic/Dockerfile | 1 + docker/images/rolling/Dockerfile | 1 + 8 files changed, 13 insertions(+) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 1d6b96d5d..62910d72f 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -18,6 +18,8 @@ project(beluga VERSION 1.0) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +# list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") + if(NOT CMAKE_BUILD_TYPE) message(STATUS "Setting build type to 'Release' as none was specified.") set(CMAKE_BUILD_TYPE @@ -47,6 +49,7 @@ find_package( find_package(range-v3 REQUIRED) find_package(Sophus REQUIRED) find_package(TBB REQUIRED) +#find_package(OpenVDB REQUIRED) add_library(${PROJECT_NAME} INTERFACE) target_include_directories( @@ -61,6 +64,7 @@ target_link_libraries( Sophus::Sophus TBB::tbb range-v3::range-v3) +#OpenVDB::openvdb) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index 8d7b0e44f..0d41e5055 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -1,11 +1,14 @@ @PACKAGE_INIT@ +#list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") + include(CMakeFindDependencyMacro) find_dependency(Eigen3 REQUIRED NO_MODULE) find_dependency(range-v3 REQUIRED) find_dependency(HDF5 COMPONENTS CXX REQUIRED) find_dependency(Sophus REQUIRED) find_dependency(TBB REQUIRED) +#find_dependency(OpenVDB REQUIRED) include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") diff --git a/beluga/package.xml b/beluga/package.xml index dc349ed27..3aa64276f 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -19,6 +19,7 @@ range-v3 sophus tbb + clang-format clang-tidy diff --git a/docker/images/humble/Dockerfile b/docker/images/humble/Dockerfile index a61d25886..713dd181b 100644 --- a/docker/images/humble/Dockerfile +++ b/docker/images/humble/Dockerfile @@ -49,6 +49,7 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ + libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/iron/Dockerfile b/docker/images/iron/Dockerfile index 28fa8268f..4abc9c01a 100644 --- a/docker/images/iron/Dockerfile +++ b/docker/images/iron/Dockerfile @@ -49,6 +49,7 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ + libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/jazzy/Dockerfile b/docker/images/jazzy/Dockerfile index b0221a1bc..f8446cbd9 100644 --- a/docker/images/jazzy/Dockerfile +++ b/docker/images/jazzy/Dockerfile @@ -49,6 +49,7 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ + libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/noetic/Dockerfile b/docker/images/noetic/Dockerfile index 02701cf58..9d1541459 100644 --- a/docker/images/noetic/Dockerfile +++ b/docker/images/noetic/Dockerfile @@ -62,6 +62,7 @@ RUN apt-get update \ python3-colcon-lcov-result \ python3-colcon-mixin \ python3-pip \ + libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/rolling/Dockerfile b/docker/images/rolling/Dockerfile index 4c53af1ba..bdf5234df 100644 --- a/docker/images/rolling/Dockerfile +++ b/docker/images/rolling/Dockerfile @@ -49,6 +49,7 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ + libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* From 0f135a175679f8df28fd3a6cf1314fc028690966 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 30 Sep 2024 23:36:49 +0800 Subject: [PATCH 28/77] Add likelihood 3D field model Signed-off-by: Pablo Vela --- .../sensor/likelihood_3d_field_model.hpp | 181 ++++++++++++++++++ .../sensor/test_likelihood_3d_field_model.cpp | 84 ++++++++ 2 files changed, 265 insertions(+) create mode 100644 beluga/include/beluga/sensor/likelihood_3d_field_model.hpp create mode 100644 beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp diff --git a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp new file mode 100644 index 000000000..9328b713e --- /dev/null +++ b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp @@ -0,0 +1,181 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL_HPP +#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL_HPP + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * \file + * \brief Implementation of a likelihood field sensor model for 3D Lidars. + */ + +namespace beluga { + +/// Parameters used to construct a Likelihood3DFieldModel instance. +/** + * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. + */ +struct Likelihood3DFieldModelParam { + /// Voxel size in meters. + double voxel_size = 0.07; + /// Maximum distance to obstacle. + /** + * When creating a distance map, if the distance to an obstacle is higher than the value specified here, + * then this value will be used. + */ + double max_obstacle_distance = 100.0; + /// Maximum range of a laser ray. + double max_laser_distance = 2.0; + /// Weight used to combine the probability of hitting an obstacle. + double z_hit = 0.5; + /// Weight used to combine the probability of random noise in perception. + double z_random = 0.5; + /// Standard deviation of a gaussian centered arounds obstacles. + /** + * Used to calculate the probability of the obstacle being hit. + */ + double sigma_hit = 0.2; +}; + +/// Likelihood field sensor model for range finders. +/** + * This model relies on a pre-computed likelihood map of the environment. + * It is less computationally intensive than the beluga::BeamSensorModel + * because no ray-tracing is required, and it can also provide better + * performance in environments with non-smooth occupation maps. See + * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4, + * for further reference. + * + * \note This class satisfies \ref SensorModelPage. + * + * \tparam OccupancyGrid Type representing an occupancy grid. + * It must satisfy \ref OccupancyGrid2Page. + */ +template +class Likelihood3DFieldModel { + public: + /// State type of a particle. + // planar movement but add Z + using state_type = Sophus::SE3d; + + /// Weight type of the particle. + using weight_type = double; + + /// Measurement type of the sensor: a point cloud for the XXXXXXXXXXX. + using measurement_type = U; + + /// Map representation type. + // openvdb, algo asi + using map_type = T; + + /// Parameter type that the constructor uses to configure the likelihood field model. + // nombre? Likelihood3DFieldModel?? el modelo es el mismo solo que los ptos son 3d + // pero el modelo sigue siendo 2D + using param_type = Likelihood3DFieldModel; + + /// Constructs a Likelihood3DFieldModel instance. + /** + * \param params Parameters to configure this instance. + * See beluga::Likelihood3DFieldModelParam for details. + * \param grid Occupancy grid representing the static map that the sensor model + * uses to compute a likelihood field for lidar hits and compute importance weights + * for particle states. + */ + explicit Likelihood3DFieldModel(const param_type& params, const map_type& map) + : params_{params}, likelihood_field_map_{std::move(map)} {} + + /// Returns the likelihood field, constructed from the provided map. + [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_map_; } + + /// Returns a state weighting function conditioned on 2D lidar hits. + /** + * \param points 2D lidar hit points in the reference frame of particle states. + * \return a state weighting function satisfying \ref StateWeightingFunctionPage + * and borrowing a reference to this sensor model (and thus their lifetime are bound). + */ + [[nodiscard]] auto operator()(measurement_type&& points) const { + return [this, points = std::move(points)](const state_type& state) -> weight_type { + // State is a particle + // by multiplying by world_to_likelihood_field_transform_ we are calculate the transform from robot to world + likelihood_field_map_.getTransform(); + const auto transform = state; + + // robot position in world coordinates + const auto x_offset = transform.translation().x(); + const auto y_offset = transform.translation().y(); + const auto z_offset = transform.translation().z(); + + // robot orientation in world coordinates + const auto cos_theta = transform.so2().unit_complex().x(); + const auto sin_theta = transform.so2().unit_complex().y(); + + const auto unknown_space_occupancy_prob = static_cast(1. / params_.max_laser_distance); + + return std::transform_reduce( + points.cbegin(), points.cend(), 1.0, std::plus{}, + [this, x_offset, y_offset, z_offset, cos_theta, sin_theta, unknown_space_occupancy_prob](const auto& point) { + // Transform the end point of the laser to the grid local coordinate system. + + const auto x = point.x * cos_theta - point.y * sin_theta + x_offset; + const auto y = point.x * sin_theta + point.y * cos_theta + y_offset; + const auto z = point.z + z_offset; + + // encontrar el punto mas cercano usnado + // probar esto sacarlo dela funcion + // DualGridaAmpler + Openvdb::ClosestSurfacePoint looker; + looker.create(grid); + std::vector distances; + std::vector points; + // output list of closest surface point distances + looker.search(points, distances); + + const auto pz = static_cast + + return pz * pz * pz; + }); + }; + } + + /// Update the sensor model with a new occupancy grid map. + /** + * This method re-computes the underlying likelihood field. + * + * \param grid New occupancy grid representing the static map. + */ + void update_map(const map_type& map) { likelihood_field_map_{std::move(map)}; } + + private: + param_type params_; + T likelihood_field_map_; + Sophus::SE3d world_to_likelihood_field_map_transform_; +}; + +} // namespace beluga + +#endif diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp new file mode 100644 index 000000000..04036b12f --- /dev/null +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -0,0 +1,84 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace { + +template +auto make_map(const double voxel_size, const std::vector& world_points) { + // Create the grid + T::Ptr grid = openvdb::tools::createLevelSet(voxel_size); + // Get an accessor for coordinate-based access to voxels + const T::Accessor accessor = grid->getAccessor(); + + // Fill the grid + const openvdb::math::Transform transform; + for (const auto& point : world_points) { + // Transform to index world + const openvdb::math::Coord ijk = transform.worldToIndexCellCentered(point); + // Set the voxel to 1 + accessor.setValue(ijk, 1.0); + // Activate the voxel + accessor.setActiveState(ijk); + } + + // Prune the grid + // Set voxels that are outside the narrow band to the background value and prune the grid + openvdb::tools::LevelSetTracker pruner(grid); + pruner.setTrimming(openvdb::TrimMode::kAll); + pruner.prune(); + + // Check grid + openvdb::tools::CheckLevelSet checker(grid); + // Check inactive values have a magnitude equal to the background value + assert(checker.checkInactiveValues() == ""); + + // Associate metadata + // vdb_print -l pcdgrid.vdb + grid->setName("map"); + grid->setGridClass(openvdb::GridClass::GRID_LEVEL_SET); + grid->setIsInWorldSpace(true); // not quite sure! + + return grid; +} + +TEST(Likelihood3DFieldModel, LikelihoodField) { + openvdb::initialize(); + // Create Grid + const double voxel_size = 0.07; + const std::vector world_points{(1.0F, 1.0F, 1.0F)}; + auto map = make_map(voxel_size, world_points); + + const auto params = beluga::LikelihoodFieldModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + auto sensor_model = beluga::Likelihood3DFieldModel{params, map}; + + ASSERT_THAT( + sensor_model.likelihood_field().data(), + testing::Pointwise(testing::DoubleNear(0.003), expected_cubed_likelihood)); +} + +} // namespace From 580ce8073dfe36949aea0a02930b0d4273a59343 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 18 Oct 2024 10:42:37 +0800 Subject: [PATCH 29/77] Add Openvdb dependencies Signed-off-by: Pablo Vela --- beluga/cmake/Config.cmake.in | 2 +- beluga/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index 0d41e5055..500383137 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -8,7 +8,7 @@ find_dependency(range-v3 REQUIRED) find_dependency(HDF5 COMPONENTS CXX REQUIRED) find_dependency(Sophus REQUIRED) find_dependency(TBB REQUIRED) -#find_dependency(OpenVDB REQUIRED) +find_dependency(OpenVDB REQUIRED) include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") diff --git a/beluga/package.xml b/beluga/package.xml index 3aa64276f..e5eab3bf6 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -19,7 +19,7 @@ range-v3 sophus tbb - + OpenVDB clang-format clang-tidy From 251bbd20b3c5c216568460057d38c8dcaadcf3c0 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 18 Oct 2024 10:46:44 +0800 Subject: [PATCH 30/77] Add likelihood 3d field model test Signed-off-by: Pablo Vela --- beluga/test/beluga/CMakeLists.txt | 2 +- .../test/beluga/sensor/test_likelihood_3d_field_model.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index ece4b8a69..c812a0762 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -52,7 +52,7 @@ add_executable( sensor/test_beam_model.cpp sensor/test_bearing_sensor_model.cpp sensor/test_landmark_sensor_model.cpp - sensor/test_lfm_with_unknown_space.cpp + sensor/test_likelihood_3d_field_model.cpp sensor/test_likelihood_field_model.cpp sensor/test_ndt_model.cpp test_3d_embedding.cpp diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index 04036b12f..d7c4fcbcd 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -73,12 +73,12 @@ TEST(Likelihood3DFieldModel, LikelihoodField) { const std::vector world_points{(1.0F, 1.0F, 1.0F)}; auto map = make_map(voxel_size, world_points); - const auto params = beluga::LikelihoodFieldModelParam{2.0, 20.0, 0.5, 0.5, 0.2}; + const auto params = beluga::LikelihoodFieldModelParam{0.07, 2.0, 20.0, 0.5, 0.5, 0.2}; auto sensor_model = beluga::Likelihood3DFieldModel{params, map}; - ASSERT_THAT( + /*ASSERT_THAT( sensor_model.likelihood_field().data(), - testing::Pointwise(testing::DoubleNear(0.003), expected_cubed_likelihood)); + testing::Pointwise(testing::DoubleNear(0.003), expected_cubed_likelihood));*/ } } // namespace From c8c21e5a93bfb310cc73a1a8ffd11e59510ace71 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 18 Oct 2024 10:49:28 +0800 Subject: [PATCH 31/77] Add likelihood 3d field model to beluga Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 109322712..930b547fa 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -54,6 +54,7 @@ * * \section SensorModelLinks See also * - beluga::LikelihoodFieldModel + * - beluga::Likelihood3DFieldModel * - beluga::BeamSensorModel * - beluga::LandmarkSensorModel * - beluga::BearingSensorModel @@ -62,6 +63,7 @@ #include #include #include +#include #include #include From aeb7b94841fbd230bf5fa48d2371a090dd51b92f Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 18 Oct 2024 10:50:36 +0800 Subject: [PATCH 32/77] Fix code Signed-off-by: Pablo Vela --- .../sensor/likelihood_3d_field_model.hpp | 48 ++++++------------- 1 file changed, 15 insertions(+), 33 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp index 9328b713e..1e8722467 100644 --- a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL_HPP -#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL_HPP +#ifndef BELUGA_SENSOR_LIKELIHOOD_3D_FIELD_MODEL_HPP +#define BELUGA_SENSOR_LIKELIHOOD_3D_FIELD_MODEL_HPP #include #include @@ -107,7 +107,12 @@ class Likelihood3DFieldModel { * for particle states. */ explicit Likelihood3DFieldModel(const param_type& params, const map_type& map) - : params_{params}, likelihood_field_map_{std::move(map)} {} + : params_{params}, likelihood_field_map_{std::move(map)} { + // encontrar el punto mas cercano usnado + // probar esto sacarlo dela funcion + // DualGridSmpler + looker.create(likelihood_field_map_); + } /// Returns the likelihood field, constructed from the provided map. [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_map_; } @@ -120,44 +125,20 @@ class Likelihood3DFieldModel { */ [[nodiscard]] auto operator()(measurement_type&& points) const { return [this, points = std::move(points)](const state_type& state) -> weight_type { - // State is a particle - // by multiplying by world_to_likelihood_field_transform_ we are calculate the transform from robot to world - likelihood_field_map_.getTransform(); - const auto transform = state; - - // robot position in world coordinates - const auto x_offset = transform.translation().x(); - const auto y_offset = transform.translation().y(); - const auto z_offset = transform.translation().z(); - - // robot orientation in world coordinates - const auto cos_theta = transform.so2().unit_complex().x(); - const auto sin_theta = transform.so2().unit_complex().y(); - + // map already in world coordinates const auto unknown_space_occupancy_prob = static_cast(1. / params_.max_laser_distance); return std::transform_reduce( points.cbegin(), points.cend(), 1.0, std::plus{}, - [this, x_offset, y_offset, z_offset, cos_theta, sin_theta, unknown_space_occupancy_prob](const auto& point) { - // Transform the end point of the laser to the grid local coordinate system. - - const auto x = point.x * cos_theta - point.y * sin_theta + x_offset; - const auto y = point.x * sin_theta + point.y * cos_theta + y_offset; - const auto z = point.z + z_offset; - - // encontrar el punto mas cercano usnado - // probar esto sacarlo dela funcion - // DualGridaAmpler - Openvdb::ClosestSurfacePoint looker; - looker.create(grid); + [this, state, unknown_space_occupancy_prob](const auto& point) { + const auto result = state * point; + std::vector distances; std::vector points; // output list of closest surface point distances - looker.search(points, distances); - - const auto pz = static_cast + // looker.search(points, distances); - return pz * pz * pz; + return 0.1; }); }; } @@ -174,6 +155,7 @@ class Likelihood3DFieldModel { param_type params_; T likelihood_field_map_; Sophus::SE3d world_to_likelihood_field_map_transform_; + Openvdb::ClosestSurfacePoint looker; }; } // namespace beluga From 97c026994c0904ac43b439e91ec319eccb30784c Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 21 Oct 2024 23:57:06 +0800 Subject: [PATCH 33/77] Create minimal pointcloud interface for testing Signed-off-by: Pablo Vela --- .../test/simple_pointcloud_interface.hpp | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp diff --git a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp new file mode 100644 index 000000000..61f8f6c49 --- /dev/null +++ b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_TEST_STATIC_SIMPLE_POINTCLOUD_INTERFACE_HPP +#define BELUGA_TEST_STATIC_SIMPLE_POINTCLOUD_INTERFACE_HPP + +#include +#include + +namespace beluga::testing { + +template +class SparsePointCloud3 { + public: + explicit SparsePointCloud3(std::vector> points, Sophus::SE3d origin = Sophus::SE3d{}) + : points_(std::move(points)), origin_(std::move(origin)) {} + + /// Get the point cloud frame origin in the filter frame. + [[nodiscard]] const auto& origin() const { return origin_; } + + /// Get the unorganized 3D point collection as an Eigen Map. + [[nodiscard]] auto points() const { + return ranges::views::iota(0, points_.size()) | + ranges::views::transform([this](int i) { return Eigen::Map>(points_.at(i)); }); + } + + private: + std::vector> points_; + Sophus::SE3d origin_; +}; + +} // namespace beluga::testing + +#endif From aace82612a4827ac0ff2c2dd542bab0ecf254958 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 21 Oct 2024 23:59:46 +0800 Subject: [PATCH 34/77] Add openvdb and clean cmakes Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 16 +++++----------- beluga/cmake/Config.cmake.in | 2 +- beluga_ros/test/cmake/ament.cmake | 6 ------ beluga_ros/test/cmake/catkin.cmake | 7 ------- 4 files changed, 6 insertions(+), 25 deletions(-) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 62910d72f..bb40543d7 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -18,7 +18,7 @@ project(beluga VERSION 1.0) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -# list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") +list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") if(NOT CMAKE_BUILD_TYPE) message(STATUS "Setting build type to 'Release' as none was specified.") @@ -29,13 +29,7 @@ endif() add_library(beluga_compile_options INTERFACE) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - target_compile_options( - beluga_compile_options - INTERFACE -Wall - -Wconversion - -Werror - -Wextra - -Wpedantic) + target_compile_options(beluga_compile_options INTERFACE -Wall) endif() if(CMAKE_BUILD_TYPE MATCHES "Debug") target_compile_options(beluga_compile_options INTERFACE -fno-inline) @@ -49,7 +43,7 @@ find_package( find_package(range-v3 REQUIRED) find_package(Sophus REQUIRED) find_package(TBB REQUIRED) -#find_package(OpenVDB REQUIRED) +find_package(OpenVDB REQUIRED) add_library(${PROJECT_NAME} INTERFACE) target_include_directories( @@ -63,8 +57,8 @@ target_link_libraries( ${HDF5_CXX_LIBRARIES} Sophus::Sophus TBB::tbb - range-v3::range-v3) -#OpenVDB::openvdb) + range-v3::range-v3 + OpenVDB::openvdb) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index 500383137..25e4c0219 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -1,6 +1,6 @@ @PACKAGE_INIT@ -#list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") +list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") include(CMakeFindDependencyMacro) find_dependency(Eigen3 REQUIRED NO_MODULE) diff --git a/beluga_ros/test/cmake/ament.cmake b/beluga_ros/test/cmake/ament.cmake index 0e94b819a..a210e15cf 100644 --- a/beluga_ros/test/cmake/ament.cmake +++ b/beluga_ros/test/cmake/ament.cmake @@ -14,7 +14,6 @@ find_package(ament_cmake_gmock REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) ament_add_gmock(test_amcl test_amcl.cpp) target_compile_options(test_amcl PRIVATE -Wno-deprecated-copy) @@ -43,8 +42,3 @@ target_link_libraries(test_particle_cloud beluga_ros) ament_add_gmock(test_point_cloud test_point_cloud.cpp) target_compile_options(test_point_cloud PRIVATE -Wno-deprecated-copy) target_link_libraries(test_point_cloud beluga_ros) - -ament_add_gmock(test_point_cloud test_point_cloud.cpp) -target_compile_options(test_point_cloud PRIVATE -Wno-deprecated-copy) -target_link_libraries(test_point_cloud beluga_ros) - diff --git a/beluga_ros/test/cmake/catkin.cmake b/beluga_ros/test/cmake/catkin.cmake index 3e0efc290..aec116941 100644 --- a/beluga_ros/test/cmake/catkin.cmake +++ b/beluga_ros/test/cmake/catkin.cmake @@ -62,10 +62,3 @@ target_link_libraries( ${PROJECT_NAME} ${catkin_LIBRARIES} gmock_main) - -catkin_add_gmock(test_point_cloud test_point_cloud.cpp) -target_link_libraries( - test_point_cloud - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gmock_main) From 48a91e4b3031ef5c2adc46573c7d4ce7b44376c9 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Tue, 22 Oct 2024 00:00:51 +0800 Subject: [PATCH 35/77] Add likelihood 3D field sensor model Signed-off-by: Pablo Vela --- .../sensor/likelihood_3d_field_model.hpp | 105 +++++++++--------- .../sensor/test_likelihood_3d_field_model.cpp | 39 ++++--- 2 files changed, 77 insertions(+), 67 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp index 1e8722467..511110a0b 100644 --- a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp @@ -20,14 +20,16 @@ #include #include -#include -#include -#include +#include +#include + +#include + #include #include #include -#include -#include +#include +#include /** * \file @@ -73,89 +75,86 @@ struct Likelihood3DFieldModelParam { * * \note This class satisfies \ref SensorModelPage. * - * \tparam OccupancyGrid Type representing an occupancy grid. - * It must satisfy \ref OccupancyGrid2Page. + * \tparam OpenVDB grid type. */ -template +template class Likelihood3DFieldModel { public: /// State type of a particle. - // planar movement but add Z using state_type = Sophus::SE3d; /// Weight type of the particle. using weight_type = double; - /// Measurement type of the sensor: a point cloud for the XXXXXXXXXXX. + /// Measurement type given by the interface. using measurement_type = U; /// Map representation type. - // openvdb, algo asi - using map_type = T; + using map_type = GridT; /// Parameter type that the constructor uses to configure the likelihood field model. - // nombre? Likelihood3DFieldModel?? el modelo es el mismo solo que los ptos son 3d - // pero el modelo sigue siendo 2D - using param_type = Likelihood3DFieldModel; + using param_type = Likelihood3DFieldModelParam; /// Constructs a Likelihood3DFieldModel instance. /** * \param params Parameters to configure this instance. * See beluga::Likelihood3DFieldModelParam for details. - * \param grid Occupancy grid representing the static map that the sensor model + * \param grid Narrow band Level set grid representing the static map that the sensor model * uses to compute a likelihood field for lidar hits and compute importance weights * for particle states. + * Currently only supports OpenVDB Level sets. */ - explicit Likelihood3DFieldModel(const param_type& params, const map_type& map) - : params_{params}, likelihood_field_map_{std::move(map)} { - // encontrar el punto mas cercano usnado - // probar esto sacarlo dela funcion - // DualGridSmpler - looker.create(likelihood_field_map_); + explicit Likelihood3DFieldModel(const param_type& params, const map_type& grid) + : params_{params}, + looker_{openvdb::tools::ClosestSurfacePoint::create(grid)}, + squared_sigma_{params.sigma_hit * params.sigma_hit}, + amplitude_{params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi()))}, + offset_{params.z_random / params.max_laser_distance} { + /// Pre-computed parameters + assert(squared_sigma_ > 0.0); + assert(amplitude_ > 0.0); + // Remember this OpenVDB DualGridSmpler } - /// Returns the likelihood field, constructed from the provided map. - [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_map_; } - - /// Returns a state weighting function conditioned on 2D lidar hits. + /// Returns a state weighting function conditioned on 3D lidar hits. /** - * \param points 2D lidar hit points in the reference frame of particle states. + * \param measurement 3D lidar measurement containing the hit points and the transform to the origin. * \return a state weighting function satisfying \ref StateWeightingFunctionPage * and borrowing a reference to this sensor model (and thus their lifetime are bound). */ - [[nodiscard]] auto operator()(measurement_type&& points) const { - return [this, points = std::move(points)](const state_type& state) -> weight_type { - // map already in world coordinates - const auto unknown_space_occupancy_prob = static_cast(1. / params_.max_laser_distance); - + [[nodiscard]] auto operator()(measurement_type&& measurement) const { + const size_t pointcloud_size = measurement.points().size(); + // Transform each point from the sensor frame to the origin frame + return [this, + points = std::move(measurement.origin() * measurement.points())](const state_type& state) -> weight_type { + std::vector nb_distances; + std::vector vdb_points; + vdb_points.resize(pointcloud_size); + + // Transform each point to every particle state + std::transform(points.cbegin(), points.cend(), vdb_points.begin(), [this, state](const auto& point) { + // OpenVDB grid already in world coordinates + const Eigen::Vector3d point_in_state_frame = state * point; + return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()}; + }); + + // Extract the distance to the closest surface for each point + looker_.search(vdb_points, nb_distances); + + // Calculates the probality based on the distance return std::transform_reduce( - points.cbegin(), points.cend(), 1.0, std::plus{}, - [this, state, unknown_space_occupancy_prob](const auto& point) { - const auto result = state * point; - - std::vector distances; - std::vector points; - // output list of closest surface point distances - // looker.search(points, distances); - - return 0.1; + nb_distances.cbegin(), nb_distances.cend(), 1.0, std::plus{}, [this](const auto& distance) { + return amplitude_ * std::exp(-(distance * distance) / squared_sigma_) + offset_; }); }; } - /// Update the sensor model with a new occupancy grid map. - /** - * This method re-computes the underlying likelihood field. - * - * \param grid New occupancy grid representing the static map. - */ - void update_map(const map_type& map) { likelihood_field_map_{std::move(map)}; } - private: param_type params_; - T likelihood_field_map_; - Sophus::SE3d world_to_likelihood_field_map_transform_; - Openvdb::ClosestSurfacePoint looker; + typename openvdb::tools::ClosestSurfacePoint::Ptr looker_; + const double squared_sigma_; + const double amplitude_; + const double offset_; }; } // namespace beluga diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index d7c4fcbcd..e5a871a26 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -18,22 +18,28 @@ #include #include +#include #include +#include #include +#include #include #include #include #include +#include "beluga/sensor/likelihood_3d_field_model.hpp" +#include "beluga/test/simple_pointcloud_interface.hpp" + namespace { -template -auto make_map(const double voxel_size, const std::vector& world_points) { +template +auto make_map(const double voxel_size, const std::vector& world_points) { // Create the grid - T::Ptr grid = openvdb::tools::createLevelSet(voxel_size); + typename GridT::Ptr grid = openvdb::createLevelSet(voxel_size); // Get an accessor for coordinate-based access to voxels - const T::Accessor accessor = grid->getAccessor(); + typename GridT::Accessor accessor = grid->getAccessor(); // Fill the grid const openvdb::math::Transform transform; @@ -48,16 +54,17 @@ auto make_map(const double voxel_size, const std::vector& world_points) { // Prune the grid // Set voxels that are outside the narrow band to the background value and prune the grid - openvdb::tools::LevelSetTracker pruner(grid); - pruner.setTrimming(openvdb::TrimMode::kAll); + openvdb::tools::LevelSetTracker pruner(*grid); + pruner.setTrimming(openvdb::tools::lstrack::TrimMode::kAll); pruner.prune(); // Check grid - openvdb::tools::CheckLevelSet checker(grid); + openvdb::tools::CheckLevelSet checker(*grid); // Check inactive values have a magnitude equal to the background value assert(checker.checkInactiveValues() == ""); // Associate metadata + // just a terminal command to remember // vdb_print -l pcdgrid.vdb grid->setName("map"); grid->setGridClass(openvdb::GridClass::GRID_LEVEL_SET); @@ -66,19 +73,23 @@ auto make_map(const double voxel_size, const std::vector& world_points) { return grid; } -TEST(Likelihood3DFieldModel, LikelihoodField) { +TEST(Likelihood3DFieldModel, Likelihood3DField) { openvdb::initialize(); // Create Grid const double voxel_size = 0.07; - const std::vector world_points{(1.0F, 1.0F, 1.0F)}; - auto map = make_map(voxel_size, world_points); + const std::vector world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)}; + auto map = make_map(voxel_size, world_points); - const auto params = beluga::LikelihoodFieldModelParam{0.07, 2.0, 20.0, 0.5, 0.5, 0.2}; - auto sensor_model = beluga::Likelihood3DFieldModel{params, map}; + const auto params = beluga::Likelihood3DFieldModelParam{voxel_size, 2.0, 20.0, 0.5, 0.5, 0.2}; + const auto pointcloud_measurement = + beluga::testing::SparsePointCloud3{std::vector>{{1.0F, 1.0F, 1.0F}}}; + auto sensor_model = + beluga::Likelihood3DFieldModel>{params, *map}; + auto asd = sensor_model(std::move(pointcloud_measurement)); /*ASSERT_THAT( - sensor_model.likelihood_field().data(), - testing::Pointwise(testing::DoubleNear(0.003), expected_cubed_likelihood));*/ + sensor_model(pointcloud_measurement), + testing::Pointwise(testing::DoubleNear(0.003), 1.0));*/ } } // namespace From d5f800dafa978b151c865815a9bd3bb8255886ea Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Tue, 22 Oct 2024 10:54:26 +0800 Subject: [PATCH 36/77] Fix return data Signed-off-by: Pablo Vela --- .../include/beluga/test/simple_pointcloud_interface.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp index 61f8f6c49..b09cd79cc 100644 --- a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp +++ b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp @@ -31,8 +31,9 @@ class SparsePointCloud3 { /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { - return ranges::views::iota(0, points_.size()) | - ranges::views::transform([this](int i) { return Eigen::Map>(points_.at(i)); }); + return ranges::views::iota(0, static_cast(points_.size())) | ranges::views::transform([this](int i) { + return Eigen::Map>(points_.data()->data()); + }); } private: From 3be87cd74c23d535ce12bb7c34f1777da2558f6c Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Tue, 22 Oct 2024 15:07:23 +0800 Subject: [PATCH 37/77] Fix cast eigen vector Signed-off-by: Pablo Vela --- .../sensor/likelihood_3d_field_model.hpp | 21 ++++++++++++------- .../sensor/test_likelihood_3d_field_model.cpp | 10 ++++----- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp index 511110a0b..396ca2a15 100644 --- a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp @@ -125,21 +125,26 @@ class Likelihood3DFieldModel { [[nodiscard]] auto operator()(measurement_type&& measurement) const { const size_t pointcloud_size = measurement.points().size(); // Transform each point from the sensor frame to the origin frame - return [this, - points = std::move(measurement.origin() * measurement.points())](const state_type& state) -> weight_type { + auto transformed_points = + ranges::views::all(measurement.points()) | ranges::views::transform([&](const auto& point) { + return measurement.origin() * point.template cast(); + }); + + return [this, pointcloud_size, points = std::move(transformed_points)](const state_type& state) -> weight_type { std::vector nb_distances; std::vector vdb_points; vdb_points.resize(pointcloud_size); // Transform each point to every particle state - std::transform(points.cbegin(), points.cend(), vdb_points.begin(), [this, state](const auto& point) { - // OpenVDB grid already in world coordinates - const Eigen::Vector3d point_in_state_frame = state * point; - return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()}; - }); + std::transform( + points.begin(), points.end(), vdb_points.begin(), [this, pointcloud_size, state](const auto& point) { + // OpenVDB grid already in world coordinates + const Eigen::Vector3d point_in_state_frame = state * point; + return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()}; + }); // Extract the distance to the closest surface for each point - looker_.search(vdb_points, nb_distances); + looker_->search(vdb_points, nb_distances); // Calculates the probality based on the distance return std::transform_reduce( diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index e5a871a26..d1e3f7ace 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -81,15 +81,13 @@ TEST(Likelihood3DFieldModel, Likelihood3DField) { auto map = make_map(voxel_size, world_points); const auto params = beluga::Likelihood3DFieldModelParam{voxel_size, 2.0, 20.0, 0.5, 0.5, 0.2}; - const auto pointcloud_measurement = - beluga::testing::SparsePointCloud3{std::vector>{{1.0F, 1.0F, 1.0F}}}; + auto pointcloud_measurement = + beluga::testing::SparsePointCloud3{std::vector>{{1.0F, 1.0F, 1.0F}}}; auto sensor_model = beluga::Likelihood3DFieldModel>{params, *map}; - auto asd = sensor_model(std::move(pointcloud_measurement)); - /*ASSERT_THAT( - sensor_model(pointcloud_measurement), - testing::Pointwise(testing::DoubleNear(0.003), 1.0));*/ + auto state_weighting_function = sensor_model(std::move(pointcloud_measurement)); + ASSERT_NEAR(1.0, state_weighting_function(Sophus::SE3d{}), 0.03); } } // namespace From a6b9e067cea9042d484af86116da12a2133784d4 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 23 Oct 2024 11:18:31 +0800 Subject: [PATCH 38/77] Fix openvdb map creation Signed-off-by: Pablo Vela --- .../sensor/test_likelihood_3d_field_model.cpp | 26 +++++++------------ 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index d1e3f7ace..6c428eb81 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -18,11 +18,8 @@ #include #include -#include #include #include -#include -#include #include #include @@ -37,12 +34,14 @@ namespace { template auto make_map(const double voxel_size, const std::vector& world_points) { // Create the grid - typename GridT::Ptr grid = openvdb::createLevelSet(voxel_size); + typename GridT::Ptr grid = openvdb::FloatGrid::create(); + grid->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size)); + // Get an accessor for coordinate-based access to voxels typename GridT::Accessor accessor = grid->getAccessor(); // Fill the grid - const openvdb::math::Transform transform; + const openvdb::math::Transform& transform = grid->transform(); for (const auto& point : world_points) { // Transform to index world const openvdb::math::Coord ijk = transform.worldToIndexCellCentered(point); @@ -52,25 +51,18 @@ auto make_map(const double voxel_size, const std::vector& world_points) { accessor.setActiveState(ijk); } - // Prune the grid - // Set voxels that are outside the narrow band to the background value and prune the grid - openvdb::tools::LevelSetTracker pruner(*grid); - pruner.setTrimming(openvdb::tools::lstrack::TrimMode::kAll); - pruner.prune(); - // Check grid openvdb::tools::CheckLevelSet checker(*grid); // Check inactive values have a magnitude equal to the background value assert(checker.checkInactiveValues() == ""); // Associate metadata - // just a terminal command to remember - // vdb_print -l pcdgrid.vdb grid->setName("map"); - grid->setGridClass(openvdb::GridClass::GRID_LEVEL_SET); - grid->setIsInWorldSpace(true); // not quite sure! - return grid; + // Transform to levelset + typename GridT::Ptr grid_levelset = openvdb::tools::topologyToLevelSet(*grid, 1, 0, 0, 0); + + return grid_levelset; } TEST(Likelihood3DFieldModel, Likelihood3DField) { @@ -82,7 +74,7 @@ TEST(Likelihood3DFieldModel, Likelihood3DField) { const auto params = beluga::Likelihood3DFieldModelParam{voxel_size, 2.0, 20.0, 0.5, 0.5, 0.2}; auto pointcloud_measurement = - beluga::testing::SparsePointCloud3{std::vector>{{1.0F, 1.0F, 1.0F}}}; + beluga::testing::SparsePointCloud3{std::vector>{{1.1F, 1.0F, 1.0F}}}; auto sensor_model = beluga::Likelihood3DFieldModel>{params, *map}; From a1a1eacfc07600965c9014a2b8823d7e4f85fb6f Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 23 Oct 2024 11:37:16 +0800 Subject: [PATCH 39/77] Fix compile with full options Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 8 +++++++- .../include/beluga/test/simple_pointcloud_interface.hpp | 5 ++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index bb40543d7..cb8d74483 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -29,7 +29,13 @@ endif() add_library(beluga_compile_options INTERFACE) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - target_compile_options(beluga_compile_options INTERFACE -Wall) + target_compile_options( + beluga_compile_options + INTERFACE -Wall + -Wconversion + -Werror + -Wextra + -Wpedantic) endif() if(CMAKE_BUILD_TYPE MATCHES "Debug") target_compile_options(beluga_compile_options INTERFACE -fno-inline) diff --git a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp index b09cd79cc..a95013840 100644 --- a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp +++ b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp @@ -31,9 +31,8 @@ class SparsePointCloud3 { /// Get the unorganized 3D point collection as an Eigen Map. [[nodiscard]] auto points() const { - return ranges::views::iota(0, static_cast(points_.size())) | ranges::views::transform([this](int i) { - return Eigen::Map>(points_.data()->data()); - }); + return ranges::views::iota(0, static_cast(points_.size())) | + ranges::views::transform([this](int i) { return Eigen::Map>(points_[i].data()); }); } private: From 015f1b79441f103c2e55d662d87fa4c144e0ef2c Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 26 Oct 2024 17:52:54 +0800 Subject: [PATCH 40/77] Fix nit Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor.hpp | 2 +- ..._model.hpp => likelihood_field_model3.hpp} | 43 +++++++++---------- .../test/simple_pointcloud_interface.hpp | 14 +++--- 3 files changed, 28 insertions(+), 31 deletions(-) rename beluga/include/beluga/sensor/{likelihood_3d_field_model.hpp => likelihood_field_model3.hpp} (81%) diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 930b547fa..d085d25e7 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -63,8 +63,8 @@ #include #include #include -#include #include +#include #include #endif diff --git a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp similarity index 81% rename from beluga/include/beluga/sensor/likelihood_3d_field_model.hpp rename to beluga/include/beluga/sensor/likelihood_field_model3.hpp index 396ca2a15..8d49270e4 100644 --- a/beluga/include/beluga/sensor/likelihood_3d_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model3.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_SENSOR_LIKELIHOOD_3D_FIELD_MODEL_HPP -#define BELUGA_SENSOR_LIKELIHOOD_3D_FIELD_MODEL_HPP +#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP +#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP #include #include @@ -38,13 +38,11 @@ namespace beluga { -/// Parameters used to construct a Likelihood3DFieldModel instance. +/// Parameters used to construct a LikelihoodFieldModel3 instance. /** * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. */ -struct Likelihood3DFieldModelParam { - /// Voxel size in meters. - double voxel_size = 0.07; +struct LikelihoodFieldModel3Param { /// Maximum distance to obstacle. /** * When creating a distance map, if the distance to an obstacle is higher than the value specified here, @@ -77,8 +75,8 @@ struct Likelihood3DFieldModelParam { * * \tparam OpenVDB grid type. */ -template -class Likelihood3DFieldModel { +template +class LikelihoodFieldModel3 { public: /// State type of a particle. using state_type = Sophus::SE3d; @@ -87,13 +85,13 @@ class Likelihood3DFieldModel { using weight_type = double; /// Measurement type given by the interface. - using measurement_type = U; + using measurement_type = PointCloud; /// Map representation type. using map_type = GridT; /// Parameter type that the constructor uses to configure the likelihood field model. - using param_type = Likelihood3DFieldModelParam; + using param_type = LikelihoodFieldModel3Param; /// Constructs a Likelihood3DFieldModel instance. /** @@ -104,16 +102,16 @@ class Likelihood3DFieldModel { * for particle states. * Currently only supports OpenVDB Level sets. */ - explicit Likelihood3DFieldModel(const param_type& params, const map_type& grid) + explicit LikelihoodFieldModel3(const param_type& params, const map_type& grid) : params_{params}, looker_{openvdb::tools::ClosestSurfacePoint::create(grid)}, - squared_sigma_{params.sigma_hit * params.sigma_hit}, + two_squared_sigma_{2 * params.sigma_hit * params.sigma_hit}, amplitude_{params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi()))}, offset_{params.z_random / params.max_laser_distance} { + openvdb::initialize(); /// Pre-computed parameters - assert(squared_sigma_ > 0.0); + assert(two_squared_sigma_ > 0.0); assert(amplitude_ > 0.0); - // Remember this OpenVDB DualGridSmpler } /// Returns a state weighting function conditioned on 3D lidar hits. @@ -133,15 +131,14 @@ class Likelihood3DFieldModel { return [this, pointcloud_size, points = std::move(transformed_points)](const state_type& state) -> weight_type { std::vector nb_distances; std::vector vdb_points; - vdb_points.resize(pointcloud_size); + vdb_points.reserve(pointcloud_size); // Transform each point to every particle state - std::transform( - points.begin(), points.end(), vdb_points.begin(), [this, pointcloud_size, state](const auto& point) { - // OpenVDB grid already in world coordinates - const Eigen::Vector3d point_in_state_frame = state * point; - return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()}; - }); + std::transform(points.begin(), points.end(), std::back_inserter(vdb_points), [this, state](const auto& point) { + // OpenVDB grid already in world coordinates + const Eigen::Vector3d point_in_state_frame = state * point; + return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()}; + }); // Extract the distance to the closest surface for each point looker_->search(vdb_points, nb_distances); @@ -149,7 +146,7 @@ class Likelihood3DFieldModel { // Calculates the probality based on the distance return std::transform_reduce( nb_distances.cbegin(), nb_distances.cend(), 1.0, std::plus{}, [this](const auto& distance) { - return amplitude_ * std::exp(-(distance * distance) / squared_sigma_) + offset_; + return amplitude_ * std::exp(-(distance * distance) / two_squared_sigma_) + offset_; }); }; } @@ -157,7 +154,7 @@ class Likelihood3DFieldModel { private: param_type params_; typename openvdb::tools::ClosestSurfacePoint::Ptr looker_; - const double squared_sigma_; + const double two_squared_sigma_; const double amplitude_; const double offset_; }; diff --git a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp index a95013840..28799471b 100644 --- a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp +++ b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp @@ -21,25 +21,25 @@ namespace beluga::testing { template -class SparsePointCloud3 { +class SimpleSparsePointCloud3 { public: - explicit SparsePointCloud3(std::vector> points, Sophus::SE3d origin = Sophus::SE3d{}) + explicit SimpleSparsePointCloud3(std::vector> points, Sophus::SE3d origin = Sophus::SE3d{}) : points_(std::move(points)), origin_(std::move(origin)) {} /// Get the point cloud frame origin in the filter frame. [[nodiscard]] const auto& origin() const { return origin_; } - /// Get the unorganized 3D point collection as an Eigen Map. - [[nodiscard]] auto points() const { - return ranges::views::iota(0, static_cast(points_.size())) | - ranges::views::transform([this](int i) { return Eigen::Map>(points_[i].data()); }); - } + /// Get the 3D points collection. + [[nodiscard]] auto& points() const { return points_; } private: std::vector> points_; Sophus::SE3d origin_; }; +using SimpleSparsePointCloud3d = SimpleSparsePointCloud3; +using SimpleSparsePointCloud3f = SimpleSparsePointCloud3; + } // namespace beluga::testing #endif From ee56b7f6b2bfe9f580931acb8ecc0e617a2d91c9 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 26 Oct 2024 17:53:45 +0800 Subject: [PATCH 41/77] Add close and far point test Signed-off-by: Pablo Vela --- .../sensor/test_likelihood_3d_field_model.cpp | 54 +++++++++++++------ 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index 6c428eb81..24e3e037b 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -26,13 +26,17 @@ #include #include -#include "beluga/sensor/likelihood_3d_field_model.hpp" +#include "beluga/sensor/likelihood_field_model3.hpp" #include "beluga/test/simple_pointcloud_interface.hpp" namespace { template auto make_map(const double voxel_size, const std::vector& world_points) { + // Parameters + constexpr int kHalfWidth = 1; + constexpr int kClosingSteps = 0; + // Create the grid typename GridT::Ptr grid = openvdb::FloatGrid::create(); grid->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size)); @@ -56,30 +60,46 @@ auto make_map(const double voxel_size, const std::vector& world_points) { // Check inactive values have a magnitude equal to the background value assert(checker.checkInactiveValues() == ""); - // Associate metadata - grid->setName("map"); - // Transform to levelset - typename GridT::Ptr grid_levelset = openvdb::tools::topologyToLevelSet(*grid, 1, 0, 0, 0); - - return grid_levelset; + return openvdb::tools::topologyToLevelSet(*grid, kHalfWidth, kClosingSteps); } -TEST(Likelihood3DFieldModel, Likelihood3DField) { +TEST(TestLikelihoodFieldModel3, Point) { openvdb::initialize(); + // Parameters + constexpr double kVoxelSize = 0.07; + constexpr double kMaxObstacleDistance = 2.0; + constexpr double kMaxLaserDistance = 20.0; + constexpr double kZHit = 0.5; + constexpr double kZRandom = 0.5; + constexpr double kSigmaHit = 0.2; + // Create Grid - const double voxel_size = 0.07; const std::vector world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)}; - auto map = make_map(voxel_size, world_points); + auto map = make_map(kVoxelSize, world_points); - const auto params = beluga::Likelihood3DFieldModelParam{voxel_size, 2.0, 20.0, 0.5, 0.5, 0.2}; - auto pointcloud_measurement = - beluga::testing::SparsePointCloud3{std::vector>{{1.1F, 1.0F, 1.0F}}}; + const auto params = + beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; auto sensor_model = - beluga::Likelihood3DFieldModel>{params, *map}; - - auto state_weighting_function = sensor_model(std::move(pointcloud_measurement)); - ASSERT_NEAR(1.0, state_weighting_function(Sophus::SE3d{}), 0.03); + beluga::LikelihoodFieldModel3{params, *map}; + + // Exact point + auto pointcloud_measurement_exact = + beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.0F, 1.0F, 1.0F}}}; + auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); + ASSERT_NEAR(1.0, state_weighting_function_exact(Sophus::SE3d{}), 0.03); + + // Close point (Inside the narrow band) + auto pointcloud_measurement_close = + beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.035F, 1.0F, 1.0F}}}; + auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); + ASSERT_NEAR(1.0, state_weighting_function_close(Sophus::SE3d{}), 0.03); + + // Far point (Outside the narrow band) + auto pointcloud_measurement_far = + beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.1F, 1.1F, 1.1F}}}; + auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); + ASSERT_NEAR(1.0, state_weighting_function_far(Sophus::SE3d{}), 0.03); } } // namespace From 01018a35fd160b54d78186949617a0c0d23e8c59 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Tue, 29 Oct 2024 14:01:42 +0800 Subject: [PATCH 42/77] Fix openvdb dependency Signed-off-by: Pablo Vela --- beluga/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/beluga/package.xml b/beluga/package.xml index e5eab3bf6..ed54aca3d 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -19,7 +19,7 @@ range-v3 sophus tbb - OpenVDB + libopenvdb-dev clang-format clang-tidy From c0b1c45045532337ccd13afa18087a40863edf78 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Tue, 29 Oct 2024 14:04:21 +0800 Subject: [PATCH 43/77] Change test parameters Signed-off-by: Pablo Vela --- .../sensor/test_likelihood_3d_field_model.cpp | 75 +++++++++++++++++-- 1 file changed, 69 insertions(+), 6 deletions(-) diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index 24e3e037b..7668dde5e 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -69,10 +69,10 @@ TEST(TestLikelihoodFieldModel3, Point) { // Parameters constexpr double kVoxelSize = 0.07; constexpr double kMaxObstacleDistance = 2.0; - constexpr double kMaxLaserDistance = 20.0; + constexpr double kMaxLaserDistance = 100.0; constexpr double kZHit = 0.5; - constexpr double kZRandom = 0.5; - constexpr double kSigmaHit = 0.2; + constexpr double kZRandom = 0.001; + constexpr double kSigmaHit = 0.22; // Create Grid const std::vector world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)}; @@ -87,19 +87,82 @@ TEST(TestLikelihoodFieldModel3, Point) { auto pointcloud_measurement_exact = beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.0F, 1.0F, 1.0F}}}; auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); - ASSERT_NEAR(1.0, state_weighting_function_exact(Sophus::SE3d{}), 0.03); + ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 1.90); // Close point (Inside the narrow band) auto pointcloud_measurement_close = beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.035F, 1.0F, 1.0F}}}; auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); - ASSERT_NEAR(1.0, state_weighting_function_close(Sophus::SE3d{}), 0.03); + ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 1.8); // Far point (Outside the narrow band) auto pointcloud_measurement_far = beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.1F, 1.1F, 1.1F}}}; auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); - ASSERT_NEAR(1.0, state_weighting_function_far(Sophus::SE3d{}), 0.03); + ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 1.65); +} + +TEST(TestLikelihoodFieldModel3, Cube) { + openvdb::initialize(); + // Parameters + constexpr double kVoxelSize = 0.07; + constexpr double kMaxObstacleDistance = 2.0; + constexpr double kMaxLaserDistance = 100.0; + constexpr double kZHit = 0.5; + constexpr double kZRandom = 0.001; + constexpr double kSigmaHit = 0.22; + + // Create Grid + const std::vector world_points{ + openvdb::math::Vec3s(1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(1.0F, 1.0F, -1.0F), + openvdb::math::Vec3s(1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(1.0F, -1.0F, -1.0F), + openvdb::math::Vec3s(-1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, -1.0F, -1.0F), + openvdb::math::Vec3s(-1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, 1.0F, -1.0F)}; + auto map = make_map(kVoxelSize, world_points); + + const auto params = + beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; + auto sensor_model = + beluga::LikelihoodFieldModel3{params, *map}; + + // Exact point + auto pointcloud_measurement_exact = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.0F, 1.0F, 1.0F}, + {1.0F, 1.0F, -1.0F}, + {1.0F, -1.0F, 1.0F}, + {1.0F, -1.0F, -1.0F}, + {-1.0F, -1.0F, 1.0F}, + {-1.0F, -1.0F, -1.0F}, + {-1.0F, 1.0F, 1.0F}, + {-1.0F, 1.0F, -1.0F}}}; + auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); + ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 8.2); + + // Close point (Inside the narrow band) + auto pointcloud_measurement_close = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.035F, 1.0F, 1.0F}, + {1.035F, 1.0F, -1.0F}, + {1.035F, -1.0F, 1.0F}, + {1.035F, -1.0F, -1.0F}, + {-1.0F, -1.0F, 1.0F}, + {-1.0F, -1.0F, -1.0F}, + {-1.0F, 1.0F, 1.0F}, + {-1.0F, 1.0F, -1.0F}}}; + auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); + ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 7.4); + + // Far point (Outside the narrow band) + auto pointcloud_measurement_far = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.1F, 1.1F, 1.1F}, + {1.1F, 1.1F, -1.1F}, + {1.1F, -1.1F, 1.1F}, + {1.1F, -1.1F, -1.1F}, + {-1.1F, -1.1F, 1.1F}, + {-1.1F, -1.1F, -1.1F}, + {-1.1F, 1.1F, 1.1F}, + {-1.1F, 1.1F, -1.1F}}}; + auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); + ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 6.85); } } // namespace From 23056f03fd88d7cd3bc7c4d4259a9bbe89ffba3d Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 9 Nov 2024 01:00:04 +0800 Subject: [PATCH 44/77] Fix points not being a container Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor/likelihood_field_model3.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_field_model3.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp index 8d49270e4..0b2028255 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model3.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model3.hpp @@ -123,10 +123,11 @@ class LikelihoodFieldModel3 { [[nodiscard]] auto operator()(measurement_type&& measurement) const { const size_t pointcloud_size = measurement.points().size(); // Transform each point from the sensor frame to the origin frame - auto transformed_points = - ranges::views::all(measurement.points()) | ranges::views::transform([&](const auto& point) { - return measurement.origin() * point.template cast(); - }); + auto transformed_points = ranges::views::all(measurement.points()) | + ranges::views::transform([&](const auto& point) { + return measurement.origin() * point.template cast(); + }) | + ranges::to(); return [this, pointcloud_size, points = std::move(transformed_points)](const state_type& state) -> weight_type { std::vector nb_distances; From 3a98181e36aaf9599dfd5119d87b462f96eba03b Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Tue, 12 Nov 2024 10:23:14 +0800 Subject: [PATCH 45/77] Change OpenVDB as an optional dependency Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 12 ++++++++---- beluga/cmake/Config.cmake.in | 3 ++- beluga/include/beluga/sensor.hpp | 5 ++++- beluga/package.xml | 1 - beluga/test/beluga/CMakeLists.txt | 2 +- 5 files changed, 15 insertions(+), 8 deletions(-) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index cb8d74483..242f2d5dc 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -18,8 +18,6 @@ project(beluga VERSION 1.0) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") - if(NOT CMAKE_BUILD_TYPE) message(STATUS "Setting build type to 'Release' as none was specified.") set(CMAKE_BUILD_TYPE @@ -49,7 +47,13 @@ find_package( find_package(range-v3 REQUIRED) find_package(Sophus REQUIRED) find_package(TBB REQUIRED) -find_package(OpenVDB REQUIRED) + +list(APPEND CMAKE_MODULE_PATH "/usr/lib/x86_64-linux-gnu/cmake/OpenVDB/") +list(APPEND CMAKE_MODULE_PATH "/usr/lib/aarch64-linux-gnu/cmake/OpenVDB") +find_package(OpenVDB QUIET) +if(OpenVDB_FOUND) + add_definitions(-USE_OPENVDB) +endif() add_library(${PROJECT_NAME} INTERFACE) target_include_directories( @@ -64,7 +68,7 @@ target_link_libraries( Sophus::Sophus TBB::tbb range-v3::range-v3 - OpenVDB::openvdb) + $,OpenVDB::openvdb,>) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index 25e4c0219..fdc510bec 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -1,6 +1,7 @@ @PACKAGE_INIT@ -list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB") +list(APPEND CMAKE_MODULE_PATH "/usr/lib/x86_64-linux-gnu/cmake/OpenVDB/") +list(APPEND CMAKE_MODULE_PATH "/usr/lib/aarch64-linux-gnu/cmake/OpenVDB") include(CMakeFindDependencyMacro) find_dependency(Eigen3 REQUIRED NO_MODULE) diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index d085d25e7..ca6fa507e 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -64,7 +64,10 @@ #include #include #include -#include #include +#ifdef USE_OPENVDB +#include +#endif + #endif diff --git a/beluga/package.xml b/beluga/package.xml index ed54aca3d..dc349ed27 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -19,7 +19,6 @@ range-v3 sophus tbb - libopenvdb-dev clang-format clang-tidy diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index c812a0762..ed0b544d2 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -16,6 +16,7 @@ file(COPY test_data DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) add_executable( test_beluga + $,sensor/test_likelihood_3d_field_model.cpp,> actions/test_assign.cpp actions/test_normalize.cpp actions/test_overlay.cpp @@ -52,7 +53,6 @@ add_executable( sensor/test_beam_model.cpp sensor/test_bearing_sensor_model.cpp sensor/test_landmark_sensor_model.cpp - sensor/test_likelihood_3d_field_model.cpp sensor/test_likelihood_field_model.cpp sensor/test_ndt_model.cpp test_3d_embedding.cpp From b8e86654c409f7540f308a5db62d8b406d90add9 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 13:18:41 +0800 Subject: [PATCH 46/77] Remove openvdb lib Signed-off-by: Pablo Vela --- docker/images/humble/Dockerfile | 1 - docker/images/noetic/Dockerfile | 1 - 2 files changed, 2 deletions(-) diff --git a/docker/images/humble/Dockerfile b/docker/images/humble/Dockerfile index 713dd181b..a61d25886 100644 --- a/docker/images/humble/Dockerfile +++ b/docker/images/humble/Dockerfile @@ -49,7 +49,6 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ - libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/noetic/Dockerfile b/docker/images/noetic/Dockerfile index 9d1541459..02701cf58 100644 --- a/docker/images/noetic/Dockerfile +++ b/docker/images/noetic/Dockerfile @@ -62,7 +62,6 @@ RUN apt-get update \ python3-colcon-lcov-result \ python3-colcon-mixin \ python3-pip \ - libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* From c4d3b4a34ae1f916b3bdeef258375b9c96f07413 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 13:19:32 +0800 Subject: [PATCH 47/77] Add boost dependency for openvdb Signed-off-by: Pablo Vela --- docker/images/iron/Dockerfile | 1 + docker/images/jazzy/Dockerfile | 1 + docker/images/rolling/Dockerfile | 1 + 3 files changed, 3 insertions(+) diff --git a/docker/images/iron/Dockerfile b/docker/images/iron/Dockerfile index 4abc9c01a..d635efb9b 100644 --- a/docker/images/iron/Dockerfile +++ b/docker/images/iron/Dockerfile @@ -49,6 +49,7 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ + libboost-all-dev \ libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/jazzy/Dockerfile b/docker/images/jazzy/Dockerfile index f8446cbd9..49688e258 100644 --- a/docker/images/jazzy/Dockerfile +++ b/docker/images/jazzy/Dockerfile @@ -49,6 +49,7 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ + libboost-all-dev \ libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/rolling/Dockerfile b/docker/images/rolling/Dockerfile index bdf5234df..1066ef1f5 100644 --- a/docker/images/rolling/Dockerfile +++ b/docker/images/rolling/Dockerfile @@ -49,6 +49,7 @@ RUN apt-get update \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ + libboost-all-dev \ libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* From b40595c267b1e9ec9a612ed0269325e8d82a7b6e Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 13:20:27 +0800 Subject: [PATCH 48/77] Change points function to const Signed-off-by: Pablo Vela --- .../beluga/include/beluga/test/simple_pointcloud_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp index 28799471b..871fba89e 100644 --- a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp +++ b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp @@ -30,7 +30,7 @@ class SimpleSparsePointCloud3 { [[nodiscard]] const auto& origin() const { return origin_; } /// Get the 3D points collection. - [[nodiscard]] auto& points() const { return points_; } + [[nodiscard]] const auto& points() const { return points_; } private: std::vector> points_; From f1e7b760dae883db0c95f9584e06d82f35e5b1de Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 13:22:03 +0800 Subject: [PATCH 49/77] Add test removed by mistake Signed-off-by: Pablo Vela --- beluga/test/beluga/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index ed0b544d2..068cb484f 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -53,6 +53,7 @@ add_executable( sensor/test_beam_model.cpp sensor/test_bearing_sensor_model.cpp sensor/test_landmark_sensor_model.cpp + sensor/test_lfm_with_unknown_space.cpp sensor/test_likelihood_field_model.cpp sensor/test_ndt_model.cpp test_3d_embedding.cpp From bef40ad2dd85beba55969b16c62adab2f0f94e79 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 13:24:56 +0800 Subject: [PATCH 50/77] Rework openvdb support options for beluga Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 45 ++++++++++++++++++++++++++------ beluga/cmake/Config.cmake.in | 4 --- beluga/include/beluga/sensor.hpp | 2 +- 3 files changed, 38 insertions(+), 13 deletions(-) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 242f2d5dc..17372d9f6 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -18,6 +18,14 @@ project(beluga VERSION 1.0) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +option( + USE_OPENVDB + "Compiles with OpenVDB, required for the 3D likelihood field sensor model" + Off) +set(OPENVDB_CMAKE_MODULE_PATH + ${OPENVDB_CMAKE_MODULE_PATH} + CACHE PATH "Path to OpenVDB CMake module") + if(NOT CMAKE_BUILD_TYPE) message(STATUS "Setting build type to 'Release' as none was specified.") set(CMAKE_BUILD_TYPE @@ -48,11 +56,31 @@ find_package(range-v3 REQUIRED) find_package(Sophus REQUIRED) find_package(TBB REQUIRED) -list(APPEND CMAKE_MODULE_PATH "/usr/lib/x86_64-linux-gnu/cmake/OpenVDB/") -list(APPEND CMAKE_MODULE_PATH "/usr/lib/aarch64-linux-gnu/cmake/OpenVDB") -find_package(OpenVDB QUIET) -if(OpenVDB_FOUND) - add_definitions(-USE_OPENVDB) +# If USE_OPENVDB is enabled, proceed with finding OpenVDB +if(USE_OPENVDB) + if(NOT OPENVDB_CMAKE_MODULE_PATH) + find_file( + OPENVDB_MODULE_PATH FindOpenVDB.cmake + PATHS /usr/lib/x86_64-linux-gnu/cmake/OpenVDB/ + /usr/lib/aarch64-linux-gnu/cmake/OpenVDB/ + NO_DEFAULT_PATH) + else() + find_file(OPENVDB_MODULE_PATH FindOpenVDB.cmake + PATHS ${OPENVDB_CMAKE_MODULE_PATH}) + endif() + + if(OPENVDB_MODULE_PATH) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE_PATH} + DIRECTORY) + message(STATUS "Found OpenVDB CMake module at ${OPENVDB_CMAKE_MODULE_PATH}") + list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) + find_package(Boost REQUIRED) + find_package(OpenVDB REQUIRED) + else() + message(FATAL_ERROR "FindOpenVDB.cmake not found in the speficied paths.") + endif() +else() + message(STATUS "USE_OPENVDB is Off, skipping OpenVDB setup.") endif() add_library(${PROJECT_NAME} INTERFACE) @@ -68,10 +96,11 @@ target_link_libraries( Sophus::Sophus TBB::tbb range-v3::range-v3 - $,OpenVDB::openvdb,>) + $,OpenVDB::openvdb,>) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) -target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG - SOPHUS_USE_BASIC_LOGGING) +target_compile_definitions( + ${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING + $<$:BELUGA_OPENVDB_SUPPORT>) add_executable(clang_tidy_findable) target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index fdc510bec..8d7b0e44f 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -1,15 +1,11 @@ @PACKAGE_INIT@ -list(APPEND CMAKE_MODULE_PATH "/usr/lib/x86_64-linux-gnu/cmake/OpenVDB/") -list(APPEND CMAKE_MODULE_PATH "/usr/lib/aarch64-linux-gnu/cmake/OpenVDB") - include(CMakeFindDependencyMacro) find_dependency(Eigen3 REQUIRED NO_MODULE) find_dependency(range-v3 REQUIRED) find_dependency(HDF5 COMPONENTS CXX REQUIRED) find_dependency(Sophus REQUIRED) find_dependency(TBB REQUIRED) -find_dependency(OpenVDB REQUIRED) include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index ca6fa507e..39dfeabf5 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -66,7 +66,7 @@ #include #include -#ifdef USE_OPENVDB +#ifdef BELUGA_OPENVDB_SUPPORT #include #endif From ab138c5bcc5c54ebcaef08a0da27ff65f75b657e Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 15:12:30 +0800 Subject: [PATCH 51/77] Update to build with openvdb flag Signed-off-by: Pablo Vela --- tools/build-and-test.sh | 7 +++++++ tools/run-clang-tidy.sh | 7 ++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/tools/build-and-test.sh b/tools/build-and-test.sh index ce7feaf0b..119e97871 100755 --- a/tools/build-and-test.sh +++ b/tools/build-and-test.sh @@ -22,11 +22,16 @@ SCRIPT_PATH=$(dirname "$(readlink -f "$0")") set -o errexit +OPEN_VDB="Off" + if [ "${ROS_DISTRO}" != "noetic" ]; then if [ "${ROS_DISTRO}" != "jazzy" ] && [ "${ROS_DISTRO}" != "rolling" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_benchmark beluga_example beluga_system_tests beluga_tools" else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools" + if [ "${ROS_DISTRO}" != "humble" ]; then + OPEN_VDB="On" + fi fi else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_example" @@ -43,6 +48,7 @@ colcon build \ build-testing-on \ ccache \ release \ + --cmake-args -DUSE_OPENVDB=${OPEN_VDB} \ --cmake-force-configure echo ::endgroup:: @@ -57,6 +63,7 @@ colcon build \ coverage-gcc \ coverage-pytest \ debug \ + --cmake-args -DUSE_OPENVDB=${OPEN_VDB} \ --cmake-force-configure echo ::endgroup:: diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index 5a96237a8..ff4bed4b9 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -20,14 +20,19 @@ set -o errexit -o xtrace +OPEN_VDB="Off" + if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" + if [ "${ROS_DISTRO}" != "humble" ]; then + OPEN_VDB="On" + fi else ROS_PACKAGES="beluga beluga_ros beluga_amcl" fi source /opt/ros/${ROS_DISTRO}/setup.sh -colcon build --packages-up-to ${ROS_PACKAGES} --event-handlers=console_cohesion+ --symlink-install --mixin ccache +colcon build --packages-up-to ${ROS_PACKAGES} --event-handlers=console_cohesion+ --symlink-install --mixin ccache --cmake-args -DUSE_OPENVDB=${OPEN_VDB} echo ${ROS_PACKAGES} | xargs -n1 echo | # NOTE: `-Wno-gnu-zero-variadic-macro-arguments` is needed due to From 88966dc297dd2e60b9d5d3b015e02b400a5305ea Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 15:13:13 +0800 Subject: [PATCH 52/77] Fix openvdb dependency Signed-off-by: Pablo Vela --- beluga/cmake/Config.cmake.in | 3 +++ 1 file changed, 3 insertions(+) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index 8d7b0e44f..f478bca8d 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -1,11 +1,14 @@ @PACKAGE_INIT@ +list(APPEND CMAKE_MODULE_PATH "@OPENVDB_CMAKE_MODULE_PATH@") + include(CMakeFindDependencyMacro) find_dependency(Eigen3 REQUIRED NO_MODULE) find_dependency(range-v3 REQUIRED) find_dependency(HDF5 COMPONENTS CXX REQUIRED) find_dependency(Sophus REQUIRED) find_dependency(TBB REQUIRED) +find_dependency(OpenVDB QUIET) include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") From a7c27aba7b97dc594db6feb9416aa6a3db5c97ca Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 14 Nov 2024 15:13:30 +0800 Subject: [PATCH 53/77] Fix clang tidy Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor/likelihood_field_model3.hpp | 2 +- .../include/beluga/test/simple_pointcloud_interface.hpp | 4 ++-- beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_field_model3.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp index 0b2028255..9cbab2388 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model3.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model3.hpp @@ -135,7 +135,7 @@ class LikelihoodFieldModel3 { vdb_points.reserve(pointcloud_size); // Transform each point to every particle state - std::transform(points.begin(), points.end(), std::back_inserter(vdb_points), [this, state](const auto& point) { + std::transform(points.begin(), points.end(), std::back_inserter(vdb_points), [state](const auto& point) { // OpenVDB grid already in world coordinates const Eigen::Vector3d point_in_state_frame = state * point; return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()}; diff --git a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp index 871fba89e..7485e282a 100644 --- a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp +++ b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_TEST_STATIC_SIMPLE_POINTCLOUD_INTERFACE_HPP -#define BELUGA_TEST_STATIC_SIMPLE_POINTCLOUD_INTERFACE_HPP +#ifndef BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP +#define BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP #include #include diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index 7668dde5e..32285de95 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -38,7 +38,7 @@ auto make_map(const double voxel_size, const std::vector& world_points) { constexpr int kClosingSteps = 0; // Create the grid - typename GridT::Ptr grid = openvdb::FloatGrid::create(); + const typename GridT::Ptr grid = openvdb::FloatGrid::create(); grid->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size)); // Get an accessor for coordinate-based access to voxels From b6073eec0142379f885540be34e25213e0a9247d Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 18 Nov 2024 16:00:06 +0800 Subject: [PATCH 54/77] Organize packages list Signed-off-by: Pablo Vela --- docker/images/iron/Dockerfile | 4 ++-- docker/images/jazzy/Dockerfile | 4 ++-- docker/images/rolling/Dockerfile | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docker/images/iron/Dockerfile b/docker/images/iron/Dockerfile index d635efb9b..0df85bbe9 100644 --- a/docker/images/iron/Dockerfile +++ b/docker/images/iron/Dockerfile @@ -44,13 +44,13 @@ RUN apt-get update \ gdb \ git \ lcov \ + libboost-all-dev \ + libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ - libboost-all-dev \ - libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/jazzy/Dockerfile b/docker/images/jazzy/Dockerfile index 49688e258..89172c499 100644 --- a/docker/images/jazzy/Dockerfile +++ b/docker/images/jazzy/Dockerfile @@ -44,13 +44,13 @@ RUN apt-get update \ gdb \ git \ lcov \ + libboost-all-dev \ + libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ - libboost-all-dev \ - libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* diff --git a/docker/images/rolling/Dockerfile b/docker/images/rolling/Dockerfile index 1066ef1f5..b1f00b8e5 100644 --- a/docker/images/rolling/Dockerfile +++ b/docker/images/rolling/Dockerfile @@ -44,13 +44,13 @@ RUN apt-get update \ gdb \ git \ lcov \ + libboost-all-dev \ + libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ python3-colcon-coveragepy-result \ python3-colcon-lcov-result \ python3-pip \ - libboost-all-dev \ - libopenvdb-dev \ tmux \ && rm -rf /var/lib/apt/lists/* From 1d8696eb5412d3ca6aaf2f210abf6836bf4b451d Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 18 Nov 2024 16:04:06 +0800 Subject: [PATCH 55/77] Generalize cmake and colcon arguments Signed-off-by: Pablo Vela --- tools/build-and-test.sh | 53 ++++++++++++++++++++++------------------- tools/run-clang-tidy.sh | 14 ++++++++--- 2 files changed, 40 insertions(+), 27 deletions(-) diff --git a/tools/build-and-test.sh b/tools/build-and-test.sh index 119e97871..86246bc8c 100755 --- a/tools/build-and-test.sh +++ b/tools/build-and-test.sh @@ -22,7 +22,7 @@ SCRIPT_PATH=$(dirname "$(readlink -f "$0")") set -o errexit -OPEN_VDB="Off" +CMAKE_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then if [ "${ROS_DISTRO}" != "jazzy" ] && [ "${ROS_DISTRO}" != "rolling" ]; then @@ -30,7 +30,7 @@ if [ "${ROS_DISTRO}" != "noetic" ]; then else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools" if [ "${ROS_DISTRO}" != "humble" ]; then - OPEN_VDB="On" + CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" fi fi else @@ -40,31 +40,36 @@ fi source /opt/ros/${ROS_DISTRO}/setup.sh echo ::group::Release Build -colcon build \ - --event-handlers console_cohesion+ \ - --packages-up-to ${ROS_PACKAGES} \ - --symlink-install \ - --mixin \ - build-testing-on \ - ccache \ - release \ - --cmake-args -DUSE_OPENVDB=${OPEN_VDB} \ - --cmake-force-configure +COLCON_EXTRA_ARGS="--event-handlers console_cohesion+ \ + --packages-up-to ${ROS_PACKAGES} \ + --symlink-install \ + --mixin \ + build-testing-on \ + ccache \ + release \ + --cmake-force-configure" +if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then + COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" +fi +colcon build ${COLCON_EXTRA_ARGS} echo ::endgroup:: + echo ::group::Debug Build -colcon build \ - --packages-up-to ${ROS_PACKAGES} \ - --event-handlers console_cohesion+ \ - --symlink-install \ - --mixin \ - build-testing-on \ - ccache \ - coverage-gcc \ - coverage-pytest \ - debug \ - --cmake-args -DUSE_OPENVDB=${OPEN_VDB} \ - --cmake-force-configure +COLCON_EXTRA_ARGS="--packages-up-to ${ROS_PACKAGES} \ + --event-handlers console_cohesion+ \ + --symlink-install \ + --mixin \ + build-testing-on \ + ccache \ + coverage-gcc \ + coverage-pytest \ + debug \ + --cmake-force-configure" +if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then + COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" +fi +colcon build ${COLCON_EXTRA_ARGS} echo ::endgroup:: LCOV_CONFIG_PATH=${SCRIPT_PATH}/../.lcovrc diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index ff4bed4b9..6c13ee1bd 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -20,19 +20,27 @@ set -o errexit -o xtrace -OPEN_VDB="Off" +CMAKE_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" if [ "${ROS_DISTRO}" != "humble" ]; then - OPEN_VDB="On" + CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" fi else ROS_PACKAGES="beluga beluga_ros beluga_amcl" fi source /opt/ros/${ROS_DISTRO}/setup.sh -colcon build --packages-up-to ${ROS_PACKAGES} --event-handlers=console_cohesion+ --symlink-install --mixin ccache --cmake-args -DUSE_OPENVDB=${OPEN_VDB} + +COLCON_EXTRA_ARGS="--packages-up-to ${ROS_PACKAGES} \ + --event-handlers=console_cohesion+ \ + --symlink-install \ + --mixin ccache" +if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then + COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" +fi +colcon build ${COLCON_EXTRA_ARGS} echo ${ROS_PACKAGES} | xargs -n1 echo | # NOTE: `-Wno-gnu-zero-variadic-macro-arguments` is needed due to From 54917a595fb44c1402e86e199be7c356ccfdc944 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 18 Nov 2024 16:08:11 +0800 Subject: [PATCH 56/77] Fix nit Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor/likelihood_field_model3.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_field_model3.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp index 9cbab2388..19babad12 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model3.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model3.hpp @@ -155,9 +155,9 @@ class LikelihoodFieldModel3 { private: param_type params_; typename openvdb::tools::ClosestSurfacePoint::Ptr looker_; - const double two_squared_sigma_; - const double amplitude_; - const double offset_; + double two_squared_sigma_; + double amplitude_; + double offset_; }; } // namespace beluga From 6759f96a4974c88ce94534dc20e7e9d845104233 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Mon, 18 Nov 2024 16:13:09 +0800 Subject: [PATCH 57/77] Change minor tweaks Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 31 ++++++++++++------------------- 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 17372d9f6..90df84117 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -59,28 +59,21 @@ find_package(TBB REQUIRED) # If USE_OPENVDB is enabled, proceed with finding OpenVDB if(USE_OPENVDB) if(NOT OPENVDB_CMAKE_MODULE_PATH) - find_file( - OPENVDB_MODULE_PATH FindOpenVDB.cmake - PATHS /usr/lib/x86_64-linux-gnu/cmake/OpenVDB/ - /usr/lib/aarch64-linux-gnu/cmake/OpenVDB/ - NO_DEFAULT_PATH) - else() find_file(OPENVDB_MODULE_PATH FindOpenVDB.cmake - PATHS ${OPENVDB_CMAKE_MODULE_PATH}) + PATHS /usr/lib/x86_64-linux-gnu/cmake/OpenVDB/) + if(OPENVDB_MODULE_PATH) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE_PATH} + DIRECTORY) + endif() endif() - - if(OPENVDB_MODULE_PATH) - get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE_PATH} - DIRECTORY) - message(STATUS "Found OpenVDB CMake module at ${OPENVDB_CMAKE_MODULE_PATH}") + if(OPENVDB_CMAKE_MODULE_PATH) list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) - find_package(Boost REQUIRED) - find_package(OpenVDB REQUIRED) - else() - message(FATAL_ERROR "FindOpenVDB.cmake not found in the speficied paths.") endif() -else() - message(STATUS "USE_OPENVDB is Off, skipping OpenVDB setup.") + find_package(Boost REQUIRED) + find_package(OpenVDB REQUIRED) + if(OPENVDB_CMAKE_MODULE_PATH) + list(POP_BACK CMAKE_MODULE_PATH) + endif() endif() add_library(${PROJECT_NAME} INTERFACE) @@ -96,7 +89,7 @@ target_link_libraries( Sophus::Sophus TBB::tbb range-v3::range-v3 - $,OpenVDB::openvdb,>) + $<$:OpenVDB::openvdb>) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) target_compile_definitions( ${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING From d4dc09eb56aa63ed9863550f2ca0bf65a2be98d1 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Wed, 20 Nov 2024 15:32:58 +0800 Subject: [PATCH 58/77] Change from closestsurface to accessors Signed-off-by: Pablo Vela --- .../beluga/sensor/likelihood_field_model3.hpp | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_field_model3.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp index 19babad12..6d7060116 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model3.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model3.hpp @@ -21,7 +21,6 @@ #include #include -#include #include @@ -104,7 +103,10 @@ class LikelihoodFieldModel3 { */ explicit LikelihoodFieldModel3(const param_type& params, const map_type& grid) : params_{params}, - looker_{openvdb::tools::ClosestSurfacePoint::create(grid)}, + grid_{openvdb::gridPtrCast(grid.deepCopyGrid())}, + accessor_{grid_->getConstAccessor()}, + transform_{grid_->transform()}, + background_{grid_->background()}, two_squared_sigma_{2 * params.sigma_hit * params.sigma_hit}, amplitude_{params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi()))}, offset_{params.z_random / params.max_laser_distance} { @@ -130,23 +132,21 @@ class LikelihoodFieldModel3 { ranges::to(); return [this, pointcloud_size, points = std::move(transformed_points)](const state_type& state) -> weight_type { - std::vector nb_distances; - std::vector vdb_points; - vdb_points.reserve(pointcloud_size); + std::vector distances; + distances.reserve(pointcloud_size); // Transform each point to every particle state - std::transform(points.begin(), points.end(), std::back_inserter(vdb_points), [state](const auto& point) { + std::transform(points.begin(), points.end(), std::back_inserter(distances), [this, state](const auto& point) { // OpenVDB grid already in world coordinates const Eigen::Vector3d point_in_state_frame = state * point; - return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()}; + const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered( + openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z())); + return accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; }); - // Extract the distance to the closest surface for each point - looker_->search(vdb_points, nb_distances); - // Calculates the probality based on the distance return std::transform_reduce( - nb_distances.cbegin(), nb_distances.cend(), 1.0, std::plus{}, [this](const auto& distance) { + distances.cbegin(), distances.cend(), 1.0, std::plus{}, [this](const auto& distance) { return amplitude_ * std::exp(-(distance * distance) / two_squared_sigma_) + offset_; }); }; @@ -154,7 +154,10 @@ class LikelihoodFieldModel3 { private: param_type params_; - typename openvdb::tools::ClosestSurfacePoint::Ptr looker_; + const typename map_type::Ptr grid_; + const typename map_type::ConstAccessor accessor_; + const openvdb::math::Transform transform_; + const typename map_type::ValueType background_; double two_squared_sigma_; double amplitude_; double offset_; From a61feed6a6f6886dca81cd62efcd3ee2e2a8f7e8 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 22 Nov 2024 13:47:23 +0800 Subject: [PATCH 59/77] Fix openvdb dependencies when building Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 19 +++++++++++++++---- beluga/cmake/Config.cmake.in | 8 +++++--- beluga/package.xml | 2 ++ 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 90df84117..1f39528a8 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -59,12 +59,23 @@ find_package(TBB REQUIRED) # If USE_OPENVDB is enabled, proceed with finding OpenVDB if(USE_OPENVDB) if(NOT OPENVDB_CMAKE_MODULE_PATH) - find_file(OPENVDB_MODULE_PATH FindOpenVDB.cmake - PATHS /usr/lib/x86_64-linux-gnu/cmake/OpenVDB/) - if(OPENVDB_MODULE_PATH) - get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE_PATH} + file( + GLOB_RECURSE + OPENVDB_MODULES + /usr/lib/*/FindOpenVDB.cmake + /usr/local/lib/*/FindOpenVDB.cmake) + list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES) + if(NUM_OPENVDB_MODULES EQUAL 1) + list( + GET + OPENVDB_MODULES + 0 + OPENVDB_MODULE) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} DIRECTORY) endif() + unset(NUM_OPENVDB_MODULES) + unset(OPENVDB_MODULES) endif() if(OPENVDB_CMAKE_MODULE_PATH) list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index f478bca8d..d7f5e9acd 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -1,14 +1,16 @@ @PACKAGE_INIT@ -list(APPEND CMAKE_MODULE_PATH "@OPENVDB_CMAKE_MODULE_PATH@") - include(CMakeFindDependencyMacro) find_dependency(Eigen3 REQUIRED NO_MODULE) find_dependency(range-v3 REQUIRED) find_dependency(HDF5 COMPONENTS CXX REQUIRED) find_dependency(Sophus REQUIRED) find_dependency(TBB REQUIRED) -find_dependency(OpenVDB QUIET) + +if("@USE_OPENVDB@") + list(APPEND CMAKE_MODULE_PATH "@OPENVDB_CMAKE_MODULE_PATH@") + find_dependency(OpenVDB REQUIRED) +endif() include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") diff --git a/beluga/package.xml b/beluga/package.xml index dc349ed27..f06af801c 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -16,6 +16,8 @@ eigen libhdf5-dev + libopenvdb-dev + libopenvdb range-v3 sophus tbb From 8a354f3b6ddda04dcabda30151aae24173667719 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 22 Nov 2024 13:48:19 +0800 Subject: [PATCH 60/77] Change nit Signed-off-by: Pablo Vela --- tools/build-and-test.sh | 52 +++++++++++++++++++++-------------------- tools/run-clang-tidy.sh | 7 ++---- 2 files changed, 29 insertions(+), 30 deletions(-) diff --git a/tools/build-and-test.sh b/tools/build-and-test.sh index 86246bc8c..0e18d93b0 100755 --- a/tools/build-and-test.sh +++ b/tools/build-and-test.sh @@ -23,6 +23,7 @@ SCRIPT_PATH=$(dirname "$(readlink -f "$0")") set -o errexit CMAKE_EXTRA_ARGS="" +COLCON_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then if [ "${ROS_DISTRO}" != "jazzy" ] && [ "${ROS_DISTRO}" != "rolling" ]; then @@ -37,39 +38,40 @@ else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_example" fi -source /opt/ros/${ROS_DISTRO}/setup.sh -echo ::group::Release Build -COLCON_EXTRA_ARGS="--event-handlers console_cohesion+ \ - --packages-up-to ${ROS_PACKAGES} \ - --symlink-install \ - --mixin \ - build-testing-on \ - ccache \ - release \ - --cmake-force-configure" if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" fi -colcon build ${COLCON_EXTRA_ARGS} + +source /opt/ros/${ROS_DISTRO}/setup.sh + +echo ::group::Release Build +colcon build \ + --event-handlers console_cohesion+ \ + --packages-up-to ${ROS_PACKAGES} \ + --symlink-install \ + --mixin \ + build-testing-on \ + ccache \ + release \ + --cmake-force-configure \ + ${COLCON_EXTRA_ARGS} echo ::endgroup:: echo ::group::Debug Build -COLCON_EXTRA_ARGS="--packages-up-to ${ROS_PACKAGES} \ - --event-handlers console_cohesion+ \ - --symlink-install \ - --mixin \ - build-testing-on \ - ccache \ - coverage-gcc \ - coverage-pytest \ - debug \ - --cmake-force-configure" -if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then - COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" -fi -colcon build ${COLCON_EXTRA_ARGS} +colcon build \ + --packages-up-to ${ROS_PACKAGES} \ + --event-handlers console_cohesion+ \ + --symlink-install \ + --mixin \ + build-testing-on \ + ccache \ + coverage-gcc \ + coverage-pytest \ + debug \ + --cmake-force-configure \ + ${COLCON_EXTRA_ARGS} echo ::endgroup:: LCOV_CONFIG_PATH=${SCRIPT_PATH}/../.lcovrc diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index 6c13ee1bd..cf9a69b25 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -21,6 +21,7 @@ set -o errexit -o xtrace CMAKE_EXTRA_ARGS="" +COLCON_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" @@ -33,14 +34,10 @@ fi source /opt/ros/${ROS_DISTRO}/setup.sh -COLCON_EXTRA_ARGS="--packages-up-to ${ROS_PACKAGES} \ - --event-handlers=console_cohesion+ \ - --symlink-install \ - --mixin ccache" if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" fi -colcon build ${COLCON_EXTRA_ARGS} +colcon build --packages-up-to ${ROS_PACKAGES} --event-handlers=console_cohesion+ --symlink-install --mixin ccache ${COLCON_EXTRA_ARGS} echo ${ROS_PACKAGES} | xargs -n1 echo | # NOTE: `-Wno-gnu-zero-variadic-macro-arguments` is needed due to From 9a4c4cbbbd1cab61622c6b0e6d2fe9e420d6a5df Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Fri, 22 Nov 2024 17:47:00 +0800 Subject: [PATCH 61/77] Fix libopenvdb not supported on noble Signed-off-by: Pablo Vela --- beluga/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/beluga/package.xml b/beluga/package.xml index f06af801c..aa992f22e 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -17,7 +17,6 @@ eigen libhdf5-dev libopenvdb-dev - libopenvdb range-v3 sophus tbb From 5f879c4bca7bce399b4768c33e65468e70ae0573 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 23 Nov 2024 10:34:07 +0800 Subject: [PATCH 62/77] Fix boost dependency Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 1 - beluga/package.xml | 2 ++ docker/images/iron/Dockerfile | 2 -- docker/images/jazzy/Dockerfile | 1 - docker/images/rolling/Dockerfile | 1 - tools/build-and-test.sh | 2 +- tools/run-clang-tidy.sh | 2 +- 7 files changed, 4 insertions(+), 7 deletions(-) diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 1f39528a8..6687622c8 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -80,7 +80,6 @@ if(USE_OPENVDB) if(OPENVDB_CMAKE_MODULE_PATH) list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) endif() - find_package(Boost REQUIRED) find_package(OpenVDB REQUIRED) if(OPENVDB_CMAKE_MODULE_PATH) list(POP_BACK CMAKE_MODULE_PATH) diff --git a/beluga/package.xml b/beluga/package.xml index aa992f22e..2b701e775 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -15,6 +15,8 @@ cmake eigen + libboost-iostreams-dev + libboost-thread-dev libhdf5-dev libopenvdb-dev range-v3 diff --git a/docker/images/iron/Dockerfile b/docker/images/iron/Dockerfile index 0df85bbe9..28fa8268f 100644 --- a/docker/images/iron/Dockerfile +++ b/docker/images/iron/Dockerfile @@ -44,8 +44,6 @@ RUN apt-get update \ gdb \ git \ lcov \ - libboost-all-dev \ - libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ python3-colcon-coveragepy-result \ diff --git a/docker/images/jazzy/Dockerfile b/docker/images/jazzy/Dockerfile index 89172c499..8e3d3b240 100644 --- a/docker/images/jazzy/Dockerfile +++ b/docker/images/jazzy/Dockerfile @@ -44,7 +44,6 @@ RUN apt-get update \ gdb \ git \ lcov \ - libboost-all-dev \ libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ diff --git a/docker/images/rolling/Dockerfile b/docker/images/rolling/Dockerfile index b1f00b8e5..7aa40845c 100644 --- a/docker/images/rolling/Dockerfile +++ b/docker/images/rolling/Dockerfile @@ -44,7 +44,6 @@ RUN apt-get update \ gdb \ git \ lcov \ - libboost-all-dev \ libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ diff --git a/tools/build-and-test.sh b/tools/build-and-test.sh index 0e18d93b0..c61b2c9ae 100755 --- a/tools/build-and-test.sh +++ b/tools/build-and-test.sh @@ -30,7 +30,7 @@ if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_benchmark beluga_example beluga_system_tests beluga_tools" else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools" - if [ "${ROS_DISTRO}" != "humble" ]; then + if [ "${ROS_DISTRO}" != "humble" && "${ROS_DISTRO}" != "iron" ]; then CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" fi fi diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index cf9a69b25..c3806e466 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -25,7 +25,7 @@ COLCON_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" - if [ "${ROS_DISTRO}" != "humble" ]; then + if [ "${ROS_DISTRO}" != "humble" && "${ROS_DISTRO}" != "iron" ]; then CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" fi else From 0c0bb8f121e6c29d94662a19156aa2a6dd501a97 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 28 Nov 2024 11:30:08 +0800 Subject: [PATCH 63/77] Fix useopenvdb flag option Signed-off-by: Pablo Vela --- tools/build-and-test.sh | 2 +- tools/run-clang-tidy.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/build-and-test.sh b/tools/build-and-test.sh index c61b2c9ae..6003f62ec 100755 --- a/tools/build-and-test.sh +++ b/tools/build-and-test.sh @@ -30,7 +30,7 @@ if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_benchmark beluga_example beluga_system_tests beluga_tools" else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools" - if [ "${ROS_DISTRO}" != "humble" && "${ROS_DISTRO}" != "iron" ]; then + if [ "${ROS_DISTRO}" != "humble" ] && [ "${ROS_DISTRO}" != "iron" ]; then CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" fi fi diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index c3806e466..24d78549a 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -25,7 +25,7 @@ COLCON_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" - if [ "${ROS_DISTRO}" != "humble" && "${ROS_DISTRO}" != "iron" ]; then + if [ "${ROS_DISTRO}" != "humble" ] && [ "${ROS_DISTRO}" != "iron" ]; then CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" fi else From 20050ab9b97a865a997e5a0878a7e4100fba1fcb Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 28 Nov 2024 11:32:32 +0800 Subject: [PATCH 64/77] Increase narrow band lenght of tests Signed-off-by: Pablo Vela --- beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp index 32285de95..622c0f59e 100644 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp @@ -34,7 +34,7 @@ namespace { template auto make_map(const double voxel_size, const std::vector& world_points) { // Parameters - constexpr int kHalfWidth = 1; + constexpr int kHalfWidth = 3; constexpr int kClosingSteps = 0; // Create the grid From 2aabb60e1d02fcf2464ebca96f3cdb47be230d3a Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 28 Nov 2024 11:33:09 +0800 Subject: [PATCH 65/77] Fix nit Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor.hpp | 2 +- .../include/beluga/sensor/likelihood_field_model3.hpp | 11 ++++------- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 39dfeabf5..8fdcb26b7 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -54,7 +54,7 @@ * * \section SensorModelLinks See also * - beluga::LikelihoodFieldModel - * - beluga::Likelihood3DFieldModel + * - beluga::LikelihoodFieldModel3 * - beluga::BeamSensorModel * - beluga::LandmarkSensorModel * - beluga::BearingSensorModel diff --git a/beluga/include/beluga/sensor/likelihood_field_model3.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp index 6d7060116..c0aecf0f5 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model3.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model3.hpp @@ -92,10 +92,10 @@ class LikelihoodFieldModel3 { /// Parameter type that the constructor uses to configure the likelihood field model. using param_type = LikelihoodFieldModel3Param; - /// Constructs a Likelihood3DFieldModel instance. + /// Constructs a LikelihoodFieldModel3 instance. /** * \param params Parameters to configure this instance. - * See beluga::Likelihood3DFieldModelParam for details. + * See beluga::LikelihoodFieldModel3Param for details. * \param grid Narrow band Level set grid representing the static map that the sensor model * uses to compute a likelihood field for lidar hits and compute importance weights * for particle states. @@ -123,17 +123,14 @@ class LikelihoodFieldModel3 { * and borrowing a reference to this sensor model (and thus their lifetime are bound). */ [[nodiscard]] auto operator()(measurement_type&& measurement) const { - const size_t pointcloud_size = measurement.points().size(); // Transform each point from the sensor frame to the origin frame - auto transformed_points = ranges::views::all(measurement.points()) | - ranges::views::transform([&](const auto& point) { + auto transformed_points = measurement.points() | ranges::views::transform([&](const auto& point) { return measurement.origin() * point.template cast(); }) | ranges::to(); - return [this, pointcloud_size, points = std::move(transformed_points)](const state_type& state) -> weight_type { + return [this, points = std::move(transformed_points)](const state_type& state) -> weight_type { std::vector distances; - distances.reserve(pointcloud_size); // Transform each point to every particle state std::transform(points.begin(), points.end(), std::back_inserter(distances), [this, state](const auto& point) { From 7f2b27ffb36380c56b781a69ee03abcb36d6e018 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 28 Nov 2024 12:37:44 +0800 Subject: [PATCH 66/77] Remove openvdb from image Signed-off-by: Pablo Vela --- docker/images/jazzy/Dockerfile | 1 - docker/images/rolling/Dockerfile | 1 - 2 files changed, 2 deletions(-) diff --git a/docker/images/jazzy/Dockerfile b/docker/images/jazzy/Dockerfile index 8e3d3b240..b0221a1bc 100644 --- a/docker/images/jazzy/Dockerfile +++ b/docker/images/jazzy/Dockerfile @@ -44,7 +44,6 @@ RUN apt-get update \ gdb \ git \ lcov \ - libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ python3-colcon-coveragepy-result \ diff --git a/docker/images/rolling/Dockerfile b/docker/images/rolling/Dockerfile index 7aa40845c..4c53af1ba 100644 --- a/docker/images/rolling/Dockerfile +++ b/docker/images/rolling/Dockerfile @@ -44,7 +44,6 @@ RUN apt-get update \ gdb \ git \ lcov \ - libopenvdb-dev \ linux-tools-common \ linux-tools-generic \ python3-colcon-coveragepy-result \ From c8d773e1e8d569bda502ff3a180149be095ca8fe Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 13:30:13 +0800 Subject: [PATCH 67/77] Change to not dynamically allocate memory Signed-off-by: Pablo Vela --- .../beluga/sensor/likelihood_field_model3.hpp | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/beluga/include/beluga/sensor/likelihood_field_model3.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp index c0aecf0f5..cc0fec13d 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model3.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model3.hpp @@ -24,6 +24,9 @@ #include +#include +#include +#include #include #include #include @@ -130,22 +133,16 @@ class LikelihoodFieldModel3 { ranges::to(); return [this, points = std::move(transformed_points)](const state_type& state) -> weight_type { - std::vector distances; - - // Transform each point to every particle state - std::transform(points.begin(), points.end(), std::back_inserter(distances), [this, state](const auto& point) { - // OpenVDB grid already in world coordinates - const Eigen::Vector3d point_in_state_frame = state * point; - const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered( - openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z())); - return accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; - }); - - // Calculates the probality based on the distance - return std::transform_reduce( - distances.cbegin(), distances.cend(), 1.0, std::plus{}, [this](const auto& distance) { - return amplitude_ * std::exp(-(distance * distance) / two_squared_sigma_) + offset_; - }); + return ranges::fold_left( + points | // + ranges::views::transform([this, &state](const auto& point) { + const Eigen::Vector3d point_in_state_frame = state * point; + const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered( + openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z())); + const auto prob = accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; + return amplitude_ * std::exp(-(prob * prob) / two_squared_sigma_) + offset_; + }), + 1.0, std::plus{}); }; } From 88a54ae1f8edd21c667eba6dfd6766c289654dde Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 15:52:01 +0800 Subject: [PATCH 68/77] Move openvdb to beluga_vdb Signed-off-by: Pablo Vela --- beluga_vdb/CMakeLists.txt | 102 +++++++++++ .../sensor/likelihood_field_model3.hpp | 162 +++++++++++++++++ beluga_vdb/package.xml | 38 ++++ beluga_vdb/test/CMakeLists.txt | 31 ++++ beluga_vdb/test/beluga_vdb/CMakeLists.txt | 23 +++ .../test/simple_pointcloud_interface.hpp | 45 +++++ .../sensor/test_likelihood_3d_field_model.cpp | 168 ++++++++++++++++++ 7 files changed, 569 insertions(+) create mode 100644 beluga_vdb/CMakeLists.txt create mode 100644 beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp create mode 100644 beluga_vdb/package.xml create mode 100644 beluga_vdb/test/CMakeLists.txt create mode 100644 beluga_vdb/test/beluga_vdb/CMakeLists.txt create mode 100644 beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp create mode 100644 beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp diff --git a/beluga_vdb/CMakeLists.txt b/beluga_vdb/CMakeLists.txt new file mode 100644 index 000000000..a13e11f99 --- /dev/null +++ b/beluga_vdb/CMakeLists.txt @@ -0,0 +1,102 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.16) + +project(beluga_vdb VERSION 1.0) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +set(OPENVDB_CMAKE_MODULE_PATH + ${OPENVDB_CMAKE_MODULE_PATH} + CACHE PATH "Path to OpenVDB CMake module") + +if(NOT CMAKE_BUILD_TYPE) + message(STATUS "Setting build type to 'Release' as none was specified.") + set(CMAKE_BUILD_TYPE + "Release" + CACHE STRING "Build type" FORCE) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options( + -Wall + -Wconversion + -Wextra + -Werror + -Wpedantic) +endif() +if(CMAKE_BUILD_TYPE MATCHES "Debug") + target_compile_options(-fno-inline) +endif() + +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(range-v3 REQUIRED) +find_package(Sophus REQUIRED) +find_package(TBB REQUIRED) +find_package(beluga REQUIRED) + +if(NOT OPENVDB_CMAKE_MODULE_PATH) + file( + GLOB_RECURSE + OPENVDB_MODULES + /usr/lib/*/FindOpenVDB.cmake + /usr/local/lib/*/FindOpenVDB.cmake) + list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES) + if(NUM_OPENVDB_MODULES EQUAL 1) + list( + GET + OPENVDB_MODULES + 0 + OPENVDB_MODULE) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} + DIRECTORY) + endif() + unset(NUM_OPENVDB_MODULES) + unset(OPENVDB_MODULES) +endif() + +if(OPENVDB_CMAKE_MODULE_PATH) + list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) +endif() + +find_package(OpenVDB REQUIRED) + +if(OPENVDB_CMAKE_MODULE_PATH) + list(POP_BACK CMAKE_MODULE_PATH) +endif() + +add_library(${PROJECT_NAME} INTERFACE) +target_include_directories( + ${PROJECT_NAME} + INTERFACE $ + $) +target_link_libraries(${PROJECT_NAME} INTERFACE beluga::beluga OpenVDB::openvdb) +target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +option(BUILD_TESTING "Build the testing tree." ON) +if(BUILD_TESTING) + message(STATUS "Build testing enabled.") + enable_testing() + add_subdirectory(test) +endif() diff --git a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp new file mode 100644 index 000000000..cc0fec13d --- /dev/null +++ b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp @@ -0,0 +1,162 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP +#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * \file + * \brief Implementation of a likelihood field sensor model for 3D Lidars. + */ + +namespace beluga { + +/// Parameters used to construct a LikelihoodFieldModel3 instance. +/** + * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. + */ +struct LikelihoodFieldModel3Param { + /// Maximum distance to obstacle. + /** + * When creating a distance map, if the distance to an obstacle is higher than the value specified here, + * then this value will be used. + */ + double max_obstacle_distance = 100.0; + /// Maximum range of a laser ray. + double max_laser_distance = 2.0; + /// Weight used to combine the probability of hitting an obstacle. + double z_hit = 0.5; + /// Weight used to combine the probability of random noise in perception. + double z_random = 0.5; + /// Standard deviation of a gaussian centered arounds obstacles. + /** + * Used to calculate the probability of the obstacle being hit. + */ + double sigma_hit = 0.2; +}; + +/// Likelihood field sensor model for range finders. +/** + * This model relies on a pre-computed likelihood map of the environment. + * It is less computationally intensive than the beluga::BeamSensorModel + * because no ray-tracing is required, and it can also provide better + * performance in environments with non-smooth occupation maps. See + * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4, + * for further reference. + * + * \note This class satisfies \ref SensorModelPage. + * + * \tparam OpenVDB grid type. + */ +template +class LikelihoodFieldModel3 { + public: + /// State type of a particle. + using state_type = Sophus::SE3d; + + /// Weight type of the particle. + using weight_type = double; + + /// Measurement type given by the interface. + using measurement_type = PointCloud; + + /// Map representation type. + using map_type = GridT; + + /// Parameter type that the constructor uses to configure the likelihood field model. + using param_type = LikelihoodFieldModel3Param; + + /// Constructs a LikelihoodFieldModel3 instance. + /** + * \param params Parameters to configure this instance. + * See beluga::LikelihoodFieldModel3Param for details. + * \param grid Narrow band Level set grid representing the static map that the sensor model + * uses to compute a likelihood field for lidar hits and compute importance weights + * for particle states. + * Currently only supports OpenVDB Level sets. + */ + explicit LikelihoodFieldModel3(const param_type& params, const map_type& grid) + : params_{params}, + grid_{openvdb::gridPtrCast(grid.deepCopyGrid())}, + accessor_{grid_->getConstAccessor()}, + transform_{grid_->transform()}, + background_{grid_->background()}, + two_squared_sigma_{2 * params.sigma_hit * params.sigma_hit}, + amplitude_{params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi()))}, + offset_{params.z_random / params.max_laser_distance} { + openvdb::initialize(); + /// Pre-computed parameters + assert(two_squared_sigma_ > 0.0); + assert(amplitude_ > 0.0); + } + + /// Returns a state weighting function conditioned on 3D lidar hits. + /** + * \param measurement 3D lidar measurement containing the hit points and the transform to the origin. + * \return a state weighting function satisfying \ref StateWeightingFunctionPage + * and borrowing a reference to this sensor model (and thus their lifetime are bound). + */ + [[nodiscard]] auto operator()(measurement_type&& measurement) const { + // Transform each point from the sensor frame to the origin frame + auto transformed_points = measurement.points() | ranges::views::transform([&](const auto& point) { + return measurement.origin() * point.template cast(); + }) | + ranges::to(); + + return [this, points = std::move(transformed_points)](const state_type& state) -> weight_type { + return ranges::fold_left( + points | // + ranges::views::transform([this, &state](const auto& point) { + const Eigen::Vector3d point_in_state_frame = state * point; + const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered( + openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z())); + const auto prob = accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; + return amplitude_ * std::exp(-(prob * prob) / two_squared_sigma_) + offset_; + }), + 1.0, std::plus{}); + }; + } + + private: + param_type params_; + const typename map_type::Ptr grid_; + const typename map_type::ConstAccessor accessor_; + const openvdb::math::Transform transform_; + const typename map_type::ValueType background_; + double two_squared_sigma_; + double amplitude_; + double offset_; +}; + +} // namespace beluga + +#endif diff --git a/beluga_vdb/package.xml b/beluga_vdb/package.xml new file mode 100644 index 000000000..55ef9861f --- /dev/null +++ b/beluga_vdb/package.xml @@ -0,0 +1,38 @@ + + + + beluga_vdb + 1.0.0 + + A generic MCL library for ROS2. + + Gerardo Puga + Ivan Paunovic + Nahuel Espinosa + + Apache License 2.0 + + cmake + + beluga + eigen + libboost-iostreams-dev + libboost-thread-dev + libopenvdb-dev + range-v3 + sophus + tbb + + clang-format + clang-tidy + gtest + libgmock-dev + + doxygen + graphviz + texlive-latex-base + + + cmake + + diff --git a/beluga_vdb/test/CMakeLists.txt b/beluga_vdb/test/CMakeLists.txt new file mode 100644 index 000000000..7f972fd5f --- /dev/null +++ b/beluga_vdb/test/CMakeLists.txt @@ -0,0 +1,31 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +find_package(GTest MODULE) +if(NOT TARGET GTest::gmock_main) + include(FetchContent) + FetchContent_Declare( + googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG 03597a01ee50ed33e9dfd640b249b4be3799d395) + # For Windows: Prevent overriding the parent project's compiler/linker + # settings + set(GTEST_FORCE_SHARED_CRT + ON + CACHE BOOL "" FORCE) + FetchContent_MakeAvailable(googletest) +endif() + +include(CTest) +add_subdirectory(beluga_vdb) diff --git a/beluga_vdb/test/beluga_vdb/CMakeLists.txt b/beluga_vdb/test/beluga_vdb/CMakeLists.txt new file mode 100644 index 000000000..a7d8dbed4 --- /dev/null +++ b/beluga_vdb/test/beluga_vdb/CMakeLists.txt @@ -0,0 +1,23 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +add_executable(test_beluga_vdb sensor/test_likelihood_3d_field_model.cpp) + +target_link_libraries(test_beluga_vdb PRIVATE ${PROJECT_NAME} beluga_vdb + GTest::gmock_main) +target_include_directories(test_beluga_vdb PRIVATE include) +target_compile_options(test_beluga_vdb PRIVATE -Wno-sign-compare) + +include(GoogleTest) +gtest_discover_tests(test_beluga_vdb) diff --git a/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp b/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp new file mode 100644 index 000000000..7485e282a --- /dev/null +++ b/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP +#define BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP + +#include +#include + +namespace beluga::testing { + +template +class SimpleSparsePointCloud3 { + public: + explicit SimpleSparsePointCloud3(std::vector> points, Sophus::SE3d origin = Sophus::SE3d{}) + : points_(std::move(points)), origin_(std::move(origin)) {} + + /// Get the point cloud frame origin in the filter frame. + [[nodiscard]] const auto& origin() const { return origin_; } + + /// Get the 3D points collection. + [[nodiscard]] const auto& points() const { return points_; } + + private: + std::vector> points_; + Sophus::SE3d origin_; +}; + +using SimpleSparsePointCloud3d = SimpleSparsePointCloud3; +using SimpleSparsePointCloud3f = SimpleSparsePointCloud3; + +} // namespace beluga::testing + +#endif diff --git a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp new file mode 100644 index 000000000..a03c77a23 --- /dev/null +++ b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp @@ -0,0 +1,168 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "beluga_vdb/sensor/likelihood_field_model3.hpp" +#include "beluga_vdb/test/simple_pointcloud_interface.hpp" + +namespace { + +template +auto make_map(const double voxel_size, const std::vector& world_points) { + // Parameters + constexpr int kHalfWidth = 3; + constexpr int kClosingSteps = 0; + + // Create the grid + const typename GridT::Ptr grid = openvdb::FloatGrid::create(); + grid->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size)); + + // Get an accessor for coordinate-based access to voxels + typename GridT::Accessor accessor = grid->getAccessor(); + + // Fill the grid + const openvdb::math::Transform& transform = grid->transform(); + for (const auto& point : world_points) { + // Transform to index world + const openvdb::math::Coord ijk = transform.worldToIndexCellCentered(point); + // Set the voxel to 1 + accessor.setValue(ijk, 1.0); + // Activate the voxel + accessor.setActiveState(ijk); + } + + // Check grid + openvdb::tools::CheckLevelSet checker(*grid); + // Check inactive values have a magnitude equal to the background value + assert(checker.checkInactiveValues() == ""); + + // Transform to levelset + return openvdb::tools::topologyToLevelSet(*grid, kHalfWidth, kClosingSteps); +} + +TEST(TestLikelihoodFieldModel3, Point) { + openvdb::initialize(); + // Parameters + constexpr double kVoxelSize = 0.07; + constexpr double kMaxObstacleDistance = 2.0; + constexpr double kMaxLaserDistance = 100.0; + constexpr double kZHit = 0.5; + constexpr double kZRandom = 0.001; + constexpr double kSigmaHit = 0.22; + + // Create Grid + const std::vector world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)}; + auto map = make_map(kVoxelSize, world_points); + + const auto params = + beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; + auto sensor_model = + beluga::LikelihoodFieldModel3{params, *map}; + + // Exact point + auto pointcloud_measurement_exact = + beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.0F, 1.0F, 1.0F}}}; + auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); + ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 1.90); + + // Close point (Inside the narrow band) + auto pointcloud_measurement_close = + beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.035F, 1.0F, 1.0F}}}; + auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); + ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 1.8); + + // Far point (Outside the narrow band) + auto pointcloud_measurement_far = + beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.1F, 1.1F, 1.1F}}}; + auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); + ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 1.65); +} + +TEST(TestLikelihoodFieldModel3, Cube) { + openvdb::initialize(); + // Parameters + constexpr double kVoxelSize = 0.07; + constexpr double kMaxObstacleDistance = 2.0; + constexpr double kMaxLaserDistance = 100.0; + constexpr double kZHit = 0.5; + constexpr double kZRandom = 0.001; + constexpr double kSigmaHit = 0.22; + + // Create Grid + const std::vector world_points{ + openvdb::math::Vec3s(1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(1.0F, 1.0F, -1.0F), + openvdb::math::Vec3s(1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(1.0F, -1.0F, -1.0F), + openvdb::math::Vec3s(-1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, -1.0F, -1.0F), + openvdb::math::Vec3s(-1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, 1.0F, -1.0F)}; + auto map = make_map(kVoxelSize, world_points); + + const auto params = + beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; + auto sensor_model = + beluga::LikelihoodFieldModel3{params, *map}; + + // Exact point + auto pointcloud_measurement_exact = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.0F, 1.0F, 1.0F}, + {1.0F, 1.0F, -1.0F}, + {1.0F, -1.0F, 1.0F}, + {1.0F, -1.0F, -1.0F}, + {-1.0F, -1.0F, 1.0F}, + {-1.0F, -1.0F, -1.0F}, + {-1.0F, 1.0F, 1.0F}, + {-1.0F, 1.0F, -1.0F}}}; + auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); + ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 8.2); + + // Close point (Inside the narrow band) + auto pointcloud_measurement_close = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.035F, 1.0F, 1.0F}, + {1.035F, 1.0F, -1.0F}, + {1.035F, -1.0F, 1.0F}, + {1.035F, -1.0F, -1.0F}, + {-1.0F, -1.0F, 1.0F}, + {-1.0F, -1.0F, -1.0F}, + {-1.0F, 1.0F, 1.0F}, + {-1.0F, 1.0F, -1.0F}}}; + auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); + ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 7.4); + + // Far point (Outside the narrow band) + auto pointcloud_measurement_far = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.1F, 1.1F, 1.1F}, + {1.1F, 1.1F, -1.1F}, + {1.1F, -1.1F, 1.1F}, + {1.1F, -1.1F, -1.1F}, + {-1.1F, -1.1F, 1.1F}, + {-1.1F, -1.1F, -1.1F}, + {-1.1F, 1.1F, 1.1F}, + {-1.1F, 1.1F, -1.1F}}}; + auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); + ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 6.85); +} + +} // namespace From ac98531643ff0587ec528fa997b89c666dc79a98 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 16:01:50 +0800 Subject: [PATCH 69/77] Remove openvdb to beluga Signed-off-by: Pablo Vela --- beluga/CMakeLists.txt | 46 +---- beluga/cmake/Config.cmake.in | 5 - beluga/include/beluga/sensor.hpp | 4 - .../beluga/sensor/likelihood_field_model3.hpp | 162 ----------------- beluga/package.xml | 3 - beluga/test/beluga/CMakeLists.txt | 1 - .../test/simple_pointcloud_interface.hpp | 45 ----- .../sensor/test_likelihood_3d_field_model.cpp | 168 ------------------ 8 files changed, 3 insertions(+), 431 deletions(-) delete mode 100644 beluga/include/beluga/sensor/likelihood_field_model3.hpp delete mode 100644 beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp delete mode 100644 beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index 6687622c8..1d6b96d5d 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -18,14 +18,6 @@ project(beluga VERSION 1.0) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -option( - USE_OPENVDB - "Compiles with OpenVDB, required for the 3D likelihood field sensor model" - Off) -set(OPENVDB_CMAKE_MODULE_PATH - ${OPENVDB_CMAKE_MODULE_PATH} - CACHE PATH "Path to OpenVDB CMake module") - if(NOT CMAKE_BUILD_TYPE) message(STATUS "Setting build type to 'Release' as none was specified.") set(CMAKE_BUILD_TYPE @@ -56,36 +48,6 @@ find_package(range-v3 REQUIRED) find_package(Sophus REQUIRED) find_package(TBB REQUIRED) -# If USE_OPENVDB is enabled, proceed with finding OpenVDB -if(USE_OPENVDB) - if(NOT OPENVDB_CMAKE_MODULE_PATH) - file( - GLOB_RECURSE - OPENVDB_MODULES - /usr/lib/*/FindOpenVDB.cmake - /usr/local/lib/*/FindOpenVDB.cmake) - list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES) - if(NUM_OPENVDB_MODULES EQUAL 1) - list( - GET - OPENVDB_MODULES - 0 - OPENVDB_MODULE) - get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} - DIRECTORY) - endif() - unset(NUM_OPENVDB_MODULES) - unset(OPENVDB_MODULES) - endif() - if(OPENVDB_CMAKE_MODULE_PATH) - list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) - endif() - find_package(OpenVDB REQUIRED) - if(OPENVDB_CMAKE_MODULE_PATH) - list(POP_BACK CMAKE_MODULE_PATH) - endif() -endif() - add_library(${PROJECT_NAME} INTERFACE) target_include_directories( ${PROJECT_NAME} @@ -98,12 +60,10 @@ target_link_libraries( ${HDF5_CXX_LIBRARIES} Sophus::Sophus TBB::tbb - range-v3::range-v3 - $<$:OpenVDB::openvdb>) + range-v3::range-v3) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) -target_compile_definitions( - ${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING - $<$:BELUGA_OPENVDB_SUPPORT>) +target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG + SOPHUS_USE_BASIC_LOGGING) add_executable(clang_tidy_findable) target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp) diff --git a/beluga/cmake/Config.cmake.in b/beluga/cmake/Config.cmake.in index d7f5e9acd..8d7b0e44f 100644 --- a/beluga/cmake/Config.cmake.in +++ b/beluga/cmake/Config.cmake.in @@ -7,11 +7,6 @@ find_dependency(HDF5 COMPONENTS CXX REQUIRED) find_dependency(Sophus REQUIRED) find_dependency(TBB REQUIRED) -if("@USE_OPENVDB@") - list(APPEND CMAKE_MODULE_PATH "@OPENVDB_CMAKE_MODULE_PATH@") - find_dependency(OpenVDB REQUIRED) -endif() - include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") set(@PROJECT_NAME@_LIBRARIES @PROJECT_NAME@::@PROJECT_NAME@) diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 8fdcb26b7..6413c5a3c 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -66,8 +66,4 @@ #include #include -#ifdef BELUGA_OPENVDB_SUPPORT -#include -#endif - #endif diff --git a/beluga/include/beluga/sensor/likelihood_field_model3.hpp b/beluga/include/beluga/sensor/likelihood_field_model3.hpp deleted file mode 100644 index cc0fec13d..000000000 --- a/beluga/include/beluga/sensor/likelihood_field_model3.hpp +++ /dev/null @@ -1,162 +0,0 @@ -// Copyright 2024 Ekumen, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP -#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP - -#include -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * \file - * \brief Implementation of a likelihood field sensor model for 3D Lidars. - */ - -namespace beluga { - -/// Parameters used to construct a LikelihoodFieldModel3 instance. -/** - * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. - */ -struct LikelihoodFieldModel3Param { - /// Maximum distance to obstacle. - /** - * When creating a distance map, if the distance to an obstacle is higher than the value specified here, - * then this value will be used. - */ - double max_obstacle_distance = 100.0; - /// Maximum range of a laser ray. - double max_laser_distance = 2.0; - /// Weight used to combine the probability of hitting an obstacle. - double z_hit = 0.5; - /// Weight used to combine the probability of random noise in perception. - double z_random = 0.5; - /// Standard deviation of a gaussian centered arounds obstacles. - /** - * Used to calculate the probability of the obstacle being hit. - */ - double sigma_hit = 0.2; -}; - -/// Likelihood field sensor model for range finders. -/** - * This model relies on a pre-computed likelihood map of the environment. - * It is less computationally intensive than the beluga::BeamSensorModel - * because no ray-tracing is required, and it can also provide better - * performance in environments with non-smooth occupation maps. See - * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4, - * for further reference. - * - * \note This class satisfies \ref SensorModelPage. - * - * \tparam OpenVDB grid type. - */ -template -class LikelihoodFieldModel3 { - public: - /// State type of a particle. - using state_type = Sophus::SE3d; - - /// Weight type of the particle. - using weight_type = double; - - /// Measurement type given by the interface. - using measurement_type = PointCloud; - - /// Map representation type. - using map_type = GridT; - - /// Parameter type that the constructor uses to configure the likelihood field model. - using param_type = LikelihoodFieldModel3Param; - - /// Constructs a LikelihoodFieldModel3 instance. - /** - * \param params Parameters to configure this instance. - * See beluga::LikelihoodFieldModel3Param for details. - * \param grid Narrow band Level set grid representing the static map that the sensor model - * uses to compute a likelihood field for lidar hits and compute importance weights - * for particle states. - * Currently only supports OpenVDB Level sets. - */ - explicit LikelihoodFieldModel3(const param_type& params, const map_type& grid) - : params_{params}, - grid_{openvdb::gridPtrCast(grid.deepCopyGrid())}, - accessor_{grid_->getConstAccessor()}, - transform_{grid_->transform()}, - background_{grid_->background()}, - two_squared_sigma_{2 * params.sigma_hit * params.sigma_hit}, - amplitude_{params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi()))}, - offset_{params.z_random / params.max_laser_distance} { - openvdb::initialize(); - /// Pre-computed parameters - assert(two_squared_sigma_ > 0.0); - assert(amplitude_ > 0.0); - } - - /// Returns a state weighting function conditioned on 3D lidar hits. - /** - * \param measurement 3D lidar measurement containing the hit points and the transform to the origin. - * \return a state weighting function satisfying \ref StateWeightingFunctionPage - * and borrowing a reference to this sensor model (and thus their lifetime are bound). - */ - [[nodiscard]] auto operator()(measurement_type&& measurement) const { - // Transform each point from the sensor frame to the origin frame - auto transformed_points = measurement.points() | ranges::views::transform([&](const auto& point) { - return measurement.origin() * point.template cast(); - }) | - ranges::to(); - - return [this, points = std::move(transformed_points)](const state_type& state) -> weight_type { - return ranges::fold_left( - points | // - ranges::views::transform([this, &state](const auto& point) { - const Eigen::Vector3d point_in_state_frame = state * point; - const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered( - openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z())); - const auto prob = accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; - return amplitude_ * std::exp(-(prob * prob) / two_squared_sigma_) + offset_; - }), - 1.0, std::plus{}); - }; - } - - private: - param_type params_; - const typename map_type::Ptr grid_; - const typename map_type::ConstAccessor accessor_; - const openvdb::math::Transform transform_; - const typename map_type::ValueType background_; - double two_squared_sigma_; - double amplitude_; - double offset_; -}; - -} // namespace beluga - -#endif diff --git a/beluga/package.xml b/beluga/package.xml index 2b701e775..dc349ed27 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -15,10 +15,7 @@ cmake eigen - libboost-iostreams-dev - libboost-thread-dev libhdf5-dev - libopenvdb-dev range-v3 sophus tbb diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 068cb484f..ece4b8a69 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -16,7 +16,6 @@ file(COPY test_data DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) add_executable( test_beluga - $,sensor/test_likelihood_3d_field_model.cpp,> actions/test_assign.cpp actions/test_normalize.cpp actions/test_overlay.cpp diff --git a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp b/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp deleted file mode 100644 index 7485e282a..000000000 --- a/beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2024 Ekumen, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP -#define BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP - -#include -#include - -namespace beluga::testing { - -template -class SimpleSparsePointCloud3 { - public: - explicit SimpleSparsePointCloud3(std::vector> points, Sophus::SE3d origin = Sophus::SE3d{}) - : points_(std::move(points)), origin_(std::move(origin)) {} - - /// Get the point cloud frame origin in the filter frame. - [[nodiscard]] const auto& origin() const { return origin_; } - - /// Get the 3D points collection. - [[nodiscard]] const auto& points() const { return points_; } - - private: - std::vector> points_; - Sophus::SE3d origin_; -}; - -using SimpleSparsePointCloud3d = SimpleSparsePointCloud3; -using SimpleSparsePointCloud3f = SimpleSparsePointCloud3; - -} // namespace beluga::testing - -#endif diff --git a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp deleted file mode 100644 index 622c0f59e..000000000 --- a/beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp +++ /dev/null @@ -1,168 +0,0 @@ -// Copyright 2024 Ekumen, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include "beluga/sensor/likelihood_field_model3.hpp" -#include "beluga/test/simple_pointcloud_interface.hpp" - -namespace { - -template -auto make_map(const double voxel_size, const std::vector& world_points) { - // Parameters - constexpr int kHalfWidth = 3; - constexpr int kClosingSteps = 0; - - // Create the grid - const typename GridT::Ptr grid = openvdb::FloatGrid::create(); - grid->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size)); - - // Get an accessor for coordinate-based access to voxels - typename GridT::Accessor accessor = grid->getAccessor(); - - // Fill the grid - const openvdb::math::Transform& transform = grid->transform(); - for (const auto& point : world_points) { - // Transform to index world - const openvdb::math::Coord ijk = transform.worldToIndexCellCentered(point); - // Set the voxel to 1 - accessor.setValue(ijk, 1.0); - // Activate the voxel - accessor.setActiveState(ijk); - } - - // Check grid - openvdb::tools::CheckLevelSet checker(*grid); - // Check inactive values have a magnitude equal to the background value - assert(checker.checkInactiveValues() == ""); - - // Transform to levelset - return openvdb::tools::topologyToLevelSet(*grid, kHalfWidth, kClosingSteps); -} - -TEST(TestLikelihoodFieldModel3, Point) { - openvdb::initialize(); - // Parameters - constexpr double kVoxelSize = 0.07; - constexpr double kMaxObstacleDistance = 2.0; - constexpr double kMaxLaserDistance = 100.0; - constexpr double kZHit = 0.5; - constexpr double kZRandom = 0.001; - constexpr double kSigmaHit = 0.22; - - // Create Grid - const std::vector world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)}; - auto map = make_map(kVoxelSize, world_points); - - const auto params = - beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; - auto sensor_model = - beluga::LikelihoodFieldModel3{params, *map}; - - // Exact point - auto pointcloud_measurement_exact = - beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.0F, 1.0F, 1.0F}}}; - auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); - ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 1.90); - - // Close point (Inside the narrow band) - auto pointcloud_measurement_close = - beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.035F, 1.0F, 1.0F}}}; - auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); - ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 1.8); - - // Far point (Outside the narrow band) - auto pointcloud_measurement_far = - beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.1F, 1.1F, 1.1F}}}; - auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); - ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 1.65); -} - -TEST(TestLikelihoodFieldModel3, Cube) { - openvdb::initialize(); - // Parameters - constexpr double kVoxelSize = 0.07; - constexpr double kMaxObstacleDistance = 2.0; - constexpr double kMaxLaserDistance = 100.0; - constexpr double kZHit = 0.5; - constexpr double kZRandom = 0.001; - constexpr double kSigmaHit = 0.22; - - // Create Grid - const std::vector world_points{ - openvdb::math::Vec3s(1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(1.0F, 1.0F, -1.0F), - openvdb::math::Vec3s(1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(1.0F, -1.0F, -1.0F), - openvdb::math::Vec3s(-1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, -1.0F, -1.0F), - openvdb::math::Vec3s(-1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, 1.0F, -1.0F)}; - auto map = make_map(kVoxelSize, world_points); - - const auto params = - beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; - auto sensor_model = - beluga::LikelihoodFieldModel3{params, *map}; - - // Exact point - auto pointcloud_measurement_exact = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ - {1.0F, 1.0F, 1.0F}, - {1.0F, 1.0F, -1.0F}, - {1.0F, -1.0F, 1.0F}, - {1.0F, -1.0F, -1.0F}, - {-1.0F, -1.0F, 1.0F}, - {-1.0F, -1.0F, -1.0F}, - {-1.0F, 1.0F, 1.0F}, - {-1.0F, 1.0F, -1.0F}}}; - auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); - ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 8.2); - - // Close point (Inside the narrow band) - auto pointcloud_measurement_close = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ - {1.035F, 1.0F, 1.0F}, - {1.035F, 1.0F, -1.0F}, - {1.035F, -1.0F, 1.0F}, - {1.035F, -1.0F, -1.0F}, - {-1.0F, -1.0F, 1.0F}, - {-1.0F, -1.0F, -1.0F}, - {-1.0F, 1.0F, 1.0F}, - {-1.0F, 1.0F, -1.0F}}}; - auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); - ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 7.4); - - // Far point (Outside the narrow band) - auto pointcloud_measurement_far = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ - {1.1F, 1.1F, 1.1F}, - {1.1F, 1.1F, -1.1F}, - {1.1F, -1.1F, 1.1F}, - {1.1F, -1.1F, -1.1F}, - {-1.1F, -1.1F, 1.1F}, - {-1.1F, -1.1F, -1.1F}, - {-1.1F, 1.1F, 1.1F}, - {-1.1F, 1.1F, -1.1F}}}; - auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); - ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 6.85); -} - -} // namespace From d802498234bbf05908b57dd7a507e23cb612fa6f Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 16:06:08 +0800 Subject: [PATCH 70/77] Add beluga_vdb package description Signed-off-by: Pablo Vela --- beluga_vdb/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/beluga_vdb/package.xml b/beluga_vdb/package.xml index 55ef9861f..e3aa54baa 100644 --- a/beluga_vdb/package.xml +++ b/beluga_vdb/package.xml @@ -4,7 +4,7 @@ beluga_vdb 1.0.0 - A generic MCL library for ROS2. + A beluga extension to facilitate the use of OpenVDB. Gerardo Puga Ivan Paunovic From dae1efd83c619d06fbd68767328a8b7caee8514d Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 16:25:23 +0800 Subject: [PATCH 71/77] Remove LikelihoodFieldModel3 in doc Signed-off-by: Pablo Vela --- beluga/include/beluga/sensor.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 6413c5a3c..109322712 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -54,7 +54,6 @@ * * \section SensorModelLinks See also * - beluga::LikelihoodFieldModel - * - beluga::LikelihoodFieldModel3 * - beluga::BeamSensorModel * - beluga::LandmarkSensorModel * - beluga::BearingSensorModel From 487901d26da49f200d04b90c27e749a8081e0871 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 16:29:14 +0800 Subject: [PATCH 72/77] Update tools with beluga_vdb Signed-off-by: Pablo Vela --- tools/build-and-test.sh | 2 +- tools/run-clang-tidy.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/build-and-test.sh b/tools/build-and-test.sh index 6003f62ec..708ab8952 100755 --- a/tools/build-and-test.sh +++ b/tools/build-and-test.sh @@ -31,7 +31,7 @@ if [ "${ROS_DISTRO}" != "noetic" ]; then else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools" if [ "${ROS_DISTRO}" != "humble" ] && [ "${ROS_DISTRO}" != "iron" ]; then - CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" + ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools beluga_vdb" fi fi else diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index 24d78549a..fca7b6e3f 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -26,7 +26,7 @@ COLCON_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" if [ "${ROS_DISTRO}" != "humble" ] && [ "${ROS_DISTRO}" != "iron" ]; then - CMAKE_EXTRA_ARGS="${CMAKE_EXTRA_ARGS} -DUSE_OPENVDB=On" + ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools beluga_vdb" fi else ROS_PACKAGES="beluga beluga_ros beluga_amcl" From 5ed11f6224f0df264cd168cbd928518637068f4d Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 16:35:39 +0800 Subject: [PATCH 73/77] Fix beluga_vdb namespace Signed-off-by: Pablo Vela --- .../sensor/likelihood_field_model3.hpp | 8 +++---- .../test/simple_pointcloud_interface.hpp | 8 +++---- .../sensor/test_likelihood_3d_field_model.cpp | 22 ++++++++++--------- 3 files changed, 20 insertions(+), 18 deletions(-) diff --git a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp index cc0fec13d..37ed750e4 100644 --- a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp +++ b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP -#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP +#ifndef BELUGA_VDB_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP +#define BELUGA_VDB_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP #include #include @@ -38,7 +38,7 @@ * \brief Implementation of a likelihood field sensor model for 3D Lidars. */ -namespace beluga { +namespace beluga_vdb { /// Parameters used to construct a LikelihoodFieldModel3 instance. /** @@ -157,6 +157,6 @@ class LikelihoodFieldModel3 { double offset_; }; -} // namespace beluga +} // namespace beluga_vdb #endif diff --git a/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp b/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp index 7485e282a..d51dbbf13 100644 --- a/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp +++ b/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP -#define BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP +#ifndef BELUGA_VDB_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP +#define BELUGA_VDB_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP #include #include -namespace beluga::testing { +namespace beluga_vdb::testing { template class SimpleSparsePointCloud3 { @@ -40,6 +40,6 @@ class SimpleSparsePointCloud3 { using SimpleSparsePointCloud3d = SimpleSparsePointCloud3; using SimpleSparsePointCloud3f = SimpleSparsePointCloud3; -} // namespace beluga::testing +} // namespace beluga_vdb::testing #endif diff --git a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp index a03c77a23..ba3deb762 100644 --- a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp @@ -79,25 +79,26 @@ TEST(TestLikelihoodFieldModel3, Point) { auto map = make_map(kVoxelSize, world_points); const auto params = - beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; + beluga_vdb::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; auto sensor_model = - beluga::LikelihoodFieldModel3{params, *map}; + beluga_vdb::LikelihoodFieldModel3{ + params, *map}; // Exact point auto pointcloud_measurement_exact = - beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.0F, 1.0F, 1.0F}}}; + beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{{1.0F, 1.0F, 1.0F}}}; auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 1.90); // Close point (Inside the narrow band) auto pointcloud_measurement_close = - beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.035F, 1.0F, 1.0F}}}; + beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{{1.035F, 1.0F, 1.0F}}}; auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 1.8); // Far point (Outside the narrow band) auto pointcloud_measurement_far = - beluga::testing::SimpleSparsePointCloud3f{std::vector>{{1.1F, 1.1F, 1.1F}}}; + beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{{1.1F, 1.1F, 1.1F}}}; auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 1.65); } @@ -121,12 +122,13 @@ TEST(TestLikelihoodFieldModel3, Cube) { auto map = make_map(kVoxelSize, world_points); const auto params = - beluga::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; + beluga_vdb::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; auto sensor_model = - beluga::LikelihoodFieldModel3{params, *map}; + beluga_vdb::LikelihoodFieldModel3{ + params, *map}; // Exact point - auto pointcloud_measurement_exact = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + auto pointcloud_measurement_exact = beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{ {1.0F, 1.0F, 1.0F}, {1.0F, 1.0F, -1.0F}, {1.0F, -1.0F, 1.0F}, @@ -139,7 +141,7 @@ TEST(TestLikelihoodFieldModel3, Cube) { ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 8.2); // Close point (Inside the narrow band) - auto pointcloud_measurement_close = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + auto pointcloud_measurement_close = beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{ {1.035F, 1.0F, 1.0F}, {1.035F, 1.0F, -1.0F}, {1.035F, -1.0F, 1.0F}, @@ -152,7 +154,7 @@ TEST(TestLikelihoodFieldModel3, Cube) { ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 7.4); // Far point (Outside the narrow band) - auto pointcloud_measurement_far = beluga::testing::SimpleSparsePointCloud3f{std::vector>{ + auto pointcloud_measurement_far = beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{ {1.1F, 1.1F, 1.1F}, {1.1F, 1.1F, -1.1F}, {1.1F, -1.1F, 1.1F}, From d6b1670313b91add9c471beb019bdf8ff7b8e647 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Sat, 7 Dec 2024 16:55:04 +0800 Subject: [PATCH 74/77] Fix beluga_vdb debug compile Signed-off-by: Pablo Vela --- beluga_vdb/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/beluga_vdb/CMakeLists.txt b/beluga_vdb/CMakeLists.txt index a13e11f99..518fcddd5 100644 --- a/beluga_vdb/CMakeLists.txt +++ b/beluga_vdb/CMakeLists.txt @@ -22,6 +22,7 @@ set(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH} CACHE PATH "Path to OpenVDB CMake module") +add_library(${PROJECT_NAME} INTERFACE) if(NOT CMAKE_BUILD_TYPE) message(STATUS "Setting build type to 'Release' as none was specified.") set(CMAKE_BUILD_TYPE @@ -38,7 +39,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") -Wpedantic) endif() if(CMAKE_BUILD_TYPE MATCHES "Debug") - target_compile_options(-fno-inline) + target_compile_options(${PROJECT_NAME} INTERFACE -fno-inline) endif() find_package(Eigen3 REQUIRED NO_MODULE) @@ -77,7 +78,6 @@ if(OPENVDB_CMAKE_MODULE_PATH) list(POP_BACK CMAKE_MODULE_PATH) endif() -add_library(${PROJECT_NAME} INTERFACE) target_include_directories( ${PROJECT_NAME} INTERFACE $ From fe6b5947669ff9f727b2c578e628bcc66ba7146c Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Tue, 10 Dec 2024 10:40:53 +0800 Subject: [PATCH 75/77] Rename prob to distance Signed-off-by: Pablo Vela --- .../include/beluga_vdb/sensor/likelihood_field_model3.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp index 37ed750e4..b841f2a75 100644 --- a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp +++ b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp @@ -139,8 +139,8 @@ class LikelihoodFieldModel3 { const Eigen::Vector3d point_in_state_frame = state * point; const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered( openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z())); - const auto prob = accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; - return amplitude_ * std::exp(-(prob * prob) / two_squared_sigma_) + offset_; + const auto distance = accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; + return amplitude_ * std::exp(-(distance * distance) / two_squared_sigma_) + offset_; }), 1.0, std::plus{}); }; From 6b95d3455aee587511ddf70ed1638c4e2d471d68 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 31 Dec 2024 17:05:57 -0300 Subject: [PATCH 76/77] Please CI Signed-off-by: Michel Hidalgo --- beluga_vdb/CMakeLists.txt | 71 +++++++++++++------ beluga_vdb/cmake/Config.cmake.in | 42 +++++++++++ beluga_vdb/src/clang_tidy_findable.cpp | 27 +++++++ .../sensor/test_likelihood_3d_field_model.cpp | 2 +- tools/run-clang-tidy.sh | 2 +- 5 files changed, 121 insertions(+), 23 deletions(-) create mode 100644 beluga_vdb/cmake/Config.cmake.in create mode 100644 beluga_vdb/src/clang_tidy_findable.cpp diff --git a/beluga_vdb/CMakeLists.txt b/beluga_vdb/CMakeLists.txt index 518fcddd5..c305ddf67 100644 --- a/beluga_vdb/CMakeLists.txt +++ b/beluga_vdb/CMakeLists.txt @@ -22,7 +22,6 @@ set(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH} CACHE PATH "Path to OpenVDB CMake module") -add_library(${PROJECT_NAME} INTERFACE) if(NOT CMAKE_BUILD_TYPE) message(STATUS "Setting build type to 'Release' as none was specified.") set(CMAKE_BUILD_TYPE @@ -30,23 +29,11 @@ if(NOT CMAKE_BUILD_TYPE) CACHE STRING "Build type" FORCE) endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options( - -Wall - -Wconversion - -Wextra - -Werror - -Wpedantic) -endif() -if(CMAKE_BUILD_TYPE MATCHES "Debug") - target_compile_options(${PROJECT_NAME} INTERFACE -fno-inline) -endif() +find_package(beluga REQUIRED) find_package(Eigen3 REQUIRED NO_MODULE) find_package(range-v3 REQUIRED) find_package(Sophus REQUIRED) -find_package(TBB REQUIRED) -find_package(beluga REQUIRED) if(NOT OPENVDB_CMAKE_MODULE_PATH) file( @@ -78,12 +65,42 @@ if(OPENVDB_CMAKE_MODULE_PATH) list(POP_BACK CMAKE_MODULE_PATH) endif() +add_library(${PROJECT_NAME} INTERFACE) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options( + ${PROJECT_NAME} + INTERFACE -Wall + -Wconversion + -Wextra + -Werror + -Wpedantic) +endif() +if(CMAKE_BUILD_TYPE MATCHES "Debug") + target_compile_options(${PROJECT_NAME} INTERFACE -fno-inline) +endif() + target_include_directories( ${PROJECT_NAME} INTERFACE $ $) target_link_libraries(${PROJECT_NAME} INTERFACE beluga::beluga OpenVDB::openvdb) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) +target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG + SOPHUS_USE_BASIC_LOGGING) + +add_executable(clang_tidy_findable) +target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp) +target_link_libraries(clang_tidy_findable PRIVATE ${PROJECT_NAME}) + +option(BUILD_TESTING "Build the testing tree." ON) +if(BUILD_TESTING) + message(STATUS "Build testing enabled.") + enable_testing() + add_subdirectory(test) +endif() + +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install( TARGETS ${PROJECT_NAME} @@ -92,11 +109,23 @@ install( LIBRARY DESTINATION lib RUNTIME DESTINATION bin) -install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +set(INSTALL_CMAKEDIR ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cmake) -option(BUILD_TESTING "Build the testing tree." ON) -if(BUILD_TESTING) - message(STATUS "Build testing enabled.") - enable_testing() - add_subdirectory(test) -endif() +install( + EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION ${INSTALL_CMAKEDIR}) + +include(CMakePackageConfigHelpers) +configure_package_config_file( + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION ${INSTALL_CMAKEDIR}) +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake + COMPATIBILITY SameMajorVersion) + +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION ${INSTALL_CMAKEDIR}) diff --git a/beluga_vdb/cmake/Config.cmake.in b/beluga_vdb/cmake/Config.cmake.in new file mode 100644 index 000000000..eddfa6d28 --- /dev/null +++ b/beluga_vdb/cmake/Config.cmake.in @@ -0,0 +1,42 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(Eigen3 REQUIRED NO_MODULE) +find_dependency(range-v3 REQUIRED) +find_dependency(Sophus REQUIRED) +find_dependency(beluga REQUIRED) + +set(OPENVDB_CMAKE_MODULE_PATH + ${OPENVDB_CMAKE_MODULE_PATH} + CACHE PATH "Path to OpenVDB CMake module") + +if(NOT OPENVDB_CMAKE_MODULE_PATH) + file( + GLOB_RECURSE + OPENVDB_MODULES + /usr/lib/*/FindOpenVDB.cmake + /usr/local/lib/*/FindOpenVDB.cmake) + list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES) + if(NUM_OPENVDB_MODULES EQUAL 1) + list(GET OPENVDB_MODULES 0 OPENVDB_MODULE) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} DIRECTORY) + endif() + unset(NUM_OPENVDB_MODULES) + unset(OPENVDB_MODULES) +endif() + +if(OPENVDB_CMAKE_MODULE_PATH) + list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) +endif() + +find_dependency(OpenVDB REQUIRED) + +if(OPENVDB_CMAKE_MODULE_PATH) + list(POP_BACK CMAKE_MODULE_PATH) +endif() + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") + +set(@PROJECT_NAME@_LIBRARIES @PROJECT_NAME@::@PROJECT_NAME@) + +check_required_components($@PROJECT_NAME@) diff --git a/beluga_vdb/src/clang_tidy_findable.cpp b/beluga_vdb/src/clang_tidy_findable.cpp new file mode 100644 index 000000000..607e856e6 --- /dev/null +++ b/beluga_vdb/src/clang_tidy_findable.cpp @@ -0,0 +1,27 @@ +// Copyright 2022-2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * This file exists in this header-only library as a workaround for + * clang-tidy to find and process the headers, since all the source files + * inside the test folder are ignored. + * It will not be exported or installed with the rest of the targets of this + * library. + */ + +#include + +int main() { + return 0; +} diff --git a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp index ba3deb762..3abd678df 100644 --- a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp @@ -56,7 +56,7 @@ auto make_map(const double voxel_size, const std::vector& world_points) { } // Check grid - openvdb::tools::CheckLevelSet checker(*grid); + const openvdb::tools::CheckLevelSet checker(*grid); // Check inactive values have a magnitude equal to the background value assert(checker.checkInactiveValues() == ""); diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index fca7b6e3f..60ea94a08 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -26,7 +26,7 @@ COLCON_EXTRA_ARGS="" if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" if [ "${ROS_DISTRO}" != "humble" ] && [ "${ROS_DISTRO}" != "iron" ]; then - ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools beluga_vdb" + ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_vdb" fi else ROS_PACKAGES="beluga beluga_ros beluga_amcl" From 533086b39dbb341b33d8508da68b805aa2197b67 Mon Sep 17 00:00:00 2001 From: Pablo Vela Date: Thu, 9 Jan 2025 10:29:28 +0800 Subject: [PATCH 77/77] Remove const to checker Signed-off-by: Pablo Vela --- .../test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp index 3abd678df..ba3deb762 100644 --- a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp +++ b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp @@ -56,7 +56,7 @@ auto make_map(const double voxel_size, const std::vector& world_points) { } // Check grid - const openvdb::tools::CheckLevelSet checker(*grid); + openvdb::tools::CheckLevelSet checker(*grid); // Check inactive values have a magnitude equal to the background value assert(checker.checkInactiveValues() == "");