LiDAR-Based SLAM with Pose Graph Optimization

This project implements a complete LiDAR SLAM pipeline on a differential-drive robot equipped with wheel encoders, an IMU, a Hokuyo UTM-30LX LiDAR, and a Kinect RGBD camera. The pipeline has four stages: encoder/IMU odometry, ICP scan matching, probabilistic occupancy and texture mapping, and GTSAM pose graph optimization with loop closure. Results are presented for two indoor datasets collected in the same building.

Dataset provided by Prof. Nikolay Atanasov as part of ECE 276A. Encoder/IMU/LiDAR data is included in the project repository; Kinect RGBD data is available separately at the course SharePoint.

Pipeline

Part 1: Encoder/IMU Odometry

Linear velocity is estimated from four wheel encoder counts using the differential-drive model. Yaw rate is taken from the IMU z-axis and interpolated to encoder timestamps. Exact discrete-time integration (sinc-based) propagates poses forward, avoiding linearization error for large heading changes.

Dataset 20 odometry trajectory

Dataset 21 odometry trajectory

Part 2: ICP Scan Matching

2D ICP aligns consecutive Hokuyo scans to refine the odometry trajectory. Each ICP step is initialized from the odometry-predicted relative transform. A key detail is the source/target convention: the newer scan is passed as source and the older as target, yielding the correctly signed relative transform for pose chaining. Steps where ICP MSE exceeds 0.05 fall back to the odometry estimate.

ICP trajectories are consistently more compact than odometry, removing wheel-slip accumulation over long straight corridors. Both datasets show the same L-shaped floor plan topology preserved through scan matching.

Dataset 20: odometry (blue) vs. ICP (red)

Dataset 21: odometry (blue) vs. ICP (red)

ICP Warm-Up: 3D Point Cloud Registration

Before applying ICP to 2D scans, the algorithm was validated on depth images of two objects. Each canonical model (blue) was aligned to four observed point clouds (red) using yaw-discretized initialization over 36 angles. The asymmetric drill aligned accurately across all viewpoints (mean MSE 1.9 x 10^-4 m²). The near-cylindrical liquid container was harder, with mean MSE 1.1 x 10^-3 m², due to rotational ambiguity in the yaw direction.

Drill PC 0

Drill PC 1

Drill PC 2

Drill PC 3

Container PC 0

Container PC 1

Container PC 2

Container PC 3

Part 3: Occupancy and Texture Mapping

A probabilistic log-odds occupancy grid is built from the ICP trajectory and LiDAR scans using Bresenham ray casting. Asymmetric updates (occupied +2.0, free -0.5) reflect the high reliability of valid Hokuyo hits. No-return beams carve free space without marking occupied endpoints, improving corridor clarity. Kinect RGBD images are projected to world frame and floor points (|z| < 0.1 m) are painted onto a co-registered texture grid.

First-scan sanity check

Dataset 20: first-scan occupancy map

Dataset 21: first-scan occupancy map

Dataset 20: ICP map progression and final maps

24%

50%

74%

100%

Dataset 20: ICP occupancy map

Dataset 20: ICP texture map

Dataset 21: ICP map progression and final maps

24%

49%

74%

100%

Dataset 21: ICP occupancy map

Dataset 21: ICP texture map

Part 4: GTSAM Pose Graph Optimization

A GTSAM factor graph is built over ICP poses with odometry (between) factors, fixed-interval loop closure factors every 10 poses, and proximity-based loop closures detected via KD-tree. All loop closure candidates pass an MSE consistency check before being added. Loop closure factors use a Huber robust kernel to limit outlier influence.

Dataset 20 had one proximity-based loop closure (MSE 0.013), providing a global anchoring constraint and yielding a 63.7% error reduction. Dataset 21 had no proximity loop closures; optimization relied entirely on fixed-interval smoothing, achieving a 54.3% reduction.

Dataset 20: GTSAM trajectory and maps

Parameter	Dataset 20	Dataset 21
Total poses	4,962	4,785
Odometry factors	4,961	4,784
Fixed-interval closures	496	478
Proximity-based closures	1	0
Initial graph error	5.724	9.149
Final graph error	2.080	4.176
Error reduction	63.7%	54.3%