feat(autoware_pointcloud_preprocessor): distortion corrector node update azimuth and distance

sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml

@@ -3,3 +3,4 @@
     base_frame: base_link
     use_imu: true
     use_3d_distortion_correction: false
+    sensor_azimuth_coordinate: ""

sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md

@@ -48,3 +48,20 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
 
 - The node requires time synchronization between the topics from lidars, twist, and IMU.
 - If you want to use a 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty.
+- The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame and the `sensor_azimuth_coordinate` parameter is set to either `velodyne` or `hesai`. The azimuth values are calculated using the `cv::fastAtan2` function. Please note that updating the azimuth and distance fields increases the execution time by approximately 13%. Additionally, due to the `cv::fastAtan2` algorithm's accuracy of about 0.3 degrees, there is a **possibility of changing beam order for high azimuth resolution LiDAR**.
+- Currently, `sensor_azimuth_coordinate` only supports `velodyne` and `hesai`. The `velodyne` and `hesai` coordinates are illustrated below. If a LiDAR uses the same coordinates as `velodyne` or `hesai`, users can use them as well.
+
+<table>
+  <tr>
+    <td><img src="./image/velodyne.drawio.png" alt="velodyne azimuth coordinate"></td>
+    <td><img src="./image/hesai.drawio.png" alt="hesai azimuth coordinate"></td>
+   </tr>
+   <tr>
+    <td><p style="text-align: center;">Velodyne azimuth coordinate.</p></td>
+    <td><p style="text-align: center;">Hesai azimuth coordinate</p></td>
+  </tr>
+</table>
+
+## References/External links
+
+<https://docs.opencv.org/3.4/db/de0/group__core__utils.html#ga7b356498dd314380a0c386b059852270>

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -36,6 +36,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include <opencv2/opencv.hpp>
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/buffer_interface.h>
 #include <tf2_ros/transform_listener.h>
@@ -50,8 +52,8 @@ namespace autoware::pointcloud_preprocessor
 class DistortionCorrectorBase
 {
 public:
-  virtual bool pointcloud_transform_exists() = 0;
-  virtual bool pointcloud_transform_needed() = 0;
+  virtual bool pointcloudTransformExists() = 0;
+  virtual bool pointcloudTransformNeeded() = 0;
   virtual std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue() = 0;
   virtual std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue() = 0;
 
@@ -62,7 +64,9 @@ class DistortionCorrectorBase
   virtual void setPointCloudTransform(
     const std::string & base_frame, const std::string & lidar_frame) = 0;
   virtual void initialize() = 0;
-  virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+  virtual void undistortPointCloud(
+    bool use_imu, std::string sensor_azimuth_coordinate,
+    sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
 };
 
 template <class T>
@@ -103,16 +107,18 @@ class DistortionCorrector : public DistortionCorrectorBase
   {
     static_tf_buffer_ = std::make_unique<autoware::universe_utils::StaticTransformBuffer>();
   }
-  bool pointcloud_transform_exists();
-  bool pointcloud_transform_needed();
+  bool pointcloudTransformExists();
+  bool pointcloudTransformNeeded();
   std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
   std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue();
   void processTwistMessage(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override;
 
   void processIMUMessage(
     const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
-  void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override;
+  void undistortPointCloud(
+    bool use_imu, std::string sensor_azimuth_coordinate,
+    sensor_msgs::msg::PointCloud2 & pointcloud) override;
   bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
 };
 

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp

@@ -52,6 +52,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
   std::string base_frame_;
   bool use_imu_;
   bool use_3d_distortion_correction_;
+  std::string sensor_azimuth_coordinate_;
 
   std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
 

sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json

@@ -20,9 +20,19 @@
           "type": "boolean",
           "description": "Use 3d distortion correction algorithm, otherwise, use 2d distortion correction algorithm.",
           "default": "false"
+        },
+        "sensor_azimuth_coordinate": {
+          "type": "string",
+          "description": "The azimuth coordinate of sensor. Currently support Hesai and Velodyne.",
+          "default": ""
         }
       },
-      "required": ["base_frame", "use_imu", "use_3d_distortion_correction"]
+      "required": [
+        "base_frame",
+        "use_imu",
+        "use_3d_distortion_correction",
+        "sensor_azimuth_coordinate"
+      ]
     }
   },
   "properties": {

sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -1,4 +1,4 @@
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -25,13 +25,13 @@
 {
 
 template <class T>
-bool DistortionCorrector<T>::pointcloud_transform_exists()
+bool DistortionCorrector<T>::pointcloudTransformExists()
 {
   return pointcloud_transform_exists_;
 }
 
 template <class T>
-bool DistortionCorrector<T>::pointcloud_transform_needed()
+bool DistortionCorrector<T>::pointcloudTransformNeeded()
 {
   return pointcloud_transform_needed_;
 }
@@ -189,75 +189,96 @@
 
 template <class T>
 void DistortionCorrector<T>::undistortPointCloud(
-  bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud)
+  bool use_imu, std::string sensor_azimuth_coordinate, sensor_msgs::msg::PointCloud2 & pointcloud)
 {
   if (!isInputValid(pointcloud)) return;
 
   sensor_msgs::PointCloud2Iterator<float> it_x(pointcloud, "x");
   sensor_msgs::PointCloud2Iterator<float> it_y(pointcloud, "y");
   sensor_msgs::PointCloud2Iterator<float> it_z(pointcloud, "z");
+  sensor_msgs::PointCloud2Iterator<float> it_azimuth(pointcloud, "azimuth");
+  sensor_msgs::PointCloud2Iterator<float> it_distance(pointcloud, "distance");
   sensor_msgs::PointCloud2ConstIterator<std::uint32_t> it_time_stamp(pointcloud, "time_stamp");
 
   double prev_time_stamp_sec{
     pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)};
   const double first_point_time_stamp_sec{
     pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)};
 
   std::deque<geometry_msgs::msg::TwistStamped>::iterator it_twist;
   std::deque<geometry_msgs::msg::Vector3Stamped>::iterator it_imu;
   getTwistAndIMUIterator(use_imu, first_point_time_stamp_sec, it_twist, it_imu);
 
   // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
   double twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds();
   double imu_stamp{0.0};
   if (use_imu && !angular_velocity_queue_.empty()) {
     imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds();
   }
 
   // If there is a point in a pointcloud that cannot be associated, record it to issue a warning
   bool is_twist_time_stamp_too_late = false;
   bool is_imu_time_stamp_too_late = false;
   bool is_twist_valid = true;
   bool is_imu_valid = true;
   double global_point_stamp;
 
   for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
     is_twist_valid = true;
     is_imu_valid = true;
 
     global_point_stamp =
       pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp);
 
     // Get closest twist information
     while (it_twist != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) {
       ++it_twist;
       twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds();
     }
     if (std::abs(global_point_stamp - twist_stamp) > 0.1) {
       is_twist_time_stamp_too_late = true;
       is_twist_valid = false;
     }
 
     // Get closest IMU information
     if (use_imu && !angular_velocity_queue_.empty()) {
       while (it_imu != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) {
         ++it_imu;
         imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds();
       }
 
       if (std::abs(global_point_stamp - imu_stamp) > 0.1) {
         is_imu_time_stamp_too_late = true;
         is_imu_valid = false;
       }
     } else {
       is_imu_valid = false;
     }
 
     float time_offset = static_cast<float>(global_point_stamp - prev_time_stamp_sec);
 
     // Undistort a single point based on the strategy
     undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
 
+    if (!sensor_azimuth_coordinate.empty() && pointcloudTransformNeeded()) {
+      // Input frame should be in the sensor frame.
+      *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
+      float cartesian_coordinate_azimuth = cv::fastAtan2(*it_y, *it_x);
+      if (sensor_azimuth_coordinate == "velodyne") {
+        *it_azimuth = (360 - cartesian_coordinate_azimuth) * M_PI / 180;
+      } else if (sensor_azimuth_coordinate == "hesai") {
+        *it_azimuth = (90 - cartesian_coordinate_azimuth) < 0
+                        ? (90 - cartesian_coordinate_azimuth + 360) * M_PI / 180
+                        : (90 - cartesian_coordinate_azimuth) * M_PI / 180;
+      } else {
+        throw std::runtime_error(
+          sensor_azimuth_coordinate + " azimuth coordinate is not supported");
+      }
+
+      ++it_azimuth;
+      ++it_distance;
+    }
+
     prev_time_stamp_sec = global_point_stamp;
   }
 

sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp

@@ -35,6 +35,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt
   base_frame_ = declare_parameter<std::string>("base_frame");
   use_imu_ = declare_parameter<bool>("use_imu");
   use_3d_distortion_correction_ = declare_parameter<bool>("use_3d_distortion_correction");
+  sensor_azimuth_coordinate_ = declare_parameter<std::string>("sensor_azimuth_coordinate");
 
   // Publisher
   {
@@ -91,9 +92,8 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou
   }
 
   distortion_corrector_->setPointCloudTransform(base_frame_, pointcloud_msg->header.frame_id);
-
   distortion_corrector_->initialize();
-  distortion_corrector_->undistortPointCloud(use_imu_, *pointcloud_msg);
+  distortion_corrector_->undistortPointCloud(use_imu_, sensor_azimuth_coordinate_, *pointcloud_msg);
 
   if (debug_publisher_) {
     auto pipeline_latency_ms =