diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 0f4cb7112f74a..b6dfbaf402cc8 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -127,6 +127,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_library(pointpainting_lib SHARED src/pointpainting_fusion/node.cpp + src/pointpainting_fusion/pointcloud_densification.cpp src/pointpainting_fusion/pointpainting_trt.cpp src/pointpainting_fusion/voxel_generator.cpp ) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp new file mode 100644 index 0000000000000..03609eb18e689 --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp @@ -0,0 +1,72 @@ +// Copyright 2024 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 IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include +#include + +namespace image_projection_based_fusion +{ +struct PointCloudWithTransform +{ + sensor_msgs::msg::PointCloud2 pointcloud_msg; + Eigen::Affine3f affine_past2world; +}; + +class PointCloudDensification +{ +public: + explicit PointCloudDensification(const centerpoint::DensificationParam & param); + + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + + double getCurrentTimestamp() const { return current_timestamp_; } + Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } + std::list::iterator getPointCloudCacheIter() + { + return pointcloud_cache_.begin(); + } + bool isCacheEnd(std::list::iterator iter) + { + return iter == pointcloud_cache_.end(); + } + unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); } + +private: + void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); + void dequeue(); + + centerpoint::DensificationParam param_; + double current_timestamp_{0.0}; + Eigen::Affine3f affine_world2current_; + std::list pointcloud_cache_; +}; + +} // namespace image_projection_based_fusion + +#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp old mode 100755 new mode 100644 index d0f44b9d58706..4cdca8e49ac7e --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -15,19 +15,35 @@ #ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ #define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ +#include +#include #include #include +#include #include namespace image_projection_based_fusion { -class VoxelGenerator : public centerpoint::VoxelGenerator + +class VoxelGenerator { public: - using centerpoint::VoxelGenerator::VoxelGenerator; + explicit VoxelGenerator( + const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config); + + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + + std::size_t generateSweepPoints(std::vector & points); + +protected: + std::unique_ptr pd_ptr_{nullptr}; - std::size_t generateSweepPoints(std::vector & points) override; + centerpoint::CenterPointConfig config_; + std::array range_; + std::array grid_size_; + std::array recip_voxel_size_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp new file mode 100644 index 0000000000000..9edf6a73cd59b --- /dev/null +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp @@ -0,0 +1,103 @@ +// Copyright 2024 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. + +#include "image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp" + +#include + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +namespace +{ +boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, + const std::string & source_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), ex.what()); + return boost::none; + } +} + +Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + +namespace image_projection_based_fusion +{ +PointCloudDensification::PointCloudDensification(const centerpoint::DensificationParam & param) +: param_(param) +{ +} + +bool PointCloudDensification::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer) +{ + const auto header = pointcloud_msg.header; + + if (param_.pointcloud_cache_size() > 1) { + auto transform_world2current = + getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); + if (!transform_world2current) { + return false; + } + auto affine_world2current = transformToEigen(transform_world2current.get()); + + enqueue(pointcloud_msg, affine_world2current); + } else { + enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); + } + + dequeue(); + + return true; +} + +void PointCloudDensification::enqueue( + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current) +{ + affine_world2current_ = affine_world2current; + current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); + PointCloudWithTransform pointcloud = {msg, affine_world2current.inverse()}; + pointcloud_cache_.push_front(pointcloud); +} + +void PointCloudDensification::dequeue() +{ + if (pointcloud_cache_.size() > param_.pointcloud_cache_size()) { + pointcloud_cache_.pop_back(); + } +} + +} // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index ea10cb1237436..cb3fc33d3e022 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -18,6 +18,32 @@ namespace image_projection_based_fusion { + +VoxelGenerator::VoxelGenerator( + const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config) +: config_(config) +{ + pd_ptr_ = std::make_unique(param); + range_[0] = config.range_min_x_; + range_[1] = config.range_min_y_; + range_[2] = config.range_min_z_; + range_[3] = config.range_max_x_; + range_[4] = config.range_max_y_; + range_[5] = config.range_max_z_; + grid_size_[0] = config.grid_size_x_; + grid_size_[1] = config.grid_size_y_; + grid_size_[2] = config.grid_size_z_; + recip_voxel_size_[0] = 1 / config.voxel_size_x_; + recip_voxel_size_[1] = 1 / config.voxel_size_y_; + recip_voxel_size_[2] = 1 / config.voxel_size_z_; +} + +bool VoxelGenerator::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) +{ + return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer); +} + size_t VoxelGenerator::generateSweepPoints(std::vector & points) { Eigen::Vector3f point_current, point_past; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp index 8cf250be0c049..55157f70fcfc3 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -15,15 +15,14 @@ #ifndef LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ #define LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ -#include -#include -#include -#include - -#include - -#include -#include +#include "lidar_centerpoint/cuda_utils.hpp" +#include "lidar_centerpoint/network/network_trt.hpp" +#include "lidar_centerpoint/postprocess/postprocess_kernel.hpp" +#include "lidar_centerpoint/preprocess/voxel_generator.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" + +#include "sensor_msgs/msg/point_cloud2.hpp" #include #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp index 5ab26d75a0a41..efe3bbb9a5482 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp @@ -42,7 +42,7 @@ #ifndef LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ #define LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ -#include +#include "cuda_runtime_api.h" #include #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp index cd1a0e58e115e..d961dd998af76 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include -#include +#include "autoware_perception_msgs/msg/detected_object_kinematics.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp index 639aa0ba8bad3..1bf77248efe08 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ #define LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ -#include -#include +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/network/tensorrt_wrapper.hpp" #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp index 3dd00c25fd9e7..ad5e222ce01e8 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ #define LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ -#include -#include +#include "cuda.h" +#include "cuda_runtime_api.h" namespace centerpoint { diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp index 7f55ab6f5e414..4af5aac7ff7fe 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp @@ -15,10 +15,9 @@ #ifndef LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ #define LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ -#include -#include - -#include +#include "NvInfer.h" +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "tensorrt_common/tensorrt_common.hpp" #include #include @@ -32,7 +31,7 @@ class TensorRTWrapper public: explicit TensorRTWrapper(const CenterPointConfig & config); - ~TensorRTWrapper(); + virtual ~TensorRTWrapper(); bool init( const std::string & onnx_path, const std::string & engine_path, const std::string & precision); diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index d96eb1ae3ed94..032627861e1b3 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -15,13 +15,13 @@ #ifndef LIDAR_CENTERPOINT__NODE_HPP_ #define LIDAR_CENTERPOINT__NODE_HPP_ +#include "lidar_centerpoint/centerpoint_trt.hpp" +#include "lidar_centerpoint/detection_class_remapper.hpp" #include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include #include #include -#include -#include #include #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp index cd2d71d33806d..5b55636a3eec4 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp @@ -15,9 +15,8 @@ #ifndef LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ #define LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#include - -#include +#include "lidar_centerpoint/utils.hpp" +#include "thrust/device_vector.h" namespace centerpoint { diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp index d790e896dea9a..e15d3022c947c 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -15,12 +15,11 @@ #ifndef LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ #define LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ -#include -#include - -#include -#include -#include +#include "cuda.h" +#include "cuda_runtime_api.h" +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/utils.hpp" +#include "thrust/device_vector.h" #include diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp index 209fc74e6c58f..c1d6a6b060955 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp @@ -15,17 +15,19 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include #include #include +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_sensor_msgs/tf2_sensor_msgs.h" +#else +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" +#endif + +#include "lidar_centerpoint/cuda_utils.hpp" namespace centerpoint { @@ -48,7 +50,10 @@ class DensificationParam struct PointCloudWithTransform { - sensor_msgs::msg::PointCloud2 pointcloud_msg; + cuda::unique_ptr points_d{nullptr}; + std_msgs::msg::Header header; + size_t num_points{0}; + size_t point_step{0}; Eigen::Affine3f affine_past2world; }; @@ -58,7 +63,8 @@ class PointCloudDensification explicit PointCloudDensification(const DensificationParam & param); bool enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream); double getCurrentTimestamp() const { return current_timestamp_; } Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } @@ -73,7 +79,8 @@ class PointCloudDensification unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); } private: - void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); + void enqueue( + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine, cudaStream_t stream); void dequeue(); DensificationParam param_; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp index 9488b67475509..3abcd89cb5c55 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp @@ -15,11 +15,15 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ -#include -#include +#include "cuda.h" +#include "cuda_runtime_api.h" namespace centerpoint { +cudaError_t generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform, int num_features, float * output_points, cudaStream_t stream); + cudaError_t generateVoxels_random_launch( const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp index be15d51e91715..3ade5e677cdbe 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp @@ -15,10 +15,10 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ -#include -#include +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include +#include "sensor_msgs/msg/point_cloud2.hpp" #include #include @@ -31,10 +31,11 @@ class VoxelGeneratorTemplate explicit VoxelGeneratorTemplate( const DensificationParam & param, const CenterPointConfig & config); - virtual std::size_t generateSweepPoints(std::vector & points) = 0; + virtual std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) = 0; bool enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream); protected: std::unique_ptr pd_ptr_{nullptr}; @@ -50,7 +51,7 @@ class VoxelGenerator : public VoxelGeneratorTemplate public: using VoxelGeneratorTemplate::VoxelGeneratorTemplate; - std::size_t generateSweepPoints(std::vector & points) override; + std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) override; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index e33d86cd0aba7..484fbcfd36657 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -17,12 +17,9 @@ // ros packages cannot be included from cuda. -#include +#include "lidar_centerpoint/utils.hpp" -#include -#include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index f7b0fe5e0c58f..6f379167b0e71 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -14,10 +14,11 @@ #include "lidar_centerpoint/centerpoint_trt.hpp" +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/network/scatter_kernel.hpp" +#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" + #include -#include -#include -#include #include #include @@ -130,14 +131,11 @@ bool CenterPointTRT::detect( bool CenterPointTRT::preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) { - bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer); + bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream_); if (!is_success) { return false; } - const auto count = vg_ptr_->generateSweepPoints(points_); - CHECK_CUDA_ERROR(cudaMemcpyAsync( - points_d_.get(), points_.data(), count * config_.point_feature_size_ * sizeof(float), - cudaMemcpyHostToDevice, stream_)); + const auto count = vg_ptr_->generateSweepPoints(points_d_.get(), stream_); CHECK_CUDA_ERROR(cudaMemsetAsync(num_voxels_d_.get(), 0, sizeof(unsigned int), stream_)); CHECK_CUDA_ERROR( cudaMemsetAsync(voxels_buffer_d_.get(), 0, voxels_buffer_size_ * sizeof(float), stream_)); diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp index ecfbb4360ecf0..9805fc7a661d1 100644 --- a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp +++ b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "lidar_centerpoint/detection_class_remapper.hpp" namespace centerpoint { diff --git a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu index 09cc83bf606fe..f139e230426ca 100644 --- a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu +++ b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu @@ -13,8 +13,7 @@ // limitations under the License. #include "lidar_centerpoint/network/scatter_kernel.hpp" - -#include +#include "lidar_centerpoint/utils.hpp" namespace { diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 91fcce9a80f78..bdfde42a12a02 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -14,7 +14,7 @@ #include "lidar_centerpoint/network/tensorrt_wrapper.hpp" -#include +#include "NvOnnxParser.h" #include #include diff --git a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu index 398e75a55c44b..c208eefe135fb 100644 --- a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu @@ -21,12 +21,10 @@ Written by Shaoshuai Shi All Rights Reserved 2019-2020. */ +#include "lidar_centerpoint/cuda_utils.hpp" #include "lidar_centerpoint/postprocess/circle_nms_kernel.hpp" - -#include -#include - -#include +#include "lidar_centerpoint/utils.hpp" +#include "thrust/host_vector.h" namespace { diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 81f620018cf04..533d04eebd433 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,10 @@ #include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include +#include +#include + namespace centerpoint { diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 34bbd2811041c..12835bab038a6 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -13,12 +13,10 @@ // limitations under the License. #include "lidar_centerpoint/postprocess/circle_nms_kernel.hpp" - -#include - -#include -#include -#include +#include "lidar_centerpoint/postprocess/postprocess_kernel.hpp" +#include "thrust/count.h" +#include "thrust/device_vector.h" +#include "thrust/sort.h" namespace { diff --git a/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp index 66c348fda50b4..2859d58d8f669 100644 --- a/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp @@ -14,20 +14,19 @@ #include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include +#include "pcl_conversions/pcl_conversions.h" +#include "pcl_ros/transforms.hpp" -#include +#include "boost/optional.hpp" -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC -#include +#include "tf2_eigen/tf2_eigen.h" #else -#include +#include "tf2_eigen/tf2_eigen.hpp" #endif -#include -#include - namespace { boost::optional getTransform( @@ -61,7 +60,8 @@ PointCloudDensification::PointCloudDensification(const DensificationParam & para } bool PointCloudDensification::enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer) + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream) { const auto header = pointcloud_msg.header; @@ -73,9 +73,9 @@ bool PointCloudDensification::enqueuePointCloud( } auto affine_world2current = transformToEigen(transform_world2current.get()); - enqueue(pointcloud_msg, affine_world2current); + enqueue(pointcloud_msg, affine_world2current, stream); } else { - enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); + enqueue(pointcloud_msg, Eigen::Affine3f::Identity(), stream); } dequeue(); @@ -84,12 +84,24 @@ bool PointCloudDensification::enqueuePointCloud( } void PointCloudDensification::enqueue( - const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current) + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current, + cudaStream_t stream) { affine_world2current_ = affine_world2current; current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); - PointCloudWithTransform pointcloud = {msg, affine_world2current.inverse()}; - pointcloud_cache_.push_front(pointcloud); + + assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 0); + auto points_d = cuda::make_unique( + sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); + CHECK_CUDA_ERROR(cudaMemcpyAsync( + points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, + cudaMemcpyHostToDevice, stream)); + + PointCloudWithTransform pointcloud = { + std::move(points_d), msg.header, msg.width * msg.height, msg.point_step, + affine_world2current.inverse()}; + + pointcloud_cache_.push_front(std::move(pointcloud)); } void PointCloudDensification::dequeue() diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 218aaee125c02..4e78176739c26 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -28,9 +28,11 @@ * limitations under the License. */ +#include "lidar_centerpoint/cuda_utils.hpp" #include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" +#include "lidar_centerpoint/utils.hpp" -#include +#include namespace { @@ -41,6 +43,51 @@ const std::size_t ENCODER_IN_FEATURE_SIZE = 9; // the same as encoder_in_featur namespace centerpoint { + +__global__ void generateSweepPoints_kernel( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, int num_features, float * output_points) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + const float input_x = input_points[point_idx * input_point_step + 0]; + const float input_y = input_points[point_idx * input_point_step + 1]; + const float input_z = input_points[point_idx * input_point_step + 2]; + + // transform_array is expected to be column-major + output_points[point_idx * num_features + 0] = transform_array[0] * input_x + + transform_array[4] * input_y + + transform_array[8] * input_z + transform_array[12]; + output_points[point_idx * num_features + 1] = transform_array[1] * input_x + + transform_array[5] * input_y + + transform_array[9] * input_z + transform_array[13]; + output_points[point_idx * num_features + 2] = transform_array[2] * input_x + + transform_array[6] * input_y + + transform_array[10] * input_z + transform_array[14]; + output_points[point_idx * num_features + 3] = time_lag; +} + +cudaError_t generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, int num_features, float * output_points, cudaStream_t stream) +{ + auto transform_d = cuda::make_unique(16); + CHECK_CUDA_ERROR(cudaMemcpyAsync( + transform_d.get(), transform_array, 16 * sizeof(float), cudaMemcpyHostToDevice, stream)); + + dim3 blocks((points_size + 256 - 1) / 256); + dim3 threads(256); + assert(num_features == 4); + + generateSweepPoints_kernel<<>>( + input_points, points_size, input_point_step, time_lag, transform_d.get(), num_features, + output_points); + + cudaError_t err = cudaGetLastError(); + return err; +} + __global__ void generateVoxels_random_kernel( const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index 07a1a2de725f5..e90474fa07327 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -14,7 +14,12 @@ #include "lidar_centerpoint/preprocess/voxel_generator.hpp" -#include +#include "lidar_centerpoint/centerpoint_trt.hpp" +#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" + +#include "sensor_msgs/point_cloud2_iterator.hpp" + +#include namespace centerpoint { @@ -38,35 +43,40 @@ VoxelGeneratorTemplate::VoxelGeneratorTemplate( } bool VoxelGeneratorTemplate::enqueuePointCloud( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer) + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, + cudaStream_t stream) { - return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer); + return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream); } -std::size_t VoxelGenerator::generateSweepPoints(std::vector & points) +std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t stream) { - Eigen::Vector3f point_current, point_past; - size_t point_counter{}; + size_t point_counter = 0; for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); pc_cache_iter++) { - auto pc_msg = pc_cache_iter->pointcloud_msg; + auto sweep_num_points = pc_cache_iter->num_points; + auto point_step = pc_cache_iter->point_step; auto affine_past2current = pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; float time_lag = static_cast( - pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); - - for (sensor_msgs::PointCloud2ConstIterator x_iter(pc_msg, "x"), y_iter(pc_msg, "y"), - z_iter(pc_msg, "z"); - x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter) { - point_past << *x_iter, *y_iter, *z_iter; - point_current = affine_past2current * point_past; + pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); - points.at(point_counter * config_.point_feature_size_) = point_current.x(); - points.at(point_counter * config_.point_feature_size_ + 1) = point_current.y(); - points.at(point_counter * config_.point_feature_size_ + 2) = point_current.z(); - points.at(point_counter * config_.point_feature_size_ + 3) = time_lag; - ++point_counter; + if (point_counter + sweep_num_points > CAPACITY_POINT) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_centerpoint"), + "Requested number of points exceeds the maximum capacity. Current points = " + << point_counter); + break; } + + static_assert(std::is_same::value); + static_assert(!Eigen::Matrix4f::IsRowMajor, "matrices should be col-major."); + generateSweepPoints_launch( + pc_cache_iter->points_d.get(), sweep_num_points, point_step / sizeof(float), time_lag, + affine_past2current.matrix().data(), config_.point_feature_size_, + points_d + config_.point_feature_size_ * point_counter, stream); + + point_counter += sweep_num_points; } return point_counter; } diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index b65c70f02bd1b..5c0931196675a 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -14,17 +14,7 @@ #include "lidar_centerpoint/node.hpp" -#include -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif +#include "pcl_ros/transforms.hpp" #include #include @@ -33,6 +23,17 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#else +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#endif + +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" +#include "lidar_centerpoint/ros_utils.hpp" +#include "lidar_centerpoint/utils.hpp" + namespace centerpoint { LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_options) diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp index d0c9123da9e77..8f35b98119afe 100644 --- a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp @@ -31,17 +31,17 @@ class PostprocessKernelTest : public testing::Test void SetUp() override; void TearDown() override; - cudaStream_t stream_{nullptr}; + cudaStream_t stream_{}; - std::unique_ptr postprocess_cuda_ptr_; - std::unique_ptr config_ptr_; + std::unique_ptr postprocess_cuda_ptr_{}; + std::unique_ptr config_ptr_{}; - cuda::unique_ptr head_out_heatmap_d_; - cuda::unique_ptr head_out_offset_d_; - cuda::unique_ptr head_out_z_d_; - cuda::unique_ptr head_out_dim_d_; - cuda::unique_ptr head_out_rot_d_; - cuda::unique_ptr head_out_vel_d_; + cuda::unique_ptr head_out_heatmap_d_{}; + cuda::unique_ptr head_out_offset_d_{}; + cuda::unique_ptr head_out_z_d_{}; + cuda::unique_ptr head_out_dim_d_{}; + cuda::unique_ptr head_out_rot_d_{}; + cuda::unique_ptr head_out_vel_d_{}; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp index 57ff51966a417..1a3e850026886 100644 --- a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp @@ -28,7 +28,7 @@ class PreprocessKernelTest : public testing::Test void SetUp() override; void TearDown() override; - cudaStream_t stream_{nullptr}; + cudaStream_t stream_{}; std::size_t max_voxel_size_{}; std::size_t max_point_in_voxel_size_{}; diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.cpp b/perception/lidar_centerpoint/test/test_voxel_generator.cpp index 5b0b3cd112e6c..6bded105536dc 100644 --- a/perception/lidar_centerpoint/test/test_voxel_generator.cpp +++ b/perception/lidar_centerpoint/test/test_voxel_generator.cpp @@ -14,9 +14,9 @@ #include "test_voxel_generator.hpp" -#include +#include "gtest/gtest.h" -#include +#include "sensor_msgs/point_cloud2_iterator.hpp" void VoxelGeneratorTest::SetUp() { @@ -95,6 +95,8 @@ void VoxelGeneratorTest::SetUp() transform2_ = transform1_; transform2_.header.stamp = cloud2_->header.stamp; transform2_.transform.translation.x = world_origin_x + delta_pointcloud_x_; + + cudaStreamCreate(&stream_); } void VoxelGeneratorTest::TearDown() @@ -117,8 +119,17 @@ TEST_F(VoxelGeneratorTest, SingleFrame) points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); - bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); - std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + auto points_d = cuda::make_unique(capacity_ * config.point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_, stream_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points_d.get(), stream_); + + cudaMemcpy( + points.data(), points_d.get(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); EXPECT_TRUE(status1); EXPECT_EQ(points_per_pointcloud_, generated_points_num); @@ -155,9 +166,18 @@ TEST_F(VoxelGeneratorTest, TwoFramesNoTf) points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); - bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); - bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); - std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + auto points_d = cuda::make_unique(capacity_ * config.point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_, stream_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_, stream_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points_d.get(), stream_); + + cudaMemcpy( + points.data(), points_d.get(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); EXPECT_FALSE(status1); EXPECT_FALSE(status2); @@ -180,12 +200,21 @@ TEST_F(VoxelGeneratorTest, TwoFrames) points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); + auto points_d = cuda::make_unique(capacity_ * config.point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + tf2_buffer_->setTransform(transform1_, "authority1"); tf2_buffer_->setTransform(transform2_, "authority1"); - bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); - bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); - std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_, stream_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_, stream_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points_d.get(), stream_); + + cudaMemcpy( + points.data(), points_d.get(), capacity_ * config.point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); EXPECT_TRUE(status1); EXPECT_TRUE(status2); diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.hpp b/perception/lidar_centerpoint/test/test_voxel_generator.hpp index 8fb7d8dffa44d..48355b8a331ba 100644 --- a/perception/lidar_centerpoint/test/test_voxel_generator.hpp +++ b/perception/lidar_centerpoint/test/test_voxel_generator.hpp @@ -33,34 +33,36 @@ class VoxelGeneratorTest : public testing::Test void TearDown() override; // These need to be public so that they can be accessed in the test cases - rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr node_{}; - std::unique_ptr cloud1_, cloud2_; - geometry_msgs::msg::TransformStamped transform1_, transform2_; + std::unique_ptr cloud1_{}, cloud2_{}; + geometry_msgs::msg::TransformStamped transform1_{}, transform2_{}; - std::unique_ptr densification_param_ptr_; - std::unique_ptr config_ptr_; + std::unique_ptr densification_param_ptr_{}; + std::unique_ptr config_ptr_{}; - std::unique_ptr tf2_buffer_; + std::unique_ptr tf2_buffer_{}; - std::string world_frame_; - std::string lidar_frame_; - std::size_t points_per_pointcloud_; - std::size_t capacity_; - double delta_pointcloud_x_; + std::string world_frame_{}; + std::string lidar_frame_{}; + std::size_t points_per_pointcloud_{}; + std::size_t capacity_{}; + double delta_pointcloud_x_{}; - std::size_t class_size_; - float point_feature_size_; - std::size_t max_voxel_size_; + std::size_t class_size_{}; + float point_feature_size_{}; + std::size_t max_voxel_size_{}; - std::vector point_cloud_range_; - std::vector voxel_size_; - std::size_t downsample_factor_; - std::size_t encoder_in_feature_size_; - float score_threshold_; - float circle_nms_dist_threshold_; - std::vector yaw_norm_thresholds_; - bool has_variance_; + std::vector point_cloud_range_{}; + std::vector voxel_size_{}; + std::size_t downsample_factor_{}; + std::size_t encoder_in_feature_size_{}; + float score_threshold_{}; + float circle_nms_dist_threshold_{}; + std::vector yaw_norm_thresholds_{}; + bool has_variance_{}; + + cudaStream_t stream_{}; }; #endif // TEST_VOXEL_GENERATOR_HPP_