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

feat(pointcloud_preprocessor): support for 3d distortion correction algorithm and refactor distortion correction node #7137

Merged

Conversation

vividf
Copy link
Contributor

@vividf vividf commented May 27, 2024

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:

  • once the node gets the TF information between imu, baselink, and lidar, it will stop getting the transformation again.
  • add 3d undistorted algorithm
  • refactor the code.
  • Add parameter file, parameter schema, and launch for this node.

Related links

#7031
#6657

Tests performed

Run the node

ros2 launch pointcloud_preprocessor distortion_corrector.launch.xml

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

Minimum Maximum Average
Time 7.15 15.29 10.09

After the PR
Test 1:

Minimum Maximum Average
Time 7.516 12.719 9.47

Test2:

Minimum Maximum Average
Time 7.5 14.9 10.5

Input: mirror pointcloud (58 frames)
distortion corrector: 3d

After the PR

Minimum Maximum Average
Time 13.02 22.26 16.05

Per point calculation time

  • test with 58 frames * 3 times
  • 2d algorithm

Before PR

Minimum Maximum Average Std Dev 50% 99%
Time 4.000000e-08 3.256100e-05 5.722510e-08 3.861742e-08 5.600000e-08 7.000000e-08

After PR

Minimum Maximum Average Std Dev 50% 99%
Time 4.400000e-08 3.476100e-05 5.898946e-08 4.029356e-08 5.800000e-08 7.200000e-08

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:

  • Blue pc : Input of the distortion corrector
  • White pc : Output of the distortion corrector
    We can see here pc which is output of the distortion corrector does not changes in direction z

After this PR:

  • Blue pc : Input of the distortion corrector
  • White pc : Output of the distortion corrector

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.

  • The PR follows the pull request guidelines.
  • The PR has been properly tested.
  • The PR has been reviewed by the code owners.

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.
  • The PR is ready for merge.

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

@github-actions github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:sensing Data acquisition from sensors, drivers, preprocessing. (auto-assigned) labels May 27, 2024
vividf added 2 commits May 27, 2024 17:34
Signed-off-by: vividf <[email protected]>
@vividf vividf marked this pull request as ready for review May 27, 2024 15:07
@knzo25
Copy link
Contributor

knzo25 commented Jun 4, 2024

@vividf
This PR and #7031 contain all the core changes to the distortion correction and thus makes no sense (to me) to have them split like this. If you only want us to check the refactor part in this PR, should the base not be the other PR rather than main? Otherwise it is really double work.

@knzo25
Copy link
Contributor

knzo25 commented Jun 4, 2024

(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)

Copy link
Contributor

@meliketanrikulu meliketanrikulu left a 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

  1. Distortion Corrector Input/Output point cloud Difference on speed bumps:
    distortion_corrector_temmuz_2
    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).
  2. 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

@knzo25
Copy link
Contributor

knzo25 commented Jul 1, 2024

@vividf
In case you have not done this before, I will leave the comment here:
It seems some of your commits were not signed off, which causes DCO to fail (it is required).
If you see the instructions, you can rebase to fix it, which is the clean way of doing things, but if you prefer, you can also click the Set DCO to pass button (please do not forget the -s from now on)

@vividf
Copy link
Contributor Author

vividf commented Jul 2, 2024

@drwnz This PR requires your approval 😃

vividf added 2 commits July 3, 2024 14:40
…f/autoware.universe into refactor/distortion_corrector_node
@vividf vividf requested a review from drwnz July 3, 2024 05:42
Copy link
Contributor

@drwnz drwnz left a comment

Choose a reason for hiding this comment

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

LGTM 👍

@drwnz
Copy link
Contributor

drwnz commented Jul 3, 2024

@vividf it would be good to sign all your commits to pass DCO.

@vividf vividf enabled auto-merge (squash) July 3, 2024 06:25
@vividf vividf disabled auto-merge July 3, 2024 06:45
@vividf vividf merged commit a7b8a10 into autowarefoundation:main Jul 3, 2024
28 of 30 checks passed
palas21 pushed a commit to palas21/autoware.universe that referenced this pull request Jul 12, 2024
…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]>
tby-udel pushed a commit to tby-udel/autoware.universe that referenced this pull request Jul 14, 2024
…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]>
KhalilSelyan pushed a commit that referenced this pull request Jul 22, 2024
…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]>
TomohitoAndo pushed a commit to tier4/autoware.universe that referenced this pull request Sep 10, 2024
…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]>
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) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) type:documentation Creating or refining documentation. (auto-assigned)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants