-
Notifications
You must be signed in to change notification settings - Fork 664
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
feat(pointcloud_preprocessor): support for 3d distortion correction algorithm and refactor distortion correction node #7137
feat(pointcloud_preprocessor): support for 3d distortion correction algorithm and refactor distortion correction node #7137
Conversation
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
(I am aware that you want to merge the other one first, in which case this PR automatically would show only the rafactor changes, but 1) main is blocked right now. 2) the PRs should be merged rather together) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Hello @vividf . I tested again with your new updates. I get same results. There doesn't seem to be any problem. Thanks for your your work. LGTM.
Related Tests
- Distortion Corrector Input/Output point cloud Difference on speed bumps:
Green: Input of 3D distortion corrector
Pink: Output of the 3D distortion corrector
The output of the Distortion Corrector approximates the location of the objects on the map. (When I visualized the map on this image, I did not add it because it looked complicated, but I observed that it approached the location on the map). - I tested with ground segmentation.
The problem caused by wobbling of the point cloud while passing through the speed bump caused ground points to be added to non-ground points. I observed that it was corrected with 3D distortion corrector.
Test video is here
White point cloud is non-ground points in the video
@vividf |
@drwnz This PR requires your approval 😃 |
...d_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
Outdated
Show resolved
Hide resolved
Signed-off-by: vividf <[email protected]>
…f/autoware.universe into refactor/distortion_corrector_node
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
LGTM 👍
@vividf it would be good to sign all your commits to pass DCO. |
Signed-off-by: vividf <[email protected]>
Signed-off-by: vividf <[email protected]>
…lgorithm and refactor distortion correction node (autowarefoundation#7137) * add support for 3d distortion correction Signed-off-by: vividf <[email protected]> * change parameter back to default and do small refactor Signed-off-by: vividf <[email protected]> * init version, need to double check Signed-off-by: vividf <[email protected]> * fix the logic error Signed-off-by: vividf <[email protected]> * temporary save, need to delete some code * init done, need to check for 3d as time is high Signed-off-by: vividf <[email protected]> * fix error Signed-off-by: vividf <[email protected]> * temporaily save * clean code Signed-off-by: vividf <[email protected]> * remove unused parameters Signed-off-by: vividf <[email protected]> * fix constructor error Signed-off-by: vividf <[email protected]> * fix spell errors Signed-off-by: vividf <[email protected]> * fix more spell errors Signed-off-by: vividf <[email protected]> * add undistorter to library Signed-off-by: vividf <[email protected]> * fix: fix cmake and change class name Signed-off-by: vividf <[email protected]> * Update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix imu to IMU Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: remove old naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * fix: fix file name and variable name Signed-off-by: vividf <[email protected]> * chore: fix invlaid virtual function definitions Signed-off-by: vividf <[email protected]> * fix: add sophus in package dependency Signed-off-by: vividf <[email protected]> * chore: remove brackets Signed-off-by: vividf <[email protected]> * chore: fix algorithm Signed-off-by: vividf <[email protected]> * chore: remove timestamp_field_name and add default parameter for 3d distortion Signed-off-by: vividf <[email protected]> * chore: fix known limits explanation Signed-off-by: vividf <[email protected]> * feat: add parameter schema and launch file for distortion correction node Signed-off-by: vividf <[email protected]> * chore: fix function name Signed-off-by: vividf <[email protected]> * chore: fix IMU function name Signed-off-by: vividf <[email protected]> * chore: fix twist and imu iterator function name Signed-off-by: vividf <[email protected]> * fix: add inline for undistortPointcloud function Signed-off-by: vividf <[email protected]> * chore: move varialbe to const Signed-off-by: vividf <[email protected]> * chore: fix grammar error Signed-off-by: vividf <[email protected]> * fix: fix inline function Signed-off-by: vividf <[email protected]> * chore: solve conflicts Signed-off-by: vividf <[email protected]> * fix: fix bug in previous code Signed-off-by: vividf <[email protected]> * fix: fix the template naming Signed-off-by: vividf <[email protected]> * fix: fix the component test Signed-off-by: vividf <[email protected]> * fix: solve conflicts Signed-off-by: vividf <[email protected]> --------- Signed-off-by: vividf <[email protected]> Co-authored-by: David Wong <[email protected]> Signed-off-by: palas21 <[email protected]>
…lgorithm and refactor distortion correction node (autowarefoundation#7137) * add support for 3d distortion correction Signed-off-by: vividf <[email protected]> * change parameter back to default and do small refactor Signed-off-by: vividf <[email protected]> * init version, need to double check Signed-off-by: vividf <[email protected]> * fix the logic error Signed-off-by: vividf <[email protected]> * temporary save, need to delete some code * init done, need to check for 3d as time is high Signed-off-by: vividf <[email protected]> * fix error Signed-off-by: vividf <[email protected]> * temporaily save * clean code Signed-off-by: vividf <[email protected]> * remove unused parameters Signed-off-by: vividf <[email protected]> * fix constructor error Signed-off-by: vividf <[email protected]> * fix spell errors Signed-off-by: vividf <[email protected]> * fix more spell errors Signed-off-by: vividf <[email protected]> * add undistorter to library Signed-off-by: vividf <[email protected]> * fix: fix cmake and change class name Signed-off-by: vividf <[email protected]> * Update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix imu to IMU Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: remove old naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * fix: fix file name and variable name Signed-off-by: vividf <[email protected]> * chore: fix invlaid virtual function definitions Signed-off-by: vividf <[email protected]> * fix: add sophus in package dependency Signed-off-by: vividf <[email protected]> * chore: remove brackets Signed-off-by: vividf <[email protected]> * chore: fix algorithm Signed-off-by: vividf <[email protected]> * chore: remove timestamp_field_name and add default parameter for 3d distortion Signed-off-by: vividf <[email protected]> * chore: fix known limits explanation Signed-off-by: vividf <[email protected]> * feat: add parameter schema and launch file for distortion correction node Signed-off-by: vividf <[email protected]> * chore: fix function name Signed-off-by: vividf <[email protected]> * chore: fix IMU function name Signed-off-by: vividf <[email protected]> * chore: fix twist and imu iterator function name Signed-off-by: vividf <[email protected]> * fix: add inline for undistortPointcloud function Signed-off-by: vividf <[email protected]> * chore: move varialbe to const Signed-off-by: vividf <[email protected]> * chore: fix grammar error Signed-off-by: vividf <[email protected]> * fix: fix inline function Signed-off-by: vividf <[email protected]> * chore: solve conflicts Signed-off-by: vividf <[email protected]> * fix: fix bug in previous code Signed-off-by: vividf <[email protected]> * fix: fix the template naming Signed-off-by: vividf <[email protected]> * fix: fix the component test Signed-off-by: vividf <[email protected]> * fix: solve conflicts Signed-off-by: vividf <[email protected]> --------- Signed-off-by: vividf <[email protected]> Co-authored-by: David Wong <[email protected]>
…lgorithm and refactor distortion correction node (#7137) * add support for 3d distortion correction Signed-off-by: vividf <[email protected]> * change parameter back to default and do small refactor Signed-off-by: vividf <[email protected]> * init version, need to double check Signed-off-by: vividf <[email protected]> * fix the logic error Signed-off-by: vividf <[email protected]> * temporary save, need to delete some code * init done, need to check for 3d as time is high Signed-off-by: vividf <[email protected]> * fix error Signed-off-by: vividf <[email protected]> * temporaily save * clean code Signed-off-by: vividf <[email protected]> * remove unused parameters Signed-off-by: vividf <[email protected]> * fix constructor error Signed-off-by: vividf <[email protected]> * fix spell errors Signed-off-by: vividf <[email protected]> * fix more spell errors Signed-off-by: vividf <[email protected]> * add undistorter to library Signed-off-by: vividf <[email protected]> * fix: fix cmake and change class name Signed-off-by: vividf <[email protected]> * Update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix imu to IMU Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: remove old naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * fix: fix file name and variable name Signed-off-by: vividf <[email protected]> * chore: fix invlaid virtual function definitions Signed-off-by: vividf <[email protected]> * fix: add sophus in package dependency Signed-off-by: vividf <[email protected]> * chore: remove brackets Signed-off-by: vividf <[email protected]> * chore: fix algorithm Signed-off-by: vividf <[email protected]> * chore: remove timestamp_field_name and add default parameter for 3d distortion Signed-off-by: vividf <[email protected]> * chore: fix known limits explanation Signed-off-by: vividf <[email protected]> * feat: add parameter schema and launch file for distortion correction node Signed-off-by: vividf <[email protected]> * chore: fix function name Signed-off-by: vividf <[email protected]> * chore: fix IMU function name Signed-off-by: vividf <[email protected]> * chore: fix twist and imu iterator function name Signed-off-by: vividf <[email protected]> * fix: add inline for undistortPointcloud function Signed-off-by: vividf <[email protected]> * chore: move varialbe to const Signed-off-by: vividf <[email protected]> * chore: fix grammar error Signed-off-by: vividf <[email protected]> * fix: fix inline function Signed-off-by: vividf <[email protected]> * chore: solve conflicts Signed-off-by: vividf <[email protected]> * fix: fix bug in previous code Signed-off-by: vividf <[email protected]> * fix: fix the template naming Signed-off-by: vividf <[email protected]> * fix: fix the component test Signed-off-by: vividf <[email protected]> * fix: solve conflicts Signed-off-by: vividf <[email protected]> --------- Signed-off-by: vividf <[email protected]> Co-authored-by: David Wong <[email protected]>
…lgorithm and refactor distortion correction node (autowarefoundation#7137) * add support for 3d distortion correction Signed-off-by: vividf <[email protected]> * change parameter back to default and do small refactor Signed-off-by: vividf <[email protected]> * init version, need to double check Signed-off-by: vividf <[email protected]> * fix the logic error Signed-off-by: vividf <[email protected]> * temporary save, need to delete some code * init done, need to check for 3d as time is high Signed-off-by: vividf <[email protected]> * fix error Signed-off-by: vividf <[email protected]> * temporaily save * clean code Signed-off-by: vividf <[email protected]> * remove unused parameters Signed-off-by: vividf <[email protected]> * fix constructor error Signed-off-by: vividf <[email protected]> * fix spell errors Signed-off-by: vividf <[email protected]> * fix more spell errors Signed-off-by: vividf <[email protected]> * add undistorter to library Signed-off-by: vividf <[email protected]> * fix: fix cmake and change class name Signed-off-by: vividf <[email protected]> * Update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: update sensing/pointcloud_preprocessor/docs/distortion-corrector.md Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix imu to IMU Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: fix company name Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: remove old naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * chore: change boolean variable naming Co-authored-by: David Wong <[email protected]> Signed-off-by: vividf <[email protected]> * fix: fix file name and variable name Signed-off-by: vividf <[email protected]> * chore: fix invlaid virtual function definitions Signed-off-by: vividf <[email protected]> * fix: add sophus in package dependency Signed-off-by: vividf <[email protected]> * chore: remove brackets Signed-off-by: vividf <[email protected]> * chore: fix algorithm Signed-off-by: vividf <[email protected]> * chore: remove timestamp_field_name and add default parameter for 3d distortion Signed-off-by: vividf <[email protected]> * chore: fix known limits explanation Signed-off-by: vividf <[email protected]> * feat: add parameter schema and launch file for distortion correction node Signed-off-by: vividf <[email protected]> * chore: fix function name Signed-off-by: vividf <[email protected]> * chore: fix IMU function name Signed-off-by: vividf <[email protected]> * chore: fix twist and imu iterator function name Signed-off-by: vividf <[email protected]> * fix: add inline for undistortPointcloud function Signed-off-by: vividf <[email protected]> * chore: move varialbe to const Signed-off-by: vividf <[email protected]> * chore: fix grammar error Signed-off-by: vividf <[email protected]> * fix: fix inline function Signed-off-by: vividf <[email protected]> * chore: solve conflicts Signed-off-by: vividf <[email protected]> * fix: fix bug in previous code Signed-off-by: vividf <[email protected]> * fix: fix the template naming Signed-off-by: vividf <[email protected]> * fix: fix the component test Signed-off-by: vividf <[email protected]> * fix: solve conflicts Signed-off-by: vividf <[email protected]> --------- Signed-off-by: vividf <[email protected]> Co-authored-by: David Wong <[email protected]>
Description
This PR solved the issue #6657.
This PR provides an option for the user to choose a 3d distortion corrector instead of using the current 2d distortion corrector.
Note that by using this, the time will increase around 50 percent.
This PR also refactored the distortion correction node to make the code cleaner and support different undistortion strategies (ex: hybrid 2d/3d) in the future.
Changes:
Related links
#7031
#6657
Tests performed
Run the node
I tested this PR and ensured the pointcloud output is the same and the processing time has a negligible time difference (the result is different every time).
Before the PR
Input: mirror pointcloud (58 frames)
distortion corrector: 2d
After the PR
Test 1:
Test2:
Input: mirror pointcloud (58 frames)
distortion corrector: 3d
After the PR
Per point calculation time
Before PR
After PR
Pointcloud comparison
@meliketanrikulu
Checked distortion corrector input point cloud and output point cloud for understanding difference coming with this PR. I did this test by viewing the moments when it passed through speed bumps.
Before this PR:
We can see here pc which is output of the distortion corrector does not changes in direction z
After this PR:
We can see here pc which is output of the distortion corrector is changes in direction z.
After seeing this change, I tested it with ground segmentation to see if it provided an improvement.
Test with Ground Segmentation:
You can see below that ground segmentation adds ground points to non-ground points when passing through speed bump before the changes occur.
Before 3d distortion corrector (ground segmentation test) Video link is here
I observed that this error did not occur after checkout the 3d distortion corrector branch.
After 3d distortion corrector (ground segmentation test) Video link is here
Notes for reviewers
Interface changes
ROS Topic Changes
ROS Parameter Changes
Effects on system behavior
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.