Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(blockage_diag): output blockage angle range bug fix #6445

Merged
merged 3 commits into from
Feb 19, 2024
Merged

fix(blockage_diag): output blockage angle range bug fix #6445

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
946ae95
fix: blockage_range_angle bug
badai-nguyen Feb 18, 2024
fc43094
chore: refactor
badai-nguyen Feb 18, 2024
26a771c
Revert "chore: refactor"
badai-nguyen Feb 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 17 additions & 24 deletions sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,113 +162,106 @@
ideal_horizontal_bins =
static_cast<uint>((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_);
pcl::PointCloud<PointXYZIRADRT>::Ptr pcl_input(new pcl::PointCloud<PointXYZIRADRT>);
pcl::fromROSMsg(*input, *pcl_input);
std::vector<float> horizontal_bin_reference(ideal_horizontal_bins);
std::vector<pcl::PointCloud<PointXYZIRADRT>> 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));
cv::Mat lidar_depth_map_8u(
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
if (pcl_input->points.empty()) {
ground_blockage_ratio_ = 1.0f;
sky_blockage_ratio_ = 1.0f;
if (ground_blockage_count_ <= 2 * blockage_count_threshold_) {
ground_blockage_count_ += 1;
}
if (sky_blockage_count_ <= 2 * blockage_count_threshold_) {
sky_blockage_count_ += 1;
}
ground_blockage_range_deg_[0] = angle_range_deg_[0];
ground_blockage_range_deg_[1] = angle_range_deg_[1];
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<int>(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<uint16_t>(p.ring, horizontal_bin) =
UINT16_MAX - distance_coefficient * p.distance;
} else if (lidar_model_ == "PandarQT") {
full_size_depth_map.at<uint16_t>(vertical_bins - p.ring - 1, horizontal_bin) =
UINT16_MAX - distance_coefficient * p.distance;
}
}
int bin_idx =
static_cast<int>((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<uint16_t>(p.ring, bin_idx) =
UINT16_MAX - distance_coefficient * p.distance;
} else if (lidar_model_ == "PandarQT") {
full_size_depth_map.at<uint16_t>(vertical_bins - p.ring - 1, bin_idx) =
UINT16_MAX - distance_coefficient * p.distance;
}
}
}
full_size_depth_map.convertTo(lidar_depth_map_8u, CV_8UC1, 1.0 / 300);
cv::Mat no_return_mask(cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask);
cv::Mat erosion_dst;
cv::Mat blockage_element = cv::getStructuringElement(
cv::MORPH_RECT, cv::Size(2 * blockage_kernel_ + 1, 2 * blockage_kernel_ + 1),
cv::Point(blockage_kernel_, blockage_kernel_));
cv::erode(no_return_mask, erosion_dst, blockage_element);
cv::dilate(erosion_dst, no_return_mask, blockage_element);
static boost::circular_buffer<cv::Mat> no_return_mask_buffer(blockage_buffering_frames_);
cv::Mat time_series_blockage_result(
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
cv::Mat time_series_blockage_mask(
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
cv::Mat no_return_mask_binarized(
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));

if (blockage_buffering_interval_ == 0) {
no_return_mask.copyTo(time_series_blockage_result);
} else {
no_return_mask_binarized = no_return_mask / 255;
if (blockage_frame_count_ >= blockage_buffering_interval_) {
no_return_mask_buffer.push_back(no_return_mask_binarized);
blockage_frame_count_ = 0;
} else {
blockage_frame_count_++;
}
for (const auto & binary_mask : no_return_mask_buffer) {
time_series_blockage_mask += binary_mask;
}
cv::inRange(
time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(),
time_series_blockage_result);
}
cv::Mat ground_no_return_mask;
cv::Mat sky_no_return_mask;
no_return_mask(cv::Rect(0, 0, ideal_horizontal_bins, horizontal_ring_id_))
.copyTo(sky_no_return_mask);
no_return_mask(
cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_))
.copyTo(ground_no_return_mask);
ground_blockage_ratio_ =
static_cast<float>(cv::countNonZero(ground_no_return_mask)) /
static_cast<float>(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_));
sky_blockage_ratio_ = static_cast<float>(cv::countNonZero(sky_no_return_mask)) /
static_cast<float>(ideal_horizontal_bins * horizontal_ring_id_);

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<float>(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<float>(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;
}
} else {
ground_blockage_count_ = 0;
}
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<float>(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<float>(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];

Check notice on line 264 in sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

BlockageDiagComponent::filter decreases in cyclomatic complexity from 25 to 24, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 264 in sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Bumpy Road Ahead 

BlockageDiagComponent::filter decreases from 8 to 7 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check notice on line 264 in sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Deep, Nested Complexity 

BlockageDiagComponent::filter is no longer above the threshold for nested complexity depth
if (sky_blockage_count_ <= 2 * blockage_count_threshold_) {
sky_blockage_count_ += 1;
}
Expand Down
Loading