From c0d2af6bb730e9dc36d658c18386968e361bd5ad Mon Sep 17 00:00:00 2001 From: tzhong518 Date: Tue, 5 Mar 2024 13:35:24 +0900 Subject: [PATCH 01/12] feat: postprocess variance Signed-off-by: tzhong518 --- .../lidar_centerpoint/centerpoint_trt.hpp | 4 +- .../postprocess/postprocess_kernel.hpp | 3 +- .../include/lidar_centerpoint/utils.hpp | 13 +++++++ .../lidar_centerpoint/lib/centerpoint_trt.cpp | 9 +++-- .../lib/postprocess/postprocess_kernel.cu | 39 +++++++++++++++++-- .../lidar_centerpoint/lib/ros_utils.cpp | 19 +++++++++ perception/lidar_centerpoint/src/node.cpp | 17 ++++++-- 7 files changed, 91 insertions(+), 13 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..758915c315a4f 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -65,7 +65,7 @@ class CenterPointTRT bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + std::vector & det_boxes3d, std::vector & det_variance); protected: void initPtr(); @@ -75,7 +75,7 @@ class CenterPointTRT void inference(); - void postProcess(std::vector & det_boxes3d); + void postProcess(std::vector & det_boxes3d, std::vector & det_variance); std::unique_ptr vg_ptr_{nullptr}; std::unique_ptr encoder_trt_ptr_{nullptr}; 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..785d4f59191a5 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -33,12 +33,13 @@ class PostProcessCUDA cudaError_t generateDetectedBoxes3D_launch( const float * out_heatmap, const float * out_offset, const float * out_z, const float * out_dim, - const float * out_rot, const float * out_vel, std::vector & det_boxes3d, + const float * out_rot, const float * out_vel, std::vector & det_boxes3d, std::vector & det_variance, cudaStream_t stream); private: CenterPointConfig config_; thrust::device_vector boxes3d_d_; + thrust::device_vector variance_d_; thrust::device_vector yaw_norm_thresholds_d_; }; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp index 84462bc9657ac..02a31db9d7992 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp @@ -35,6 +35,19 @@ struct Box3D float vel_y; }; +struct Variance +{ + float x_variance; + float y_variance; + float z_variance; + float length_variance; + float width_variance; + float height_variance; + float yaw_variance; + float vel_x_variance; + float vel_y_variance; +}; + // cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b); diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 2804e172b73fa..2ded3db5c042c 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -107,7 +107,7 @@ void CenterPointTRT::initPtr() bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + std::vector & det_boxes3d, std::vector & det_variance) { CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); @@ -122,7 +122,7 @@ bool CenterPointTRT::detect( inference(); - postProcess(det_boxes3d); + postProcess(det_boxes3d, det_variance); return true; } @@ -192,11 +192,12 @@ void CenterPointTRT::inference() head_trt_ptr_->context_->enqueueV2(head_buffers.data(), stream_, nullptr); } -void CenterPointTRT::postProcess(std::vector & det_boxes3d) +void CenterPointTRT::postProcess( + std::vector & det_boxes3d, std::vector & det_variance) { CHECK_CUDA_ERROR(post_proc_ptr_->generateDetectedBoxes3D_launch( head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), - head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_)); + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, det_variance, stream_)); if (det_boxes3d.size() == 0) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); } diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index ea2111862759a..9916b276294a9 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -58,7 +58,7 @@ __global__ void generateBoxes3D_kernel( const float * out_rot, const float * out_vel, const float voxel_size_x, const float voxel_size_y, const float range_min_x, const float range_min_y, const std::size_t down_grid_size_x, const std::size_t down_grid_size_y, const std::size_t downsample_factor, const int class_size, - const float * yaw_norm_thresholds, Box3D * det_boxes3d) + const float * yaw_norm_thresholds, Box3D * det_boxes3d, Variance * det_variance) { // generate boxes3d from the outputs of the network. // shape of out_*: (N, DOWN_GRID_SIZE_Y, DOWN_GRID_SIZE_X) @@ -96,6 +96,20 @@ __global__ void generateBoxes3D_kernel( const float vel_x = out_vel[down_grid_size * 0 + idx]; const float vel_y = out_vel[down_grid_size * 1 + idx]; + const float offset_x_variance = out_offset[down_grid_size * 2 + idx]; + const float offset_y_variance = out_offset[down_grid_size * 3 + idx]; + // const float x_variance = voxel_size_x * downsample_factor * (xi + offset_x) + range_min_x; + // const float y_variance = voxel_size_y * downsample_factor * (yi + offset_y) + range_min_y; + const float z_variance = out_z[idx]; + const float w_variance = out_dim[down_grid_size * 3 + idx]; + const float l_variance = out_dim[down_grid_size * 4 + idx]; + const float h_variance = out_dim[down_grid_size * 5 + idx]; + const float yaw_sin_log_variance = out_rot[down_grid_size * 2 + idx]; + const float yaw_cos_log_variance = out_rot[down_grid_size * 3 + idx]; + // const float yaw_norm_variance = sqrtf(yaw_sin * yaw_sin + yaw_cos * yaw_cos); + const float vel_x_variance = out_vel[down_grid_size * 2 + idx]; + const float vel_y_variance = out_vel[down_grid_size * 3 + idx]; + det_boxes3d[idx].label = label; det_boxes3d[idx].score = yaw_norm >= yaw_norm_thresholds[label] ? max_score : 0.f; det_boxes3d[idx].x = x; @@ -107,6 +121,17 @@ __global__ void generateBoxes3D_kernel( det_boxes3d[idx].yaw = atan2f(yaw_sin, yaw_cos); det_boxes3d[idx].vel_x = vel_x; det_boxes3d[idx].vel_y = vel_y; + + det_variance[idx].x_variance = offset_x_variance; + det_variance[idx].y_variance = offset_y_variance; + det_variance[idx].z_variance = z_variance; + det_variance[idx].length_variance = l_variance; + det_variance[idx].width_variance = w_variance; + det_variance[idx].height_variance = h_variance; + det_variance[idx].yaw_variance = (powf(yaw_cos,2) * expf(yaw_sin_log_variance) + powf(yaw_sin,2) * expf(yaw_cos_log_variance)) / + (powf((powf(yaw_sin,2) + powf(yaw_cos,2)),2)); + det_variance[idx].vel_x_variance = vel_x_variance; + det_variance[idx].vel_y_variance = vel_y_variance; } PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(config) @@ -120,7 +145,7 @@ PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(con // cspell: ignore divup cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( const float * out_heatmap, const float * out_offset, const float * out_z, const float * out_dim, - const float * out_rot, const float * out_vel, std::vector & det_boxes3d, + const float * out_rot, const float * out_vel, std::vector & det_boxes3d, std::vector & det_variance, cudaStream_t stream) { dim3 blocks( @@ -132,7 +157,8 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), - thrust::raw_pointer_cast(boxes3d_d_.data())); + thrust::raw_pointer_cast(boxes3d_d_.data()), + thrust::raw_pointer_cast(variance_d_.data())); // suppress by score const auto num_det_boxes3d = thrust::count_if( @@ -158,10 +184,17 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( thrust::copy_if( thrust::device, det_boxes3d_d.begin(), det_boxes3d_d.end(), final_keep_mask_d.begin(), final_det_boxes3d_d.begin(), is_kept()); + thrust::device_vector final_det_variance_d(num_final_det_boxes3d); + thrust::copy_if( + thrust::device, variance_d_.begin(), variance_d_.end(), final_keep_mask_d.begin(), + final_det_variance_d.begin(), is_kept()); + // memcpy device to host det_boxes3d.resize(num_final_det_boxes3d); thrust::copy(final_det_boxes3d_d.begin(), final_det_boxes3d_d.end(), det_boxes3d.begin()); + det_variance.resize(num_final_det_boxes3d); + thrust::copy(final_det_variance_d.begin(), final_det_variance_d.end(), det_variance.begin()); return cudaGetLastError(); } diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 6bb788ec097b5..5815559f9bd68 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -55,6 +55,7 @@ void box3DToDetectedObject( tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + // obj.kinematrc.pose_with_covariance.covariance = obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); @@ -92,4 +93,22 @@ uint8_t getSemanticType(const std::string & class_name) } } +// std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( +// const radar_msgs::msg::RadarTrack & radar_track) +// { +// using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; +// using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; +// std::array pose_covariance{}; +// pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; +// pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; +// pose_covariance[POSE_IDX::X_Z] = radar_track.position_covariance[RADAR_IDX::X_Z]; +// pose_covariance[POSE_IDX::Y_X] = radar_track.position_covariance[RADAR_IDX::X_Y]; +// pose_covariance[POSE_IDX::Y_Y] = radar_track.position_covariance[RADAR_IDX::Y_Y]; +// pose_covariance[POSE_IDX::Y_Z] = radar_track.position_covariance[RADAR_IDX::Y_Z]; +// pose_covariance[POSE_IDX::Z_X] = radar_track.position_covariance[RADAR_IDX::X_Z]; +// pose_covariance[POSE_IDX::Z_Y] = radar_track.position_covariance[RADAR_IDX::Y_Z]; +// pose_covariance[POSE_IDX::Z_Z] = radar_track.position_covariance[RADAR_IDX::Z_Z]; +// return pose_covariance; +// } + } // namespace centerpoint diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 65bdafe94fe7d..516538e63d8b5 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -143,17 +143,28 @@ void LidarCenterPointNode::pointCloudCallback( } std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); + std::vector det_variance; + bool is_success = + detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d, det_variance); if (!is_success) { return; } + std::cout << "-------------------------det_variance:" << det_variance.size() << std::endl; + // << det_variance[0] << std::endl; + std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); - for (const auto & box3d : det_boxes3d) { + // for (const auto & box3d : det_boxes3d) { + // autoware_auto_perception_msgs::msg::DetectedObject obj; + // box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + // raw_objects.emplace_back(obj); + // } + for (size_t i = 0; i < det_boxes3d.size(); ++i) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + box3DToDetectedObject(det_boxes3d[i], class_names_, has_twist_, obj); raw_objects.emplace_back(obj); + std::cout << i << " /// x_variance:" << det_variance[i].x_variance << std::endl; } autoware_auto_perception_msgs::msg::DetectedObjects output_msg; From d26bc0d17bd8bfb32904077cb27ae9ab9b5425e4 Mon Sep 17 00:00:00 2001 From: tzhong518 Date: Thu, 7 Mar 2024 14:13:00 +0900 Subject: [PATCH 02/12] feat: output variance Signed-off-by: tzhong518 --- .../config/centerpoint_sigma.param.yaml | 26 +++++++++++++ .../lidar_centerpoint/centerpoint_config.hpp | 10 ++--- .../lidar_centerpoint/centerpoint_trt.hpp | 4 +- .../postprocess/postprocess_kernel.hpp | 3 +- .../include/lidar_centerpoint/ros_utils.hpp | 4 ++ .../include/lidar_centerpoint/utils.hpp | 4 +- .../lidar_centerpoint/lib/centerpoint_trt.cpp | 9 ++--- .../lib/postprocess/postprocess_kernel.cu | 37 +++++++----------- .../lidar_centerpoint/lib/ros_utils.cpp | 39 ++++++++++--------- perception/lidar_centerpoint/src/node.cpp | 17 ++------ 10 files changed, 80 insertions(+), 73 deletions(-) create mode 100644 perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml diff --git a/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml b/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml new file mode 100644 index 0000000000000..82d969c17f3d2 --- /dev/null +++ b/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 4 + max_voxel_size: 40000 + point_cloud_range: [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0] + voxel_size: [0.32, 0.32, 10.0] + downsample_factor: 1 + encoder_in_feature_size: 9 + # 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 + has_twist: true + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # 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" diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 363184ecacfa9..2dd665cb76819 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -97,11 +97,11 @@ class CenterPointConfig std::size_t encoder_in_feature_size_{9}; const std::size_t encoder_out_feature_size_{32}; const std::size_t head_out_size_{6}; - const std::size_t head_out_offset_size_{2}; - const std::size_t head_out_z_size_{1}; - const std::size_t head_out_dim_size_{3}; - const std::size_t head_out_rot_size_{2}; - const std::size_t head_out_vel_size_{2}; + const std::size_t head_out_offset_size_{4}; + const std::size_t head_out_z_size_{2}; + const std::size_t head_out_dim_size_{6}; + const std::size_t head_out_rot_size_{4}; + const std::size_t head_out_vel_size_{4}; // post-process params float score_threshold_{0.35f}; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp index 758915c315a4f..8cf250be0c049 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -65,7 +65,7 @@ class CenterPointTRT bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d, std::vector & det_variance); + std::vector & det_boxes3d); protected: void initPtr(); @@ -75,7 +75,7 @@ class CenterPointTRT void inference(); - void postProcess(std::vector & det_boxes3d, std::vector & det_variance); + void postProcess(std::vector & det_boxes3d); std::unique_ptr vg_ptr_{nullptr}; std::unique_ptr encoder_trt_ptr_{nullptr}; 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 785d4f59191a5..d790e896dea9a 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -33,13 +33,12 @@ class PostProcessCUDA cudaError_t generateDetectedBoxes3D_launch( const float * out_heatmap, const float * out_offset, const float * out_z, const float * out_dim, - const float * out_rot, const float * out_vel, std::vector & det_boxes3d, std::vector & det_variance, + const float * out_rot, const float * out_vel, std::vector & det_boxes3d, cudaStream_t stream); private: CenterPointConfig config_; thrust::device_vector boxes3d_d_; - thrust::device_vector variance_d_; thrust::device_vector yaw_norm_thresholds_d_; }; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index 69b75b614498e..46c1ac353dfea 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -36,6 +36,10 @@ void box3DToDetectedObject( uint8_t getSemanticType(const std::string & class_name); +std::array convertPoseCovarianceMatrix(const Box3D & box3d); + +std::array convertTwistCovarianceMatrix(const Box3D & box3d); + bool isCarLikeVehicleLabel(const uint8_t label); } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp index 02a31db9d7992..f3dd30f754989 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp @@ -33,10 +33,8 @@ struct Box3D float yaw; float vel_x; float vel_y; -}; -struct Variance -{ + // variance float x_variance; float y_variance; float z_variance; diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 2ded3db5c042c..2804e172b73fa 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -107,7 +107,7 @@ void CenterPointTRT::initPtr() bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d, std::vector & det_variance) + std::vector & det_boxes3d) { CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); @@ -122,7 +122,7 @@ bool CenterPointTRT::detect( inference(); - postProcess(det_boxes3d, det_variance); + postProcess(det_boxes3d); return true; } @@ -192,12 +192,11 @@ void CenterPointTRT::inference() head_trt_ptr_->context_->enqueueV2(head_buffers.data(), stream_, nullptr); } -void CenterPointTRT::postProcess( - std::vector & det_boxes3d, std::vector & det_variance) +void CenterPointTRT::postProcess(std::vector & det_boxes3d) { CHECK_CUDA_ERROR(post_proc_ptr_->generateDetectedBoxes3D_launch( head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), - head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, det_variance, stream_)); + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_)); if (det_boxes3d.size() == 0) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); } diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 9916b276294a9..a55dee6b4d2f4 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -58,7 +58,7 @@ __global__ void generateBoxes3D_kernel( const float * out_rot, const float * out_vel, const float voxel_size_x, const float voxel_size_y, const float range_min_x, const float range_min_y, const std::size_t down_grid_size_x, const std::size_t down_grid_size_y, const std::size_t downsample_factor, const int class_size, - const float * yaw_norm_thresholds, Box3D * det_boxes3d, Variance * det_variance) + const float * yaw_norm_thresholds, Box3D * det_boxes3d) { // generate boxes3d from the outputs of the network. // shape of out_*: (N, DOWN_GRID_SIZE_Y, DOWN_GRID_SIZE_X) @@ -98,15 +98,12 @@ __global__ void generateBoxes3D_kernel( const float offset_x_variance = out_offset[down_grid_size * 2 + idx]; const float offset_y_variance = out_offset[down_grid_size * 3 + idx]; - // const float x_variance = voxel_size_x * downsample_factor * (xi + offset_x) + range_min_x; - // const float y_variance = voxel_size_y * downsample_factor * (yi + offset_y) + range_min_y; - const float z_variance = out_z[idx]; + const float z_variance = out_z[down_grid_size * 1 + idx]; const float w_variance = out_dim[down_grid_size * 3 + idx]; const float l_variance = out_dim[down_grid_size * 4 + idx]; const float h_variance = out_dim[down_grid_size * 5 + idx]; const float yaw_sin_log_variance = out_rot[down_grid_size * 2 + idx]; const float yaw_cos_log_variance = out_rot[down_grid_size * 3 + idx]; - // const float yaw_norm_variance = sqrtf(yaw_sin * yaw_sin + yaw_cos * yaw_cos); const float vel_x_variance = out_vel[down_grid_size * 2 + idx]; const float vel_y_variance = out_vel[down_grid_size * 3 + idx]; @@ -122,16 +119,16 @@ __global__ void generateBoxes3D_kernel( det_boxes3d[idx].vel_x = vel_x; det_boxes3d[idx].vel_y = vel_y; - det_variance[idx].x_variance = offset_x_variance; - det_variance[idx].y_variance = offset_y_variance; - det_variance[idx].z_variance = z_variance; - det_variance[idx].length_variance = l_variance; - det_variance[idx].width_variance = w_variance; - det_variance[idx].height_variance = h_variance; - det_variance[idx].yaw_variance = (powf(yaw_cos,2) * expf(yaw_sin_log_variance) + powf(yaw_sin,2) * expf(yaw_cos_log_variance)) / + det_boxes3d[idx].x_variance = expf(offset_x_variance); + det_boxes3d[idx].y_variance = expf(offset_y_variance); + det_boxes3d[idx].z_variance = expf(z_variance); + det_boxes3d[idx].length_variance = expf(l_variance); + det_boxes3d[idx].width_variance = expf(w_variance); + det_boxes3d[idx].height_variance = expf(h_variance); + det_boxes3d[idx].yaw_variance = (powf(yaw_cos,2) * expf(yaw_sin_log_variance) + powf(yaw_sin,2) * expf(yaw_cos_log_variance)) / (powf((powf(yaw_sin,2) + powf(yaw_cos,2)),2)); - det_variance[idx].vel_x_variance = vel_x_variance; - det_variance[idx].vel_y_variance = vel_y_variance; + det_boxes3d[idx].vel_x_variance = expf(vel_x_variance); + det_boxes3d[idx].vel_y_variance = expf(vel_y_variance); } PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(config) @@ -145,7 +142,7 @@ PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(con // cspell: ignore divup cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( const float * out_heatmap, const float * out_offset, const float * out_z, const float * out_dim, - const float * out_rot, const float * out_vel, std::vector & det_boxes3d, std::vector & det_variance, + const float * out_rot, const float * out_vel, std::vector & det_boxes3d, cudaStream_t stream) { dim3 blocks( @@ -157,8 +154,7 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), - thrust::raw_pointer_cast(boxes3d_d_.data()), - thrust::raw_pointer_cast(variance_d_.data())); + thrust::raw_pointer_cast(boxes3d_d_.data())); // suppress by score const auto num_det_boxes3d = thrust::count_if( @@ -184,17 +180,10 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( thrust::copy_if( thrust::device, det_boxes3d_d.begin(), det_boxes3d_d.end(), final_keep_mask_d.begin(), final_det_boxes3d_d.begin(), is_kept()); - thrust::device_vector final_det_variance_d(num_final_det_boxes3d); - thrust::copy_if( - thrust::device, variance_d_.begin(), variance_d_.end(), final_keep_mask_d.begin(), - final_det_variance_d.begin(), is_kept()); - // memcpy device to host det_boxes3d.resize(num_final_det_boxes3d); thrust::copy(final_det_boxes3d_d.begin(), final_det_boxes3d_d.end(), det_boxes3d.begin()); - det_variance.resize(num_final_det_boxes3d); - thrust::copy(final_det_variance_d.begin(), final_det_variance_d.end(), det_variance.begin()); return cudaGetLastError(); } diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 5815559f9bd68..827c98224280c 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -55,7 +55,7 @@ void box3DToDetectedObject( tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - // obj.kinematrc.pose_with_covariance.covariance = + obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); @@ -69,6 +69,7 @@ void box3DToDetectedObject( twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); obj.kinematics.twist_with_covariance.twist = twist; obj.kinematics.has_twist = has_twist; + obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d); } } @@ -93,22 +94,24 @@ uint8_t getSemanticType(const std::string & class_name) } } -// std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( -// const radar_msgs::msg::RadarTrack & radar_track) -// { -// using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -// using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; -// std::array pose_covariance{}; -// pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; -// pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; -// pose_covariance[POSE_IDX::X_Z] = radar_track.position_covariance[RADAR_IDX::X_Z]; -// pose_covariance[POSE_IDX::Y_X] = radar_track.position_covariance[RADAR_IDX::X_Y]; -// pose_covariance[POSE_IDX::Y_Y] = radar_track.position_covariance[RADAR_IDX::Y_Y]; -// pose_covariance[POSE_IDX::Y_Z] = radar_track.position_covariance[RADAR_IDX::Y_Z]; -// pose_covariance[POSE_IDX::Z_X] = radar_track.position_covariance[RADAR_IDX::X_Z]; -// pose_covariance[POSE_IDX::Z_Y] = radar_track.position_covariance[RADAR_IDX::Y_Z]; -// pose_covariance[POSE_IDX::Z_Z] = radar_track.position_covariance[RADAR_IDX::Z_Z]; -// return pose_covariance; -// } +std::array convertPoseCovarianceMatrix(const Box3D & box3d) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + std::array pose_covariance{}; + pose_covariance[POSE_IDX::X_X] = box3d.x_variance; + pose_covariance[POSE_IDX::Y_Y] = box3d.y_variance; + pose_covariance[POSE_IDX::Z_Z] = box3d.z_variance; + pose_covariance[POSE_IDX::YAW_YAW] = box3d.yaw_variance; + return pose_covariance; +} + +std::array convertTwistCovarianceMatrix(const Box3D & box3d) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + std::array twist_covariance{}; + twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; + twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; + return twist_covariance; +} } // namespace centerpoint diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 516538e63d8b5..65bdafe94fe7d 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -143,28 +143,17 @@ void LidarCenterPointNode::pointCloudCallback( } std::vector det_boxes3d; - std::vector det_variance; - bool is_success = - detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d, det_variance); + bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { return; } - std::cout << "-------------------------det_variance:" << det_variance.size() << std::endl; - // << det_variance[0] << std::endl; - std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); - // for (const auto & box3d : det_boxes3d) { - // autoware_auto_perception_msgs::msg::DetectedObject obj; - // box3DToDetectedObject(box3d, class_names_, has_twist_, obj); - // raw_objects.emplace_back(obj); - // } - for (size_t i = 0; i < det_boxes3d.size(); ++i) { + for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(det_boxes3d[i], class_names_, has_twist_, obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, obj); raw_objects.emplace_back(obj); - std::cout << i << " /// x_variance:" << det_variance[i].x_variance << std::endl; } autoware_auto_perception_msgs::msg::DetectedObjects output_msg; From 9a2abaa5bb878982527f15ea0a9a1779f922c85f Mon Sep 17 00:00:00 2001 From: tzhong518 Date: Fri, 8 Mar 2024 14:08:59 +0900 Subject: [PATCH 03/12] feat: add has_variance to config Signed-off-by: tzhong518 --- .../config/centerpoint.param.yaml | 1 + .../config/centerpoint_sigma.param.yaml | 1 + .../config/centerpoint_tiny.param.yaml | 1 + .../lidar_centerpoint/centerpoint_config.hpp | 22 +- .../include/lidar_centerpoint/node.hpp | 1 + .../include/lidar_centerpoint/ros_utils.hpp | 2 +- .../lib/postprocess/postprocess_kernel.cu | 48 +++-- .../lidar_centerpoint/lib/ros_utils.cpp | 12 +- .../lidar_centerpoint/src/node copy.cpp | 200 ++++++++++++++++++ perception/lidar_centerpoint/src/node.cpp | 5 +- 10 files changed, 258 insertions(+), 35 deletions(-) create mode 100644 perception/lidar_centerpoint/src/node copy.cpp diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index a9c3174846d0d..38c57285e552d 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -14,6 +14,7 @@ iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] score_threshold: 0.35 + has_variance: false has_twist: false trt_precision: fp16 densification_num_past_frames: 1 diff --git a/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml b/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml index 82d969c17f3d2..c217f6321381a 100644 --- a/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml @@ -14,6 +14,7 @@ iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] score_threshold: 0.35 + has_variance: true has_twist: true trt_precision: fp16 densification_num_past_frames: 1 diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 474d0e2b695ac..35e11e61e9634 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -14,6 +14,7 @@ iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] score_threshold: 0.35 + has_variance: false has_twist: false trt_precision: fp16 densification_num_past_frames: 1 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 2dd665cb76819..19fdbe7a8b9c2 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -28,7 +28,7 @@ class CenterPointConfig 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 std::vector yaw_norm_thresholds, const bool has_variance) { class_size_ = class_size; point_feature_size_ = point_feature_size; @@ -49,6 +49,15 @@ class CenterPointConfig downsample_factor_ = downsample_factor; encoder_in_feature_size_ = encoder_in_feature_size; + if (has_variance) { + has_variance_ = true; + head_out_offset_size_ = 4; + head_out_z_size_ = 2; + head_out_dim_size_ = 6; + head_out_rot_size_ = 4; + head_out_vel_size_ = 4; + } + if (score_threshold > 0 && score_threshold < 1) { score_threshold_ = score_threshold; } @@ -97,11 +106,12 @@ class CenterPointConfig std::size_t encoder_in_feature_size_{9}; const std::size_t encoder_out_feature_size_{32}; const std::size_t head_out_size_{6}; - const std::size_t head_out_offset_size_{4}; - const std::size_t head_out_z_size_{2}; - const std::size_t head_out_dim_size_{6}; - const std::size_t head_out_rot_size_{4}; - const std::size_t head_out_vel_size_{4}; + bool has_variance_{false}; + std::size_t head_out_offset_size_{2}; + std::size_t head_out_z_size_{1}; + std::size_t head_out_dim_size_{3}; + std::size_t head_out_rot_size_{2}; + std::size_t head_out_vel_size_{2}; // post-process params float score_threshold_{0.35f}; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 963579e5763c2..d9740df515db2 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -53,6 +53,7 @@ class LidarCenterPointNode : public rclcpp::Node float score_threshold_{0.0}; std::vector class_names_; + bool has_variance_{false}; bool has_twist_{false}; NonMaximumSuppression iou_bev_nms_; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index 46c1ac353dfea..4eb4318c4f3f9 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -32,7 +32,7 @@ namespace centerpoint void box3DToDetectedObject( const Box3D & box3d, const std::vector & class_names, const bool has_twist, - autoware_auto_perception_msgs::msg::DetectedObject & obj); + const bool has_variance, autoware_auto_perception_msgs::msg::DetectedObject & obj); uint8_t getSemanticType(const std::string & class_name); diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index a55dee6b4d2f4..feaeda62d6282 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -58,7 +58,7 @@ __global__ void generateBoxes3D_kernel( const float * out_rot, const float * out_vel, const float voxel_size_x, const float voxel_size_y, const float range_min_x, const float range_min_y, const std::size_t down_grid_size_x, const std::size_t down_grid_size_y, const std::size_t downsample_factor, const int class_size, - const float * yaw_norm_thresholds, Box3D * det_boxes3d) + const bool has_variance, const float * yaw_norm_thresholds, Box3D * det_boxes3d) { // generate boxes3d from the outputs of the network. // shape of out_*: (N, DOWN_GRID_SIZE_Y, DOWN_GRID_SIZE_X) @@ -96,17 +96,6 @@ __global__ void generateBoxes3D_kernel( const float vel_x = out_vel[down_grid_size * 0 + idx]; const float vel_y = out_vel[down_grid_size * 1 + idx]; - const float offset_x_variance = out_offset[down_grid_size * 2 + idx]; - const float offset_y_variance = out_offset[down_grid_size * 3 + idx]; - const float z_variance = out_z[down_grid_size * 1 + idx]; - const float w_variance = out_dim[down_grid_size * 3 + idx]; - const float l_variance = out_dim[down_grid_size * 4 + idx]; - const float h_variance = out_dim[down_grid_size * 5 + idx]; - const float yaw_sin_log_variance = out_rot[down_grid_size * 2 + idx]; - const float yaw_cos_log_variance = out_rot[down_grid_size * 3 + idx]; - const float vel_x_variance = out_vel[down_grid_size * 2 + idx]; - const float vel_y_variance = out_vel[down_grid_size * 3 + idx]; - det_boxes3d[idx].label = label; det_boxes3d[idx].score = yaw_norm >= yaw_norm_thresholds[label] ? max_score : 0.f; det_boxes3d[idx].x = x; @@ -119,16 +108,29 @@ __global__ void generateBoxes3D_kernel( det_boxes3d[idx].vel_x = vel_x; det_boxes3d[idx].vel_y = vel_y; - det_boxes3d[idx].x_variance = expf(offset_x_variance); - det_boxes3d[idx].y_variance = expf(offset_y_variance); - det_boxes3d[idx].z_variance = expf(z_variance); - det_boxes3d[idx].length_variance = expf(l_variance); - det_boxes3d[idx].width_variance = expf(w_variance); - det_boxes3d[idx].height_variance = expf(h_variance); - det_boxes3d[idx].yaw_variance = (powf(yaw_cos,2) * expf(yaw_sin_log_variance) + powf(yaw_sin,2) * expf(yaw_cos_log_variance)) / - (powf((powf(yaw_sin,2) + powf(yaw_cos,2)),2)); - det_boxes3d[idx].vel_x_variance = expf(vel_x_variance); - det_boxes3d[idx].vel_y_variance = expf(vel_y_variance); + if (has_variance) { + const float offset_x_variance = out_offset[down_grid_size * 2 + idx]; + const float offset_y_variance = out_offset[down_grid_size * 3 + idx]; + const float z_variance = out_z[down_grid_size * 1 + idx]; + const float w_variance = out_dim[down_grid_size * 3 + idx]; + const float l_variance = out_dim[down_grid_size * 4 + idx]; + const float h_variance = out_dim[down_grid_size * 5 + idx]; + const float yaw_sin_log_variance = out_rot[down_grid_size * 2 + idx]; + const float yaw_cos_log_variance = out_rot[down_grid_size * 3 + idx]; + const float vel_x_variance = out_vel[down_grid_size * 2 + idx]; + const float vel_y_variance = out_vel[down_grid_size * 3 + idx]; + + det_boxes3d[idx].x_variance = expf(offset_x_variance); + det_boxes3d[idx].y_variance = expf(offset_y_variance); + det_boxes3d[idx].z_variance = expf(z_variance); + det_boxes3d[idx].length_variance = expf(l_variance); + det_boxes3d[idx].width_variance = expf(w_variance); + det_boxes3d[idx].height_variance = expf(h_variance); + det_boxes3d[idx].yaw_variance = (powf(yaw_cos,2) * expf(yaw_sin_log_variance) + powf(yaw_sin,2) * expf(yaw_cos_log_variance)) / + (powf((powf(yaw_sin,2) + powf(yaw_cos,2)),2)); + det_boxes3d[idx].vel_x_variance = expf(vel_x_variance); + det_boxes3d[idx].vel_y_variance = expf(vel_y_variance); + } } PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(config) @@ -152,7 +154,7 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( generateBoxes3D_kernel<<>>( out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_, config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, - config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, + config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, config_.has_variance_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), thrust::raw_pointer_cast(boxes3d_d_.data())); diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 827c98224280c..c4fb4abd8d753 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -25,7 +25,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; void box3DToDetectedObject( const Box3D & box3d, const std::vector & class_names, const bool has_twist, - autoware_auto_perception_msgs::msg::DetectedObject & obj) + const bool has_variance, autoware_auto_perception_msgs::msg::DetectedObject & obj) { // TODO(yukke42): the value of classification confidence of DNN, not probability. obj.existence_probability = box3d.score; @@ -55,10 +55,13 @@ void box3DToDetectedObject( tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + if (has_variance) { + obj.kinematics.has_position_covariance = has_variance; + obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); + } // twist if (has_twist) { @@ -69,7 +72,10 @@ void box3DToDetectedObject( twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); obj.kinematics.twist_with_covariance.twist = twist; obj.kinematics.has_twist = has_twist; - obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d); + if (has_variance) { + obj.kinematics.has_twist_covariance = has_variance; + obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d); + } } } diff --git a/perception/lidar_centerpoint/src/node copy.cpp b/perception/lidar_centerpoint/src/node copy.cpp new file mode 100644 index 0000000000000..dcab7eea9b52a --- /dev/null +++ b/perception/lidar_centerpoint/src/node copy.cpp @@ -0,0 +1,200 @@ +// Copyright 2021 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 "lidar_centerpoint/node.hpp" + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +#include +#include +#include + +namespace centerpoint +{ +LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_options) +: Node("lidar_center_point", node_options), tf_buffer_(this->get_clock()) +{ + const float score_threshold = + static_cast(this->declare_parameter("score_threshold", 0.35)); + const float circle_nms_dist_threshold = + static_cast(this->declare_parameter("circle_nms_dist_threshold")); + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds"); + const std::string densification_world_frame_id = + this->declare_parameter("densification_world_frame_id", "map"); + const int densification_num_past_frames = + this->declare_parameter("densification_num_past_frames", 1); + const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); + const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); + const std::string encoder_engine_path = + this->declare_parameter("encoder_engine_path"); + const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); + const std::string head_engine_path = this->declare_parameter("head_engine_path"); + class_names_ = this->declare_parameter>("class_names"); + has_twist_ = this->declare_parameter("has_twist", false); + const std::size_t point_feature_size = + static_cast(this->declare_parameter("point_feature_size")); + const std::size_t max_voxel_size = + static_cast(this->declare_parameter("max_voxel_size")); + const auto point_cloud_range = this->declare_parameter>("point_cloud_range"); + const auto voxel_size = this->declare_parameter>("voxel_size"); + const std::size_t downsample_factor = + static_cast(this->declare_parameter("downsample_factor")); + const std::size_t encoder_in_feature_size = + static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + { + NMSParams p; + p.nms_type_ = NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names"); + p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + iou_bev_nms_.setParameters(p); + } + + NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); + NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); + DensificationParam densification_param( + densification_world_frame_id, densification_num_past_frames); + + if (point_cloud_range.size() != 6) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_centerpoint"), + "The size of point_cloud_range != 6: use the default parameters."); + } + if (voxel_size.size() != 3) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_centerpoint"), + "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); + detector_ptr_ = + std::make_unique(encoder_param, head_param, densification_param, config); + + pointcloud_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), + std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1)); + objects_pub_ = this->create_publisher( + "~/output/objects", rclcpp::QoS{1}); + + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + if (this->declare_parameter("build_only", false)) { + RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } +} + +void LidarCenterPointNode::pointCloudCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg) +{ + const auto objects_sub_count = + objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + if (stop_watch_ptr_) { + stop_watch_ptr_->toc("processing_time", true); + } + + std::vector det_boxes3d; + std::vector det_variance; + bool is_success = + detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d, det_variance); + if (!is_success) { + return; + } + + std::cout << "-------------------------det_variance:" << det_variance.size() << std::endl; + // << det_variance[0] << std::endl; + + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); + // for (const auto & box3d : det_boxes3d) { + // autoware_auto_perception_msgs::msg::DetectedObject obj; + // box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + // raw_objects.emplace_back(obj); + // } + for (size_t i = 0; i < det_boxes3d.size(); ++i) { + autoware_auto_perception_msgs::msg::DetectedObject obj; + box3DToDetectedObject(det_boxes3d[i], class_names_, has_twist_, obj); + raw_objects.emplace_back(obj); + std::cout << i << " /// x_variance:" << det_variance[i].x_variance << std::endl; + } + + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = input_pointcloud_msg->header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + + detection_class_remapper_.mapClasses(output_msg); + + if (objects_sub_count > 0) { + objects_pub_->publish(output_msg); + } + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/processing_time_ms", processing_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } +} + +} // namespace centerpoint + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(centerpoint::LidarCenterPointNode) diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 65bdafe94fe7d..40457afcfd860 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -55,6 +55,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); const std::string head_engine_path = this->declare_parameter("head_engine_path"); class_names_ = this->declare_parameter>("class_names"); + has_variance_ = this->declare_parameter("has_variance", false); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -102,7 +103,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti 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); + yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); @@ -152,7 +153,7 @@ void LidarCenterPointNode::pointCloudCallback( raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, has_variance_, obj); raw_objects.emplace_back(obj); } From c5e8b7064c5cdb29c54efb9afb805bc9f464ab8f Mon Sep 17 00:00:00 2001 From: tzhong518 Date: Tue, 12 Mar 2024 12:34:56 +0900 Subject: [PATCH 04/12] fix: single_inference node Signed-off-by: tzhong518 --- .../lib/postprocess/postprocess_kernel.cu | 4 +- .../lidar_centerpoint/src/node copy.cpp | 200 ------------------ 2 files changed, 2 insertions(+), 202 deletions(-) delete mode 100644 perception/lidar_centerpoint/src/node copy.cpp diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index feaeda62d6282..848aad0984352 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -120,8 +120,8 @@ __global__ void generateBoxes3D_kernel( const float vel_x_variance = out_vel[down_grid_size * 2 + idx]; const float vel_y_variance = out_vel[down_grid_size * 3 + idx]; - det_boxes3d[idx].x_variance = expf(offset_x_variance); - det_boxes3d[idx].y_variance = expf(offset_y_variance); + det_boxes3d[idx].x_variance = voxel_size_x * downsample_factor * expf(offset_x_variance); + det_boxes3d[idx].y_variance = voxel_size_x * downsample_factor * expf(offset_y_variance); det_boxes3d[idx].z_variance = expf(z_variance); det_boxes3d[idx].length_variance = expf(l_variance); det_boxes3d[idx].width_variance = expf(w_variance); diff --git a/perception/lidar_centerpoint/src/node copy.cpp b/perception/lidar_centerpoint/src/node copy.cpp deleted file mode 100644 index dcab7eea9b52a..0000000000000 --- a/perception/lidar_centerpoint/src/node copy.cpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright 2021 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 "lidar_centerpoint/node.hpp" - -#include -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include - -#include -#include -#include - -namespace centerpoint -{ -LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_options) -: Node("lidar_center_point", node_options), tf_buffer_(this->get_clock()) -{ - const float score_threshold = - static_cast(this->declare_parameter("score_threshold", 0.35)); - const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold")); - const auto yaw_norm_thresholds = - this->declare_parameter>("yaw_norm_thresholds"); - const std::string densification_world_frame_id = - this->declare_parameter("densification_world_frame_id", "map"); - const int densification_num_past_frames = - this->declare_parameter("densification_num_past_frames", 1); - const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); - const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); - const std::string encoder_engine_path = - this->declare_parameter("encoder_engine_path"); - const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); - const std::string head_engine_path = this->declare_parameter("head_engine_path"); - class_names_ = this->declare_parameter>("class_names"); - has_twist_ = this->declare_parameter("has_twist", false); - const std::size_t point_feature_size = - static_cast(this->declare_parameter("point_feature_size")); - const std::size_t max_voxel_size = - static_cast(this->declare_parameter("max_voxel_size")); - const auto point_cloud_range = this->declare_parameter>("point_cloud_range"); - const auto voxel_size = this->declare_parameter>("voxel_size"); - const std::size_t downsample_factor = - static_cast(this->declare_parameter("downsample_factor")); - const std::size_t encoder_in_feature_size = - static_cast(this->declare_parameter("encoder_in_feature_size")); - const auto allow_remapping_by_area_matrix = - this->declare_parameter>("allow_remapping_by_area_matrix"); - const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); - const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - - detection_class_remapper_.setParameters( - allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); - - { - NMSParams p; - p.nms_type_ = NMS_TYPE::IoU_BEV; - p.target_class_names_ = - this->declare_parameter>("iou_nms_target_class_names"); - p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); - p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); - iou_bev_nms_.setParameters(p); - } - - NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); - NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); - DensificationParam densification_param( - densification_world_frame_id, densification_num_past_frames); - - if (point_cloud_range.size() != 6) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("lidar_centerpoint"), - "The size of point_cloud_range != 6: use the default parameters."); - } - if (voxel_size.size() != 3) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("lidar_centerpoint"), - "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); - detector_ptr_ = - std::make_unique(encoder_param, head_param, densification_param, config); - - pointcloud_sub_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), - std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1)); - objects_pub_ = this->create_publisher( - "~/output/objects", rclcpp::QoS{1}); - - // initialize debug tool - { - using tier4_autoware_utils::DebugPublisher; - using tier4_autoware_utils::StopWatch; - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint"); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); - } - - if (this->declare_parameter("build_only", false)) { - RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); - rclcpp::shutdown(); - } -} - -void LidarCenterPointNode::pointCloudCallback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg) -{ - const auto objects_sub_count = - objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count(); - if (objects_sub_count < 1) { - return; - } - - if (stop_watch_ptr_) { - stop_watch_ptr_->toc("processing_time", true); - } - - std::vector det_boxes3d; - std::vector det_variance; - bool is_success = - detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d, det_variance); - if (!is_success) { - return; - } - - std::cout << "-------------------------det_variance:" << det_variance.size() << std::endl; - // << det_variance[0] << std::endl; - - std::vector raw_objects; - raw_objects.reserve(det_boxes3d.size()); - // for (const auto & box3d : det_boxes3d) { - // autoware_auto_perception_msgs::msg::DetectedObject obj; - // box3DToDetectedObject(box3d, class_names_, has_twist_, obj); - // raw_objects.emplace_back(obj); - // } - for (size_t i = 0; i < det_boxes3d.size(); ++i) { - autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(det_boxes3d[i], class_names_, has_twist_, obj); - raw_objects.emplace_back(obj); - std::cout << i << " /// x_variance:" << det_variance[i].x_variance << std::endl; - } - - autoware_auto_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = input_pointcloud_msg->header; - output_msg.objects = iou_bev_nms_.apply(raw_objects); - - detection_class_remapper_.mapClasses(output_msg); - - if (objects_sub_count > 0) { - objects_pub_->publish(output_msg); - } - - // add processing time for debug - if (debug_publisher_ptr_ && stop_watch_ptr_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - const double pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) - .count(); - debug_publisher_ptr_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( - "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); - } -} - -} // namespace centerpoint - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(centerpoint::LidarCenterPointNode) From e9f3917f5a1ef940e11b63c0660b7574cf49eaac Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 8 Mar 2024 05:42:12 +0000 Subject: [PATCH 05/12] style(pre-commit): autofix --- .../lib/postprocess/postprocess_kernel.cu | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 848aad0984352..6184ab10c9e16 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -111,7 +111,7 @@ __global__ void generateBoxes3D_kernel( if (has_variance) { const float offset_x_variance = out_offset[down_grid_size * 2 + idx]; const float offset_y_variance = out_offset[down_grid_size * 3 + idx]; - const float z_variance = out_z[down_grid_size * 1 + idx]; + const float z_variance = out_z[down_grid_size * 1 + idx]; const float w_variance = out_dim[down_grid_size * 3 + idx]; const float l_variance = out_dim[down_grid_size * 4 + idx]; const float h_variance = out_dim[down_grid_size * 5 + idx]; @@ -126,8 +126,9 @@ __global__ void generateBoxes3D_kernel( det_boxes3d[idx].length_variance = expf(l_variance); det_boxes3d[idx].width_variance = expf(w_variance); det_boxes3d[idx].height_variance = expf(h_variance); - det_boxes3d[idx].yaw_variance = (powf(yaw_cos,2) * expf(yaw_sin_log_variance) + powf(yaw_sin,2) * expf(yaw_cos_log_variance)) / - (powf((powf(yaw_sin,2) + powf(yaw_cos,2)),2)); + det_boxes3d[idx].yaw_variance = (powf(yaw_cos, 2) * expf(yaw_sin_log_variance) + + powf(yaw_sin, 2) * expf(yaw_cos_log_variance)) / + (powf((powf(yaw_sin, 2) + powf(yaw_cos, 2)), 2)); det_boxes3d[idx].vel_x_variance = expf(vel_x_variance); det_boxes3d[idx].vel_y_variance = expf(vel_y_variance); } @@ -154,8 +155,8 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( generateBoxes3D_kernel<<>>( out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_, config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, - config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, config_.has_variance_, - thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), + config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, + config_.has_variance_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), thrust::raw_pointer_cast(boxes3d_d_.data())); // suppress by score From a7708a465ae3eb75698f001f02fee2e3107b7105 Mon Sep 17 00:00:00 2001 From: tzhong518 Date: Wed, 3 Apr 2024 12:03:47 +0900 Subject: [PATCH 06/12] fix: add to pointpainting param Signed-off-by: tzhong518 --- .../config/pointpainting.param.yaml | 1 + .../pointpainting_fusion/node.hpp | 1 + .../schema/pointpainting.schema.json | 5 +++++ .../src/pointpainting_fusion/node.cpp | 5 +++-- 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 3abaffb243d96..53ac6f4cafc28 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -16,6 +16,7 @@ downsample_factor: 1 encoder_in_feature_size: 12 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + has_variance: false has_twist: false densification_params: world_frame_id: "map" diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index 78ae152141a00..c39dd7ac8bc33 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -60,6 +60,7 @@ class PointPaintingFusionNode std::map class_index_; std::map> isClassTable_; std::vector pointcloud_range; + bool has_variance_{false}; bool has_twist_{false}; centerpoint::NonMaximumSuppression iou_bev_nms_; diff --git a/perception/image_projection_based_fusion/schema/pointpainting.schema.json b/perception/image_projection_based_fusion/schema/pointpainting.schema.json index 036628d72e70a..bc8f0512263b6 100644 --- a/perception/image_projection_based_fusion/schema/pointpainting.schema.json +++ b/perception/image_projection_based_fusion/schema/pointpainting.schema.json @@ -60,6 +60,11 @@ "minimum": 0.0, "maximum": 1.0 }, + "has_variance": { + "type": "boolean", + "description": "Indicates whether the model outputs variance value.", + "default": false + }, "has_twist": { "type": "boolean", "description": "Indicates whether the model outputs twist value.", 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 14783a46ba8b4..f9c6dfb882f8f 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -140,6 +140,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_.erase(cls); } } + has_variance_ = this->declare_parameter("has_variance", false); has_twist_ = this->declare_parameter("model_params.has_twist"); const std::size_t point_feature_size = static_cast( this->declare_parameter("model_params.point_feature_size")); @@ -189,7 +190,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt 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); + yaw_norm_thresholds, has_variance_); // create detector detector_ptr_ = std::make_unique( @@ -401,7 +402,7 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, has_variance_, obj); raw_objects.emplace_back(obj); } From 29c655d22966abd6f755562d0525aca0e36929ef Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Thu, 4 Apr 2024 10:10:28 +0900 Subject: [PATCH 07/12] Update perception/lidar_centerpoint/src/node.cpp Co-authored-by: Yoshi Ri --- perception/lidar_centerpoint/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 40457afcfd860..2f66ba6dcc791 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -55,7 +55,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); const std::string head_engine_path = this->declare_parameter("head_engine_path"); class_names_ = this->declare_parameter>("class_names"); - has_variance_ = this->declare_parameter("has_variance", false); + has_variance_ = this->declare_parameter("has_variance"); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); From 375c023fff64ce4adc87ba2d989e8450d69c7326 Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Thu, 4 Apr 2024 10:10:36 +0900 Subject: [PATCH 08/12] Update perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp Co-authored-by: Yoshi Ri --- .../src/pointpainting_fusion/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f9c6dfb882f8f..c4c5941be5fec 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -140,7 +140,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_.erase(cls); } } - has_variance_ = this->declare_parameter("has_variance", false); + has_variance_ = this->declare_parameter("has_variance"); has_twist_ = this->declare_parameter("model_params.has_twist"); const std::size_t point_feature_size = static_cast( this->declare_parameter("model_params.point_feature_size")); From 4ff31ff48b0c2bf1755e13ba7f0a71f74092c995 Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Thu, 4 Apr 2024 10:10:55 +0900 Subject: [PATCH 09/12] Update perception/lidar_centerpoint/src/node.cpp Co-authored-by: Yoshi Ri --- perception/lidar_centerpoint/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 2f66ba6dcc791..0606a2ec55c9e 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -56,7 +56,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const std::string head_engine_path = this->declare_parameter("head_engine_path"); class_names_ = this->declare_parameter>("class_names"); has_variance_ = this->declare_parameter("has_variance"); - has_twist_ = this->declare_parameter("has_twist", false); + has_twist_ = this->declare_parameter("has_twist"); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); const std::size_t max_voxel_size = From 44ded7ab50f0d98a8f1abc2dcc5b990e26f14898 Mon Sep 17 00:00:00 2001 From: tzhong518 Date: Thu, 4 Apr 2024 10:21:24 +0900 Subject: [PATCH 10/12] fix: add options Signed-off-by: tzhong518 --- .../detection/detector/lidar_dnn_detector.launch.xml | 2 +- launch/tier4_perception_launch/launch/perception.launch.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index f9b65fd5c30eb..1a770708d75fd 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index c6213ee313c2b..d9e18e76bc4c4 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -36,7 +36,7 @@ - + From 8a2373163811186045c81cf3c71f7ed28831e92c Mon Sep 17 00:00:00 2001 From: tzhong518 Date: Thu, 4 Apr 2024 13:46:20 +0900 Subject: [PATCH 11/12] fix: avoid powf Signed-off-by: tzhong518 --- .../lib/postprocess/postprocess_kernel.cu | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 6184ab10c9e16..34bbd2811041c 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -126,9 +126,12 @@ __global__ void generateBoxes3D_kernel( det_boxes3d[idx].length_variance = expf(l_variance); det_boxes3d[idx].width_variance = expf(w_variance); det_boxes3d[idx].height_variance = expf(h_variance); - det_boxes3d[idx].yaw_variance = (powf(yaw_cos, 2) * expf(yaw_sin_log_variance) + - powf(yaw_sin, 2) * expf(yaw_cos_log_variance)) / - (powf((powf(yaw_sin, 2) + powf(yaw_cos, 2)), 2)); + const float yaw_sin_sq = yaw_sin * yaw_sin; + const float yaw_cos_sq = yaw_cos * yaw_cos; + const float yaw_norm_sq = (yaw_sin_sq + yaw_cos_sq) * (yaw_sin_sq + yaw_cos_sq); + det_boxes3d[idx].yaw_variance = + (yaw_cos_sq * expf(yaw_sin_log_variance) + yaw_sin_sq * expf(yaw_cos_log_variance)) / + yaw_norm_sq; det_boxes3d[idx].vel_x_variance = expf(vel_x_variance); det_boxes3d[idx].vel_y_variance = expf(vel_y_variance); } From 62274ea5e0ffb6f86f75ebfee645bbc6896dd42f Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Fri, 12 Apr 2024 13:11:24 +0900 Subject: [PATCH 12/12] Update launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml Co-authored-by: Taekjin LEE --- .../detection/detector/lidar_dnn_detector.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index 1a770708d75fd..ab89da7112b66 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -5,7 +5,7 @@ - +