How to Implement SLAM: Mapping the World in Real-Time

Step-by-Step Guide

1. 1

   Understand the SLAM Problem

   SLAM is a chicken-and-egg problem: to localize you need a map; to map you need to localize. SLAM solves both simultaneously using sensor data and motion estimates. Key sensors: LiDAR (precise range), cameras (rich visual info), IMU (orientation), wheel odometry (relative motion).

   text Copy

   SLAM State Estimation:
   State: x = [pose, map]
   Prediction: x_t = f(x_{t-1}, u_t) + noise
   Update: z_t = h(x_t, m) + noise
   Belief: bel(x_t) = p(x_t | z_{1:t}, u_{1:t})

2. 2

   Implement LiDAR SLAM

   2D algorithms: Gmapping (particle filter), Hector SLAM (scan matching, no odometry), Cartographer (graph-based, Google). 3D: LOAM, LeGO-LOAM, LIO-SAM (LiDAR-Inertial). LiDAR SLAM is robust in feature-rich environments but struggles in long corridors.

   💡

   Tip: Google Cartographer is recommended for 2D LiDAR SLAM in ROS2. For 3D, LIO-SAM combines LiDAR and IMU for robust odometry.
3. 3

   Implement Visual SLAM

   Monocular: ORB-SLAM3 (feature-based, most robust). Stereo: two cameras for depth. RGB-D: depth cameras (RealSense) — RTAB-Map is the ROS2 standard. Visual SLAM is cheaper than LiDAR but sensitive to lighting and textureless surfaces.

   ⚠️

   Warning: Monocular SLAM has scale ambiguity — cannot determine absolute scale without IMU, known object size, or stereo. Use stereo, RGB-D, or visual-inertial for metric scale.
4. 4

   Master Scan Matching

   Scan matching aligns consecutive scans for motion estimation. ICP (Iterative Closest Point) minimizes point cloud distance. NDT represents scans as Gaussian distributions. Correlative scan matching (Cartographer) is more robust but slower.

   python Copy

   # ICP — simplified
   import numpy as np
   from scipy.spatial.distance import cdist
   
   def icp(source, target, max_iter=50, tol=1e-6):
       for i in range(max_iter):
           dist = cdist(source, target)
           idx = np.argmin(dist, axis=1)
           matched = target[idx]
           cs, ct = source.mean(0), matched.mean(0)
           H = (source-cs).T @ (matched-ct)
           U, S, Vt = np.linalg.svd(H)
           R = Vt.T @ U.T
           t = ct - R @ cs
           source = (R @ source.T).T + t
           if np.linalg.norm(t) < tol: break
       return source

5. 5

   Understand Pose Graph Optimization

   SLAM is a graph optimization. Nodes are robot poses; edges are constraints (odometry between consecutive poses, loop closures between revisited locations). Optimizer adjusts all poses to minimize residuals. Libraries: g2o, GTSAM, Ceres Solver.

   

6. 6

   Detect Loop Closures

   Loop closure corrects drift by detecting revisited locations. Methods: scan-based (compare LiDAR scans), visual (bag-of-words image matching, DBoW2), geometric (descriptor matching). False positives are catastrophic — verify with geometric consistency before accepting.

   ⚠️

   Warning: False loop closures destroy maps. Always verify with RANSAC pose estimation before adding to the pose graph. One false closure can warp the entire map.
7. 7

   Build Occupancy Grid Maps

   Occupancy grids are 2D grids where each cell has an occupancy probability (0-1). Built by raycasting LiDAR beams: cells along beam are free, endpoint is occupied. Resolution: 5cm indoor, 10-20cm outdoor. Used by navigation stack for path planning.

   python Copy

   # Occupancy grid update
   import numpy as np
   def update_grid(grid, scan, pose, res):
       for angle, r in scan:
           ex = pose[0] + r*np.cos(pose[2]+angle)
           ey = pose[1] + r*np.sin(pose[2]+angle)
           cells = bresenham(pose[:2], [ex,ey], res)
           for c in cells[:-1]: grid[c] += -0.4  # free
           grid[cells[-1]] += 0.85  # occupied
       return grid

8. 8

   Use the ROS2 Nav2 Stack

   Nav2 provides: AMCL localization, path planning (NavFn, Theta*, RRT), local planning (DWB, MPPI), costmaps (local+global), behavior trees. Provide a map from SLAM, configure costmaps, set navigation goals via action interface.

   💡

   Tip: Use Nav2 with Cartographer: run Cartographer for mapping (teleop to explore), save the map, then switch to AMCL + Nav2 for autonomous navigation.
9. 9

   Handle Dynamic Environments

   Standard SLAM assumes static environments. For dynamic objects: detect and remove moving points (velocity checks, temporal consistency), or use SLAMMOT (SLAM with moving object tracking), or semantic SLAM (label and handle objects differently).

10. 10

    Evaluate SLAM Performance

    Metrics: ATE (Absolute Trajectory Error vs ground truth), RPE (Relative Pose Error — drift per distance), map quality (visual inspection), runtime (CPU, memory). Datasets: KITTI, EuRoC, TUM RGB-D. Tools: evo, RViz2.

    text Copy

    SLAM Evaluation:
    ATE: Global trajectory accuracy
    RPE: Drift per unit distance
    Datasets: KITTI, EuRoC, TUM RGB-D
    Tools: evo, RViz2, rqt_bag

11. 11

    Deploy SLAM on a Real Robot

    Choose sensors (2D LiDAR indoor, 3D LiDAR outdoor, camera for visual), select algorithm (Cartographer 2D, LIO-SAM 3D, ORB-SLAM3 visual), tune parameters, test in target environment. Hardware: mini PC (Intel NUC, Jetson) for real-time SLAM.

    ⚠️

    Warning: SLAM that works in one building may fail in another due to different geometry, lighting, or dynamics. Test in the actual deployment environment.