From 946ae95ce43b5009ff321ee404b51535b5d1ebc0 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Sun, 18 Feb 2024 23:03:53 +0900 Subject: [PATCH 1/3] fix: blockage_range_angle bug Signed-off-by: badai-nguyen --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 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 06da91e61b3bd..0dd7088f80262 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -255,9 +255,11 @@ void BlockageDiagComponent::filter( if (ground_blockage_ratio_ > blockage_ratio_threshold_) { cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); - ground_blockage_range_deg_[0] = static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; + ground_blockage_range_deg_[0] = + ground_blockage_bb.x * horizontal_resolution_ + angle_range_deg_[0]; ground_blockage_range_deg_[1] = - static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + (ground_blockage_bb.x + ground_blockage_bb.width) * horizontal_resolution_ + + angle_range_deg_[0]; if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { ground_blockage_count_ += 1; } @@ -266,9 +268,9 @@ void BlockageDiagComponent::filter( } if (sky_blockage_ratio_ > blockage_ratio_threshold_) { cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); - sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; + sky_blockage_range_deg_[0] = sky_blockage_bx.x * horizontal_resolution_ + angle_range_deg_[0]; sky_blockage_range_deg_[1] = - static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; + (sky_blockage_bx.x + sky_blockage_bx.width) * horizontal_resolution_ + angle_range_deg_[0]; if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { sky_blockage_count_ += 1; } From fc430946f1a9075bee36402a36180965e3b028a1 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Sun, 18 Feb 2024 23:51:36 +0900 Subject: [PATCH 2/3] chore: refactor Signed-off-by: badai-nguyen --- .../blockage_diag/blockage_diag_nodelet.cpp | 31 +++++++------------ 1 file changed, 11 insertions(+), 20 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 0dd7088f80262..61ee97295ded2 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -163,8 +163,6 @@ void BlockageDiagComponent::filter( static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - std::vector horizontal_bin_reference(ideal_horizontal_bins); - std::vector> each_ring_pointcloud(vertical_bins); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); @@ -184,25 +182,18 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (int i = 0; i < ideal_horizontal_bins; ++i) { - horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; - } for (const auto p : pcl_input->points) { - for (int horizontal_bin = 0; - horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { - if ( - (p.azimuth / 100 > - (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && - (p.azimuth / 100 <= - (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { - if (lidar_model_ == "Pandar40P") { - full_size_depth_map.at(p.ring, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } else if (lidar_model_ == "PandarQT") { - full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } - } + int bin_idx = + static_cast((p.azimuth / 100. - angle_range_deg_[0]) / horizontal_resolution_); + if (bin_idx < 0 || bin_idx >= ideal_horizontal_bins) { + continue; + } + if (lidar_model_ == "Pandar40P") { + full_size_depth_map.at(p.ring, bin_idx) = + UINT16_MAX - distance_coefficient * p.distance; + } else if (lidar_model_ == "PandarQT") { + full_size_depth_map.at(vertical_bins - p.ring - 1, bin_idx) = + UINT16_MAX - distance_coefficient * p.distance; } } } From 26a771cd7f5e1d5f56003fb1179b219b3d5ce654 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 19 Feb 2024 09:49:43 +0900 Subject: [PATCH 3/3] Revert "chore: refactor" This reverts commit fc430946f1a9075bee36402a36180965e3b028a1. --- .../blockage_diag/blockage_diag_nodelet.cpp | 31 ++++++++++++------- 1 file changed, 20 insertions(+), 11 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 61ee97295ded2..0dd7088f80262 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -163,6 +163,8 @@ void BlockageDiagComponent::filter( static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); + std::vector horizontal_bin_reference(ideal_horizontal_bins); + std::vector> each_ring_pointcloud(vertical_bins); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); @@ -182,18 +184,25 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { + for (int i = 0; i < ideal_horizontal_bins; ++i) { + horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; + } for (const auto p : pcl_input->points) { - int bin_idx = - static_cast((p.azimuth / 100. - angle_range_deg_[0]) / horizontal_resolution_); - if (bin_idx < 0 || bin_idx >= ideal_horizontal_bins) { - continue; - } - if (lidar_model_ == "Pandar40P") { - full_size_depth_map.at(p.ring, bin_idx) = - UINT16_MAX - distance_coefficient * p.distance; - } else if (lidar_model_ == "PandarQT") { - full_size_depth_map.at(vertical_bins - p.ring - 1, bin_idx) = - UINT16_MAX - distance_coefficient * p.distance; + for (int horizontal_bin = 0; + horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { + if ( + (p.azimuth / 100 > + (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && + (p.azimuth / 100 <= + (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { + if (lidar_model_ == "Pandar40P") { + full_size_depth_map.at(p.ring, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } else if (lidar_model_ == "PandarQT") { + full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } + } } } }