support to undistort point cloud in lidar odometry/localization (#52)

* support undistort_point_cloud in lidar odometry

* support undistort_point_cloud in lidar localization

* update lidar odometry

* publish undistorted pointcloud for mapping

* update code style

lidar_localization/include/lidar_localization/lidar_localization.hpp

@@ -43,7 +43,7 @@ class LidarLocalization
   void set_extrinsic(const Eigen::Matrix4d & T_base_lidar);
   bool add_gnss_data(const localization_common::GnssData & gnss_data);
   bool add_gnss_odom(const localization_common::OdomData & gnss_odom);
-  bool update(const localization_common::LidarData<pcl::PointXYZ> & lidar_data);
+  bool update(const localization_common::LidarData<localization_common::PointXYZIRT> & lidar_data);
   pcl::PointCloud<pcl::PointXYZ>::Ptr get_current_scan();
   localization_common::OdomData get_current_odom();
   pcl::PointCloud<pcl::PointXYZ>::Ptr get_global_map();

lidar_localization/include/lidar_localization/lidar_localization_node.hpp

@@ -28,6 +28,7 @@
 #include "localization_common/publisher/cloud_publisher.hpp"
 #include "localization_common/publisher/odometry_publisher.hpp"
 #include "localization_common/extrinsics_manager.hpp"
+#include "localization_common/sensor_data_utils.hpp"
 #include "lidar_localization/lidar_localization.hpp"
 
 namespace lidar_localization
@@ -61,13 +62,15 @@ class LidarLocalizationNode
   Eigen::Matrix4d T_base_lidar_ = Eigen::Matrix4d::Identity();
   bool is_valid_extrinsics_{false};
   bool publish_tf_{false};
+  bool undistort_point_cloud_{false};
   // lidar_localization and process thread
   std::shared_ptr<LidarLocalization> lidar_localization_;
   std::unique_ptr<std::thread> run_thread_;
   bool exit_{false};
   // data
-  std::deque<localization_common::LidarData<pcl::PointXYZ>> lidar_data_buffer_;
+  std::deque<localization_common::LidarData<localization_common::PointXYZIRT>> lidar_data_buffer_;
   std::deque<localization_common::GnssData> gnss_data_buffer_;
   std::deque<localization_common::OdomData> gnss_odom_buffer_;
+  localization_common::TwistData last_twist_;
 };
 }  // namespace lidar_localization

lidar_localization/launch/lidar_localization.launch.py

@@ -49,6 +49,7 @@ def generate_launch_description():
         parameters=[
             {
                 "lidar_localization_config": lidar_localization_config,
+                "undistort_point_cloud": False,
                 "data_path": data_dir,
                 "publish_tf": True,
                 "base_frame_id": "base_link",
@@ -66,7 +67,7 @@ def generate_launch_description():
                 "trajectory_path": data_dir + "/trajectory",
                 "odom_names": ["ground_truth", "lidar_pose"],
                 "odom_topics": ["synced_gnss/pose", "localization/lidar/pose"],
-                "reference_odom_name": "lidar_pose",
+                "reference_odom_name": "ground_truth",
             }
         ],
         output="screen",

lidar_localization/src/lidar_localization.cpp

@@ -86,13 +86,18 @@ bool LidarLocalization::add_gnss_odom(const localization_common::OdomData & gnss
   return true;
 }
 
-bool LidarLocalization::update(const localization_common::LidarData<pcl::PointXYZ> & lidar_data)
+bool LidarLocalization::update(
+  const localization_common::LidarData<localization_common::PointXYZIRT> & lidar_data)
 {
   has_new_local_map_ = false;
-  current_lidar_data_ = lidar_data;
+  current_lidar_data_.time = lidar_data.time;
+  current_lidar_data_.point_cloud =
+    pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::copyPointCloud(*lidar_data.point_cloud, *current_lidar_data_.point_cloud);
   // remove invalid measurements
   std::vector<int> indices;
-  pcl::removeNaNFromPointCloud(*lidar_data.point_cloud, *lidar_data.point_cloud, indices);
+  pcl::removeNaNFromPointCloud(
+    *current_lidar_data_.point_cloud, *current_lidar_data_.point_cloud, indices);
   // initialize if not
   if (!has_inited_) {
     if (init_global_localization()) {
@@ -146,6 +151,15 @@ localization_common::OdomData LidarLocalization::get_current_odom()
   localization_common::OdomData odom;
   odom.time = current_lidar_frame_.time;
   odom.pose = current_lidar_frame_.pose * T_lidar_base_;
+  if (history_frames_.size() >= 2) {
+    auto & frame1 = history_frames_[history_frames_.size() - 2];
+    auto & frame2 = history_frames_[history_frames_.size() - 1];
+    auto twist = localization_common::estimate_twist_by_pose(
+      frame1.pose, frame2.pose, frame1.time,
+      frame2.time);
+    odom.linear_velocity = twist.linear_velocity;
+    odom.angular_velocity = twist.angular_velocity;
+  }
   return odom;
 }
 

lidar_localization/src/lidar_localization_node.cpp

@@ -15,7 +15,10 @@
 #include "lidar_localization/lidar_localization_node.hpp"
 
 #include <filesystem>
+
 #include "localization_common/msg_utils.hpp"
+#include "localization_common/sensor_data_utils.hpp"
+#include "localization_common/lidar_utils.hpp"
 
 namespace lidar_localization
 {
@@ -25,11 +28,13 @@ LidarLocalizationNode::LidarLocalizationNode(rclcpp::Node::SharedPtr node)
   std::string data_path;
   node->declare_parameter("lidar_localization_config", lidar_localization_config);
   node->declare_parameter("data_path", data_path);
+  node->declare_parameter("undistort_point_cloud", undistort_point_cloud_);
   node->declare_parameter("publish_tf", publish_tf_);
   node->declare_parameter("base_frame_id", base_frame_id_);
   node->declare_parameter("lidar_frame_id", lidar_frame_id_);
   node->get_parameter("lidar_localization_config", lidar_localization_config);
   node->get_parameter("data_path", data_path);
+  node->get_parameter("undistort_point_cloud", undistort_point_cloud_);
   node->get_parameter("publish_tf", publish_tf_);
   node->get_parameter("base_frame_id", base_frame_id_);
   node->get_parameter("lidar_frame_id", lidar_frame_id_);
@@ -120,6 +125,10 @@ bool LidarLocalizationNode::run()
   // process lidar data
   if (!lidar_data_buffer_.empty()) {
     auto current_lidar_data = lidar_data_buffer_.front();
+    // undistort point cloud
+    if (undistort_point_cloud_) {
+      localization_common::undistort_point_cloud(current_lidar_data, last_twist_);
+    }
     if (lidar_localization_->update(current_lidar_data)) {
       publish_data();
     }
@@ -142,6 +151,7 @@ bool LidarLocalizationNode::publish_data()
   // publish lidar pose
   auto odom = lidar_localization_->get_current_odom();
   lidar_pose_pub_->publish(odom);
+  last_twist_ = localization_common::get_twist_from_odom(odom);
   // publish tf
   if (publish_tf_) {
     geometry_msgs::msg::TransformStamped msg;

lidar_odometry/include/lidar_odometry/lidar_odometry_node.hpp

@@ -27,6 +27,7 @@
 #include "localization_common/subscriber/odometry_subscriber.hpp"
 #include "localization_common/extrinsics_manager.hpp"
 #include "localization_common/msg_utils.hpp"
+#include "localization_common/sensor_data_utils.hpp"
 #include "localization_common/odom_data_buffer.hpp"
 #include "localization_common/tic_toc.hpp"
 #include "lidar_odometry/simple_odometry.hpp"
@@ -59,6 +60,7 @@ class LidarOdometryNode
   // pub & sub
   std::shared_ptr<localization_common::CloudSubscriber> cloud_sub_;
   std::shared_ptr<localization_common::OdometrySubscriber> reference_odom_sub_;
+  std::shared_ptr<localization_common::CloudPublisher> undistorted_scan_pub_;
   std::shared_ptr<localization_common::CloudPublisher> current_scan_pub_;
   std::shared_ptr<localization_common::CloudPublisher> local_map_pub_;
   std::shared_ptr<localization_common::CloudPublisher> loam_feature_pub_;
@@ -80,9 +82,12 @@ class LidarOdometryNode
   // data
   std::deque<LidarMsgData> lidar_data_buffer_;
   std::shared_ptr<localization_common::OdomDataBuffer> ref_odom_buffer_;
+  localization_common::TwistData last_twist_;
   // params
   Eigen::Matrix4d T_map_odom_ = Eigen::Matrix4d::Identity();
   bool use_initial_pose_from_topic_{false};
+  bool undistort_point_cloud_{false};
+  bool publish_undistorted_point_cloud_{false};
   bool inited_{false};
   // debug
   localization_common::AdvancedTicToc elapsed_time_statistics_;

lidar_odometry/include/lidar_odometry/simple_odometry.hpp

@@ -41,7 +41,7 @@ class SimpleOdometry
   explicit SimpleOdometry(const YAML::Node & config);
   ~SimpleOdometry() = default;
   void set_extrinsic(const Eigen::Matrix4d & T_base_lidar);
-  bool update(const localization_common::LidarData<pcl::PointXYZ> & lidar_data);
+  bool update(const localization_common::LidarData<localization_common::PointXYZIRT> & lidar_data);
   localization_common::OdomData get_current_odom();
   pcl::PointCloud<pcl::PointXYZ>::Ptr get_current_scan();
   pcl::PointCloud<pcl::PointXYZ>::Ptr get_local_map();

lidar_odometry/launch/lidar_odometry.launch.py

@@ -47,6 +47,8 @@ def generate_launch_description():
         parameters=[
             {
                 "lidar_odometry_config": lidar_odometry_config,
+                "undistort_point_cloud": False,
+                "publish_undistorted_point_cloud": False,
                 "publish_tf": True,
                 "use_initial_pose_from_topic": True,
                 "base_frame_id": "base_link",

lidar_odometry/src/lidar_odometry_node.cpp

@@ -18,6 +18,7 @@
 #include <filesystem>
 
 #include "localization_common/sensor_data_utils.hpp"
+#include "localization_common/lidar_utils.hpp"
 
 namespace lidar_odometry
 {
@@ -33,11 +34,15 @@ LidarOdometryNode::LidarOdometryNode(rclcpp::Node::SharedPtr node)
   node->declare_parameter("lidar_odometry_config", lidar_odometry_config);
   node->declare_parameter("publish_tf", publish_tf_);
   node->declare_parameter("use_initial_pose_from_topic", use_initial_pose_from_topic_);
+  node->declare_parameter("undistort_point_cloud", undistort_point_cloud_);
+  node->declare_parameter("publish_undistorted_point_cloud", publish_undistorted_point_cloud_);
   node->declare_parameter("base_frame_id", base_frame_id_);
   node->declare_parameter("lidar_frame_id", lidar_frame_id_);
   node->get_parameter("lidar_odometry_config", lidar_odometry_config);
   node->get_parameter("publish_tf", publish_tf_);
   node->get_parameter("use_initial_pose_from_topic", use_initial_pose_from_topic_);
+  node->get_parameter("undistort_point_cloud", undistort_point_cloud_);
+  node->get_parameter("publish_undistorted_point_cloud", publish_undistorted_point_cloud_);
   node->get_parameter("base_frame_id", base_frame_id_);
   node->get_parameter("lidar_frame_id", lidar_frame_id_);
   RCLCPP_INFO(node->get_logger(), "lidar_odometry_config: [%s]", lidar_odometry_config.c_str());
@@ -67,6 +72,8 @@ LidarOdometryNode::LidarOdometryNode(rclcpp::Node::SharedPtr node)
     reference_odom_sub_ =
       std::make_shared<localization_common::OdometrySubscriber>(node, "reference_odom", 10000);
   }
+  undistorted_scan_pub_ = std::make_shared<localization_common::CloudPublisher>(
+    node, "lidar_odometry/undistorted_pointcloud", lidar_frame_id_, 100);
   current_scan_pub_ = std::make_shared<localization_common::CloudPublisher>(
     node, "lidar_odometry/current_scan", "map", 100);
   local_map_pub_ = std::make_shared<localization_common::CloudPublisher>(
@@ -185,12 +192,18 @@ void LidarOdometryNode::set_extrinsics_for_odometry(
 bool LidarOdometryNode::update_odometry(OdometryMethod method, const LidarMsgData & msg_data)
 {
   elapsed_time_statistics_.tic("update_odometry");
+  auto lidar_data = cloud_sub_->to_lidar_data<localization_common::PointXYZIRT>(msg_data);
+  // undistort point cloud
+  if (undistort_point_cloud_) {
+    localization_common::undistort_point_cloud(lidar_data, last_twist_);
+    if (publish_undistorted_point_cloud_) {
+      undistorted_scan_pub_->publish(lidar_data);
+    }
+  }
   bool success = false;
   if (method == OdometryMethod::Simple) {
-    auto lidar_data = cloud_sub_->to_lidar_data<pcl::PointXYZ>(msg_data);
     success = simple_odometry_->update(lidar_data);
   } else if (method == OdometryMethod::Loam) {
-    auto lidar_data = cloud_sub_->to_lidar_data<localization_common::PointXYZIRT>(msg_data);
     success = loam_odometry_->update(lidar_data);
   }
   elapsed_time_statistics_.toc("update_odometry");
@@ -226,9 +239,10 @@ void LidarOdometryNode::publish_odom(const localization_common::OdomData & odom)
 void LidarOdometryNode::publish_data(OdometryMethod method)
 {
   elapsed_time_statistics_.tic("publish_data");
+  localization_common::OdomData odom;
   if (method == OdometryMethod::Simple) {
     // publish odom
-    auto odom = align_odom_to_map(simple_odometry_->get_current_odom());
+    odom = align_odom_to_map(simple_odometry_->get_current_odom());
     publish_odom(odom);
     // publish point cloud
     if (current_scan_pub_->has_subscribers()) {
@@ -243,7 +257,7 @@ void LidarOdometryNode::publish_data(OdometryMethod method)
     }
   } else if (method == OdometryMethod::Loam) {
     // publish odom
-    auto odom = align_odom_to_map(loam_odometry_->get_current_odom());
+    odom = align_odom_to_map(loam_odometry_->get_current_odom());
     publish_odom(odom);
     // publish point cloud
     if (current_scan_pub_->has_subscribers()) {
@@ -257,6 +271,7 @@ void LidarOdometryNode::publish_data(OdometryMethod method)
       loam_feature_pub_->publish(*feature_scan);
     }
   }
+  last_twist_ = localization_common::get_twist_from_odom(odom);
   elapsed_time_statistics_.toc("publish_data");
 }
 

lidar_odometry/src/loam_odometry.cpp

@@ -14,6 +14,8 @@
 
 #include "lidar_odometry/loam_odometry.hpp"
 
+#include "localization_common/sensor_data_utils.hpp"
+
 namespace lidar_odometry
 {
 
@@ -89,6 +91,13 @@ localization_common::OdomData LoamOdometry::get_current_odom()
   localization_common::OdomData odom;
   odom.time = current_frame_.time;
   odom.pose = current_frame_.pose * T_lidar_base_;
+  if (history_poses_.size() >= 2) {
+    auto & pose1 = history_poses_[history_poses_.size() - 2];
+    auto & pose2 = history_poses_[history_poses_.size() - 1];
+    auto twist = localization_common::estimate_twist_by_pose(pose1, pose2);
+    odom.linear_velocity = twist.linear_velocity;
+    odom.angular_velocity = twist.angular_velocity;
+  }
   return odom;
 }
 

lidar_odometry/src/simple_odometry.cpp

@@ -16,6 +16,8 @@
 
 #include <pcl/common/transforms.h>
 
+#include "localization_common/sensor_data_utils.hpp"
+
 namespace lidar_odometry
 {
 SimpleOdometry::SimpleOdometry(const YAML::Node & config)
@@ -48,11 +50,14 @@ void SimpleOdometry::set_extrinsic(const Eigen::Matrix4d & T_base_lidar)
 }
 
 
-bool SimpleOdometry::update(const localization_common::LidarData<pcl::PointXYZ> & lidar_data)
+bool SimpleOdometry::update(
+  const localization_common::LidarData<localization_common::PointXYZIRT> & lidar_data)
 {
   has_new_local_map_ = false;
   current_frame_.time = lidar_data.time;
-  current_frame_.point_cloud = lidar_data.point_cloud;
+  current_frame_.point_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+    new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::copyPointCloud(*lidar_data.point_cloud, *current_frame_.point_cloud);
   if (key_frames_.empty()) {
     // initialize the first frame
     current_frame_.pose = T_base_lidar_;
@@ -82,6 +87,13 @@ localization_common::OdomData SimpleOdometry::get_current_odom()
   localization_common::OdomData odom;
   odom.time = current_frame_.time;
   odom.pose = current_frame_.pose * T_lidar_base_;
+  if (history_poses_.size() >= 2) {
+    auto & pose1 = history_poses_[history_poses_.size() - 2];
+    auto & pose2 = history_poses_[history_poses_.size() - 1];
+    auto twist = localization_common::estimate_twist_by_pose(pose1, pose2);
+    odom.linear_velocity = twist.linear_velocity;
+    odom.angular_velocity = twist.angular_velocity;
+  }
   return odom;
 }
 

localization_common/include/localization_common/kitti_preprocess_node.hpp

@@ -75,8 +75,6 @@ class KittiPreprocessNode
   // map origin (latitude, longitude, altitude)
   bool use_manual_map_origin_{true};
   std::vector<double> map_origin_{48.982545, 8.390366, 116.382141};
-  //
-  bool undistort_point_cloud_{false};
   // data
   std::deque<LidarData<pcl::PointXYZI>> lidar_data_buffer_;
   std::deque<ImuData2> imu_data_buffer_;

localization_common/include/localization_common/sensor_data_utils.hpp

@@ -19,6 +19,7 @@
 #include "localization_common/sensor_data/twist_data.hpp"
 #include "localization_common/sensor_data/imu_data.hpp"
 #include "localization_common/sensor_data/odom_data.hpp"
+#include "localization_common/sensor_data/pose_data.hpp"
 
 namespace localization_common
 {
@@ -45,4 +46,11 @@ ImuData interpolate_imu(const ImuData & data1, const ImuData & data2, double tim
 
 OdomData interpolate_odom(const OdomData & data1, const OdomData & data2, double time);
 
+TwistData get_twist_from_odom(const OdomData & odom);
+
+TwistData estimate_twist_by_pose(
+  const Eigen::Matrix4d & pose1, const Eigen::Matrix4d & pose2, double t1, double t2);
+
+TwistData estimate_twist_by_pose(const PoseData & pose1, const PoseData & pose2);
+
 }  // namespace localization_common

localization_common/src/kitti_preprocess_node.cpp

@@ -25,11 +25,9 @@ KittiPreprocessNode::KittiPreprocessNode(rclcpp::Node::SharedPtr node)
   node->declare_parameter("use_manual_map_origin", use_manual_map_origin_);
   node->declare_parameter("map_origin", map_origin_);
   node->declare_parameter("publish_tf", publish_tf_);
-  node->declare_parameter("undistort_point_cloud", undistort_point_cloud_);
   node->get_parameter("use_manual_map_origin", use_manual_map_origin_);
   node->get_parameter("map_origin", map_origin_);
   node->get_parameter("publish_tf", publish_tf_);
-  node->get_parameter("undistort_point_cloud", undistort_point_cloud_);
   if (map_origin_.size() != 3) {
     RCLCPP_FATAL(
       node->get_logger(), "map_origin's size must be 3 for (latitude, longitude, altitude)");
@@ -132,19 +130,6 @@ bool KittiPreprocessNode::run()
     // convert lidar data
     LidarData<PointXYZIRT> current_lidar_data;
     convert_velodyne64(lidar_data, current_lidar_data, 0.1, false);
-    // undistort point cloud
-    if (undistort_point_cloud_) {
-      // get sync gnss odom
-      OdomData synced_odom;
-      gnss_odom_buffer_->get_interpolated_data(current_lidar_data.time, synced_odom);
-      auto odom_lidar = transform_odom(synced_odom, T_base_lidar_);
-      TwistData twist_lidar;
-      twist_lidar.time = odom_lidar.time;
-      twist_lidar.linear_velocity = odom_lidar.linear_velocity;
-      twist_lidar.angular_velocity = odom_lidar.angular_velocity;
-      // undistort
-      undistort_point_cloud(current_lidar_data, twist_lidar);
-    }
     // publish lidar point cloud
     cloud_pub_->publish(current_lidar_data);
     // update buffer

localization_common/src/lidar_utils.cpp

@@ -124,7 +124,7 @@ bool undistort_point_cloud(LidarData<PointXYZIRT> & lidar_data, const TwistData
     auto & p = lidar_data.point_cloud->points[i];
     Eigen::Vector3d point(p.x, p.y, p.z);
     // lidar motion
-    Eigen::Matrix4d T = integrate_twist(twist_data, p.time - lidar_data.time, true);
+    Eigen::Matrix4d T = integrate_twist(twist_data, p.time - lidar_data.time, false);
     // Rp + t
     Eigen::Vector3d undistort_point = T.block<3, 3>(0, 0) * point + T.block<3, 1>(0, 3);
     // update point xyz