From 2c763b8a86b9588b918391e4a59f908e73b93761 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Wed, 17 Jul 2024 17:10:04 +0900 Subject: [PATCH] fix: fixed compilation errors after cherrypicks fix: ml detector buffer capacity and parameterization (#7936) * fix: increased the buffer capacity in ML-based detectors (1M -> 2M due to new models) but parameterized to users can adapt it to their use cases Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added schema parts Signed-off-by: Kenzo Lobos-Tsunekawa * chore: points are countable ! Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa --- .../config/pointpainting.param.yaml | 1 + .../pointpainting_fusion/pointpainting_trt.hpp | 1 - .../src/pointpainting_fusion/node.cpp | 7 ++++--- .../lidar_centerpoint/config/centerpoint.param.yaml | 1 + .../config/centerpoint_tiny.param.yaml | 1 + .../lidar_centerpoint/centerpoint_config.hpp | 13 ++++++++----- .../include/lidar_centerpoint/centerpoint_trt.hpp | 2 -- .../lidar_centerpoint/lib/centerpoint_trt.cpp | 4 ++-- .../lib/preprocess/voxel_generator.cpp | 2 +- .../schema/centerpoint.schemal.json | 6 ++++++ perception/lidar_centerpoint/src/node.cpp | 7 ++++--- .../lidar_transfusion/config/transfusion.param.yaml | 1 + .../lidar_transfusion/transfusion_config.hpp | 9 ++++++--- .../schema/transfusion.schema.json | 6 ++++++ .../src/lidar_transfusion_node.cpp | 8 ++++++-- 15 files changed, 47 insertions(+), 22 deletions(-) diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 30136bc8f47d0..9227e4fa9319a 100644 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/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 diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index d5892de6d7edb..c8318f79bbeed 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/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: diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 3fb2aca2171f7..4df660e65a8c1 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -110,6 +110,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt this->declare_parameter("densification_params.num_past_frames"); // network param const std::string trt_precision = this->declare_parameter("trt_precision"); + const std::size_t cloud_capacity = this->declare_parameter("cloud_capacity"); const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); const std::string encoder_engine_path = this->declare_parameter("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( diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index 17ed9a447cdb8..53d77e8d1c42c 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/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 diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 17ed9a447cdb8..53d77e8d1c42c 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/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 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 19fdbe7a8b9c2..7b560cf46e2e3 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/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 & point_cloud_range, const std::vector & 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 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 & point_cloud_range, + const std::vector & 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 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(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 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp index df8100fb6b80e..52ae86951c7cf 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -31,8 +31,6 @@ namespace centerpoint { -static constexpr size_t CAPACITY_POINT = 2000000; - class NetworkParam { public: diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 0e3791e061f62..7047bf25711de 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -85,7 +85,7 @@ void CenterPointTRT::initPtr() mask_size_ = config_.grid_size_x_ * config_.grid_size_y_; // host - points_.resize(CAPACITY_POINT * config_.point_feature_size_); + points_.resize(config_.cloud_capacity_ * config_.point_feature_size_); // device voxels_d_ = cuda::make_unique(voxels_size_); @@ -100,7 +100,7 @@ void CenterPointTRT::initPtr() head_out_dim_d_ = cuda::make_unique(grid_xy_size * config_.head_out_dim_size_); head_out_rot_d_ = cuda::make_unique(grid_xy_size * config_.head_out_rot_size_); head_out_vel_d_ = cuda::make_unique(grid_xy_size * config_.head_out_vel_size_); - points_d_ = cuda::make_unique(CAPACITY_POINT * config_.point_feature_size_); + points_d_ = cuda::make_unique(config_.cloud_capacity_ * config_.point_feature_size_); voxels_buffer_d_ = cuda::make_unique(voxels_buffer_size_); mask_d_ = cuda::make_unique(mask_size_); num_voxels_d_ = cuda::make_unique(1); diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index e90474fa07327..8e91c3b0b36cc 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -61,7 +61,7 @@ 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) { + if (point_counter + sweep_num_points > config_.cloud_capacity_) { RCLCPP_WARN_STREAM( rclcpp::get_logger("lidar_centerpoint"), "Requested number of points exceeds the maximum capacity. Current points = " diff --git a/perception/lidar_centerpoint/schema/centerpoint.schemal.json b/perception/lidar_centerpoint/schema/centerpoint.schemal.json index b11c115cc2466..c3268f0e90d9b 100644 --- a/perception/lidar_centerpoint/schema/centerpoint.schemal.json +++ b/perception/lidar_centerpoint/schema/centerpoint.schemal.json @@ -12,6 +12,12 @@ "default": "fp16", "enum": ["fp32", "fp16"] }, + "cloud_capacity": { + "type": "integer", + "description": "Capacity of the point cloud buffer (should be set to at least the maximum theoretical number of points).", + "default": 2000000, + "minimum": 1 + }, "post_process_params": { "type": "object", "properties": { diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 8bf62e3e0168b..53554d0a3becf 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -50,6 +50,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const int densification_num_past_frames = this->declare_parameter("densification_params.num_past_frames"); const std::string trt_precision = this->declare_parameter("trt_precision"); + const std::size_t cloud_capacity = this->declare_parameter("cloud_capacity"); const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path"); @@ -104,9 +105,9 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti "The size of voxel_size != 3: use the default parameters."); } CenterPointConfig config( - class_names_.size(), point_feature_size, max_voxel_size, point_cloud_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, point_cloud_range, + voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, + circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/lidar_transfusion/config/transfusion.param.yaml index feabe359caf1f..2f1d56872dad3 100644 --- a/perception/lidar_transfusion/config/transfusion.param.yaml +++ b/perception/lidar_transfusion/config/transfusion.param.yaml @@ -3,6 +3,7 @@ # network class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] trt_precision: fp16 + cloud_capacity: 2000000 voxels_num: [5000, 30000, 60000] # [min, opt, max] point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] # [x_min, y_min, z_min, x_max, y_max, z_max] voxel_size: [0.3, 0.3, 8.0] # [x, y, z] diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp index 31976de56a9da..c6a55d1043a92 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp @@ -25,10 +25,13 @@ class TransfusionConfig { public: TransfusionConfig( - const std::vector & voxels_num, const std::vector & point_cloud_range, - const std::vector & voxel_size, const float circle_nms_dist_threshold, + const std::size_t cloud_capacity, const std::vector & voxels_num, + const std::vector & point_cloud_range, const std::vector & voxel_size, + const std::size_t num_proposals, const float circle_nms_dist_threshold, const std::vector & yaw_norm_thresholds, const float score_threshold) { + cloud_capacity_ = cloud_capacity; + if (voxels_num.size() == 3) { max_voxels_ = voxels_num[2]; @@ -79,7 +82,7 @@ class TransfusionConfig } ///// INPUT PARAMETERS ///// - const std::size_t cloud_capacity_{1000000}; + std::size_t cloud_capacity_{}; ///// KERNEL PARAMETERS ///// const std::size_t threads_for_voxel_{256}; // threads number for a block const std::size_t points_per_voxel_{20}; diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/lidar_transfusion/schema/transfusion.schema.json index 41d8d887236a8..0bbbdc888a261 100644 --- a/perception/lidar_transfusion/schema/transfusion.schema.json +++ b/perception/lidar_transfusion/schema/transfusion.schema.json @@ -21,6 +21,12 @@ "default": "fp16", "enum": ["fp16", "fp32"] }, + "cloud_capacity": { + "type": "integer", + "description": "Capacity of the point cloud buffer (should be set to at least the maximum theoretical number of points).", + "default": 2000000, + "minimum": 1 + }, "voxels_num": { "type": "array", "items": { diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp index e3ea6b3780de8..36cd7e2693413 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -27,10 +27,14 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) class_names_ = this->declare_parameter>("class_names", descriptor); const std::string trt_precision = this->declare_parameter("trt_precision", descriptor); + const std::size_t cloud_capacity = + this->declare_parameter("cloud_capacity", descriptor); const auto voxels_num = this->declare_parameter>("voxels_num", descriptor); const auto point_cloud_range = this->declare_parameter>("point_cloud_range", descriptor); const auto voxel_size = this->declare_parameter>("voxel_size", descriptor); + const std::size_t num_proposals = + this->declare_parameter("num_proposals", descriptor); const std::string onnx_path = this->declare_parameter("onnx_path", descriptor); const std::string engine_path = this->declare_parameter("engine_path", descriptor); @@ -73,8 +77,8 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); TransfusionConfig config( - voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, - score_threshold); + voxels_num, point_cloud_range, voxel_size, num_proposals, circle_nms_dist_threshold, + yaw_norm_thresholds, score_threshold); const auto allow_remapping_by_area_matrix = this->declare_parameter>("allow_remapping_by_area_matrix", descriptor);