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.

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(crop_box_filter): can cause error in the subsequent module #881

Conversation

plane-li
Copy link
Contributor

Signed-off-by: plane.li plane.li@autocore.ai

Description

the pr is to fix the "crop_box_filter node "which can cause error in the subsequent module.
related issue: #705

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.

  • There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

Signed-off-by: plane.li <plane.li@autocore.ai>
@@ -92,7 +83,7 @@ class CropBoxFilterComponent : public pointcloud_preprocessor::Filter
rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);

public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Copy link
Contributor

Choose a reason for hiding this comment

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

I think this can't be changed due to Rolling/Humble support.
https://github.com/autowarefoundation/autoware.universe/pull/804/files#r860083464

@codecov
Copy link

codecov bot commented May 10, 2022

Codecov Report

Merging #881 (e07f8c7) into main (c092de0) will decrease coverage by 0.00%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main    #881      +/-   ##
========================================
- Coverage   9.53%   9.53%   -0.01%     
========================================
  Files        931     931              
  Lines      57663   57696      +33     
  Branches   10401   10401              
========================================
  Hits        5500    5500              
- Misses     47636   47669      +33     
  Partials    4527    4527              
Flag Coverage Δ *Carryforward flag
differential 0.00% <0.00%> (?)
total 9.53% <0.00%> (ø) Carriedforward from c092de0

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files Coverage Δ
...cessor/crop_box_filter/crop_box_filter_nodelet.hpp 0.00% <0.00%> (ø)
...or/src/crop_box_filter/crop_box_filter_nodelet.cpp 0.00% <0.00%> (ø)

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update c092de0...e07f8c7. Read the comment docs.

@wep21 wep21 requested review from yukkysaito and satoshi-ota May 10, 2022 12:05
Comment on lines +97 to +103
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
impl_.setIndices(indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter(pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
Copy link
Contributor

Choose a reason for hiding this comment

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

As far as I know, there are two reasons not to use pcl cropbox.

Copy link
Contributor

Choose a reason for hiding this comment

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

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Oh, Now I understand why not the PCL.
However,at present, we have tried many times and found that the crop_box_filter can not be used before "down_sample" and "ground_filter". I think the reason is that "down_sample" and "ground_filter" nodes behind the crop_box_filter use the PCL. The may be some wrong parameters:
https://github.com/autowarefoundation/autoware.universe/blob/main/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp#L124-L130
more details at #705

@yukkysaito says "The conversion is too heavy since the data size is large." I think we can use "pcl down sample" before "pcl crop box filter" in the xxxx.launch.py to solve the problem.

At present ,Function availability is more important than performance.

Copy link
Contributor

Choose a reason for hiding this comment

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

However,at present, we have tried many times and found that the crop_box_filter can not be used before "down_sample" and "ground_filter". I think the reason is that "down_sample" and "ground_filter" nodes behind the crop_box_filter use the PCL.

Thank you for the reply.
It may have some wrong paramえters. I guess we need to see what is wrong.

@yukkysaito says "The conversion is too heavy since the data size is large." I think we can use "pcl down sample" before "pcl crop box filter" in the xxxx.launch.py to solve the problem.

Yes. It can solve to use "cropbox filter" after "downsample". But I think it is better to be able to use it even before downsample.
Also, there are three problems with downsampling.

  • Many DNN algorithms do not assume downsampling.
  • INTENSITY is removed by downsample filter(voxel grid filter).
  • Errors are increased in the distance to obstacles. It is harder to guarantee not to hit an obstacle.

At present ,Function availability is more important than performance.

I agree with you 👍

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Also, there are three problems with downsampling.

  • Many DNN algorithms do not assume downsampling.
  • INTENSITY is removed by downsample filter(voxel grid filter).
  • Errors are increased in the distance to obstacles. It is harder to guarantee not to hit an obstacle.

At present:
Our DNN directly uses the point cloud published by lidar_driver without any pcl handler.
and compared to pcl_crop_box_filter,DNN consumes more and more computing power and memory.

And in euclidean_cluster and ndt_scan_matcher , INTENSITY is useless.

Copy link
Member

Choose a reason for hiding this comment

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

I think the original error about invalid point comes from the fact that current implementation assumes that incoming points have height=1. (i.e., unordered points). Rather than changing the cropbox filter itself, I would suggest to change L126
from

  output.height = input->height;

to

  output.height = 1;

This is reasonable because the pointcloud would become unordered points once you delete certain number of points after cropbox filtering.

Copy link
Contributor Author

@plane-li plane-li May 13, 2022

Choose a reason for hiding this comment

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

Thank u! We have tried this modification method before,and it can not cause errors in subsequent nodes("down_sample" and "ground_filter"). But I'm not sure if other nodes using PCL will have errors.
@mitsudome-r

And I think modify the height to 1 is a nice idea and can solve the "larger calculation cost“ problem.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

thank you!
@mitsudome-r
By viewing relevant materials ,I think height to 1 is useful.

The below code is nice.
output.width = static_cast<uint32_t>(output.data.size() / output.height / output.point_step);
output.row_step = static_cast<uint32_t>(output.data.size() / output.height);

Beacuse the PCL is to heavy, I will do an other PR(change the height to 1) to fix the issue.

@yukkysaito yukkysaito added the component:sensing Data acquisition from sensors, drivers, preprocessing. (auto-assigned) label May 10, 2022
@plane-li plane-li changed the title fix: crop_box_filter fix(crop_box_filter): can cause error in the subsequent module May 11, 2022
@plane-li
Copy link
Contributor Author

New PR without PCL #951. I close this PR.

@plane-li plane-li closed this May 24, 2022
iwatake2222 pushed a commit to iwatake2222/autoware.universe that referenced this pull request Jan 17, 2025
…ion for approval (autowarefoundation#881)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:sensing Data acquisition from sensors, drivers, preprocessing. (auto-assigned)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants