Added Relative pose graph ImuPreintegration

script/save_benchmark_result.py

@@ -15,6 +15,7 @@
 # Rerun imports
 import rerun as rr
 import numpy as np
+import csv
 
 class PathSubscriber(Node):
     def __init__(self):
@@ -62,6 +63,37 @@ def laser_path_callback(self, msg):
         self.laser_last_message_time = time.time()
         self.get_logger().info(f'Received laser path message with {len(msg.poses)} poses')
 
+    def append_pose_to_csv(self, pose_stamped, source="imu"):
+        output_file = f"{source}_poses.csv"
+        file_exists = os.path.isfile(output_file)
+
+        with open(output_file, mode='a', newline='') as f:
+            writer = csv.writer(f)
+
+            # Write header if file is new
+            if not file_exists:
+                writer.writerow([
+                    "timestamp",
+                    "p_w_b_x", "p_w_b_y", "p_w_b_z",
+                    "q_w_b_x", "q_w_b_y", "q_w_b_z", "q_w_b_w"
+                ])
+
+            # Get timestamp from pose_stamped.header.stamp
+            stamp = pose_stamped.header.stamp
+            timestamp = stamp.sec + stamp.nanosec * 1e-9
+
+            # Write pose data
+            writer.writerow([
+                timestamp,
+                pose_stamped.pose.position.x,
+                pose_stamped.pose.position.y,
+                pose_stamped.pose.position.z,
+                pose_stamped.pose.orientation.x,
+                pose_stamped.pose.orientation.y,
+                pose_stamped.pose.orientation.z,
+                pose_stamped.pose.orientation.w
+            ])
+
     def imu_path_callback(self, msg):
         """Callback for receiving IMU Path messages"""
         self.imu_path_messages.append(msg)
@@ -70,6 +102,7 @@ def imu_path_callback(self, msg):
         # Add all poses from this message to the global list
         for pose in msg.poses:
             self.all_imu_poses.append(pose)
+            self.append_pose_to_csv(pose, source="imu")
 
         self.get_logger().info(f'Received IMU path message with {len(msg.poses)} poses, total: {len(self.all_imu_poses)}')
 

super_odometry/CMakeLists.txt

@@ -48,10 +48,28 @@ find_package(Sophus REQUIRED)
 find_package(PCL REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(Ceres REQUIRED)
-find_package(GTSAM REQUIRED QUIET)
 find_package(TBB REQUIRED)
 find_package(livox_ros_driver2 REQUIRED)
 
+# GTSAM and gtsam_unstable configuration
+find_package(GTSAM REQUIRED)
+
+# Check if gtsam_unstable is available
+find_library(GTSAM_UNSTABLE_LIBRARY 
+  NAMES gtsam_unstable
+  HINTS ${GTSAM_LIBRARY_DIRS} /usr/local/lib /usr/lib
+)
+
+if(GTSAM_UNSTABLE_LIBRARY)
+  message(STATUS "Found gtsam_unstable: ${GTSAM_UNSTABLE_LIBRARY}")
+  set(GTSAM_LIBRARIES gtsam gtsam_unstable)
+  add_definitions(-DGTSAM_UNSTABLE_AVAILABLE)
+else()
+  message(WARNING "gtsam_unstable not found. Fixed-Lag Smoother will not be available.")
+  message(WARNING "To enable Fixed-Lag Smoother, rebuild GTSAM with -DGTSAM_BUILD_UNSTABLE=ON")
+  set(GTSAM_LIBRARIES gtsam)
+endif()
+
 # Include directories
 include_directories(
   include
@@ -67,19 +85,15 @@ include_directories(
 
 # Link directories
 link_directories(
-  include
   ${PCL_LIBRARY_DIRS}
   ${OpenCV_LIBRARY_DIRS}
   ${GTSAM_LIBRARY_DIRS}
 )
 
-set(GTSAM_LIBRARIES gtsam)
-
-
 # SuperOdom Library
 add_library(SuperOdomLib SHARED
   src/FeatureExtraction/featureExtraction.cpp
-  src/ImuPreintegration/imuPreintegration.cpp
+  src/ImuPreintegration/imuPreintegration_relative.cpp
   src/LaserMapping/laserMapping.cpp
   src/LaserMapping/lidarOptimization.cpp
   src/LaserMapping/LocalMap.cpp
@@ -92,10 +106,15 @@ add_library(SuperOdomLib SHARED
 
 # Set up library include directory
 target_include_directories(SuperOdomLib PUBLIC  
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/SuperOdomLib>  
-    $<INSTALL_INTERFACE:include/SuperOdomLib>  # <prefix>/include/mylib
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>  
+    $<INSTALL_INTERFACE:include>
 )
 
+# Add compile definitions for configuration
+if(GTSAM_UNSTABLE_LIBRARY)
+  target_compile_definitions(SuperOdomLib PUBLIC GTSAM_UNSTABLE_AVAILABLE)
+endif()
+
 ament_target_dependencies(SuperOdomLib
   rclcpp
   std_msgs
@@ -115,38 +134,58 @@ ament_target_dependencies(SuperOdomLib
 target_link_libraries(SuperOdomLib
   ${PCL_LIBRARIES}
   ${OpenCV_LIBRARIES}
-  gtsam
- ${CERES_LIBRARIES}
+  ${GTSAM_LIBRARIES}
+  ${CERES_LIBRARIES}
+  ${TBB_LIBRARIES}
+  pthread
 )
 
 # IMU PreIntegration node
 add_executable(imu_preintegration_node src/imuPreintegration_node.cpp)
-ament_target_dependencies(imu_preintegration_node rclcpp nav_msgs sensor_msgs tf2_ros)
+ament_target_dependencies(imu_preintegration_node 
+  rclcpp 
+  nav_msgs 
+  sensor_msgs 
+  tf2_ros
+  std_msgs
+)
 target_link_libraries(imu_preintegration_node
-    gtsam
-    SuperOdomLib
+  ${GTSAM_LIBRARIES}
+  SuperOdomLib
+  pthread
 )
 
-
-
 # Feature Extraction node
 add_executable(feature_extraction_node src/featureExtraction_node.cpp)
-ament_target_dependencies(feature_extraction_node rclcpp nav_msgs sensor_msgs super_odometry_msgs livox_ros_driver2)
+ament_target_dependencies(feature_extraction_node 
+  rclcpp 
+  nav_msgs 
+  sensor_msgs 
+  super_odometry_msgs 
+  livox_ros_driver2
+)
 target_link_libraries(feature_extraction_node 
   ${PCL_LIBRARIES}
   ${TBB_LIBRARIES}
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
   SuperOdomLib
 )
 
 # LiDAR Mapping node
 add_executable(laser_mapping_node src/laserMapping_node.cpp)
-ament_target_dependencies(laser_mapping_node rclcpp std_msgs nav_msgs sensor_msgs geometry_msgs tf2 tf2_ros)
+ament_target_dependencies(laser_mapping_node 
+  rclcpp 
+  std_msgs 
+  nav_msgs 
+  sensor_msgs 
+  geometry_msgs 
+  tf2 
+  tf2_ros
+  visualization_msgs
+)
 target_link_libraries(laser_mapping_node
-    ${PCL_LIBRARIES}
-    ${CERES_LIBRARIES}
-    SuperOdomLib
+  ${PCL_LIBRARIES}
+  ${CERES_LIBRARIES}
+  SuperOdomLib
 )
 
 # Install Exported Libraries
@@ -168,8 +207,21 @@ install(
 
 ament_export_include_directories(include)
 ament_export_libraries(SuperOdomLib)
+ament_export_dependencies(
+  rclcpp
+  std_msgs
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  super_odometry_msgs
+  visualization_msgs
+  pcl_conversions
+)
 
-#Install executables
+# Install executables
 install(TARGETS
   imu_preintegration_node
   feature_extraction_node