The problem
Automated driving needs vehicle position with lane-level accuracy where GNSS is degraded, such as in urban canyons, tunnels and under flyovers. Lidar SLAM builds a map while estimating motion, and map-based localisation reuses that map. Both drift or fail in degenerate geometry such as long featureless roads, and their accuracy must be measured rigorously.
Objectives
- Implement lidar odometry with scan-to-scan and scan-to-map registration.
- Add a pose graph with loop closure and optimisation.
- Build maps from KITTI odometry sequences and evaluate drift.
- Implement localisation against a prior map and test robustness.
- Measure runtime and identify degenerate cases.
What you need
Tools and toolboxes
- ROS 2 — Nodes for odometry, mapping and localisation, bag playback, TF frames and RViz visualisation.
- C++ — Performance-critical registration and pose graph code.
- Python — Evaluation scripts, trajectory alignment and plots.
- Open3D — ICP and point cloud utilities for prototyping and comparison.
- Lidar Toolbox — Optional MATLAB comparison with pcregisterndt and pcregistericp.
Software
- ROS 2 with RViz
- C++ with PCL or Open3D and a pose graph library such as GTSAM or g2o
- KITTI odometry dataset with ground-truth poses
- Python 3 with NumPy, SciPy and Matplotlib
Extra parts and data
- A multi-core PC with sufficient RAM for large maps — Map building and optimisation over long sequences need memory and CPU.
Key concepts
- Scan registration
- Finding the rigid transform that aligns two point clouds. ICP iteratively matches nearest points; NDT matches points to local normal distributions on a voxel grid.
- Pose graph
- A graph whose nodes are vehicle poses and whose edges are relative transforms from odometry and loop closures. Optimisation adjusts poses to agree with all edges.
- Loop closure
- Recognising a previously visited place and adding a constraint between current and past poses, which removes accumulated drift.
- Absolute and relative trajectory error
- ATE measures global position error after aligning trajectories; relative error measures drift over fixed path lengths, as in the KITTI odometry benchmark.
Step-by-step guide
- 1
Define requirements and evaluation
State the questions: what drift does lidar odometry alone have, how much does loop closure reduce it, and how accurately and robustly can a vehicle localise against a stored map? Set targets such as relative translation error of a few tenths of a per cent and localisation error under 0.2 m. Choose KITTI odometry sequences with ground truth for mapping and others for localisation tests, and define ATE and relative error metrics.
Checkpoint: A plan with questions, targets, sequence allocation and metric definitions.
- 2
Set up data playback in ROS 2
Convert KITTI scans and timestamps into ROS 2 bag files or write a playback node that publishes sensor_msgs/PointCloud2 at 10 Hz with correct frame IDs. Publish the ground-truth trajectory as a separate topic for visualisation. Set up TF frames (map, odom, base_link, lidar) and confirm in RViz that scans and ground truth appear correctly. Use simulated time from the bag so all nodes share the same clock, and record the setup in a launch file that starts the whole pipeline with one command.
Checkpoint: RViz showing scans and the ground-truth path playing back at the correct rate.
- 3
Implement lidar odometry
Pre-process each scan: crop, remove the ego vehicle and voxel-downsample. Implement scan-to-scan registration with point-to-plane ICP and an NDT alternative, initialising with a constant-velocity prediction. Add scan-to-local-map registration against a sliding window of recent keyframes for lower drift. Publish odometry and record the estimated trajectory. Keep the keyframe selection and local-map size as parameters, since they strongly affect drift and runtime. Log the registration fitness score and iteration count for each scan, so poor matches can be found later when you analyse where drift grows.
Checkpoint: An odometry trajectory that follows the ground truth closely over the first few hundred metres.
- 4
Evaluate odometry drift
Compute relative translation and rotation error over path segments from 100 to 800 m, as in the KITTI benchmark, and ATE after aligning with ground truth. Compare scan-to-scan, scan-to-map, ICP and NDT. Record runtime per scan and plot error against path length. Identify segments where drift grows fastest and look at their geometry. Use the standard KITTI evaluation procedure for relative error so your numbers can be compared with published results on the same sequences.
Checkpoint: A table of relative error and runtime per variant and plots of drift against distance.
- 5
Add a pose graph and loop closure
Create keyframes every few metres and add odometry edges with covariances. Detect loop closure candidates by position proximity in the current estimate and a place descriptor such as Scan Context, then verify with ICP and a fitness threshold. Add verified loop edges and optimise the graph with GTSAM or g2o, using a robust kernel to reduce the impact of false loops.
Checkpoint: A corrected trajectory after optimisation on a sequence with loops, visibly closer to ground truth.
- 6
Build and evaluate maps
Build a global map by transforming keyframe scans with optimised poses and downsampling. Compute ATE before and after loop closure, and inspect the map for double walls and blurred structures, which indicate residual errors. Test false-loop robustness by injecting wrong loop edges and checking whether the robust kernel rejects them. Measure map size in memory and point count at the chosen voxel size, since the same map is used for localisation later. Save the map in a standard point cloud format so it can be reloaded and inspected independently.
Checkpoint: ATE before and after loop closure and results of the false-loop injection test.
- 7
Implement map-based localisation
Load a map built from one sequence and localise a different drive over the same area against it, using NDT scan-to-map matching with motion prediction. Handle initialisation with a rough initial pose, such as noisy GNSS emulated by adding noise to ground truth. Publish the pose and a matching score. Where sequences do not overlap, split a long sequence into mapping and localisation portions over repeated roads.
Checkpoint: A localisation trajectory on an unseen drive with its error against ground truth.
- 8
Test robustness and degeneracy
Evaluate localisation with added point dropout, reduced beam count, dynamic objects that were not in the map and simulated map changes, such as removing parked cars. Detect degenerate geometry, for example by analysing the eigenvalues of the registration Hessian, and report where it occurs, such as long straight roads with few features. Measure recovery after a deliberate initial pose error.
Checkpoint: Robustness results and a map of degenerate segments with eigenvalue evidence.
- 9
Measure runtime and resource use
Profile CPU time per scan for odometry, loop detection, optimisation and localisation, and memory use for the map. Check whether localisation keeps up with 10 Hz lidar in real time and how map resolution trades accuracy against memory. Report results for at least two map voxel sizes. Run the measurements on the same machine with no other heavy processes, and report the CPU model and core count. Show the worst-case time per scan as well as the average, because occasional slow optimisation steps can make localisation fall behind the lidar.
Checkpoint: A runtime and memory table showing whether each component meets real time.
- 10
Write the thesis chapter
Present the system architecture, odometry, pose graph, loop closure, mapping and localisation results, robustness and degeneracy analysis, and runtime. Compare your relative error with the variants you implemented and, cautiously, with published methods on the same sequences. Release ROS 2 packages and evaluation scripts. State the limitations: KITTI only, a single lidar type and no fusion with IMU or wheel odometry.
Checkpoint: A chapter with reproducible ROS 2 packages and full evaluation results.
Testing & validation
- Relative translation and rotation error over KITTI path segments.
- ATE before and after loop closure, and false-loop rejection.
- Map-based localisation error on unseen drives and under degradations.
- Runtime per component against the 10 Hz lidar rate.
Expected results
A ROS 2 lidar SLAM and localisation system with measured odometry drift, loop-closure gains, localisation accuracy on unseen drives, robustness and degeneracy analysis and runtime figures.
Research angle for your PhD
Research questions: how do registration choices and loop closure affect drift, how robust is map-based localisation to map changes and sensor degradation, and can degeneracy be detected early enough to switch strategy? Metrics: KITTI relative error, ATE, localisation error, degeneracy detection accuracy, runtime. Baselines: scan-to-scan ICP and NDT odometry. A degeneracy-aware localisation method with measured robustness gains on public sequences and released ROS 2 code is a solid robotics conference or journal contribution.
Take it further
- Fuse IMU and wheel odometry in a factor graph and measure gains in degenerate segments.
- Run the same pipeline on simulated CARLA drives with exact ground truth.
- Add semantic filtering of dynamic objects before registration.
- Detect map changes and update the map incrementally.
Viva and review questions
Why does lidar odometry drift?
Each registration has a small error, and chaining many relative transforms accumulates those errors over distance.
How does loop closure reduce drift?
It adds a constraint between the current pose and a past pose at the same place, and pose graph optimisation spreads the correction along the trajectory.
What is a degenerate scene for registration?
One where geometry does not constrain all six degrees of freedom, such as a long straight tunnel, so motion along the road is poorly observed.
Why use a robust kernel in the pose graph?
A wrong loop closure would bend the whole map; a robust kernel down-weights edges with large residuals so a single false loop has limited effect.
Learn it at DIYguru
These DIYguru courses and programmes teach what this project needs.