Update for Exporting Keyframes

CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
+find_package(rmw REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -33,8 +34,15 @@ find_package(OpenMP REQUIRED)
 
 include_directories(
   include/lio_sam
+  ${rclcpp_INCLUDE_DIRS}
+  ${rmw_INCLUDE_DIRS}
+  ${CMAKE_CURRENT_BINARY_DIR}     # so you can #include "lio_sam/msg/cloud_info.hpp"
+  ${PCL_INCLUDE_DIRS}
 )
 
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
 rosidl_generate_interfaces(${PROJECT_NAME} "msg/CloudInfo.msg" "srv/SaveMap.srv" DEPENDENCIES std_msgs sensor_msgs)
 
 
@@ -60,6 +68,17 @@ else()
   target_link_libraries(${PROJECT_NAME}_mapOptimization gtsam "${cpp_typesupport_target}")
 endif()
 
+add_executable(export_keyframes src/export_keyframes.cpp)
+ament_target_dependencies(export_keyframes
+  rclcpp
+  rmw            # <-- add this
+  nav_msgs
+)
+
+target_link_libraries(export_keyframes
+  ${cpp_typesupport_target}
+)
+
 
 install(
   DIRECTORY launch
@@ -96,8 +115,64 @@ install(
   DESTINATION include
 )
 
+install(TARGETS export_keyframes DESTINATION lib/${PROJECT_NAME})
+
 ament_export_include_directories(include)
 
+
+add_executable(deskew_cloud src/deskew_cloud.cpp)
+ament_target_dependencies(deskew_cloud
+  rclcpp
+  sensor_msgs
+  pcl_conversions
+  pcl_msgs
+  tf2
+  tf2_sensor_msgs
+)
+target_link_libraries(deskew_cloud
+  ${PCL_LIBRARIES}
+  ${cpp_typesupport_target}
+)
+install(TARGETS deskew_cloud
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# (Optional) Service‐based saver that grabs the final pair on demand
+add_executable(save_map src/save_map.cpp)
+ament_target_dependencies(save_map
+  rclcpp
+  nav_msgs
+  sensor_msgs
+  pcl_conversions
+  pcl_msgs
+)
+target_link_libraries(save_map
+  ${PCL_LIBRARIES}
+  ${cpp_typesupport_target}
+)
+install(TARGETS save_map
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
+add_executable(aligned_saver src/aligned_saver.cpp)
+ament_target_dependencies(aligned_saver
+  rclcpp
+  nav_msgs
+  sensor_msgs
+  pcl_conversions
+  pcl_msgs
+)
+target_link_libraries(aligned_saver
+  ${PCL_LIBRARIES}
+  ${cpp_typesupport_target}  # for any CloudInfo types if you switch to that
+)
+install(
+  TARGETS aligned_saver
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

README_Export_Keyframe.md

@@ -0,0 +1,60 @@
+# Exporting Undistorted Point Clouds and Keyframe Poses from LIO-SAM
+## Written by Yingyu Wang June 10, 2025
+
+This document describes how to export undistorted point clouds and their associated poses from LIO-SAM for further analysis or visualization.
+
+## Overview
+
+The export functionality consists of three main components:
+1. `aligned_saver`: Exports undistorted point clouds of keyframes to PCD files
+2. `export_keyframes`: Exports keyframe poses to a CSV file
+3. `export gps/odometry topic`: A script written by python to export gps/odometry topic
+
+## Prerequisites
+
+- ROS 2 environment is properly set up
+- LIO-SAM is built and installed
+- The dataset is ready for processing
+
+## Usage
+
+
+### Step 1: Export Point Clouds
+```bash
+ros2 run lio_sam aligned_saver
+```
+This will save undistorted point clouds of keyframes as PCD files.
+
+### Step 2: Export Keyframe Poses
+```bash
+ros2 run lio_sam export_keyframes
+```
+This will save the poses of keyframes to a CSV file.
+
+### Step 3: Export gps/odometry (optional)
+```bash
+cd src/lio_sam
+python3 script_export_gps_odom.py
+```
+
+### Step 4: Launch LIO-SAM
+```bash
+ros2 launch lio_sam run.launch.py
+```
+
+## Output Files
+
+Exported point clouds and associated poses are saved in the `/output` directory;
+Exported odometry/gps is save in the `src/lio_sam` directory.
+
+### Point Cloud Files
+- Format: PCD files
+- Naming convention: `id_time.pcd`
+- Contains: Undistorted point clouds for each keyframe
+
+### Pose File
+- Format: CSV file
+- Contains: Timestamp and 6-DOF pose information for each keyframe (idx,sec,nsec,x,y,z,qx,qy,qz,qw)
+
+
+

script_export_gps_odom.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+import csv
+import os
+import math
+
+class GPSOdomToCSV(Node):
+    def __init__(self):
+        super().__init__('gps_odom_to_csv')
+
+        # Subscribe to GPS odometry data
+        self.sub = self.create_subscription(
+            Odometry, '/odometry/gps',
+            self.gps_callback, 10)
+
+        # Initialize CSV file
+        self.csv_path = os.path.join(
+            os.getcwd(), 'gps_odom_output.csv'
+        )
+        # Write header if file doesn't exist
+        if not os.path.exists(self.csv_path):
+            with open(self.csv_path, 'w', newline='') as f:
+                writer = csv.writer(f)
+                writer.writerow([
+                    'timestamp',  # ROS timestamp (seconds)
+                    'x',          # x coordinate (m)
+                    'y',          # y coordinate (m)
+                    'z',          # z coordinate (m)
+                    'roll',       # roll angle (rad)
+                    'pitch',      # pitch angle (rad)
+                    'yaw',        # yaw angle (rad)
+                    'cov_xx',     # x covariance
+                    'cov_yy',     # y covariance
+                    'cov_zz'      # z covariance
+                ])
+        self.get_logger().info(f"CSV log file: {self.csv_path}")
+
+    def gps_callback(self, msg: Odometry):
+        # Get position
+        x = msg.pose.pose.position.x
+        y = msg.pose.pose.position.y
+        z = msg.pose.pose.position.z
+
+        # Get orientation (quaternion to euler angles)
+        qx = msg.pose.pose.orientation.x
+        qy = msg.pose.pose.orientation.y
+        qz = msg.pose.pose.orientation.z
+        qw = msg.pose.pose.orientation.w
+
+        # Convert quaternion to euler angles
+        roll, pitch, yaw = self.quaternion_to_euler(qx, qy, qz, qw)
+
+        # Get covariance
+        cov_xx = msg.pose.covariance[0]  # x covariance
+        cov_yy = msg.pose.covariance[7]  # y covariance
+        cov_zz = msg.pose.covariance[14] # z covariance
+
+        # Write to CSV
+        t = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+        with open(self.csv_path, 'a', newline='') as f:
+            writer = csv.writer(f)
+            writer.writerow([
+                f"{t:.9f}",
+                f"{x:.6f}",
+                f"{y:.6f}",
+                f"{z:.6f}",
+                f"{roll:.6f}",
+                f"{pitch:.6f}",
+                f"{yaw:.6f}",
+                f"{cov_xx:.6f}",
+                f"{cov_yy:.6f}",
+                f"{cov_zz:.6f}"
+            ])
+
+    def quaternion_to_euler(self, x, y, z, w):
+        # Convert quaternion to euler angles
+        t0 = +2.0 * (w * x + y * z)
+        t1 = +1.0 - 2.0 * (x * x + y * y)
+        roll = math.atan2(t0, t1)
+
+        t2 = +2.0 * (w * y - z * x)
+        t2 = +1.0 if t2 > +1.0 else t2
+        t2 = -1.0 if t2 < -1.0 else t2
+        pitch = math.asin(t2)
+
+        t3 = +2.0 * (w * z + x * y)
+        t4 = +1.0 - 2.0 * (y * y + z * z)
+        yaw = math.atan2(t3, t4)
+
+        return roll, pitch, yaw
+
+    def destroy_node(self):
+        super().destroy_node()
+        self.get_logger().info("Node destroyed, CSV writing completed.")
+
+def main():
+    rclpy.init()
+    node = GPSOdomToCSV()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main() 

src/aligned_saver.cpp

@@ -0,0 +1,103 @@
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <fstream>
+#include <iomanip>
+#include <sstream>
+#include <filesystem>
+
+namespace fs = std::filesystem;
+using std::placeholders::_1;
+
+class AlignedSaver : public rclcpp::Node {
+public:
+  AlignedSaver()
+  : Node("aligned_saver"), keyframe_idx_(0)
+  {
+    // Create output directory
+    output_dir_ = "output";
+    try {
+      fs::create_directories(output_dir_);
+      RCLCPP_INFO(get_logger(), "Created output directory '%s'", output_dir_.c_str());
+    } catch (const fs::filesystem_error &e) {
+      RCLCPP_ERROR(get_logger(), "Failed to create output directory '%s': %s", output_dir_.c_str(), e.what());
+    }
+
+    // Subscribe to full trajectory updates (key-frames only)
+    path_sub_ = create_subscription<nav_msgs::msg::Path>(
+      "/lio_sam/mapping/path", rclcpp::QoS(1).best_effort(),
+      std::bind(&AlignedSaver::onPath, this, _1));
+
+    // Subscribe to deskewed point cloud stream (best-effort)
+    cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
+      "/lio_sam/deskew/cloud_deskewed", rclcpp::SensorDataQoS(),
+      std::bind(&AlignedSaver::onCloud, this, _1));
+
+    RCLCPP_INFO(get_logger(), "AlignedSaver started: saving outputs under '%s'", output_dir_.c_str());
+  }
+
+private:
+  void onCloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
+    last_cloud_ = msg;
+  }
+
+  void onPath(const nav_msgs::msg::Path::SharedPtr msg) {
+    if (msg->poses.empty()) return;
+
+    // 1) Overwrite the full trajectory CSV
+    fs::path csv_path = fs::path(output_dir_) / "keyframes.csv";
+    std::ofstream fs_csv(csv_path.string(), std::ios::trunc);
+    fs_csv << "idx,sec,nsec,x,y,z,qx,qy,qz,qw\n";
+    for (size_t i = 0; i < msg->poses.size(); ++i) {
+      const auto &p   = msg->poses[i];
+      const auto &t   = p.header.stamp;
+      const auto &pos = p.pose.position;
+      const auto &o   = p.pose.orientation;
+      fs_csv << i << ','
+             << t.sec << ',' << t.nanosec << ','
+             << pos.x  << ',' << pos.y  << ',' << pos.z  << ','
+             << o.x    << ',' << o.y    << ',' << o.z    << ',' << o.w
+             << '\n';
+    }
+    RCLCPP_INFO(get_logger(), "Overwrote '%s' with %zu poses", csv_path.c_str(), msg->poses.size());
+
+    // 2) Save the cached cloud corresponding to this key-frame
+    if (!last_cloud_) {
+      RCLCPP_WARN(get_logger(), "No cached cloud yet – skipping PCD for keyframe %d", keyframe_idx_);
+      return;
+    }
+    auto ts = msg->poses.back().header.stamp;
+    std::ostringstream ss;
+    ss << std::setw(5) << std::setfill('0') << keyframe_idx_
+       << '_' << ts.sec << '.' << std::setw(9) << std::setfill('0') << ts.nanosec
+       << ".pcd";
+    fs::path pcd_path = fs::path(output_dir_) / ss.str();
+
+    pcl::PointCloud<pcl::PointXYZI> cloud;
+    pcl::fromROSMsg(*last_cloud_, cloud);
+    if (pcl::io::savePCDFileBinary(pcd_path.string(), cloud) == 0) {
+      RCLCPP_INFO(get_logger(), "Saved '%s' (%zu points)", pcd_path.c_str(), cloud.size());
+    } else {
+      RCLCPP_ERROR(get_logger(), "Failed to save '%s'", pcd_path.c_str());
+    }
+
+    ++keyframe_idx_;
+  }
+
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr           path_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
+  sensor_msgs::msg::PointCloud2::SharedPtr                       last_cloud_;
+  int                                                            keyframe_idx_;
+  std::string                                                    output_dir_;
+};
+
+int main(int argc, char ** argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AlignedSaver>());
+  rclcpp::shutdown();
+  return 0;
+}
+