feat(autoware_cuda_pointcloud_preprocessor): a cuda-accelerated pointcloud preprocessor

sensing/autoware_cuda_pointcloud_preprocessor/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_cuda_pointcloud_preprocessor)
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(CUDA)
+
+if(NOT ${CUDA_FOUND})
+  message(WARNING "cuda was not found, so the autoware_cuda_pointcloud_preprocessor package will not be built.")
+  return()
+elseif(CMAKE_BUILD_TYPE STREQUAL "Debug")
+  set(CMAKE_CUDA_FLAGS ${CMAKE_CUDA_FLAGS} "-g -G")
+  set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-g -G")
+endif()
+
+ament_auto_find_build_dependencies()
+
+# Default to C++17
+if (NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif ()
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function)
+endif ()
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+include_directories(
+    include
+    SYSTEM
+    ${CUDA_INCLUDE_DIRS}
+)
+
+list(APPEND CUDA_NVCC_FLAGS "--expt-relaxed-constexpr -diag-suppress 20012") # cSpell: ignore expt
+
+cuda_add_library(cuda_pointcloud_preprocessor_lib SHARED
+  src/cuda_pointcloud_preprocessor/cuda_pointcloud_preprocessor.cu
+  src/cuda_pointcloud_preprocessor/common_kernels.cu
+  src/cuda_pointcloud_preprocessor/organize_kernels.cu
+  src/cuda_pointcloud_preprocessor/outlier_kernels.cu
+  src/cuda_pointcloud_preprocessor/undistort_kernels.cu
+)
+
+target_link_libraries(cuda_pointcloud_preprocessor_lib
+  ${autoware_pointcloud_preprocessor_TARGETS}
+)
+
+target_include_directories(cuda_pointcloud_preprocessor_lib SYSTEM PRIVATE
+  ${autoware_pointcloud_preprocessor_INCLUDE_DIRS}
+  ${autoware_point_types_INCLUDE_DIRS}
+  ${cuda_blackboard_INCLUDE_DIRS}
+  ${diagnostic_msgs_INCLUDE_DIRS}
+  ${geometry_msgs_INCLUDE_DIRS}
+  ${rclcpp_INCLUDE_DIRS}
+  ${rclcpp_components_INCLUDE_DIRS}
+  ${rcl_interfaces_INCLUDE_DIRS}
+  ${sensor_msgs_INCLUDE_DIRS}
+  ${tf2_INCLUDE_DIRS}
+  ${tf2_msgs_INCLUDE_DIRS}
+)
+
+
+# Targets
+ament_auto_add_library(cuda_pointcloud_preprocessor SHARED
+  src/cuda_pointcloud_preprocessor/cuda_pointcloud_preprocessor_node.cpp
+)
+
+target_link_libraries(cuda_pointcloud_preprocessor
+  ${CUDA_LIBRARIES}
+  cuda_pointcloud_preprocessor_lib
+)
+
+rclcpp_components_register_node(cuda_pointcloud_preprocessor
+  PLUGIN "autoware::cuda_pointcloud_preprocessor::CudaPointcloudPreprocessorNode"
+  EXECUTABLE cuda_pointcloud_preprocessor_node
+)
+
+install(DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_auto_package()
+
+# Set ROS_DISTRO macros
+set(ROS_DISTRO $ENV{ROS_DISTRO})
+if(${ROS_DISTRO} STREQUAL "rolling")
+    add_compile_definitions(ROS_DISTRO_ROLLING)
+elseif(${ROS_DISTRO} STREQUAL "foxy")
+    add_compile_definitions(ROS_DISTRO_FOXY)
+elseif(${ROS_DISTRO} STREQUAL "galactic")
+    add_compile_definitions(ROS_DISTRO_GALACTIC)
+elseif(${ROS_DISTRO} STREQUAL "humble")
+    add_compile_definitions(ROS_DISTRO_HUMBLE)
+endif()

sensing/autoware_cuda_pointcloud_preprocessor/README.md

@@ -0,0 +1,19 @@
+# autoware_cuda_pointcloud_preprocessor
+
+## Purpose
+
+The pointcloud preprocessing implemented in `autoware_pointcloud_preprocessor` has been thoroughly tested in autoware. However, the latency it introduces does not scale well with modern LiDAR devices due to the high number of points they introduce.
+
+To alleviate this issue, this package reimplements most of the pipeline presented in `autoware_pointcloud_preprocessor` leveraging the use of GPGPUs. In particular, this package makes use of CUDA to provide accelerated versions of the already established implementations, while also maintaining compatibility with normal ROS nodes/topics. <!-- cSpell: ignore GPGPUs >
+
+## Inner-workings / Algorithms
+
+A detailed description of each filter's algorithm is available in the following links.
+
+| Filter Name                  | Description                                                                                                                                  | Detail                                       |
+| ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------- |
+| cuda_pointcloud_preprocessor | Implements the cropping, distortion correction, and outlier filtering (ring-based) of the `autoware_pointcloud_preprocessor`'s CPU versions. | [link](docs/cuda-pointcloud-preprocessor.md) |
+
+## (Optional) Future extensions / Unimplemented parts
+
+The subsample filters implemented in `autoware_pointcloud_preprocessor` will have similar counterparts in this package.

sensing/autoware_cuda_pointcloud_preprocessor/config/cuda_pointcloud_preprocessor.param.yaml

@@ -0,0 +1,16 @@
+/**:
+  ros__parameters:
+
+    base_frame: base_link
+    use_imu: true
+    use_3d_distortion_correction: false
+    distance_ratio: 1.03
+    object_length_threshold: 0.1
+    num_points_threshold: 4
+
+    crop_box.min_x: []
+    crop_box.min_y: []
+    crop_box.min_z: []
+    crop_box.max_x: []
+    crop_box.max_y: []
+    crop_box.max_z: []

sensing/autoware_cuda_pointcloud_preprocessor/docs/cuda-pointcloud-preprocessor.md

@@ -0,0 +1,45 @@
+# cuda_pointcloud_preprocessor
+
+## Purpose
+
+This node implements all standard pointcloud preprocessing algorithms applied to a single LiDAR's pointcloud in CUDA.
+In particular, this node implements:
+
+- box cropping (ego-vehicle and ego-vehicle's mirrors)
+- distortion correction
+- ring-based outlier filtering
+
+## Inner-workings / Algorithms
+
+As this node reimplements the functionalities of the CPU-version algorithms, please have a look at the documentations of [crop-box](../../autoware_pointcloud_preprocessor/docs/crop-box-filter.md), [distortion correction](../../autoware_pointcloud_preprocessor/docs/distortion-corrector.md), and [ring-based outlier filter](../../autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md) for more information about these algorithms.
+
+In addition to the individual algorithms previously mentioned, this node uses the `cuda_blackboard`, a cuda transport layer that enables a zero-copy mechanism between GPU and GPU memory for both input and output.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                      | Type                                             | Description                               |
+| ------------------------- | ------------------------------------------------ | ----------------------------------------- |
+| `~/input/pointcloud`      | `sensor_msgs::msg::PointCloud2`                  | Input pointcloud's topic.                 |
+| `~/input/pointcloud/cuda` | `negotiated_interfaces/msg/NegotiatedTopicsInfo` | Input pointcloud's type negotiation topic |
+| `~/input/twist`           | `geometry_msgs::msg::TwistWithCovarianceStamped` | Topic of the twist information.           |
+| `~/input/imu`             | `sensor_msgs::msg::Imu`                          | Topic of the IMU data.                    |
+
+### Output
+
+| Name                       | Type                                             | Description                              |
+| -------------------------- | ------------------------------------------------ | ---------------------------------------- |
+| `~/output/pointcloud`      | `sensor_msgs::msg::PointCloud2`                  | Processed pointcloud's topic             |
+| `~/output/pointcloud/cuda` | `negotiated_interfaces/msg/NegotiatedTopicsInfo` | Processed pointcloud's negotiation topic |
+
+## Parameters
+
+### Core Parameters
+
+{{ json_to_markdown("sensing/autoware_cuda_pointcloud_preprocessor/schema/cuda_pointcloud_preprocessor.schema.schema.json") }}
+
+## Assumptions / Known limits
+
+- The CUDA implementations, while following the original CPU ones, will not offer the same numerical results, and small approximations were needed to maximize GPU usage.
+- This node expects that the input pointcloud follows the `autoware::point_types::PointXYZIRCAEDT` layout and the output pointcloud will use the `autoware::point_types::PointXYZIRC` layout defined in the `autoware_point_types` package.

sensing/autoware_cuda_pointcloud_preprocessor/include/autoware/cuda_pointcloud_preprocessor/common_kernels.hpp

@@ -0,0 +1,45 @@
+// Copyright 2025 TIER IV, 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 AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__COMMON_KERNELS_HPP_
+#define AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__COMMON_KERNELS_HPP_
+
+#include "autoware/cuda_pointcloud_preprocessor/point_types.hpp"
+#include "autoware/cuda_pointcloud_preprocessor/types.hpp"
+
+#include <cuda_runtime.h>
+
+namespace autoware::cuda_pointcloud_preprocessor
+{
+void transformPointsLaunch(
+  const InputPointType * input_points, InputPointType * output_points, int num_points,
+  TransformStruct transform, int threads_per_block, int blocks_per_grid, cudaStream_t & stream);
+
+void cropBoxLaunch(
+  InputPointType * d_points, std::uint32_t * output_mask, int num_points,
+  const CropBoxParameters * crop_box_parameters_ptr, int num_crop_boxes, int threads_per_block,
+  int blocks_per_grid, cudaStream_t & stream);
+
+void combineMasksLaunch(
+  const std::uint32_t * mask1, const std::uint32_t * mask2, int num_points,
+  std::uint32_t * output_mask, int threads_per_block, int blocks_per_grid, cudaStream_t & stream);
+
+void extractPointsLaunch(
+  InputPointType * input_points, std::uint32_t * masks, std::uint32_t * indices, int num_points,
+  OutputPointType * output_points, int threads_per_block, int blocks_per_grid,
+  cudaStream_t & stream);
+
+}  // namespace autoware::cuda_pointcloud_preprocessor
+
+#endif  // AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__COMMON_KERNELS_HPP_

sensing/autoware_cuda_pointcloud_preprocessor/include/autoware/cuda_pointcloud_preprocessor/cuda_pointcloud_preprocessor.hpp

@@ -0,0 +1,107 @@
+// Copyright 2025 TIER IV, 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 AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_POINTCLOUD_PREPROCESSOR_HPP_
+#define AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_POINTCLOUD_PREPROCESSOR_HPP_
+
+#include "autoware/cuda_pointcloud_preprocessor/point_types.hpp"
+#include "autoware/cuda_pointcloud_preprocessor/types.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <cuda_blackboard/cuda_pointcloud2.hpp>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <thrust/device_vector.h>
+
+#include <cstdint>
+#include <deque>
+
+namespace autoware::cuda_pointcloud_preprocessor
+{
+
+class CudaPointcloudPreprocessor
+{
+public:
+  enum class UndistortionType { Invalid, Undistortion2D, Undistortion3D };
+
+  CudaPointcloudPreprocessor();
+  ~CudaPointcloudPreprocessor() = default;
+
+  void setCropBoxParameters(const std::vector<CropBoxParameters> & crop_box_parameters);
+  void setRingOutlierFilterParameters(const RingOutlierFilterParameters & ring_outlier_parameters);
+  void setUndistortionType(const UndistortionType & undistortion_type);
+
+  void preallocateOutput();
+
+  std::unique_ptr<cuda_blackboard::CudaPointCloud2> process(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg_ptr,
+    const geometry_msgs::msg::TransformStamped & transform_msg,
+    const std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> & twist_queue,
+    const std::deque<geometry_msgs::msg::Vector3Stamped> & angular_velocity_queue,
+    const std::uint32_t first_point_rel_stamp_nsec);
+
+private:
+  void organizePointcloud();
+
+  CropBoxParameters self_crop_box_parameters_{};
+  CropBoxParameters mirror_crop_box_parameters_{};
+  RingOutlierFilterParameters ring_outlier_parameters_{};
+  UndistortionType undistortion_type_{UndistortionType::Invalid};
+
+  int num_rings_{};
+  int max_points_per_ring_{};
+  std::size_t num_raw_points_{};
+  std::size_t num_organized_points_{};
+
+  std::vector<sensor_msgs::msg::PointField> point_fields_{};
+  std::unique_ptr<cuda_blackboard::CudaPointCloud2> output_pointcloud_ptr_{};
+
+  cudaStream_t stream_;
+  int max_blocks_per_grid_{};
+  const int threads_per_block_{256};
+  cudaMemPool_t device_memory_pool_;
+
+  // Organizing buffers
+  thrust::device_vector<InputPointType> device_input_points_;
+  thrust::device_vector<InputPointType> device_organized_points_;
+  thrust::device_vector<std::int32_t> device_ring_index_;
+  thrust::device_vector<std::uint32_t> device_indexes_tensor_;
+  thrust::device_vector<std::uint32_t> device_sorted_indexes_tensor_;
+  thrust::device_vector<std::int32_t> device_segment_offsets_;
+  thrust::device_vector<std::int32_t> device_max_ring_;
+  thrust::device_vector<std::int32_t> device_max_points_per_ring_;
+
+  thrust::device_vector<std::uint8_t> device_sort_workspace_;
+  std::size_t sort_workspace_bytes_{0};
+
+  // Pointcloud preprocessing buffers
+  thrust::device_vector<InputPointType> device_transformed_points_{};
+  thrust::device_vector<OutputPointType> device_output_points_{};
+  thrust::device_vector<std::uint32_t> device_crop_mask_{};
+  thrust::device_vector<std::uint32_t> device_ring_outlier_mask_{};
+  thrust::device_vector<std::uint32_t> device_indices_{};
+  thrust::device_vector<TwistStruct2D> device_twist_2d_structs_{};
+  thrust::device_vector<TwistStruct3D> device_twist_3d_structs_{};
+  thrust::device_vector<CropBoxParameters> host_crop_box_structs_{};
+  thrust::device_vector<CropBoxParameters> device_crop_box_structs_{};
+};
+
+}  // namespace autoware::cuda_pointcloud_preprocessor
+
+#endif  // AUTOWARE__CUDA_POINTCLOUD_PREPROCESSOR__CUDA_POINTCLOUD_PREPROCESSOR_HPP_