From ca1370df58c032ed0e79911167ffac08687e65ec Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Sun, 12 May 2024 23:41:15 +0900 Subject: [PATCH 1/8] feat: accelerated preprocessing for centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa --- .../preprocess/pointcloud_densification.hpp | 13 +++-- .../preprocess/preprocess_kernel.hpp | 4 ++ .../preprocess/voxel_generator.hpp | 7 +-- .../lidar_centerpoint/lib/centerpoint_trt.cpp | 7 +-- .../preprocess/pointcloud_densification.cpp | 25 +++++++--- .../lib/preprocess/preprocess_kernel.cu | 49 ++++++++++++++++++- .../lib/preprocess/voxel_generator.cpp | 33 ++++++------- 7 files changed, 101 insertions(+), 37 deletions(-) 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..e24d6d387d14b 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp @@ -15,6 +15,8 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#include + #include #include #ifdef ROS_DISTRO_GALACTIC @@ -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 streams); 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..f5f2b3402f0e8 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp @@ -20,6 +20,10 @@ 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..fac34c94f5a4d 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp @@ -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/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 2804e172b73fa..0b37ab8d4a99d 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -130,14 +130,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/preprocess/pointcloud_densification.cpp b/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp index 66c348fda50b4..de0d8f7ed1689 100644 --- a/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp @@ -61,7 +61,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 +74,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 +85,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 118e31f892d72..670a7c7836cac 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -28,10 +28,11 @@ * limitations under the License. */ -#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" - +#include +#include #include +#include namespace { const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32; // the same as max_point_in_voxel_size_ in config @@ -41,6 +42,50 @@ 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]; + + output_points[point_idx * num_features + 0] = transform_array[0] * input_x + + transform_array[1] * input_y + + transform_array[2] * input_z + transform_array[3]; + output_points[point_idx * num_features + 1] = transform_array[4] * input_x + + transform_array[5] * input_y + + transform_array[6] * input_z + transform_array[7]; + output_points[point_idx * num_features + 2] = transform_array[8] * input_x + + transform_array[9] * input_y + + transform_array[10] * input_z + transform_array[11]; + 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..dfca54fcd642d 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -14,6 +14,8 @@ #include "lidar_centerpoint/preprocess/voxel_generator.hpp" +#include + #include namespace centerpoint @@ -38,35 +40,30 @@ 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()); + pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->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; + 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 + point_counter, + stream); - 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; - } + point_counter += sweep_num_points; } return point_counter; } From f3187e9ce8364e86d4539b3ef1fa65223f6a35b9 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 3 Jun 2024 15:06:00 +0900 Subject: [PATCH 2/8] chore: changed includes to quotes instead of brackets Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lidar_centerpoint/centerpoint_trt.hpp | 17 ++++++------- .../include/lidar_centerpoint/cuda_utils.hpp | 2 +- .../detection_class_remapper.hpp | 8 +++--- .../lidar_centerpoint/network/network_trt.hpp | 4 +-- .../network/scatter_kernel.hpp | 4 +-- .../network/tensorrt_wrapper.hpp | 7 +++--- .../include/lidar_centerpoint/node.hpp | 25 +++++++++---------- .../postprocess/circle_nms_kernel.hpp | 5 ++-- .../postprocess/postprocess_kernel.hpp | 11 ++++---- .../preprocess/pointcloud_densification.hpp | 16 ++++++------ .../preprocess/preprocess_kernel.hpp | 4 +-- .../preprocess/voxel_generator.hpp | 6 ++--- .../include/lidar_centerpoint/ros_utils.hpp | 10 ++++---- .../lidar_centerpoint/lib/centerpoint_trt.cpp | 8 +++--- .../lib/detection_class_remapper.cpp | 2 +- .../lib/network/scatter_kernel.cu | 3 +-- .../lib/network/tensorrt_wrapper.cpp | 2 +- .../lib/postprocess/circle_nms_kernel.cu | 8 +++--- .../postprocess/non_maximum_suppression.cpp | 1 + .../lib/postprocess/postprocess_kernel.cu | 10 +++----- .../preprocess/pointcloud_densification.cpp | 15 ++++++----- .../lib/preprocess/preprocess_kernel.cu | 7 +++--- .../lib/preprocess/voxel_generator.cpp | 4 +-- perception/lidar_centerpoint/src/node.cpp | 23 +++++++++-------- 24 files changed, 97 insertions(+), 105 deletions(-) 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 18a78f122e170..2bd3c87b734fe 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_auto_perception_msgs/msg/detected_object_kinematics.hpp" +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_auto_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..c1de417e4c7d9 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 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index d9740df515db2..42437559857d7 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -15,20 +15,19 @@ #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 -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "tier4_autoware_utils/ros/published_time_publisher.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp" +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_auto_perception_msgs/msg/shape.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #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 e24d6d387d14b..f314ea3bec615 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp @@ -15,19 +15,19 @@ #ifndef LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ #define LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#include +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC -#include +#include "tf2_sensor_msgs/tf2_sensor_msgs.h" #else -#include +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" #endif -#include -#include -#include +#include "lidar_centerpoint/cuda_utils.hpp" namespace centerpoint { 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 f5f2b3402f0e8..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,8 +15,8 @@ #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 { 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 fac34c94f5a4d..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 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index 4eb4318c4f3f9..a41d01d10d1d4 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -17,12 +17,12 @@ // ros packages cannot be included from cuda. -#include +#include "lidar_centerpoint/utils.hpp" -#include -#include -#include -#include +#include "autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp" +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_auto_perception_msgs/msg/shape.hpp" #include #include diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 0b37ab8d4a99d..45a4dbb62ef86 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -14,10 +14,10 @@ #include "lidar_centerpoint/centerpoint_trt.hpp" -#include -#include -#include -#include +#include "lidar_centerpoint/centerpoint_config.hpp" +#include "lidar_centerpoint/network/scatter_kernel.hpp" +#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" +#include "tier4_autoware_utils/math/constants.hpp" #include #include diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp index 4637dc5537f37..fc0d67b9385eb 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 d4d7b1379776a..d1de14ff823f0 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -17,6 +17,7 @@ #include "object_recognition_utils/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" + 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 de0d8f7ed1689..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( diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 670a7c7836cac..69f94f238b2ce 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -28,11 +28,12 @@ * limitations under the License. */ -#include -#include -#include +#include "lidar_centerpoint/cuda_utils.hpp" +#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" +#include "lidar_centerpoint/utils.hpp" #include + namespace { const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32; // the same as max_point_in_voxel_size_ in config diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index dfca54fcd642d..d86a2e2ad5d33 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -14,9 +14,9 @@ #include "lidar_centerpoint/preprocess/voxel_generator.hpp" -#include +#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" -#include +#include "sensor_msgs/point_cloud2_iterator.hpp" namespace centerpoint { diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index d0abf72d888d3..8f05f99600243 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) From 0705553d0de04e2ad398527af1603631ad57b096 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 3 Jun 2024 15:09:19 +0900 Subject: [PATCH 3/8] chore: fixed variable name Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lidar_centerpoint/preprocess/pointcloud_densification.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f314ea3bec615..c1d6a6b060955 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp @@ -64,7 +64,7 @@ class PointCloudDensification bool enqueuePointCloud( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - cudaStream_t streams); + cudaStream_t stream); double getCurrentTimestamp() const { return current_timestamp_; } Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } From 80112b1be14753f94cde12910e28d1b7fda91366 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 3 Jun 2024 15:37:19 +0900 Subject: [PATCH 4/8] fix: addressed the case where the points in the cache is over the maximum capacity Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lidar_centerpoint/lib/preprocess/voxel_generator.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index d86a2e2ad5d33..41c58536dac6e 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -14,6 +14,7 @@ #include "lidar_centerpoint/preprocess/voxel_generator.hpp" +#include "lidar_centerpoint/centerpoint_trt.hpp" #include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -58,6 +59,14 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s float time_lag = static_cast( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); + 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; + } + 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 + point_counter, From d0ea5f0ae60b084d19aa12b11a22d38f2f878569 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 3 Jun 2024 15:52:08 +0900 Subject: [PATCH 5/8] fix: fixed the offset of the output buffer in the kernel Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lidar_centerpoint/lib/preprocess/voxel_generator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index 41c58536dac6e..ab862d59f084f 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -69,8 +69,8 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s 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 + point_counter, - stream); + affine_past2current.matrix().data(), config_.point_feature_size_, + points_d + config_.point_feature_size_ * point_counter, stream); point_counter += sweep_num_points; } From 1b0c0608279e8c5cf74c97e442438c8c462a75a7 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 3 Jun 2024 16:49:06 +0900 Subject: [PATCH 6/8] chore: reimplemented old logic in the pointpainting package since the new centerpoint logic is incompatible Signed-off-by: Kenzo Lobos-Tsunekawa --- .../CMakeLists.txt | 1 + .../pointcloud_densification.hpp | 72 ++++++++++++ .../pointpainting_fusion/voxel_generator.hpp | 22 +++- .../pointcloud_densification.cpp | 103 ++++++++++++++++++ .../pointpainting_fusion/voxel_generator.cpp | 26 +++++ 5 files changed, 221 insertions(+), 3 deletions(-) create mode 100644 perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp mode change 100755 => 100644 perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp create mode 100644 perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index b13f68b07181e..a94efbb326989 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; From af3993f1c97158dc8e9f013b52c950a3002d1ed7 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 6 Jun 2024 10:54:17 +0900 Subject: [PATCH 7/8] fix: eigen matrices are column major by default. this commit fixes the code that assumed row-major, implements check for data layout, and updates tests Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lib/preprocess/preprocess_kernel.cu | 15 +++--- .../lib/preprocess/voxel_generator.cpp | 4 ++ .../test/test_postprocess_kernel.hpp | 18 +++---- .../test/test_preprocess_kernel.hpp | 2 +- .../test/test_voxel_generator.cpp | 49 +++++++++++++++---- .../test/test_voxel_generator.hpp | 46 ++++++++--------- 6 files changed, 85 insertions(+), 49 deletions(-) diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index daae4a331a476..4e78176739c26 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -55,15 +55,16 @@ __global__ void generateSweepPoints_kernel( 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[1] * input_y + - transform_array[2] * input_z + transform_array[3]; - output_points[point_idx * num_features + 1] = transform_array[4] * 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[6] * input_z + transform_array[7]; - output_points[point_idx * num_features + 2] = transform_array[8] * input_x + - transform_array[9] * input_y + - transform_array[10] * input_z + transform_array[11]; + 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; } diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index ab862d59f084f..e90474fa07327 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -19,6 +19,8 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" +#include + namespace centerpoint { VoxelGeneratorTemplate::VoxelGeneratorTemplate( @@ -67,6 +69,8 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s 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_, 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_ From 27c03cf8f9ec7beba6de549f82ee527e7d0aaef1 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 10 Jun 2024 15:21:27 +0900 Subject: [PATCH 8/8] chore: fixed missing virtual destructor Signed-off-by: Kenzo Lobos-Tsunekawa --- .../include/lidar_centerpoint/network/tensorrt_wrapper.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c1de417e4c7d9..4af5aac7ff7fe 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp @@ -31,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);