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

[vividf]

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.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

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.

[knzo25]

@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]

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

[meliketanrikulu]

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:
   
   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]

@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]

@drwnz This PR requires your approval 😃

[drwnz]

LGTM 👍

[drwnz]

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