factor_graph_slam

GPS-denied multimodal SLAM for a wheeled ground platform ("the trike"). LiDAR + IMU + stereo camera fused in a GTSAM iSAM2 factor graph, with YOLO-World detections turned into semantic landmarks. GPS is recorded but kept out of the graph and republished as a separate path for drift evaluation.

ROS 2 Jazzy, Python.

Packages

• trike_lidar_odom — KISS-ICP wrapper.
• trike_semantic — YOLO-World stereo detector and 3D back-projection.
• trike_backend — iSAM2 factor graph, occupancy grid, drift evaluator, viz.
• trike_bringup — launch files, RViz configs, static extrinsics.

Install

sudo apt install ros-jazzy-desktop ros-jazzy-cv-bridge ros-jazzy-tf2-ros
python3 -m venv --system-site-packages .venv
source .venv/bin/activate
pip install -r requirements.txt

Build and run

source env.sh
colcon build --symlink-install
source install/setup.bash

# main SLAM stack + RViz
ros2 launch trike_bringup trike_slam.launch.py

# optional: second RViz window with only SLAM path + GPS + drift arrows
ros2 launch trike_bringup drift_overlay.launch.py

# play a bag in another terminal
ros2 bag play <bag_dir> --rate 0.5

Run artefacts (trajectory CSV, drift JSONL, occupancy snapshot, etc.) are written under $TRIKE_MAPS_ROOT/run_<timestamp>/. Defaults to $PWD/maps/ if unset.

Offline drift report

python -m trike_backend.drift_plot $TRIKE_MAPS_ROOT/run_<timestamp>/

Produces drift_report.pdf next to the JSONL.