feat(pointcloud_preprocessor): update azimuth and distance in distortion_correction_node

[vividf]

Description

This PR solves issue #3896
This PR updates the azimuth and distance value based on the undistorted xyz value. The user can decide to use this feature by setting the parameter update_azimuth_and_distance to true.

The distance is calculated with the undistorted xyz value.
The azimuth is calculated by using an undistorted xy value with the cv::fastAtan2. By using cv::fastAtan2 instead of atan2, it can fasten the calculation 6 times faster with negligible error.

For different frames:

1. base_link: If the frame of the input pointcloud is base_link, even the update_azimuth_and_distance is set to true, we won't update the azimuth and distance, as it didn't make sense to update these value when the frame is in base_link.

2. sensor_frame: If the frame of the input pointcloud is sensor_frames, we will update the azimuth value and distance value for each point. However different sensors have different coordinate systems after the nebula driver. For example, Velodyne pointcloud defines the x-axis as 0 degrees, but the y-axis as 270 degrees. Hesai pointcloud defines the y-axis as 0 degrees and the x-axis as 90 degrees. Both of them are rotating clockwise.

To solve this problem, we update the azimuth value based on the Cartesian coordinate system, in which the x-axis is 0 degrees and the y-axis is 90 degrees. For instance, the below result is recorded when the input pointcloud of the distortion correction node is /sensing/lidar/top/pointcloud_raw_ex , and the frame_id is velodyne_top.

before distortion correction (Velodyne):
x: -11.3042, y: -3.0857, z: -1.90711, azimuth: 16200, distance: 11.872
after distortion correction:
x: -11.2044, y: -3.67424, z: -1.89818, azimuth: 19815.9, distance: 11.9432

Based on this situation, I write documentation in the readme to notify the user that they should be aware of the azimuth of the Lidar.

Tests performed

Testing rosbag is from the sample rosbag: https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/rosbag-replay-simulation/

Test with launching ros2 launch autoware_launch logging_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-rosbag vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

Result of the updated azimuth and distance value:

x: 7.65626, y : -10.5918, z: 1.58103, azimuth: 30585.4, distance: 13.1645

Notes for reviewers

@drwnz
The user can only change the parameter via the console with this single PR.
I also set the parameter in the nebula_node_container.launch.py in this PR (autowarefoundation/sample_sensor_kit_launch#81), which can change the parameter more easily.
I also want to do the same thing for the https://github.com/RobotecAI/awsim_sensor_kit_launch/blob/main/common_awsim_sensor_launch/launch/velodyne_node_container.launch.py#L132C1-L140C64

Interface changes

Added a new parameter update_azimuth_and_distance for the user to decide whether they want to update the azimuth and distance value.

Effects on system behavior

The default value of the update_azimuth_and_distance is False, which won't cause any change to the system. If the user wants to update the azimuth and distance, then they can set the parameter to True via launch file or console ros2 param set /distortion_corrector_node update_azimuth_and_distance True.

Once the update_azimuth_and_distance is set to True, the time cost will increase by around 13% for the distortion correction node (around 1 ms on my laptop).

Performance analysis:
Below testing runs the rosbag with a total of 81 frames of pointcloud.

1. Without only additional updates (only XYZ)

test1: 0.00715894
test2: 0.00704335
test3: 0.0070083
avg: 0.00707019666

2. With additional distance and azimuth (cv::fastAtan2) update

test1: 0.008114
test2: 0.00785841
test3: 0.00807316
avg: 0.00801519, increasing 13% from no updates

5. With additional distance and azimuth (atan2) update

test1: 0.0116086
test2: 0.0122381
test3: 0.0117111
avg: 0.0118526, increasing 63% from no updates

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.

[yukkysaito]

@badai-nguyen @miursh Can you review this?

[badai-nguyen]

@vividf Thank you for your PR.
Could you also fix spell-check error as it is required?
You also could check those spell errors locally by installing https://github.com/tier4/cspell-dicts

[badai-nguyen]

@vividf
As fastAtan2 function description, the accuracy is 0.3 deg which greater than VLS128 azimuth resolution (~0.2deg) so do you think some beams' order in a ring might be changed?
Anyway, IMO, even it's happened, it changes only 1 beam so it will not affect other filters result, such as Dual_return_filter which uses rertified topic as input. @miursh @drwnz Do you have any concern about this?

[drwnz]

I think it should be fine for now - if we run into issues we could optionally use a custom look up table implementation or use a method which has higher accuracy (at higher computational cost) but as you mention, I doubt that this will cause any problems for the modules which actually use azimuth.

[badai-nguyen]

@drwnz
I agree that it's acceptable to use fastAtan2 for now.
@vividf I think it is better to add the reference about fastAtan accuracy and a warning about posibility of changing beam order for high azimuth resolution LiDAR in filter README?

[drwnz]

Recommended:
When setting the parameter update_azimuth_and_distance to true, the node will calculate the per-point azimuth and distance values based on the undistorted XYZ using the atan2 function. The azimuth value will have its origin along the x-axis. Please make sure the frame coordinates are the same as the coordinate system mentioned above. For Autoware users, the frame_id of the input pointcloud is base_link, in which the coordinate system is the same as the coordinate system mentioned above, therefore it will not cause any issues. Also note that by setting the parameter update_azimuth_and_distance to true, the processing time will increase by approximately 13%.

[vividf]

Thanks! I updated the README, could you please help me review it again :).

[drwnz]

Please see the comments about README. Thanks!

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 7 lines in your changes are missing coverage. Please review.

Project coverage is 14.78%. Comparing base (b8c9701) to head (7b51d4f).
Report is 266 commits behind head on main.

❗ Current head 7b51d4f differs from pull request most recent head 55682dd. Consider uploading reports for the commit 55682dd to get more accurate results

Files	Patch %	Lines
.../src/distortion_corrector/distortion_corrector.cpp	0.00%	7 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5560      +/-   ##
==========================================
- Coverage   14.79%   14.78%   -0.01%     
==========================================
  Files        1915     1917       +2     
  Lines      132361   132052     -309     
  Branches    39339    39228     -111     
==========================================
- Hits        19580    19523      -57     
+ Misses      90946    90740     -206     
+ Partials    21835    21789      -46     

Flag	Coverage Δ		*Carryforward flag
differential	5.19% <0.00%> (?)
total	14.78% <ø> (-0.01%)	⬇️	Carriedforward from ebb4172

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

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.