Feat/slam: 3D Lidar-Inertial SLAM pipeline with iSAM2 Factor Graph

[shayaf84]

This branch develops a 3D Lidar-Inertial SLAM pipeline with integrated GPS factors.
Functionalities:

• Implements the ROS2 package urc_slam for 3D Localization and point-cloud mapping using an iSAM2 optimized factor graph developed with GTSAM
• Publishes optimized odometry, a keyframe path, and an accumulated point cloud map for future traversability mapping
• Maintains SE(3) pose, velocity, and IMU bias variables, with a random-walk bias evolution model
• Preintegrates IMU angular velocity+linear acceleration between keyframes and added as an IMU factor
• Registers relative Lidar transforms with Generalized ICP and added transform to the graph as relative pose factors
• Runs a ROS2 navsat_transform node to convert GPS to slam map_frame
• Add GPS factors to the graph

Limitations:

• Lidar extrinsics are currently identity and need to be measured
• IMU is initialized to identity orientation + zero initial velocity and bias
• No loop closures yet, will be added for the future
• Stored point clouds grow with mission length, no optimization implemented in this version
• Recovery from prolonged registration failure is not implemented in this version

[Hiptostee]

lgtm 👍🛰️