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

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -9,6 +9,7 @@
 
 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 @@
   ${Boost_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
 )
 
@@ -76,6 +78,7 @@
   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/undistorter.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 @@
   faster_voxel_grid_downsample_filter
   ${Boost_LIBRARIES}
   ${OpenCV_LIBRARIES}
+  ${Sophus_LIBRARIES}
   ${PCL_LIBRARIES}
 )
 

sensing/pointcloud_preprocessor/docs/distortion-corrector.md

@@ -2,46 +2,47 @@
 
 ## 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 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
 
-- 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 utilizes twist information (linear velocity and angular velocity) from the `~/input/twist` topic. If the user sets `use_imu` to true, the node will replace the twist's angular velocity with the angular velocity from IMU. Afterward, the node will undistort all of the points one by one based on the velocity information.
 
-The offset equation is given by
-$ TimeOffset = (55.296 \mu s _SequenceIndex) + (2.304 \mu s_ DataPointIndex) $
+The node also supports two different kinds of distortion methods: 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 points. On the other hand, the 3D distortion corrector utilizes all linear and angular velocity components to correct the points.
 
-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/points` | `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/points` | `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 |
+| Name                           | Type     | Default Value | Description                                                 |
+| ------------------------------ | -------- | ------------- | ----------------------------------------------------------- |
+| `timestamp_field_name`         | `string` | `time_stamp`  | Name of time stamp field.                                   |
+| `use_imu`                      | `bool`   | `true`        | Use gyroscope for yaw rate if true, else use vehicle status |
+| `use_3d_distortion_correction` | `bool`   |               | Use 3d correction if true, otherwise use 2d correction      |
 
 ## Assumptions / Known limits
+
+- The node requires that time synchronization works well between the pointcloud, twist, and IMU.
+- If you want to use 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,31 +15,17 @@
 #ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_
 #define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_
 
+#include "pointcloud_preprocessor/distortion_corrector/undistorter.hpp"
+
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.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 <sensor_msgs/point_cloud2_iterator.hpp>
-
-#include <tf2/convert.h>
-#include <tf2/transform_datatypes.h>
-
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
 
-// Include tier4 autoware utils
-#include <tier4_autoware_utils/ros/debug_publisher.hpp>
-#include <tier4_autoware_utils/system/stop_watch.hpp>
-
-#include <deque>
 #include <memory>
 #include <string>
 
@@ -54,16 +40,6 @@
   explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options);
 
 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);
-
-  bool undistortPointCloud(const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points);
-
   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_;
@@ -72,15 +48,16 @@
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
 
-  tf2_ros::Buffer tf2_buffer_{get_clock()};
-  tf2_ros::TransformListener tf2_listener_{tf2_buffer_};
-
-  std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
-  std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
-
   std::string base_link_frame_ = "base_link";
-  std::string time_stamp_field_name_;
   bool use_imu_;
+  bool use_3d_distortion_correction_;
+
+  std::unique_ptr<UndistorterBase> undistorter_;
+
+  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);
 };
 
 }  // namespace pointcloud_preprocessor

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

@@ -0,0 +1,175 @@
+// 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__UNDISTORTER_HPP_
+#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_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>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
+#include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/buffer_interface.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <deque>
+#include <memory>
+#include <string>
+
+namespace pointcloud_preprocessor
+{
+
+class UndistorterBase
+{
+public:
+  virtual void processTwistMessage(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0;
+  virtual void processIMUMessage(
+    const std::string & base_link_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) = 0;
+  virtual void setPointCloudTransform(
+    const std::string & base_link_frame, const std::string & lidar_frame) = 0;
+  virtual void initialize() = 0;
+  virtual void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) = 0;
+};
+
+template <class Derived>
+class Undistorter : public UndistorterBase
+{
+public:
+  bool is_pointcloud_transform_needed_{false};
+  bool is_pointcloud_transform_exist_{false};
+  bool is_imu_transform_exist_{false};
+  rclcpp::Node * node_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
+  std::deque<geometry_msgs::msg::Vector3Stamped> angular_velocity_queue_;
+
+  explicit Undistorter(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_link_frame,
+    const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override;
+  void getIMUTransformation(
+    const std::string & base_link_frame, const std::string & imu_frame,
+    geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr);
+  void storeIMUToQueue(
+    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 getIteratorOfTwistAndIMU(
+    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 warnIfTimestampsTooLate(
+    bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_is_too_late);
+
+  virtual void initialize() = 0;
+  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 & time_offset,
+    bool & is_twist_valid, bool & is_imu_valid)
+  {
+    static_cast<Derived *>(this)->undistortPointImplementation(
+      it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
+  };
+
+  virtual void setPointCloudTransform(
+    const std::string & base_link_frame, const std::string & lidar_frame) = 0;
+};
+
+class Undistorter2D : public Undistorter<Undistorter2D>
+{
+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_;
+
+public:
+  explicit Undistorter2D(rclcpp::Node * node) : Undistorter(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, float & time_offset,
+    bool & is_twist_valid, bool & is_imu_valid);
+
+  void setPointCloudTransform(
+    const std::string & base_link_frame, const std::string & lidar_frame) override;
+};
+
+class Undistorter3D : public Undistorter<Undistorter3D>
+{
+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_;
+
+  // TF
+  Eigen::Matrix4f eigen_lidar_to_base_link_;
+  Eigen::Matrix4f eigen_base_link_to_lidar_;
+
+public:
+  explicit Undistorter3D(rclcpp::Node * node) : Undistorter(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, float & time_offset,
+    bool & is_twist_valid, bool & is_imu_valid);
+  void setPointCloudTransform(
+    const std::string & base_link_frame, const std::string & lidar_frame) override;
+};
+
+}  // namespace pointcloud_preprocessor
+
+#endif  // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__UNDISTORTER_HPP_