feat(pointcloud_preprocessor): support for 3d distortion correction algorithm and refactor distortion correction node

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -9,6 +9,7 @@ set(CGAL_DATA_DIR ".")
 
 find_package(OpenCV REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(Sophus REQUIRED)
 find_package(Boost REQUIRED)
 find_package(PCL REQUIRED)
 find_package(CGAL REQUIRED COMPONENTS Core)
@@ -19,6 +20,7 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
 )
 
@@ -76,6 +78,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp
   src/vector_map_filter/lanelet2_map_filter_nodelet.cpp
   src/distortion_corrector/distortion_corrector.cpp
+  src/distortion_corrector/distortion_corrector_node.cpp
   src/blockage_diag/blockage_diag_nodelet.cpp
   src/polygon_remover/polygon_remover.cpp
   src/vector_map_filter/vector_map_inside_area_filter.cpp
@@ -86,6 +89,7 @@ target_link_libraries(pointcloud_preprocessor_filter
   faster_voxel_grid_downsample_filter
   ${Boost_LIBRARIES}
   ${OpenCV_LIBRARIES}
+  ${Sophus_LIBRARIES}
   ${PCL_LIBRARIES}
 )
 

sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    base_frame: base_link
+    use_imu: true
+    use_3d_distortion_correction: false

sensing/pointcloud_preprocessor/docs/distortion-corrector.md

@@ -2,46 +2,49 @@
 
 ## Purpose
 
-The `distortion_corrector` is a node that compensates pointcloud distortion caused by ego vehicle's movement during 1 scan.
+The `distortion_corrector` is a node that compensates for pointcloud distortion caused by the ego-vehicle's movement during one 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
 
-- Use the equations below (specific to the Velodyne 32C sensor) to obtain an accurate timestamp for each scan data point.
-- Use twist information to determine the distance the ego-vehicle has traveled between the time that the scan started and the corrected timestamp of each point, and then correct the position of the point.
+The node uses twist information (linear and angular velocity) from the `~/input/twist` topic to correct each point in the point cloud. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU.
 
-The offset equation is given by
-$ TimeOffset = (55.296 \mu s _SequenceIndex) + (2.304 \mu s_ DataPointIndex) $
+The node supports two different modes of distortion correction: 2D distortion correction and 3D distortion correction. The main difference is that the 2D distortion corrector only utilizes the x-axis of linear velocity and the z-axis of angular velocity to correct the point positions. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the point positions.
 
-To calculate the exact point time, add the TimeOffset to the timestamp.
-$ ExactPointTime = TimeStamp + TimeOffset $
+Please note that the processing time difference between the two distortion methods is significant; the 3D corrector takes 50% more time than the 2D corrector. Therefore, it is recommended that in general cases, users should set `use_3d_distortion_correction` to `false`. However, in scenarios such as a vehicle going over speed bumps, using the 3D corrector can be beneficial.
 
 ![distortion corrector figure](./image/distortion_corrector.jpg)
 
 ## Inputs / Outputs
 
 ### Input
 
-| Name             | Type                                             | Description      |
-| ---------------- | ------------------------------------------------ | ---------------- |
-| `~/input/points` | `sensor_msgs::msg::PointCloud2`                  | reference points |
-| `~/input/twist`  | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist            |
-| `~/input/imu`    | `sensor_msgs::msg::Imu`                          | imu data         |
+| Name                 | Type                                             | Description                        |
+| -------------------- | ------------------------------------------------ | ---------------------------------- |
+| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2`                  | Topic of the distorted pointcloud. |
+| `~/input/twist`      | `geometry_msgs::msg::TwistWithCovarianceStamped` | Topic of the twist information.    |
+| `~/input/imu`        | `sensor_msgs::msg::Imu`                          | Topic of the IMU data.             |
 
 ### Output
 
-| Name              | Type                            | Description     |
-| ----------------- | ------------------------------- | --------------- |
-| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
+| Name                  | Type                            | Description                         |
+| --------------------- | ------------------------------- | ----------------------------------- |
+| `~/output/pointcloud` | `sensor_msgs::msg::PointCloud2` | Topic of the undistorted pointcloud |
 
 ## Parameters
 
 ### 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 |
+{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector.schema.json") }}
+
+## Launch
+
+```bash
+ros2 launch pointcloud_preprocessor distortion_corrector.launch.xml
+```
 
 ## Assumptions / Known limits
+
+- 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.

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

@@ -1,4 +1,4 @@
-// Copyright 2020 Tier IV, Inc.
+// 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.
@@ -15,7 +15,9 @@
 #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_
 #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_
 
+#include <Eigen/Core>
 #include <rclcpp/rclcpp.hpp>
+#include <sophus/se3.hpp>
 
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
@@ -33,54 +35,132 @@
 #endif
 
 #include <tf2_ros/buffer.h>
+#include <tf2_ros/buffer_interface.h>
 #include <tf2_ros/transform_listener.h>
 
-// Include tier4 autoware utils
-#include <autoware/universe_utils/ros/debug_publisher.hpp>
-#include <autoware/universe_utils/system/stop_watch.hpp>
-
 #include <deque>
 #include <memory>
 #include <string>
 
 namespace pointcloud_preprocessor
 {
-using rcl_interfaces::msg::SetParametersResult;
-using sensor_msgs::msg::PointCloud2;
 
-class DistortionCorrectorComponent : public rclcpp::Node
+class DistortionCorrectorBase
 {
 public:
-  explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options);
+  virtual void processTwistMessage(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0;
+  virtual void processIMUMessage(
+    const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0;
+  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;
+};
 
-private:
-  void onPointCloud(PointCloud2::UniquePtr points_msg);
-  void onTwistWithCovarianceStamped(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg);
-  void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
-  bool getTransform(
-    const std::string & target_frame, const std::string & source_frame,
-    tf2::Transform * tf2_transform_ptr);
+template <class T>
+class DistortionCorrector : public DistortionCorrectorBase
+{
+public:
+  bool pointcloud_transform_needed_{false};
+  bool pointcloud_transform_exists_{false};
+  bool imu_transform_exists_{false};
+  rclcpp::Node * node_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
 
-  bool undistortPointCloud(const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points);
+  std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
+  std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
+
+  explicit DistortionCorrector(rclcpp::Node * node)
+  : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
+  {
+  }
+  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 getIMUTransformation(
+    const std::string & base_frame, const std::string & imu_frame,
+    geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr);
+  void enqueueIMU(
+    const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg,
+    geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr);
+
+  bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud);
+  void getTwistAndIMUIterator(
+    bool use_imu, double first_point_time_stamp_sec,
+    std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
+    std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu);
+  void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override;
+  void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late);
+  void undistortPoint(
+    sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
+    sensor_msgs::PointCloud2Iterator<float> & it_z,
+    std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
+    std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, float const & time_offset,
+    const bool & is_twist_valid, const bool & is_imu_valid)
+  {
+    static_cast<T *>(this)->undistortPointImplementation(
+      it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
+  };
+};
 
-  rclcpp::Subscription<PointCloud2>::SharedPtr input_points_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
-  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
-  rclcpp::Publisher<PointCloud2>::SharedPtr undistorted_points_pub_;
+class DistortionCorrector2D : public DistortionCorrector<DistortionCorrector2D>
+{
+private:
+  // defined outside of for loop for performance reasons.
+  tf2::Quaternion baselink_quat_;
+  tf2::Transform baselink_tf_odom_;
+  tf2::Vector3 point_tf_;
+  tf2::Vector3 undistorted_point_tf_;
+  float theta_;
+  float x_;
+  float y_;
+
+  // TF
+  tf2::Transform tf2_lidar_to_base_link_;
+  tf2::Transform tf2_base_link_to_lidar_;
 
-  std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
-  std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
+public:
+  explicit DistortionCorrector2D(rclcpp::Node * node) : DistortionCorrector(node) {}
+  void initialize() override;
+  void undistortPointImplementation(
+    sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
+    sensor_msgs::PointCloud2Iterator<float> & it_z,
+    std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
+    std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset,
+    const bool & is_twist_valid, const bool & is_imu_valid);
+
+  void setPointCloudTransform(
+    const std::string & base_frame, const std::string & lidar_frame) override;
+};
 
-  tf2_ros::Buffer tf2_buffer_{get_clock()};
-  tf2_ros::TransformListener tf2_listener_{tf2_buffer_};
+class DistortionCorrector3D : public DistortionCorrector<DistortionCorrector3D>
+{
+private:
+  // defined outside of for loop for performance reasons.
+  Eigen::Vector4f point_eigen_;
+  Eigen::Vector4f undistorted_point_eigen_;
+  Eigen::Matrix4f transformation_matrix_;
+  Eigen::Matrix4f prev_transformation_matrix_;
 
-  std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
-  std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
+  // TF
+  Eigen::Matrix4f eigen_lidar_to_base_link_;
+  Eigen::Matrix4f eigen_base_link_to_lidar_;
 
-  std::string base_link_frame_ = "base_link";
-  std::string time_stamp_field_name_;
-  bool use_imu_;
+public:
+  explicit DistortionCorrector3D(rclcpp::Node * node) : DistortionCorrector(node) {}
+  void initialize() override;
+  void undistortPointImplementation(
+    sensor_msgs::PointCloud2Iterator<float> & it_x, sensor_msgs::PointCloud2Iterator<float> & it_y,
+    sensor_msgs::PointCloud2Iterator<float> & it_z,
+    std::deque<geometry_msgs::msg::TwistStamped>::iterator & it_twist,
+    std::deque<geometry_msgs::msg::Vector3Stamped>::iterator & it_imu, const float & time_offset,
+    const bool & is_twist_valid, const bool & is_imu_valid);
+  void setPointCloudTransform(
+    const std::string & base_frame, const std::string & lidar_frame) override;
 };
 
 }  // namespace pointcloud_preprocessor

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

@@ -0,0 +1,65 @@
+// 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_
+#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_
+
+#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
+
+#include <autoware/universe_utils/ros/debug_publisher.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <memory>
+#include <string>
+
+namespace pointcloud_preprocessor
+{
+using rcl_interfaces::msg::SetParametersResult;
+using sensor_msgs::msg::PointCloud2;
+
+class DistortionCorrectorComponent : public rclcpp::Node
+{
+public:
+  explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Subscription<PointCloud2>::SharedPtr pointcloud_sub_;
+
+  rclcpp::Publisher<PointCloud2>::SharedPtr undistorted_pointcloud_pub_;
+
+  std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
+
+  std::string base_frame_;
+  bool use_imu_;
+  bool use_3d_distortion_correction_;
+
+  std::unique_ptr<DistortionCorrectorBase> distortion_corrector_;
+
+  void onPointCloud(PointCloud2::UniquePtr points_msg);
+  void onTwist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg);
+  void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg);
+};
+
+}  // namespace pointcloud_preprocessor
+
+#endif  // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_

sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml

@@ -0,0 +1,16 @@
+<launch>
+  <arg name="input/pointcloud" default="/sensing/lidar/top/mirror_cropped/pointcloud_ex"/>
+  <arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+  <arg name="input/imu" default="/sensing/imu/imu_data"/>
+  <arg name="output/pointcloud" default="/sensing/lidar/top/rectified/pointcloud_ex"/>
+
+  <!-- Parameter -->
+  <arg name="param_file" default="$(find-pkg-share pointcloud_preprocessor)/config/distortion_corrector_node.param.yaml"/>
+  <node pkg="pointcloud_preprocessor" exec="distortion_corrector_node" name="distortion_corrector_node" output="screen">
+    <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+    <remap from="~/input/twist" to="$(var input/twist)"/>
+    <remap from="~/input/imu" to="$(var input/imu)"/>
+    <remap from="~/output/pointcloud" to="$(var output/pointcloud)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

sensing/pointcloud_preprocessor/package.xml

@@ -42,6 +42,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
+  <depend>sophus</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>