fix(detected_object_validation): add 3d pointcloud based validator #5327
Conversation
Signed-off-by: badai-nguyen <[email protected]>
Signed-off-by: badai-nguyen <[email protected]>
github-actions
bot
added
the
component:perception
Signed-off-by: badai-nguyen <[email protected]>
badai-nguyen
force-pushed
the
fix/add_3d_point_based_validator
branch
from
6a8448b to
d5b7bed
Compare
|
I'll review 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 + | ||
| 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); | ||
| } | ||
| } |
There was a problem hiding this comment.
Since most of them are the same as callback functions of XY-plane, callback functions may be common.
There was a problem hiding this comment.
@yukkysaito thank you for your comment. I changed at e3cbf6c
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
@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.
perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp
Outdated
Show resolved
Hide resolved
| points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / | ||
| object_distance + |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
@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.
There was a problem hiding this comment.
@badai-nguyen I agree.
Could you please leave a light comment in the code? 🙏
|
WalkthroughThe changes introduce support for 3D shape-based validation of obstacle point clouds in the Changes
Uplevel your code reviews with CodeRabbit ProCodeRabbit ProIf you like this project, please support us by purchasing the Pro version. The Pro version has advanced context, superior noise reduction and several proprietary improvements compared to the open source version. Moreover, CodeRabbit Pro is free for open source projects. |
![github-actions[bot]](https://avatars.githubusercontent.com/in/15368?s=60&v=4){kind=small}
There was a problem hiding this comment.
Files selected (4)
- perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp (1)
- perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp (1)
- perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml (1)
- perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp (6)
Files not summarized due to errors (1)
- perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp (diff tokens exceeds limit)
Files skipped from review due to trivial changes (2)
- perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml
- perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp (diff too large)
Review comments generated (2)
- Review: 2
- LGTM: 0
Tips
Chat with 
CodeRabbit Bot (@coderabbitai)
- Reply on review comments left by this bot to ask follow-up questions. A review comment is a comment on a diff or a file.
- Invite the bot into a review comment chain by tagging
@coderabbitaiin a reply.
Code suggestions
- The bot may make code suggestions, but please review them carefully before committing since the line number ranges may be misaligned.
- You can edit the comment made by the bot and manually tweak the suggestion if it is slightly off.
Pausing incremental reviews
- Add
@coderabbitai: ignoreanywhere in the PR description to pause further reviews from the bot.
| void addNeighborPointcloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & input) | ||
| { | ||
| for (const auto & point : *input) { | ||
| neighbor_pointcloud_->push_back(point); | ||
| } | ||
| } |
There was a problem hiding this comment.
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:
-
It would be more efficient to reserve space for the new points in the
neighbor_pointcloud_before adding them. This can be done using thereservemethod to avoid unnecessary reallocations. -
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);
}
}
}There was a problem hiding this comment.
@badai-nguyen I think the point is a decent one and we might as well reflect it.
...lidation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
Outdated
Show resolved
Hide resolved
Signed-off-by: badai-nguyen <[email protected]>
Signed-off-by: badai-nguyen <[email protected]>
This reverts commit e3cbf6c.
github-actions
bot
added
the
type:documentation
Signed-off-by: badai-nguyen <[email protected]>
badai-nguyen
force-pushed
the
fix/add_3d_point_based_validator
branch
from
4b6acb9 to
7ad22a7
Compare
Signed-off-by: badai-nguyen <[email protected]>
badai-nguyen
added
the
run:build-and-test-differential
Codecov ReportAttention:
Additional details and impacted files@@ Coverage Diff @@
## main #5327 +/- ##
==========================================
- Coverage 14.78% 14.77% -0.02%
==========================================
Files 1644 1645 +1
Lines 113941 114060 +119
Branches 35160 35160
==========================================
Hits 16849 16849
- Misses 78136 78255 +119
Partials 18956 18956
*This pull request uses carry forward flags. Click here to find out more. ☔ View full report in Codecov by Sentry. |
|
@yukkysaito I refactored and changed to a single bind function at 731e768. Could you review this PR again? |
Signed-off-by: badai-nguyen <[email protected]>
…utowarefoundation#5327) * fix: add 3d validation bind function Signed-off-by: badai-nguyen <[email protected]> * fix: check validation point within shape Signed-off-by: badai-nguyen <[email protected]> * fix: add 2d validator option param Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * Revert "chore: refactor" This reverts commit e3cbf6c. * chore: update docs and param file Signed-off-by: badai-nguyen <[email protected]> * refactor: change to abstract class Signed-off-by: badai-nguyen <[email protected]> * chore: add maintainer Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
Description
Related link
TIER IV INTERNAL LINK
Tests performed
Tested by logging_simulator
CPU usage comparison: there was not significant change in cpu usage of obstacle-pointcloud-based-validator-node
Not applicable.
Effects on system behavior
Not applicable.
Pre-review checklist for the PR author
The PR author must check the checkboxes below when creating the PR.
In-review checklist for the PR reviewers
The PR reviewers must check the checkboxes below before approval.
Post-review checklist for the PR author
The PR author must check the checkboxes below before merging.
After all checkboxes are checked, anyone who has write access can merge the PR.
Summary by CodeRabbit
ObstaclePointCloudBasedValidatorclass.addNeighborPointcloudto theDebuggerclass, allowing users to add neighbor point clouds.obstacle_pointcloud_based_validator.launch.xml) to choose between 2D and 3D validation methods.ObstaclePointCloudBasedValidatorclass to reflect the changes and provide usage instructions.