Add pointcloud API contract and ROS interface (#431)

### Proposed changes

Added a ROS point cloud interface for float data type and it respective
testcase

#### Type of change

- [ ] 🐛 Bugfix (change which fixes an issue)
- [x] 🚀 Feature (change which adds functionality)
- [ ] 📚 Documentation (change which fixes or extends documentation)


### Checklist

_Put an `x` in the boxes that apply. This is simply a reminder of what
we will require before merging your code._

- [x] Lint and unit tests (if any) pass locally with my changes
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have added necessary documentation (if appropriate)
- [x] All commits have been signed for
[DCO](https://developercertificate.org/)

---------

Signed-off-by: Pablo Vela <[email protected]>
Co-authored-by: Michel Hidalgo <[email protected]>

beluga/include/beluga/eigen_compatibility.hpp

@@ -22,6 +22,10 @@ namespace Eigen {  // NOLINT(readability-identifier-naming)
 /// Type alias for single-column matrices, available starting Eigen 3.4
 template <typename Scalar, int Dims>
 using Vector = Eigen::Matrix<Scalar, Dims, 1>;
+template <typename Scalar>
+using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
+template <typename Scalar>
+using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
 }  // namespace Eigen
 #endif
 

beluga/include/beluga/sensor/data/point_cloud.hpp

@@ -0,0 +1,60 @@
+// 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_DATA_POINT_CLOUD_HPP
+#define BELUGA_SENSOR_DATA_POINT_CLOUD_HPP
+
+#include <cmath>
+
+#include <ciabatta/ciabatta.hpp>
+
+#include <sophus/types.hpp>
+
+/**
+ * \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.
+ */
+
+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<Eigen::Matrix3X>` type,
+ * - `p.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local
+ *   frame in the sensor space;
+ */
+
+/// Point Cloud 3D base type.
+/**
+ * When instantiated, it satisfies \ref PointCloudPage.
+ *
+ * \tparam Derived Concrete point cloud type. It must define
+ * `Derived::ranges()`, `Derived::angles()`,
+ * as described in \ref PointCloudPage.
+ */
+template <typename Derived>
+class BasePointCloud : public ciabatta::ciabatta_top<Derived> {};
+
+}  // namespace beluga
+
+#endif

beluga/include/beluga/sensor/data/sparse_point_cloud.hpp

@@ -0,0 +1,60 @@
+// 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_DATA_SPARSE_POINT_CLOUD_HPP
+#define BELUGA_SENSOR_DATA_SPARSE_POINT_CLOUD_HPP
+
+#include <cmath>
+
+#include <ciabatta/ciabatta.hpp>
+
+#include <sophus/types.hpp>
+
+/**
+ * \file
+ * \brief Implementation of a sparse point cloud interface without memory-aligned data.
+ *
+ * \details
+ * Assumes data is aligned as X, Y, Z, other datafields .
+ * Only X, Y & Z must be the same type (float or double).
+ * Other datafields can be different types.
+ */
+
+namespace beluga {
+
+/**
+ * \page SparsePointCloudPage 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<Eigen::Vector3>` type,
+ * - `p.origin()` returns the transform, of `Sophus::SE3d` type, from the global to local
+ *   frame in the sensor space;
+ */
+
+/// Sparse Point Cloud 3D base type.
+/**
+ * When instantiated, it satisfies \ref PointCloudPage.
+ *
+ * \tparam Derived Concrete point cloud type. It must define
+ * `Derived::ranges()`, `Derived::angles()`,
+ * as described in \ref PointCloudPage.
+ */
+template <typename Derived>
+class BaseSparsePointCloud : public ciabatta::ciabatta_top<Derived> {};
+
+}  // namespace beluga
+
+#endif

beluga_ros/include/beluga_ros/beluga_ros.hpp

@@ -29,6 +29,8 @@
 #include <beluga_ros/laser_scan.hpp>
 #include <beluga_ros/occupancy_grid.hpp>
 #include <beluga_ros/particle_cloud.hpp>
+#include <beluga_ros/point_cloud.hpp>
+#include <beluga_ros/sparse_point_cloud.hpp>
 #include <beluga_ros/tf2_eigen.hpp>
 #include <beluga_ros/tf2_sophus.hpp>
 

beluga_ros/include/beluga_ros/messages.hpp

@@ -23,6 +23,10 @@
 #include <geometry_msgs/msg/transform.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/point_field.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <sensor_msgs/point_field_conversion.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -36,6 +40,10 @@
 #include <geometry_msgs/Transform.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/PointField.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/point_field_conversion.h>
 #include <std_msgs/ColorRGBA.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
@@ -55,6 +63,14 @@ namespace msg {
 using ColorRGBA = std_msgs::msg::ColorRGBA;
 using LaserScan = sensor_msgs::msg::LaserScan;
 using LaserScanConstSharedPtr = LaserScan::ConstSharedPtr;
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+using PointCloud2ConstSharedPtr = PointCloud2::ConstSharedPtr;
+template <typename Scalar>
+using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator<Scalar>;
+template <typename Scalar>
+using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator<Scalar>;
+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;
@@ -70,6 +86,14 @@ using Transform = geometry_msgs::msg::Transform;
 using ColorRGBA = std_msgs::ColorRGBA;
 using LaserScan = sensor_msgs::LaserScan;
 using LaserScanConstSharedPtr = LaserScan::ConstPtr;
+using PointCloud2 = sensor_msgs::PointCloud2;
+using PointCloud2ConstSharedPtr = PointCloud2::ConstPtr;
+template <typename Scalar>
+using PointCloud2ConstIterator = sensor_msgs::PointCloud2ConstIterator<Scalar>;
+template <typename Scalar>
+using PointCloud2Iterator = sensor_msgs::PointCloud2Iterator<Scalar>;
+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;

beluga_ros/include/beluga_ros/point_cloud.hpp

@@ -0,0 +1,104 @@
+// 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_HPP
+#define BELUGA_ROS_POINT_CLOUD_HPP
+
+#include <range/v3/view/iota.hpp>
+
+#include <beluga/sensor/data/point_cloud.hpp>
+#include <beluga/views/take_evenly.hpp>
+#include <beluga_ros/messages.hpp>
+
+#include <sophus/se3.hpp>
+
+#include <Eigen/Dense>
+
+#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.
+ */
+
+namespace beluga_ros {
+
+/// Thin wrapper type for 3D `sensor_msgs/PointCloud2` messages.
+template <typename T>
+class PointCloud3 : public beluga::BasePointCloud<PointCloud3<T>> {
+ public:
+  /// PointCloud type
+  using Scalar = T;
+
+  /// Check type is float or double
+  static_assert(
+      std::is_same_v<Scalar, float> || std::is_same_v<Scalar, double>,
+      "Pointcloud3 only supports float or double datatype");
+
+  /// 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)) {
+    assert(cloud_ != nullptr);
+    constexpr uint8_t fieldType = sensor_msgs::typeAsPointFieldType<T>::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<int>(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<Eigen::Matrix3X>.
+  [[nodiscard]] auto points() const {
+    const beluga_ros::msg::PointCloud2ConstIterator<Scalar> iter_points(*cloud_, "x");
+    return Eigen::Map<const Eigen::Matrix3X<Scalar>, 0, Eigen::OuterStride<>>(
+        &iter_points[0], 3, cloud_->width * cloud_->height, stride_);
+  }
+
+ private:
+  beluga_ros::msg::PointCloud2ConstSharedPtr cloud_;
+  int stride_;
+  Sophus::SE3d origin_;
+};
+
+}  // namespace beluga_ros
+
+#endif  // BELUGA_ROS_POINT_CLOUD_HPP

beluga_ros/include/beluga_ros/sparse_point_cloud.hpp

@@ -0,0 +1,99 @@
+// 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_SPARSE_POINT_CLOUD_HPP
+#define BELUGA_ROS_SPARSE_POINT_CLOUD_HPP
+
+#include <range/v3/view/iota.hpp>
+
+#include <beluga/sensor/data/sparse_point_cloud.hpp>
+#include <beluga/views/take_evenly.hpp>
+#include <beluga_ros/messages.hpp>
+
+#include <sophus/se3.hpp>
+
+#include <Eigen/Dense>
+
+#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 <typename T>
+class SparsePointCloud3 : public beluga::BaseSparsePointCloud<SparsePointCloud3<T>> {
+ public:
+  /// PointCloud type
+  using Scalar = T;
+
+  /// Check type is float or double
+  static_assert(
+      std::is_same_v<Scalar, float> || std::is_same_v<Scalar, double>,
+      "PointcloudSparse3 only supports float or double datatype");
+
+  /// Constructor.
+  ///
+  /// \param cloud Point cloud message.
+  /// \param origin Point cloud frame origin in the filter frame.
+  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<T>::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");
+    }
+  }
+
+  /// 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<Eigen::Vector3>.
+  [[nodiscard]] auto points() const {
+    beluga_ros::msg::PointCloud2ConstIterator<Scalar> iter_points(*cloud_, "x");
+    return ranges::views::iota(0, static_cast<int>(cloud_->width * cloud_->height)) |
+           ranges::views::transform([iter_points](int i) mutable {
+             return Eigen::Map<const Eigen::Vector3<Scalar>>(&(iter_points + i)[0]);
+           });
+  }
+
+ private:
+  beluga_ros::msg::PointCloud2ConstSharedPtr cloud_;
+  Sophus::SE3d origin_;
+};
+
+}  // namespace beluga_ros
+
+#endif  // BELUGA_ROS_SPARSE_POINT_CLOUD_HPP