From 08d9e45e13c2f9cbc20ce9c3a3c21d83945cbfbd Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 13 Nov 2023 13:36:28 +0900 Subject: [PATCH 01/17] distortion node is able to update azimuth and distance value based on new xyz --- .../distortion_corrector/distortion_corrector.hpp | 2 ++ .../distortion_corrector/distortion_corrector.cpp | 13 +++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 618957a2ac783..87c2f6b98fcd3 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -42,6 +42,7 @@ #include #include #include +#include namespace pointcloud_preprocessor { @@ -81,6 +82,7 @@ class DistortionCorrectorComponent : public rclcpp::Node std::string base_link_frame_ = "base_link"; std::string time_stamp_field_name_; bool use_imu_; + bool update_azimuth_and_distance_; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index dd38b85a2b56d..3284428bb1fbd 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -39,6 +39,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); + update_azimuth_and_distance_ = declare_parameter("update_azimuth_and_distance", false); // Publisher undistorted_points_pub_ = @@ -123,6 +124,8 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms return; } + update_azimuth_and_distance_ = get_parameter("update_azimuth_and_distance").as_bool(); + tf2::Transform tf2_base_link_to_sensor{}; getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor); @@ -192,6 +195,8 @@ bool DistortionCorrectorComponent::undistortPointCloud( sensor_msgs::PointCloud2Iterator it_x(points, "x"); sensor_msgs::PointCloud2Iterator it_y(points, "y"); sensor_msgs::PointCloud2Iterator it_z(points, "z"); + sensor_msgs::PointCloud2Iterator it_azimuth(points, "azimuth"); + sensor_msgs::PointCloud2Iterator it_distance(points, "distance"); sensor_msgs::PointCloud2ConstIterator it_time_stamp(points, time_stamp_field_name_); float theta{0.0f}; @@ -232,7 +237,7 @@ bool DistortionCorrectorComponent::undistortPointCloud( // For performance, avoid transform computation if unnecessary bool need_transform = points.header.frame_id != base_link_frame_; - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_azimuth, ++it_distance, ++it_time_stamp) { while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { ++twist_it; twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); @@ -302,6 +307,10 @@ bool DistortionCorrectorComponent::undistortPointCloud( *it_y = static_cast(undistorted_point.getY()); *it_z = static_cast(undistorted_point.getZ()); + if (update_azimuth_and_distance_ && !need_transform) { + *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z); + *it_azimuth = cv::fastAtan2(*it_y, *it_x) * 100; + } prev_time_stamp_sec = *it_time_stamp; } return true; @@ -310,4 +319,4 @@ bool DistortionCorrectorComponent::undistortPointCloud( } // namespace pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) \ No newline at end of file From 404e59a346a024c5f531fbe7b41a4abfdeabba0d Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 13 Nov 2023 13:39:19 +0900 Subject: [PATCH 02/17] wrong opencv head file --- .../distortion_corrector/distortion_corrector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 87c2f6b98fcd3..f34498201fd1c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include namespace pointcloud_preprocessor { From 6a1c6dac141bf026a38966bf6dc89c5f39872568 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 13 Nov 2023 14:33:10 +0900 Subject: [PATCH 03/17] update the distortion node docs --- .../docs/distortion-corrector.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index dcb03cc792cae..9b28b0d3367ee 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -39,9 +39,9 @@ $ ExactPointTime = TimeStamp + TimeOffset $ ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------- | ------ | ------------- | ----------------------------------------------------------- | -| `timestamp_field_name` | string | "time_stamp" | time stamp field name | -| `use_imu` | bool | true | use gyroscope for yaw rate if true, else use vehicle status | - +| Name | Type | Default Value | Description | +| ----------------------------- | ------ | ------------- | ----------------------------------------------------------- | +| `timestamp_field_name` | string | "time_stamp" | time stamp field name | +| `use_imu` | bool | true | use gyroscope for yaw rate if true, else use vehicle status | +| `update_azimuth_and_distance_`| bool | false | update the azimuth and distance based on undistorted xyz | ## Assumptions / Known limits From c99dbb7b01be443bf63d85c2f675f7cab2814dce Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 13 Nov 2023 14:36:30 +0900 Subject: [PATCH 04/17] update the distortion node docs --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 9b28b0d3367ee..1cbd20940e789 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -43,5 +43,5 @@ $ ExactPointTime = TimeStamp + TimeOffset $ | ----------------------------- | ------ | ------------- | ----------------------------------------------------------- | | `timestamp_field_name` | string | "time_stamp" | time stamp field name | | `use_imu` | bool | true | use gyroscope for yaw rate if true, else use vehicle status | -| `update_azimuth_and_distance_`| bool | false | update the azimuth and distance based on undistorted xyz | +| `update_azimuth_and_distance`| bool | false | update the azimuth and distance based on undistorted xyz | ## Assumptions / Known limits From 598bd425c0dcdbbfbc7536ebec42d36ebf77d1af Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 13 Nov 2023 14:59:05 +0900 Subject: [PATCH 05/17] fix if else condition to update value even in sensor frame --- .../src/distortion_corrector/distortion_corrector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 3284428bb1fbd..0abaa4da45c08 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -307,7 +307,7 @@ bool DistortionCorrectorComponent::undistortPointCloud( *it_y = static_cast(undistorted_point.getY()); *it_z = static_cast(undistorted_point.getZ()); - if (update_azimuth_and_distance_ && !need_transform) { + if (update_azimuth_and_distance_) { *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z); *it_azimuth = cv::fastAtan2(*it_y, *it_x) * 100; } From 447a71df84b38fe5a8437259c05bc33ad4299071 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 13 Nov 2023 16:36:29 +0900 Subject: [PATCH 06/17] update readme for distortion node --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 1cbd20940e789..905b452e5c497 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -44,4 +44,6 @@ $ ExactPointTime = TimeStamp + TimeOffset $ | `timestamp_field_name` | string | "time_stamp" | time stamp field name | | `use_imu` | bool | true | use gyroscope for yaw rate if true, else use vehicle status | | `update_azimuth_and_distance`| bool | false | update the azimuth and distance based on undistorted xyz | + ## Assumptions / Known limits +When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using atan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinates are same as the coordinate system mentioned in the above. For autoware user, the frame_id of input pointcloud is base_link, which the coordinate system is same as the coordinate system mentioned above, therefore it won't casue any issues. Also note that by setting the parameter `update_azimuth_and_distance` to True, the time will incrase around 13%. \ No newline at end of file From ab81c478cb325c9a2b30c7c13fa57bd43116948c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 13 Nov 2023 07:54:38 +0000 Subject: [PATCH 07/17] style(pre-commit): autofix --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 5 +++-- .../distortion_corrector/distortion_corrector.hpp | 2 +- .../src/distortion_corrector/distortion_corrector.cpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 905b452e5c497..03b04bf0811e9 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -43,7 +43,8 @@ $ ExactPointTime = TimeStamp + TimeOffset $ | ----------------------------- | ------ | ------------- | ----------------------------------------------------------- | | `timestamp_field_name` | string | "time_stamp" | time stamp field name | | `use_imu` | bool | true | use gyroscope for yaw rate if true, else use vehicle status | -| `update_azimuth_and_distance`| bool | false | update the azimuth and distance based on undistorted xyz | +| `update_azimuth_and_distance` | bool | false | update the azimuth and distance based on undistorted xyz | ## Assumptions / Known limits -When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using atan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinates are same as the coordinate system mentioned in the above. For autoware user, the frame_id of input pointcloud is base_link, which the coordinate system is same as the coordinate system mentioned above, therefore it won't casue any issues. Also note that by setting the parameter `update_azimuth_and_distance` to True, the time will incrase around 13%. \ No newline at end of file + +When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using atan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinates are same as the coordinate system mentioned in the above. For autoware user, the frame_id of input pointcloud is base_link, which the coordinate system is same as the coordinate system mentioned above, therefore it won't casue any issues. Also note that by setting the parameter `update_azimuth_and_distance` to True, the time will incrase around 13%. diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index f34498201fd1c..cab62613cc4a7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -36,13 +36,13 @@ #include // Include tier4 autoware utils +#include #include #include #include #include #include -#include namespace pointcloud_preprocessor { diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 0abaa4da45c08..4c107c41970a3 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -319,4 +319,4 @@ bool DistortionCorrectorComponent::undistortPointCloud( } // namespace pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) From 1ed9e2904aabe4f2c82edcecf6327c95e99c57ed Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 14 Nov 2023 14:12:27 +0900 Subject: [PATCH 08/17] Update distortion-corrector.md --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 03b04bf0811e9..5139d89ceb019 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -4,7 +4,7 @@ The `distortion_corrector` is a node that compensates pointcloud distortion caused by ego vehicle's movement during 1 scan. -Since the LiDAR sensor scans by rotating an internal laser, the resulting point cloud will be distorted if the ego-vehicle moves during a single scan (as shown by the figure below). The node corrects this by interpolating sensor data using odometry of ego-vehicle. +Since the LiDAR sensor scans by rotating an internal laser, the resulting point cloud will be distorted if the ego-vehicle moves during a single scan (as shown by the figure below). The node corrects this by interpolating sensor data using the odometry of the ego-vehicle. ## Inner-workings / Algorithms @@ -47,4 +47,4 @@ $ ExactPointTime = TimeStamp + TimeOffset $ ## Assumptions / Known limits -When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using atan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinates are same as the coordinate system mentioned in the above. For autoware user, the frame_id of input pointcloud is base_link, which the coordinate system is same as the coordinate system mentioned above, therefore it won't casue any issues. Also note that by setting the parameter `update_azimuth_and_distance` to True, the time will incrase around 13%. +When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using atan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinates are the same as the coordinate system mentioned above. For autoware users, the frame_id of input pointcloud is base_link, which the coordinate system is the same as the coordinate system mentioned above, therefore it won't cause any issues. Also note that by setting the parameter `update_azimuth_and_distance` to True, the time will increase by around 13%. From 8502d895dec672e13f1526db83e161c18e31c3fc Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 14 Nov 2023 16:09:27 +0900 Subject: [PATCH 09/17] remove parameter default value Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> --- .../src/distortion_corrector/distortion_corrector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 4c107c41970a3..2ac9460bfadf7 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -39,7 +39,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); - update_azimuth_and_distance_ = declare_parameter("update_azimuth_and_distance", false); + update_azimuth_and_distance_ = declare_parameter("update_azimuth_and_distance"); // Publisher undistorted_points_pub_ = From 2058788014a212d06d7789de0922cf9409ea4ae1 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 14 Nov 2023 16:10:02 +0900 Subject: [PATCH 10/17] remove get_parameter Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> --- .../src/distortion_corrector/distortion_corrector.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 2ac9460bfadf7..beb1f2fddc3ce 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -124,7 +124,6 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms return; } - update_azimuth_and_distance_ = get_parameter("update_azimuth_and_distance").as_bool(); tf2::Transform tf2_base_link_to_sensor{}; getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor); From dffbd56350dfa34a59a6b2accf4b6165f19fbc30 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Nov 2023 07:12:57 +0000 Subject: [PATCH 11/17] style(pre-commit): autofix --- .../src/distortion_corrector/distortion_corrector.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index beb1f2fddc3ce..a4424a21cc1a3 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -124,7 +124,6 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms return; } - tf2::Transform tf2_base_link_to_sensor{}; getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor); From d779a36dcd55d88c6a133a3d3b1ce9e1d112cae8 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 15 Nov 2023 15:32:58 +0900 Subject: [PATCH 12/17] Update distortion-corrector.md --- .../pointcloud_preprocessor/docs/distortion-corrector.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 5139d89ceb019..95c52c8d511eb 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -47,4 +47,9 @@ $ ExactPointTime = TimeStamp + TimeOffset $ ## Assumptions / Known limits -When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using atan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinates are the same as the coordinate system mentioned above. For autoware users, the frame_id of input pointcloud is base_link, which the coordinate system is the same as the coordinate system mentioned above, therefore it won't cause any issues. Also note that by setting the parameter `update_azimuth_and_distance` to True, the time will increase by around 13%. +When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using cv::fastAtan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinate of the input pointcloud is the same as the coordinate system mentioned above. For autoware users, when you launch the pointcloud preprocessor, the frame_id of input pointcloud is base_link, which the coordinate system is the same as the coordinate system mentioned above, therefore it won't cause any issues. + +Please also note that by setting the parameter `update_azimuth_and_distance` to True, the time will increase by around 13% and there is the **possibility of changing beam order for high azimuth resolution LiDAR**. + +## References/External links +https://docs.opencv.org/3.4/db/de0/group__core__utils.html#ga7b356498dd314380a0c386b059852270 From e2b95263bdf133ab0eab1d5158b18a4356e3da8f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 15 Nov 2023 06:34:49 +0000 Subject: [PATCH 13/17] style(pre-commit): autofix --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 95c52c8d511eb..2a41d8d956edb 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -47,9 +47,10 @@ $ ExactPointTime = TimeStamp + TimeOffset $ ## Assumptions / Known limits -When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using cv::fastAtan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinate of the input pointcloud is the same as the coordinate system mentioned above. For autoware users, when you launch the pointcloud preprocessor, the frame_id of input pointcloud is base_link, which the coordinate system is the same as the coordinate system mentioned above, therefore it won't cause any issues. +When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using cv::fastAtan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinate of the input pointcloud is the same as the coordinate system mentioned above. For autoware users, when you launch the pointcloud preprocessor, the frame_id of input pointcloud is base_link, which the coordinate system is the same as the coordinate system mentioned above, therefore it won't cause any issues. Please also note that by setting the parameter `update_azimuth_and_distance` to True, the time will increase by around 13% and there is the **possibility of changing beam order for high azimuth resolution LiDAR**. ## References/External links -https://docs.opencv.org/3.4/db/de0/group__core__utils.html#ga7b356498dd314380a0c386b059852270 + + From 78dde17aceffa3f49944da9593a0061b8d7599e8 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 15 Nov 2023 16:24:50 +0900 Subject: [PATCH 14/17] Update distortion-corrector.md --- .../pointcloud_preprocessor/docs/distortion-corrector.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 2a41d8d956edb..d9c23e96c3772 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -47,10 +47,10 @@ $ ExactPointTime = TimeStamp + TimeOffset $ ## Assumptions / Known limits -When setting the parameter `update_azimuth_and_distance` to True, the node will calculate the azimuth value based on the undistorted XYZ by using cv::fastAtan2, which will return the azimuth value with the coordinate system that the x-axis is 0 degrees and the y-axis is 90 degrees. Please make sure the frame coordinate of the input pointcloud is the same as the coordinate system mentioned above. For autoware users, when you launch the pointcloud preprocessor, the frame_id of input pointcloud is base_link, which the coordinate system is the same as the coordinate system mentioned above, therefore it won't cause any issues. +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 cv:fastAtan2 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. -Please also note that by setting the parameter `update_azimuth_and_distance` to True, the time will increase by around 13% and there is the **possibility of changing beam order for high azimuth resolution LiDAR**. +Please note that by setting the parameter `update_azimuth_and_distance` to `true`, the time will increase by around 13%. +Also note that due to the cv::fastAtan2 algorithm (accuracy is about 0.3 degrees), there is the **possibility of changing beam order for high azimuth resolution LiDAR**. ## References/External links - From ef03a19b33db313e9217a7fc3c10bce5d064549a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 15 Nov 2023 07:27:06 +0000 Subject: [PATCH 15/17] style(pre-commit): autofix --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index d9c23e96c3772..5c076c8db2a4b 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -49,8 +49,9 @@ $ ExactPointTime = TimeStamp + TimeOffset $ 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 cv:fastAtan2 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. -Please note that by setting the parameter `update_azimuth_and_distance` to `true`, the time will increase by around 13%. +Please note that by setting the parameter `update_azimuth_and_distance` to `true`, the time will increase by around 13%. Also note that due to the cv::fastAtan2 algorithm (accuracy is about 0.3 degrees), there is the **possibility of changing beam order for high azimuth resolution LiDAR**. ## References/External links + From 49299583c72c7d6dc5a5ae507e0bae9a2d86b401 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 15 Nov 2023 16:28:07 +0900 Subject: [PATCH 16/17] Update distortion-corrector.md --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 5c076c8db2a4b..ca993557cbd13 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -47,10 +47,10 @@ $ ExactPointTime = TimeStamp + TimeOffset $ ## Assumptions / Known limits -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 cv:fastAtan2 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. +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 `cv:fastAtan2` 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. Please note that by setting the parameter `update_azimuth_and_distance` to `true`, the time will increase by around 13%. -Also note that due to the cv::fastAtan2 algorithm (accuracy is about 0.3 degrees), there is the **possibility of changing beam order for high azimuth resolution LiDAR**. +Also note that due to the `cv::fastAtan2` algorithm (accuracy is about 0.3 degrees), there is the **possibility of changing beam order for high azimuth resolution LiDAR**. ## References/External links From 55682dda2e6ce44e3906f559cc7fe8bae9cb3055 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 14 Mar 2024 10:29:27 +0900 Subject: [PATCH 17/17] modify ring outlier filter node for supporting counter clockwise azimuth Signed-off-by: vividf --- .../distortion_corrector/distortion_corrector.hpp | 1 + .../src/distortion_corrector/distortion_corrector.cpp | 4 ++-- .../src/outlier_filter/ring_outlier_filter_nodelet.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index cab62613cc4a7..8a1e7c7464ca2 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -83,6 +83,7 @@ class DistortionCorrectorComponent : public rclcpp::Node std::string time_stamp_field_name_; bool use_imu_; bool update_azimuth_and_distance_; + float azimuth_factor_ = 100.0; }; } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 62969a93b2333..46996ef68e6bf 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -315,9 +315,9 @@ bool DistortionCorrectorComponent::undistortPointCloud( *it_y = static_cast(undistorted_point.getY()); *it_z = static_cast(undistorted_point.getZ()); - if (update_azimuth_and_distance_) { + if (update_azimuth_and_distance_ && need_transform) { *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z); - *it_azimuth = cv::fastAtan2(*it_y, *it_x) * 100; + *it_azimuth = cv::fastAtan2(*it_y, *it_x) * azimuth_factor_; } prev_time_stamp_sec = *it_time_stamp; } diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index e0c8443cfe57b..710d694155624 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -116,8 +116,8 @@ void RingOutlierFilterComponent::faster_filter( *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); const float & next_azimuth = *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); - float azimuth_diff = next_azimuth - current_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + float azimuth_diff = std::abs(next_azimuth - current_azimuth); + azimuth_diff = azimuth_diff > 18000.f ? 36000.f - azimuth_diff : azimuth_diff; const float & current_distance = *reinterpret_cast(&input->data[current_data_idx + distance_offset]);