feat(pointcloud_preprocessor): support 3d distortion corrector for distortion corrector 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.

Related links

#6657

Tests performed

Time comparison

Input: mirrored pointcloud (58 frames)
distortion corrector: 2d

Before the PR

	Minimum	Maximum	Average
Time	7.15	15.29	10.09

After the PR

	Minimum	Maximum	Average
Time	7.36	14.01	10.40

---

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

After the PR

	Minimum	Maximum	Average
Time	13.02	22.26	16.05

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.

[vividf]

@meliketanrikulu (cc @xmfcx )
Please provide your address for me to add you to the package maintainer list.
Thanks.

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-7031/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-7031/sensing/pointcloud_preprocessor/distortion-corrector/

[meliketanrikulu]

/review

[github-actions]

PR Review 🔍

⏱️ Estimated effort to review [1-5]	4, because the PR introduces significant changes to the distortion correction logic, including the addition of 3D distortion correction capabilities. The changes impact core functionality and require careful review of mathematical computations and transformations, as well as performance implications due to increased computation time.
🧪 Relevant tests	No
⚡ Possible issues	Possible Bug: The PR introduces a new parameter use_3d_distortion_correction but does not specify a default value in the declare_parameter call. This could lead to runtime errors if the parameter is not set externally.
	Performance Concern: The PR notes a 50% increase in processing time when using 3D distortion correction. This significant increase might affect the real-time capabilities of the system, especially in compute-constrained environments.
🔒 Security concerns	No

Code feedback:

---

relevant file	sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion	Consider initializing use_3d_distortion_correction_ with a default value to ensure predictable behavior when the parameter is not set externally. This change enhances robustness and prevents potential runtime errors. [important]
relevant line	use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction");

---

relevant file	sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion	To improve the performance of the 3D distortion correction, consider optimizing the matrix operations or exploring parallel processing techniques. This could help mitigate the performance impact noted in the PR description. [important]
relevant line	Eigen::Matrix4f transformation_matrix;

---

relevant file	sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion	Add error handling for the new transformations and calculations introduced for 3D distortion correction. This will ensure that any failures in transformation calculations are caught and handled gracefully, improving the reliability of the system. [important]
relevant line	transformation_matrix = Sophus::SE3f::exp(twist).matrix();

---

relevant file	sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion	Refactor the undistortPointCloud function to separate the 2D and 3D distortion correction logic into distinct methods. This separation can improve code readability and maintainability. [medium]
relevant line	if (use_3d_distortion_correction_) {

---

[meliketanrikulu]

PR Review 🔍

⏱️ Estimated effort to review [1-5]
4, because the PR introduces significant changes to the distortion correction logic, including the addition of 3D distortion correction capabilities. The changes impact core functionality and require careful review of mathematical computations and transformations, as well as performance implications due to increased computation time.

🧪 Relevant tests
No

⚡ Possible issues
Possible Bug: The PR introduces a new parameter use_3d_distortion_correction but does not specify a default value in the declare_parameter call. This could lead to runtime errors if the parameter is not set externally.

Performance Concern: The PR notes a 50% increase in processing time when using 3D distortion correction. This significant increase might affect the real-time capabilities of the system, especially in compute-constrained environments.

🔒 Security concerns
No

Code feedback:
relevant file sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion       ** Consider initializing use_3d_distortion_correction_ with a default value to ensure predictable behavior when the parameter is not set externally. This change enhances robustness and prevents potential runtime errors. [important] **
relevant line use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction");
relevant file sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion       ** To improve the performance of the 3D distortion correction, consider optimizing the matrix operations or exploring parallel processing techniques. This could help mitigate the performance impact noted in the PR description. [important] **
relevant line Eigen::Matrix4f transformation_matrix;
relevant file sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion       ** Add error handling for the new transformations and calculations introduced for 3D distortion correction. This will ensure that any failures in transformation calculations are caught and handled gracefully, improving the reliability of the system. [important] **
relevant line transformation_matrix = Sophus::SE3f::exp(twist).matrix();
relevant file sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
suggestion       ** Refactor the undistortPointCloud function to separate the 2D and 3D distortion correction logic into distinct methods. This separation can improve code readability and maintainability. [medium] **
relevant line if (use_3d_distortion_correction_) {

@vividf could you check this review

[meliketanrikulu]

Hello @vividf . I tested PR and I saw same results so I shared same videos and images.
Problem related this issue is input twist topic. Because default distortion corrector in autoware uses /sensing/vehicle_velocity_converter/twist_with_covariance topic. But this topic does not includes angular velocity fields. These fields are empty. I tested with /localization/twist_estimator/twist_with_covariance topic.
But /localization/twist_estimator/twist_with_covariance is localization package's (gyro_odometer) output. So I am not sure about using this directly as an input. Is there any suggestion for decide to input topic. What do you think about this.

[vividf]

@meliketanrikulu
True that probably we shouldn't take the twist information from localization.
The /sensing/vehicle_velocity_converter/twist_with_covariance topic did include the angular velocity field but only z-axis of the angular velocity has value. (I get from the Autoware sample-rosbag)

Also, usually the angular velocity from IMU (x, y, z) will replace the twist angular velocity.

Could you test it with the normal input and see if it still solve the problem?
Thanks!

[meliketanrikulu]

@meliketanrikulu True that probably we shouldn't take the twist information from localization. The /sensing/vehicle_velocity_converter/twist_with_covariance topic did include the angular velocity field but only z-axis of the angular velocity has value. (I get from the Autoware sample-rosbag)

Also, usually the angular velocity from IMU (x, y, z) will replace the twist angular velocity.

Could you test it with the normal input and see if it still solve the problem? Thanks!

Correct. I tested again with /sensing/vehicle_velocity_converter/twist_with_covariance topic and using imu. Its working when use_imu is true .
However current vehicle velocity converter node can not provide angular x ,y . Because it uses VelocityReport msg to fill twist msg. There is no information inside this message to fill these fields. If people want to use only twist msg (use_imu == false ) they can not use /sensing/vehicle_velocity_converter/twist_with_covariance topic.
We can add note on the README.md like this: If you want to use 3D distortion corrector without imu, please check that the linear and angular velocity fields of your twist message are not empty.

[meliketanrikulu]

@meliketanrikulu (cc @xmfcx ) Please provide your address for me to add you to the package maintainer list. Thanks.
Melike Tanrıkulu
[email protected]

[meliketanrikulu]

@vividf . I see that you changed your code according to this review. However, there is no parameter file for the other parameters in this node and they are initialized in the code. There is no config file created for this package. A new issue may be created for this in the future. However, depending on the current structure, I recommend that you enter the default value here as before. Because in this way the parameter value will not be set at all. Can you revert this change. We talked about this with @xmfcx and he approved it.

[knzo25]

@vividf

I am concerned a little about

Input: mirrored pointcloud (58 frames)
distortion corrector: 2d

Can you run experiments on a whole bag of XX1 and X2? (recording the processing times. or even better, oprofile/gperf/pef reports)
I see that even when 3d mode is disabled is still uses sophus and matrices. Since the code is quite short it can be specialized instead of handling both cases to keep using scalars.

[vividf]

@meliketanrikulu
Thanks a lot for your advice. I change them in the commit 026edf3

[vividf]

@knzo25
Thanks for the comment.
I will optimize the node again and avoid using the Eigen if the mode is disabled.
Btw, I didn't think when the mode is disabled, it is using Sophus.

[vividf]

To all the reviewers,
for this PR, please only focus on the algorithm concept and the result of using this algorithm.

As the new feature makes the node more complicated to understand and there are some redundant variables that might not be used for the 2D method, I create new PR (#7137) that refactors the whole distortion correction node.
Afterward, I will merge them together after both of the PRs are approved.

[knzo25]

@vividf
I was doing to approve, but the ci/cd is not passing in the spelling (I do not really see the reason for separating the PRs though).

[vividf]

I will close this PR to avoid duplicated reviews.
Please review this #7137