GO-SLAM: Graph Optimization + Generalized ICP SLAM

GO-SLAM = Graph Optimization + Generalized ICP SLAM

A complete SLAM system implementation combining:

• Generalized Iterative Closest Point (GICP) for robust odometry estimation
• Graph Optimization with pose graph back-end for global trajectory correction
• Loop Closure Detection for long-term drift elimination

Built from scratch with custom Levenberg-Marquardt solvers for both GICP alignment and pose graph optimization.

Features

• GICP Front-End: Scan-to-scan matching with Levenberg-Marquardt optimization
• Pose Graph Back-End: Global trajectory optimization using custom LM solver
• Loop Closure: Automatic loop detection and verification
• ROS 2 Integration: Full ROS 2 Humble support with standard message types
• KITTI Benchmarking: Tools for evaluation against ground truth

Architecture

┌─────────────┐
│ Point Cloud │
└──────┬──────┘
       │
       ▼
┌─────────────┐     ┌──────────────┐
│    GICP     │────▶│  Odometry    │
│  (LM-based) │     │  Estimate    │
└─────────────┘     └──────┬───────┘
                           │
                           ▼
                    ┌──────────────┐
                    │  Keyframe    │
                    │  Manager     │
                    └──────┬───────┘
                           │
              ┌────────────┴────────────┐
              │                         │
              ▼                         ▼
       ┌─────────────┐          ┌─────────────┐
       │ Loop Closure│          │ Pose Graph  │
       │  Detection  │─────────▶│ Optimization│
       └─────────────┘          └─────────────┘

Dependencies

System Requirements

• Ubuntu 22.04 (Jammy)
• ROS 2 Humble
• C++17 compiler

Libraries

sudo apt install \
    ros-humble-pcl-conversions \
    ros-humble-tf2-eigen \
    ros-humble-sophus \
    libpcl-dev \
    libeigen3-dev

Building

cd /home/panav/ros2_ws/src/go-slam
mkdir -p build && cd build
cmake .. && make -j4

Or using colcon (from workspace root):

cd /home/panav/ros2_ws
colcon build --packages-select go_slam

KITTI Dataset Setup

Download Required Files

From KITTI Odometry Benchmark:

1. Velodyne Laser Data (80 GB) - Required
2. Calibration Files (1 MB) - Required
3. Ground Truth Poses (4 MB) - For benchmarking

Directory Structure

/path/to/kitti/
├── sequences/
│   ├── 00/
│   │   ├── velodyne/
│   │   │   ├── 000000.bin
│   │   │   ├── 000001.bin
│   │   │   └── ...
│   │   ├── poses.txt
│   │   └── calib.txt
│   ├── 01/
│   └── ...
└── poses/
    ├── 00.txt
    ├── 01.txt
    └── ...

Running

Option 1: With ROS 2 Bag Files (Recommended for Custom Data)

Quick Start:

# 1. Check your bag file
ros2 bag info /path/to/your_bag.db3

# 2. Launch SLAM with bag playback
ros2 launch go_slam slam_bag.launch.py \
    bag_file:=/path/to/your_bag.db3 \
    lidar_topic:=/velodyne_points

# 3. Visualize in RViz (automatically launched)
# Add displays: /kitti/point_cloud, /slam/path, /odom
# Set Fixed Frame to 'map'

Parameters:

• bag_file: Path to your ROS 2 bag file
• lidar_topic: Point cloud topic name (default: /velodyne_points)
• playback_rate: Speed multiplier (default: 1.0)
• launch_rviz: Launch RViz visualization (default: true)

Examples:

# Slower playback for debugging
ros2 launch go_slam slam_bag.launch.py \
    bag_file:=~/bags/warehouse.db3 \
    playback_rate:=0.5

# Custom topic without RViz
ros2 launch go_slam slam_bag.launch.py \
    bag_file:=~/bags/outdoor.db3 \
    lidar_topic:=/ouster/points \
    launch_rviz:=false

See docs/BAG_USAGE.md for detailed instructions.

Option 2: With KITTI Dataset

1. Update Launch File

Edit launch/slam.launch.py:

{'dataset_path': '/your/path/to/kitti'},
{'sequence': '00'},  # Choose sequence 00-10
{'publish_rate': 10.0}  # Hz

2. Launch SLAM

cd /home/panav/ros2_ws/src/go-slam/build
./slam_node

Or with launch file:

ros2 launch go_slam slam.launch.py

3. Visualize in RViz2

rviz2

Add displays:

• PointCloud2: Topic /kitti/point_cloud
• Path: Topic /slam/path
• Odometry: Topic /odom

Set Fixed Frame to map.

Benchmarking

Save Your Trajectory

The system automatically saves poses. To manually save:

// In slam_node.cpp, add to constructor:
trajectory_saver_ = std::make_unique<TrajectorySaver>("estimated_poses.txt");

// In createKeyframe(), after optimization:
trajectory_saver_->savePose(kf.pose);

Run Benchmark

cd /home/panav/ros2_ws/src/go-slam
./scripts/benchmark_kitti.py \
    --gt /path/to/kitti/poses/00.txt \
    --est estimated_poses.txt \
    --plot \
    --output results/

Metrics Computed

• ATE (Absolute Trajectory Error): Overall drift
• RPE (Relative Pose Error): Frame-to-frame accuracy
• Trajectory plots: Visual comparison
• Error plots: Error over time

Parameters

GICP Parameters

• voxel_size: Downsampling resolution (default: 0.5m)
• max_iterations: GICP iterations (default: 20)

Keyframe Parameters

• kf_dist_threshold: Distance between keyframes (default: 2.0m)
• kf_angle_threshold: Rotation between keyframes (default: 0.2 rad)

Loop Closure Parameters

• radius: Search radius for candidates (default: 15.0m)
• min_id_diff: Minimum keyframe separation (default: 50)

Testing

LM Solver Test

cd build
./test_lm_solver

Expected output:

=== LM Solver Test: Exponential Curve Fitting ===
...
=== Test PASSED ===

Project Structure

slick-lo/
├── include/go_slam/
│   ├── gicp.hpp              # GICP algorithm
│   ├── lm_solver.hpp         # Levenberg-Marquardt solver
│   ├── pose_graph.hpp        # Pose graph optimization
│   ├── loop_closure.hpp      # Loop closure detection
│   ├── slam_node.hpp         # Main SLAM node
│   ├── kitti_loader.hpp      # KITTI dataset loader
│   ├── visualizer.hpp        # RViz visualization
│   ├── stats_logger.hpp      # Statistics logging
│   ├── logger.hpp            # Logging utilities
│   ├── trajectory_saver.hpp  # Pose output
│   ├── types.hpp             # Type definitions
│   └── cuda/                 # GPU acceleration headers
│       ├── gpu_gicp.hpp
│       ├── cuda_utils.cuh
│       └── cuda_types.cuh
├── src/
│   ├── slam_node.cpp         # Main SLAM node
│   ├── gicp.cpp              # CPU GICP implementation
│   ├── pose_graph.cpp        # Pose graph optimization
│   ├── loop_closure.cpp      # Loop closure detection
│   ├── kitti_loader.cpp      # KITTI dataset I/O
│   ├── kitti_player_node.cpp # KITTI playback node
│   ├── visualizer.cpp        # RViz visualization
│   ├── stats_logger.cpp      # Statistics logging
│   ├── logger.cpp            # Logging utilities
│   ├── trajectory_saver.cpp  # Trajectory output
│   └── cuda/                 # GPU (CUDA) implementations
│       ├── gpu_gicp.cu
│       ├── gpu_spatial_hash.cu
│       ├── gpu_covariance.cu
│       ├── gpu_voxel_filter.cu
│       └── gpu_knn.cu
├── launch/
│   ├── slam.launch.py        # Live SLAM launch
│   └── slam_bag.launch.py    # Bag file playback launch
├── config/
│   └── slam_viz.rviz         # RViz configuration
├── test/
│   └── test_lm_solver.cpp    # LM solver unit tests
├── scripts/                  # Utility & evaluation scripts
├── docs/                     # Documentation guides
├── eval/                     # KITTI evaluation pipeline
├── CMakeLists.txt
├── package.xml
├── LICENSE
└── README.md

Algorithm Details

GICP with LM Damping

The GICP implementation uses:

• Point-to-plane metric via SVD-regularized covariances
• Levenberg-Marquardt damping for robust convergence
• Adaptive lambda: Increases on failure, decreases on success
• Voxel grid filtering: For speed and uniform density

Pose Graph Optimization

• Vertices: Keyframe poses (SE3)
• Edges: Odometry + loop closure constraints
• Optimization: Custom LM solver with sparse Jacobian
• Information weighting: Confidence-based edge weights

Loop Closure

1. Candidate search: Proximity-based (upgradable to KD-tree)
2. Geometric verification: GICP alignment
3. Fitness check: Mean squared error threshold
4. Constraint creation: Relative pose + information matrix

Performance Tips

1. Adjust voxel size: Larger = faster, smaller = more accurate
2. Tune keyframe thresholds: More keyframes = better map, slower optimization
3. Loop closure radius: Smaller = faster, larger = more loops detected
4. Multi-threading: Move optimization to background thread (future work)

Known Limitations

1. Memory: Stores all keyframe clouds in RAM
2. Loop search: Linear scan (O(n) per query)
3. No map visualization: publishKeyframes() is a stub
4. Single-threaded: Optimization blocks odometry

Future Improvements

1. Implement global map publishing
2. Add KD-tree for loop candidate search
3. Multi-threaded back-end optimization
4. Analytical Jacobians for pose graph
5. Scan-to-map matching (vs scan-to-scan)
6. Adaptive information matrices

Citation

If you use this code, please cite:

@software{go_slam_2026,
  author = {Panav Arpit Raaj},
  title = {GO-SLAM: GICP-based Odometry SLAM},
  year = {2026},
  url = {https://github.com/Pana1v/go-slam}
}

License

Apache License 2.0 - See LICENSE for details.

References

1. Segal, A., Haehnel, D., & Thrun, S. (2009). "Generalized-ICP"
2. Grisetti, G., et al. (2010). "g2o: A general framework for graph optimization"
3. Geiger, A., et al. (2012). "Are we ready for autonomous driving? The KITTI vision benchmark suite"

Contact

For questions or issues, please open an issue on GitHub.