feat(lidar_centerpoint): accelerated preprocessing for centerpoint

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
   )

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 <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#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/preprocess/pointcloud_densification.hpp>
+
+#include <list>
+#include <string>
+#include <utility>
+
+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<PointCloudWithTransform>::iterator getPointCloudCacheIter()
+  {
+    return pointcloud_cache_.begin();
+  }
+  bool isCacheEnd(std::list<PointCloudWithTransform>::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<PointCloudWithTransform> pointcloud_cache_;
+};
+
+}  // namespace image_projection_based_fusion
+
+#endif  // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_

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 <image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp>
+#include <lidar_centerpoint/preprocess/pointcloud_densification.hpp>
 #include <lidar_centerpoint/preprocess/voxel_generator.hpp>
 
 #include <bitset>
+#include <memory>
 #include <vector>
 
 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<float> & points);
+
+protected:
+  std::unique_ptr<PointCloudDensification> pd_ptr_{nullptr};
 
-  std::size_t generateSweepPoints(std::vector<float> & points) override;
+  centerpoint::CenterPointConfig config_;
+  std::array<float, 6> range_;
+  std::array<int, 3> grid_size_;
+  std::array<float, 3> recip_voxel_size_;
 };
 }  // namespace image_projection_based_fusion
 

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 <pcl_ros/transforms.hpp>
+
+#include <boost/optional.hpp>
+
+#include <pcl_conversions/pcl_conversions.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+#endif
+
+#include <string>
+#include <utility>
+
+namespace
+{
+boost::optional<geometry_msgs::msg::Transform> 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<float>();
+  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

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<PointCloudDensification>(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<float> & points)
 {
   Eigen::Vector3f point_current, point_past;

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 <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 <sensor_msgs/msg/point_cloud2.hpp>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
+#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 <memory>
 #include <string>

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 <cuda_runtime_api.h>
+#include "cuda_runtime_api.h"
 
 #include <memory>
 #include <sstream>

perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp

@@ -17,10 +17,10 @@
 
 #include <Eigen/Core>
 
-#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 "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 <vector>
 

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 <lidar_centerpoint/centerpoint_config.hpp>
-#include <lidar_centerpoint/network/tensorrt_wrapper.hpp>
+#include "lidar_centerpoint/centerpoint_config.hpp"
+#include "lidar_centerpoint/network/tensorrt_wrapper.hpp"
 
 #include <vector>
 

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 <cuda.h>
-#include <cuda_runtime_api.h>
+#include "cuda.h"
+#include "cuda_runtime_api.h"
 
 namespace centerpoint
 {

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 <lidar_centerpoint/centerpoint_config.hpp>
-#include <tensorrt_common/tensorrt_common.hpp>
-
-#include <NvInfer.h>
+#include "NvInfer.h"
+#include "lidar_centerpoint/centerpoint_config.hpp"
+#include "tensorrt_common/tensorrt_common.hpp"
 
 #include <iostream>
 #include <memory>
@@ -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);

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 <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
-#include <lidar_centerpoint/centerpoint_trt.hpp>
-#include <lidar_centerpoint/detection_class_remapper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>

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 <lidar_centerpoint/utils.hpp>
-
-#include <thrust/device_vector.h>
+#include "lidar_centerpoint/utils.hpp"
+#include "thrust/device_vector.h"
 
 namespace centerpoint
 {

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 <lidar_centerpoint/centerpoint_config.hpp>
-#include <lidar_centerpoint/utils.hpp>
-
-#include <cuda.h>
-#include <cuda_runtime_api.h>
-#include <thrust/device_vector.h>
+#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 <vector>