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(detected_object_validation): add 3d pointcloud based validator #5327

Merged
merged 9 commits into from
Nov 15, 2023
Merged

fix(detected_object_validation): add 3d pointcloud based validator #5327

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
1afed95
fix: add 3d validation bind function
badai-nguyen Oct 10, 2023
c0bca36
fix: check validation point within shape
badai-nguyen Oct 17, 2023
d5b7bed
fix: add 2d validator option param
badai-nguyen Oct 16, 2023
e3cbf6c
chore: refactor
badai-nguyen Oct 17, 2023
c3317b6
chore: refactor
badai-nguyen Oct 18, 2023
e3389ee
Revert "chore: refactor"
badai-nguyen Oct 30, 2023
7ad22a7
chore: update docs and param file
badai-nguyen Oct 30, 2023
731e768
refactor: change to abstract class
badai-nguyen Nov 13, 2023
be15671
chore: add maintainer
badai-nguyen Nov 15, 2023
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
6 changes: 6 additions & 0 deletions ...ption/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,12 @@ class Debugger
}
}

void addNeighborPointcloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & input)
{
for (const auto & point : *input) {
neighbor_pointcloud_->push_back(point);
}
}
yukkysaito marked this conversation as resolved.
Show resolved Hide resolved
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Image description CodeRabbit

The addNeighborPointcloud method in the Debugger class is added to add points from an input point cloud to the neighbor_pointcloud_ member variable. However, there are a couple of improvements that can be made:

  1. It would be more efficient to reserve space for the new points in the neighbor_pointcloud_ before adding them. This can be done using the reserve method to avoid unnecessary reallocations.

  2. It's important to check if the input pointer is valid before accessing its contents. This can be done by adding a null check before the loop.

Here's the updated code:

void addNeighborPointcloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input)
{
  if (input)
  {
    neighbor_pointcloud_->reserve(neighbor_pointcloud_->size() + input->size());
    for (const auto& point : *input)
    {
      neighbor_pointcloud_->push_back(point);
    }
  }
}

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@badai-nguyen I think the point is a decent one and we might as well reflect it.

void addPointcloudWithinPolygon(const pcl::PointCloud<pcl::PointXYZ>::Ptr & input)
{
// pcl::PointCloud<pcl::PointXYZ>::Ptr input_xyz = toXYZ(input);
Expand Down
11 changes: 10 additions & 1 deletion ...ation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,15 +61,24 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
PointsNumThresholdParam points_num_threshold_param_;

std::shared_ptr<Debugger> debugger_;
bool using_2d_validator_;

private:
void onObjectsAndObstaclePointCloud(
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud);
void on3dObjectsAndObstaclePointCloud(
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud);
std::optional<size_t> getPointCloudNumWithinPolygon(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud);
std::optional<float> getMaxRadius(
std::optional<size_t> getPointCloudNumWithinShape(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud);
std::optional<float> getMaxRadius2D(
const autoware_auto_perception_msgs::msg::DetectedObject & object);
std::optional<float> getMaxRadius3D(
yukkysaito marked this conversation as resolved.
Show resolved Hide resolved
const autoware_auto_perception_msgs::msg::DetectedObject & object);
};
} // namespace obstacle_pointcloud_based_validator
Expand Down
1 change: 1 addition & 0 deletions perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,5 +11,6 @@
<remap from="~/output/objects" to="$(var output/objects)"/>
<param name="enable_debugger" value="false"/>
<param from="$(var obstacle_pointcloud_based_validator_param_path)"/>
<param name="using_2d_validator" value="true"/>
</node>
</launch>
156 changes: 146 additions & 10 deletions perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 Tier IV, Inc.

Check warning on line 1 in perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Overall Code Complexity 

This module has a mean cyclomatic complexity of 4.45 across 11 functions. The mean complexity threshold is 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 Down Expand Up @@ -85,24 +85,109 @@
tf_listener_(tf_buffer_),
sync_(SyncPolicy(10), objects_sub_, obstacle_pointcloud_sub_)
{
using std::placeholders::_1;
using std::placeholders::_2;
sync_.registerCallback(
std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2));
objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
"~/output/objects", rclcpp::QoS{1});

points_num_threshold_param_.min_points_num =
declare_parameter<std::vector<int64_t>>("min_points_num");
points_num_threshold_param_.max_points_num =
declare_parameter<std::vector<int64_t>>("max_points_num");
points_num_threshold_param_.min_points_and_distance_ratio =
declare_parameter<std::vector<double>>("min_points_and_distance_ratio");

using_2d_validator_ = declare_parameter<bool>("using_2d_validator");

using std::placeholders::_1;
using std::placeholders::_2;

if (using_2d_validator_) {
sync_.registerCallback(
std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2));
} else {
sync_.registerCallback(
std::bind(&ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud, this, _1, _2));
}

objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
"~/output/objects", rclcpp::QoS{1});

const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
}
void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud(
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud)
{
autoware_auto_perception_msgs::msg::DetectedObjects output, removed_objects;
output.header = input_objects->header;
removed_objects.header = input_objects->header;

// Transform to pointcloud frame
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
if (!object_recognition_utils::transformObjects(
*input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_,
transformed_objects)) {
return;
}

// Convert to PCL
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud);
if (obstacle_pointcloud->empty()) {
return;
}

// Create Kd-tree to search neighbor pointcloud to reduce cost
pcl::search::Search<pcl::PointXYZ>::Ptr kdtree =
pcl::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(false);
kdtree->setInputCloud(obstacle_pointcloud);
for (size_t i = 0; i < transformed_objects.objects.size(); ++i) {
const auto & transformed_object = transformed_objects.objects.at(i);
const auto object_label_id = transformed_object.classification.front().label;
const auto & object = input_objects->objects.at(i);
const auto & transformed_object_position =
transformed_object.kinematics.pose_with_covariance.pose.position;
const auto search_radius = getMaxRadius3D(transformed_object);
if (!search_radius) {
output.objects.push_back(object);
continue;
}
// Search neighbor pointcloud to reduce cost
pcl::PointCloud<pcl::PointXYZ>::Ptr neighbor_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> indices;
std::vector<float> distances;
pcl::PointXYZ trans_obj_position_pcl;
trans_obj_position_pcl.x = transformed_object_position.x;
trans_obj_position_pcl.y = transformed_object_position.y;
trans_obj_position_pcl.z = transformed_object_position.z;
kdtree->radiusSearch(trans_obj_position_pcl, search_radius.value(), indices, distances);
for (const auto & index : indices) {
neighbor_pointcloud->push_back(obstacle_pointcloud->at(index));
}

if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud);
// Filter object that have few pointcloud in them
const auto num = getPointCloudNumWithinShape(transformed_object, neighbor_pointcloud);
const auto object_distance =
std::hypot(transformed_object_position.x, transformed_object_position.y);
size_t min_pointcloud_num = std::clamp(
static_cast<size_t>(
points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) /
object_distance +
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The formula here is correct in 2D, but incorrect in 3D.
I don't think it will be a big problem with the 2D formula since it is after height crop, but you should leave it in the comments just in case.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@yukkysaito
IMO:

  • Since this is to adapt validation threshold as pointcloud density decreasing as far distance so the formular should be common for 2D and 3D. The min_points_num and max_points_num might be better changing for 2D and 3D.
  • The big difference happens when vehicle is in front of a up/down slope and the LiDAR resolution are largely changed in vertical axis.
  • As you mentioned, since we have height crop so I will leave it as is.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@badai-nguyen I agree.
Could you please leave a light comment in the code? 🙏

0.5f),
static_cast<size_t>(points_num_threshold_param_.min_points_num.at(object_label_id)),
static_cast<size_t>(points_num_threshold_param_.max_points_num.at(object_label_id)));
if (num) {
(min_pointcloud_num <= num.value()) ? output.objects.push_back(object)
: removed_objects.objects.push_back(object);
} else {
output.objects.push_back(object);
}
}
objects_pub_->publish(output);
if (debugger_) {
debugger_->publishRemovedObjects(removed_objects);
debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);
debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header);
}
}

Check warning on line 190 in perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Code Duplication 

The module contains 2 functions with similar structure: ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud,ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud. Avoid duplicated, aka copy-pasted, code inside the module. More duplication lowers the code health.

Check warning on line 190 in perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud has a cyclomatic complexity of 11, 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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since most of them are the same as callback functions of XY-plane, callback functions may be common.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@yukkysaito thank you for your comment. I changed at e3cbf6c

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm sorry, I didn't communicate it well.
It was a little different from my image.

Most parts of the callback function are the same for 2D and 3D, so I would be happy if you could abstract and absorb only the parts that make a difference in the interface (using template, function pointer, etc.). If it is difficult, it might be better to have separate callbacks.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@yukkysaito I am sorry for late response. Currently, I did not find the efficient way to refactor as your comment yet, so I would like revert to the separate callbacks way first. I will consider to refactor this ASAP in another PR.

void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud)
Expand Down Expand Up @@ -140,7 +225,7 @@
const auto & object = input_objects->objects.at(i);
const auto & transformed_object_position =
transformed_object.kinematics.pose_with_covariance.pose.position;
const auto search_radius = getMaxRadius(transformed_object);
const auto search_radius = getMaxRadius2D(transformed_object);
if (!search_radius) {
output.objects.push_back(object);
continue;
Expand All @@ -158,7 +243,6 @@
if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud);

// Filter object that have few pointcloud in them.
// TODO(badai-nguyen) add 3d validator option
const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud);
const auto object_distance =
std::hypot(transformed_object_position.x, transformed_object_position.y);
Expand All @@ -184,6 +268,48 @@
debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header);
}
}
std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinShape(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<pcl::Vertices> vertices_array;
pcl::Vertices vertices;

auto const & object_position = object.kinematics.pose_with_covariance.pose.position;
auto const object_height = object.shape.dimensions.z;
auto z_min = object_position.z - object_height / 2.0f;
auto z_max = object_position.z + object_height / 2.0f;

Polygon2d poly2d =
tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape);
if (bg::is_empty(poly2d)) return std::nullopt;

pcl::PointCloud<pcl::PointXYZ>::Ptr poly3d(new pcl::PointCloud<pcl::PointXYZ>);

for (size_t i = 0; i < poly2d.outer().size(); ++i) {
vertices.vertices.emplace_back(i);
vertices_array.emplace_back(vertices);
poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0);
}
pcl::CropHull<pcl::PointXYZ> cropper;
cropper.setInputCloud(pointcloud);
cropper.setDim(2);
cropper.setHullIndices(vertices_array);
cropper.setHullCloud(poly3d);
cropper.setCropOutside(true);
cropper.filter(*cropped_pointcloud_2d);

pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud_3d(new pcl::PointCloud<pcl::PointXYZ>);
cropped_pointcloud_3d->reserve(cropped_pointcloud_2d->size());
for (const auto & point : *cropped_pointcloud_2d) {
if (point.z > z_min && point.z < z_max) {
cropped_pointcloud_3d->push_back(point);
}
}
if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud_3d);
return cropped_pointcloud_3d->size();
}

std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
Expand Down Expand Up @@ -217,7 +343,7 @@
return cropped_pointcloud->size();
}

std::optional<float> ObstaclePointCloudBasedValidator::getMaxRadius(
std::optional<float> ObstaclePointCloudBasedValidator::getMaxRadius2D(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) {
Expand All @@ -234,6 +360,16 @@
return std::nullopt;
}
}
std::optional<float> ObstaclePointCloudBasedValidator::getMaxRadius3D(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
const auto max_dist_2d = getMaxRadius2D(object);
if (!max_dist_2d) {
return std::nullopt;
}
const auto object_height = object.shape.dimensions.z;
return std::hypot(max_dist_2d.value(), object_height / 2.0);
}

} // namespace obstacle_pointcloud_based_validator

Expand Down
Loading