feat(pointcloud_preprocessor): support for 3d distortion correction a…

…lgorithm 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
@@ -85,6 +88,7 @@ target_link_libraries(pointcloud_preprocessor_filter
   pointcloud_preprocessor_filter_base
   faster_voxel_grid_downsample_filter
   ${Boost_LIBRARIES}
+  ${Sophus_LIBRARIES}
   ${OpenCV_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/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,56 +35,134 @@
 #endif
 
 #include <tf2_ros/buffer.h>
+#include <tf2_ros/buffer_interface.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>
 
 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<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
-  std::unique_ptr<tier4_autoware_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
 
-#endif  // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_
+#endif  // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_

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 <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_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<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_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

@@ -35,6 +35,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>