feat(pointcloud_preprocessor): update azimuth and distance in distortion_correction_node

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
 
@@ -39,9 +39,18 @@ $ 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
+
+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>

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -36,6 +36,7 @@
 #include <tf2_ros/transform_listener.h>
 
 // Include tier4 autoware utils
+#include <opencv2/opencv.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
@@ -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

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -39,6 +39,7 @@
   // 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<bool>("update_azimuth_and_distance");
 
   // Publisher
   undistorted_points_pub_ =
@@ -192,116 +193,122 @@
   sensor_msgs::PointCloud2Iterator<float> it_x(points, "x");
   sensor_msgs::PointCloud2Iterator<float> it_y(points, "y");
   sensor_msgs::PointCloud2Iterator<float> it_z(points, "z");
+  sensor_msgs::PointCloud2Iterator<float> it_azimuth(points, "azimuth");
+  sensor_msgs::PointCloud2Iterator<float> it_distance(points, "distance");
   sensor_msgs::PointCloud2ConstIterator<double> it_time_stamp(points, time_stamp_field_name_);
 
   float theta{0.0f};
   float x{0.0f};
   float y{0.0f};
   double prev_time_stamp_sec{*it_time_stamp};
   const double first_point_time_stamp_sec{*it_time_stamp};
 
   auto twist_it = std::lower_bound(
     std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec,
     [](const geometry_msgs::msg::TwistStamped & x, const double t) {
       return rclcpp::Time(x.header.stamp).seconds() < t;
     });
   twist_it = twist_it == std::end(twist_queue_) ? std::end(twist_queue_) - 1 : twist_it;
 
   decltype(angular_velocity_queue_)::iterator imu_it;
   if (use_imu_ && !angular_velocity_queue_.empty()) {
     imu_it = std::lower_bound(
       std::begin(angular_velocity_queue_), std::end(angular_velocity_queue_),
       first_point_time_stamp_sec, [](const geometry_msgs::msg::Vector3Stamped & x, const double t) {
         return rclcpp::Time(x.header.stamp).seconds() < t;
       });
     imu_it =
       imu_it == std::end(angular_velocity_queue_) ? std::end(angular_velocity_queue_) - 1 : imu_it;
   }
 
   const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()};
 
   // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
   double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds();
 
   // For performance, instantiate outside of the for-loop
   tf2::Quaternion baselink_quat{};
   tf2::Transform baselink_tf_odom{};
   tf2::Vector3 point{};
   tf2::Vector3 undistorted_point{};
 
   // 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();
     }
 
     float v{static_cast<float>(twist_it->twist.linear.x)};
     float w{static_cast<float>(twist_it->twist.angular.z)};
 
     if (std::abs(*it_time_stamp - twist_stamp) > 0.1) {
       RCLCPP_WARN_STREAM_THROTTLE(
         get_logger(), *get_clock(), 10000 /* ms */,
         "twist time_stamp is too late. Could not interpolate.");
       v = 0.0f;
       w = 0.0f;
     }
 
     if (use_imu_ && !angular_velocity_queue_.empty()) {
       // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
       double imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
 
       for (;
            (imu_it != std::end(angular_velocity_queue_) - 1 &&
             *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds());
            ++imu_it) {
       }
 
       while (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) {
         ++imu_it;
         imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
       }
 
       if (std::abs(*it_time_stamp - imu_stamp) > 0.1) {
         RCLCPP_WARN_STREAM_THROTTLE(
           get_logger(), *get_clock(), 10000 /* ms */,
           "imu time_stamp is too late. Could not interpolate.");
       } else {
         w = static_cast<float>(imu_it->vector.z);
       }
     }
 
     const auto time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
     point.setValue(*it_x, *it_y, *it_z);
 
     if (need_transform) {
       point = tf2_base_link_to_sensor_inv * point;
     }
 
     theta += w * time_offset;
     baselink_quat.setValue(
       0, 0, tier4_autoware_utils::sin(theta * 0.5f),
       tier4_autoware_utils::cos(theta * 0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta);
     const float dis = v * time_offset;
     x += dis * tier4_autoware_utils::cos(theta);
     y += dis * tier4_autoware_utils::sin(theta);
 
     baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0));
     baselink_tf_odom.setRotation(baselink_quat);
 
     undistorted_point = baselink_tf_odom * point;
 
     if (need_transform) {
       undistorted_point = tf2_base_link_to_sensor * undistorted_point;
     }
 
     *it_x = static_cast<float>(undistorted_point.getX());
     *it_y = static_cast<float>(undistorted_point.getY());
     *it_z = static_cast<float>(undistorted_point.getZ());
 
+    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;
+    }
     prev_time_stamp_sec = *it_time_stamp;
   }
   return true;