feat(centerpoint): cherrypicked upstream changes for x2_gen2/v0.29.0

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/config/pointpainting.param.yaml

@@ -5,6 +5,7 @@
     head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
     head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
     trt_precision: fp16
+    cloud_capacity: 2000000
     post_process_params:
       # post-process params
       circle_nms_dist_threshold: 0.3

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/pointpainting_trt.hpp

@@ -23,7 +23,6 @@
 
 namespace image_projection_based_fusion
 {
-static constexpr size_t CAPACITY_POINT = 2000000;
 class PointPaintingTRT : public centerpoint::CenterPointTRT
 {
 public:

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/node.cpp

@@ -110,6 +110,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
     this->declare_parameter<int>("densification_params.num_past_frames");
   // network param
   const std::string trt_precision = this->declare_parameter<std::string>("trt_precision");
+  const std::size_t cloud_capacity = this->declare_parameter<std::int64_t>("cloud_capacity");
   const std::string encoder_onnx_path = this->declare_parameter<std::string>("encoder_onnx_path");
   const std::string encoder_engine_path =
     this->declare_parameter<std::string>("encoder_engine_path");
@@ -188,9 +189,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
   centerpoint::DensificationParam densification_param(
     densification_world_frame_id, densification_num_past_frames);
   centerpoint::CenterPointConfig config(
-    class_names_.size(), point_feature_size, max_voxel_size, pointcloud_range, voxel_size,
-    downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold,
-    yaw_norm_thresholds, has_variance_);
+    class_names_.size(), point_feature_size, cloud_capacity, max_voxel_size, pointcloud_range,
+    voxel_size, downsample_factor, encoder_in_feature_size, score_threshold,
+    circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_);
 
   // create detector
   detector_ptr_ = std::make_unique<image_projection_based_fusion::PointPaintingTRT>(

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/README.md

@@ -28,6 +28,22 @@ We trained the models using <https://github.com/open-mmlab/mmdetection3d>.
 
 ## Parameters
 
+### ML Model Parameters
+
+Note that these parameters are associated with ONNX file, predefined during the training phase. Be careful to change ONNX file as well when changing this parameter. Also, whenever you update the ONNX file, do NOT forget to check these values.
+
+| Name                                   | Type         | Default Value                                    | Description                                                           |
+| -------------------------------------- | ------------ | ------------------------------------------------ | --------------------------------------------------------------------- |
+| `model_params.class_names`             | list[string] | ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] | list of class names for model outputs                                 |
+| `model_params.point_feature_size`      | int          | `4`                                              | number of features per point in the point cloud                       |
+| `model_params.max_voxel_size`          | int          | `40000`                                          | maximum number of voxels                                              |
+| `model_params.point_cloud_range`       | list[double] | [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0]            | detection range [min_x, min_y, min_z, max_x, max_y, max_z] [m]        |
+| `model_params.voxel_size`              | list[double] | [0.32, 0.32, 10.0]                               | size of each voxel [x, y, z] [m]                                      |
+| `model_params.downsample_factor`       | int          | `1`                                              | downsample factor for coordinates                                     |
+| `model_params.encoder_in_feature_size` | int          | `9`                                              | number of input features to the encoder                               |
+| `model_params.has_variance`            | bool         | `false`                                          | true if the model outputs pose variance as well as pose for each bbox |
+| `model_params.has_twist`               | bool         | `false`                                          | true if the model outputs velocity as well as pose for each bbox      |
+
 ### Core Parameters
 
 | Name                                             | Type         | Default Value             | Description                                                   |

perception/lidar_centerpoint/config/centerpoint.param.yaml

@@ -6,6 +6,7 @@
     head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
     head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
     trt_precision: fp16
+    cloud_capacity: 2000000
     post_process_params:
       # post-process params
       circle_nms_dist_threshold: 0.5

perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml

@@ -0,0 +1,21 @@
+/**:
+  ros__parameters:
+
+    # weight files
+    encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"
+    encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine"
+    head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
+    head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
+    trt_precision: fp16
+    cloud_capacity: 2000000
+    post_process_params:
+      # post-process params
+      circle_nms_dist_threshold: 0.5
+      iou_nms_target_class_names: ["CAR"]
+      iou_nms_search_distance_2d: 10.0
+      iou_nms_threshold: 0.1
+      yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
+      score_threshold: 0.35
+    densification_params:
+      world_frame_id: map
+      num_past_frames: 1

perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml

@@ -6,6 +6,7 @@
     head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
     head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
     trt_precision: fp16
+    cloud_capacity: 2000000
     post_process_params:
       # post-process params
       circle_nms_dist_threshold: 0.5

perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp

@@ -24,14 +24,16 @@ class CenterPointConfig
 {
 public:
   explicit CenterPointConfig(
-    const std::size_t class_size, const float point_feature_size, const std::size_t max_voxel_size,
-    const std::vector<double> & point_cloud_range, const std::vector<double> & voxel_size,
-    const std::size_t downsample_factor, const std::size_t encoder_in_feature_size,
-    const float score_threshold, const float circle_nms_dist_threshold,
-    const std::vector<double> yaw_norm_thresholds, const bool has_variance)
+    const std::size_t class_size, const float point_feature_size, const std::size_t cloud_capacity,
+    const std::size_t max_voxel_size, const std::vector<double> & point_cloud_range,
+    const std::vector<double> & voxel_size, const std::size_t downsample_factor,
+    const std::size_t encoder_in_feature_size, const float score_threshold,
+    const float circle_nms_dist_threshold, const std::vector<double> yaw_norm_thresholds,
+    const bool has_variance)
   {
     class_size_ = class_size;
     point_feature_size_ = point_feature_size;
+    cloud_capacity_ = cloud_capacity;
     max_voxel_size_ = max_voxel_size;
     if (point_cloud_range.size() == 6) {
       range_min_x_ = static_cast<float>(point_cloud_range[0]);
@@ -85,6 +87,7 @@ class CenterPointConfig
   };
 
   // input params
+  std::size_t cloud_capacity_{};
   std::size_t class_size_{3};
   const std::size_t point_dim_size_{3};  // x, y and z
   std::size_t point_feature_size_{4};    // x, y, z and time-lag