From 52c78bb5af8693e38d545e1c7c52ef4a01870e88 Mon Sep 17 00:00:00 2001 From: swift_file Date: Wed, 22 Jun 2022 16:08:58 +0900 Subject: [PATCH] update:calc blockage_ratio by multi frame blockage mask Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 8d2572bea46d5..dc6314acd078e 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -159,12 +159,7 @@ void BlockageDiagComponent::filter( cv::Point(erode_kernel_, erode_kernel_)); cv::erode(no_return_mask, erosion_dst, element); cv::dilate(erosion_dst, no_return_mask, element); - cv::Mat ground_no_return_mask; - cv::Mat sky_no_return_mask; - no_return_mask(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)).copyTo(sky_no_return_mask); - no_return_mask( - cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) - .copyTo(ground_no_return_mask); + static boost::circular_buffer no_return_mask_buffer(buffering_frames_); @@ -191,6 +186,12 @@ void BlockageDiagComponent::filter( } else { no_return_mask.copyTo(no_return_mask_result); } + cv::Mat ground_no_return_mask; + cv::Mat sky_no_return_mask; + no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)).copyTo(sky_no_return_mask); + no_return_mask_result( + cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) + .copyTo(ground_no_return_mask); ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_));