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(ground_segmentation): add omp parallel for #5791

Closed
wants to merge 3 commits into from
Closed

fix(ground_segmentation): add omp parallel for #5791

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
627020c
fix: add loop for parallel
badai-nguyen Dec 4, 2023
5cb8eab
chore: refactor
badai-nguyen Dec 6, 2023
696d2c3
Merge branch 'main' into fix/ground_segmentation_parallel
badai-nguyen Feb 26, 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
7 changes: 4 additions & 3 deletions perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -219,10 +219,10 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
void classifyPointCloud(
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
pcl::PointIndices & out_no_ground_indices);
std::vector<pcl::PointIndices> & out_no_ground_indices);
void classifyPointCloudGridScan(
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
pcl::PointIndices & out_no_ground_indices);
std::vector<pcl::PointIndices> & out_no_ground_indices);
/*!
* Re-classifies point of ground cluster based on their height
* @param gnd_cluster Input ground cluster for re-checking
Expand All @@ -240,7 +240,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
* @param out_object_cloud_ptr Resulting PointCloud with the indices kept
*/
void extractObjectPoints(
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
const std::vector<pcl::PointIndices> & in_indices,
pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);

/** \brief Parameter service callback result : needed to be hold */
Expand Down
53 changes: 32 additions & 21 deletions perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc. All rights reserved.

Check notice on line 1 in perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 7.29 to 7.36, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -19,6 +19,8 @@
#include <tier4_autoware_utils/math/unit_conversion.hpp>
#include <vehicle_info_util/vehicle_info_util.hpp>

#include <omp.h>

#include <memory>
#include <string>
#include <vector>
Expand Down Expand Up @@ -283,126 +285,129 @@
}
void ScanGroundFilterComponent::classifyPointCloudGridScan(
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
pcl::PointIndices & out_no_ground_indices)
std::vector<pcl::PointIndices> & out_no_ground_indices)
{
out_no_ground_indices.indices.clear();
out_no_ground_indices.resize(in_radial_ordered_clouds.size());
#pragma omp parallel for
for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) {
auto & radial_no_ground_indices = out_no_ground_indices[i];
radial_no_ground_indices.indices.clear();
PointsCentroid ground_cluster;
ground_cluster.initialize();
std::vector<GridCenter> gnd_grids;
GridCenter curr_gnd_grid;

// check empty ray
if (in_radial_ordered_clouds[i].size() == 0) {
continue;
}

// check the first point in ray
auto * p = &in_radial_ordered_clouds[i][0];
PointRef * prev_p;
prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point
// // check the first point in ray
auto p = &in_radial_ordered_clouds[i][0];
auto prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point

bool initialized_first_gnd_grid = false;
bool prev_list_init = false;

for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) {
p = &in_radial_ordered_clouds[i][j];
float global_slope_p = std::atan(p->orig_point->z / p->radius);
float non_ground_height_threshold_local = non_ground_height_threshold_;
if (p->orig_point->x < low_priority_region_x_) {
non_ground_height_threshold_local =
non_ground_height_threshold_ * abs(p->orig_point->x / low_priority_region_x_);
}
// classify first grid's point cloud
if (
!initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ &&
p->orig_point->z > non_ground_height_threshold_local) {
out_no_ground_indices.indices.push_back(p->orig_index);
radial_no_ground_indices.indices.push_back(p->orig_index);
p->point_state = PointLabel::NON_GROUND;
prev_p = p;
continue;
}

if (
!initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ &&
abs(p->orig_point->z) < non_ground_height_threshold_local) {
ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index);
p->point_state = PointLabel::GROUND;
initialized_first_gnd_grid = static_cast<bool>(p->grid_id - prev_p->grid_id);
prev_p = p;
continue;
}

if (!initialized_first_gnd_grid) {
prev_p = p;
continue;
}

// initialize lists of previous gnd grids
if (prev_list_init == false && initialized_first_gnd_grid == true) {
float h = ground_cluster.getAverageHeight();
float r = ground_cluster.getAverageRadius();
initializeFirstGndGrids(h, r, p->grid_id, gnd_grids);
prev_list_init = true;
}

if (prev_list_init == false && initialized_first_gnd_grid == false) {
// assume first gnd grid is zero
initializeFirstGndGrids(0.0f, p->radius, p->grid_id, gnd_grids);
prev_list_init = true;
}

// move to new grid
if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) {
// check if the prev grid have ground point cloud
if (use_recheck_ground_cluster_) {
recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
recheckGroundCluster(
ground_cluster, non_ground_height_threshold_, radial_no_ground_indices);
}
curr_gnd_grid.radius = ground_cluster.getAverageRadius();
curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();
curr_gnd_grid.max_height = ground_cluster.getMaxHeight();
curr_gnd_grid.grid_id = prev_p->grid_id;
gnd_grids.push_back(curr_gnd_grid);
ground_cluster.initialize();
}
// classify
if (p->orig_point->z - gnd_grids.back().avg_height > detection_range_z_max_) {
p->point_state = PointLabel::OUT_OF_RANGE;
prev_p = p;
continue;
}
float points_xy_distance = std::hypot(
p->orig_point->x - prev_p->orig_point->x, p->orig_point->y - prev_p->orig_point->y);
if (
prev_p->point_state == PointLabel::NON_GROUND &&
points_xy_distance < split_points_distance_tolerance_ &&
p->orig_point->z > prev_p->orig_point->z) {
p->point_state = PointLabel::NON_GROUND;
out_no_ground_indices.indices.push_back(p->orig_index);
radial_no_ground_indices.indices.push_back(p->orig_index);
prev_p = p;
continue;
}

if (global_slope_p > global_slope_max_angle_rad_) {
out_no_ground_indices.indices.push_back(p->orig_index);
radial_no_ground_indices.indices.push_back(p->orig_index);
prev_p = p;
continue;
}
// gnd grid is continuous, the last gnd grid is close
uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id +
gnd_grid_buffer_size_ + gnd_grid_continual_thresh_;
if (
p->grid_id < next_gnd_grid_id_thresh &&
p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) {
checkContinuousGndGrid(*p, gnd_grids);

} else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) {
checkDiscontinuousGndGrid(*p, gnd_grids);
} else {
checkBreakGndGrid(*p, gnd_grids);
}
if (p->point_state == PointLabel::NON_GROUND) {
out_no_ground_indices.indices.push_back(p->orig_index);
radial_no_ground_indices.indices.push_back(p->orig_index);

Check warning on line 410 in perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

ScanGroundFilterComponent::classifyPointCloudGridScan already has high cyclomatic complexity, and now it increases in Lines of Code from 109 to 112. 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.
} else if (p->point_state == PointLabel::GROUND) {
ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index);
}
Expand All @@ -413,17 +418,20 @@

void ScanGroundFilterComponent::classifyPointCloud(
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
pcl::PointIndices & out_no_ground_indices)
std::vector<pcl::PointIndices> & out_no_ground_indices)
{
out_no_ground_indices.indices.clear();
out_no_ground_indices.resize(in_radial_ordered_clouds.size());

Check warning on line 423 in perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp#L423 

Added line #L423 was not covered by tests

const pcl::PointXYZ init_ground_point(0, 0, 0);
pcl::PointXYZ virtual_ground_point(0, 0, 0);
calcVirtualGroundOrigin(virtual_ground_point);

// point classification algorithm
// sweep through each radial division
// point classification algorithm
// sweep through each radial division
#pragma omp parallel for

Check warning on line 431 in perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp#L431 

Added line #L431 was not covered by tests
for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) {
auto & radial_no_ground_indices = out_no_ground_indices[i];
radial_no_ground_indices.indices.clear();
float prev_gnd_radius = 0.0f;
float prev_gnd_slope = 0.0f;
float points_distance = 0.0f;
Expand Down Expand Up @@ -498,12 +506,12 @@
non_ground_cluster.initialize();
}
if (p->point_state == PointLabel::NON_GROUND) {
out_no_ground_indices.indices.push_back(p->orig_index);
radial_no_ground_indices.indices.push_back(p->orig_index);
} else if ( // NOLINT
(prev_point_label == PointLabel::NON_GROUND) &&
(p->point_state == PointLabel::POINT_FOLLOW)) {
p->point_state = PointLabel::NON_GROUND;
out_no_ground_indices.indices.push_back(p->orig_index);
radial_no_ground_indices.indices.push_back(p->orig_index);

Check warning on line 514 in perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

ScanGroundFilterComponent::classifyPointCloud already has high cyclomatic complexity, and now it increases in Lines of Code from 98 to 101. 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.
} else if ( // NOLINT
(prev_point_label == PointLabel::GROUND) && (p->point_state == PointLabel::POINT_FOLLOW)) {
p->point_state = PointLabel::GROUND;
Expand All @@ -527,11 +535,14 @@
}

void ScanGroundFilterComponent::extractObjectPoints(
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
const std::vector<pcl::PointIndices> & in_indices,
pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr)
{
for (const auto & i : in_indices.indices) {
out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
for (const auto & indices : in_indices) {
for (const auto & i : indices.indices) {
out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
}
}
}

Expand All @@ -546,7 +557,7 @@

std::vector<PointCloudRefVector> radial_ordered_points;

pcl::PointIndices no_ground_indices;
std::vector<pcl::PointIndices> no_ground_indices;
pcl::PointCloud<pcl::PointXYZ>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size());

Expand Down
Loading