Foundations: The Six Ideas Everything Else Is Built On

0.1The Problem, Stated Like a Human

Imagine you wake up in an unfamiliar building with a notepad. Your job: draw a floor plan of the building and mark, at every moment, exactly where you are standing on that floor plan.

Notice the trap. To draw the walls in the right place, you need to know where you are when you see them. But to know where you are, you need a map to locate yourself against. Each answer depends on the other. This chicken-and-egg is the whole problem, and it has a name.

Why does a robot need this at all? Because GPS does not work indoors, under bridges, in warehouses, in tunnels, or precisely enough for a robot that must pass through a 90 cm doorway. A robot with no SLAM only knows "I told my wheels to spin." It has no idea what actually happened.

0.2Idea 1: Odometry, or Counting Your Steps

The simplest way to track yourself: start at a known point and add up your own motion. Wheels turned 10 times, times wheel circumference, means you moved 3.1 m forward. This is odometry (from the Greek hodos, "path"), also called dead reckoning.

Odometry comes in four main flavors, and every SLAM system runs at least one of them:

• Wheel odometry. Encoders count wheel revolutions; kinematics turns counts into motion. Nearly free, but it measures what the wheels did, not what the robot did. Slip on dust, carpet, or a threshold and the two diverge silently.
• Inertial odometry. Integrate an IMU's gyroscope for orientation and accelerometer (twice) for position. Superb over a tenth of a second, hopeless alone over a minute: any accelerometer bias integrates into a position error that grows with time squared.
• Visual odometry. Track features between camera frames and solve for the motion that explains their shift. A good implementation drifts around 0.5 to 1% of distance traveled. Dies in darkness, blur, and on blank walls.
• Lidar odometry. Align each laser scan to the previous one (scan matching, Chapter 3). Centimeter-level in geometry-rich scenes, degenerate in a long straight corridor where every scan looks identical.

All four share the same disease. Position is the sum of every increment, so per-step errors accumulate instead of canceling. That accumulation is called drift, and it is the villain of this entire book. Pure translation noise grows roughly like a random walk, with the square root of distance. Heading error is far nastier: it grows linearly with everything you drive afterwards.

Here is the lever-arm intuition worth memorizing. A heading error of θ radians turns into θ·d meters of sideways error after driving d meters. One degree is about 0.017 radians, so a single 1° gyro hiccup costs 1.7 cm per meter, which is 1.7 m after a 100 m aisle. A 1.7 cm translation error after the same aisle would be a rounding error. This is why every serious odometry stack spends its best sensor (usually the gyro) on rotation, and why IMU gyro calibration gets a whole section in Chapter 2.

Odometry

Estimating your motion incrementally from your own sensors: wheel encoders, an IMU, camera frames, or lidar scans compared to the previous one.

IMU (Inertial Measurement Unit)

A chip with accelerometers and gyroscopes. Measures acceleration and rotation rate hundreds of times per second. Great for a fraction of a second; drifts badly if integrated alone, because errors get integrated twice on the way to position.

Drift

Accumulated error in an incremental estimate. Small per step, unbounded over time.

Pose

Position plus orientation. In 2D: (x, y, heading θ). In 3D: (x, y, z) plus three rotation angles, hence "6 degrees of freedom" (6-DOF).

Concrete stakes: 1% drift sounds great until you have driven 500 meters and you are 5 meters off, which is the difference between "docked at the charger" and "in the pond."

0.3Idea 2: Loop Closure, or Recognizing Where You Have Been

Here is the escape hatch from drift. Suppose, while blindfold-walking, you occasionally reach out and touch a landmark: "ah, this is the same doorframe I touched five minutes ago." The instant you recognize a previously visited place, you learn something powerful. However much your step-counting says you have wandered, you are actually back here. You can now go back and correct your whole path, smearing the accumulated error across every step you took in between.

That recognition event is called a loop closure, and the correction it triggers is what upgrades mere odometry into SLAM.

How does a robot recognize a place it cannot "see" the way you do? The universal recipe has two stages. First, place recognition: compress each sensor snapshot into a compact fingerprint and search a database of past fingerprints for near-matches. For images the classic fingerprint is a bag of visual words (DBoW2); for lidar, a polar signature of the scan such as Scan Context. Second, geometric verification: take the candidate match and try to actually align the two scans or images. If no sensible relative pose explains both views, the candidate is rejected. Fingerprints propose; geometry disposes.

Place recognition

Answering "have I seen this place before?" from sensor data alone, usually by matching compact descriptors against a database of past views.

Perceptual aliasing

Two different places that look identical to the sensor. The main source of false loop closures.

Geometric verification

Confirming a candidate loop by actually aligning the two views and checking the alignment quality before adding the constraint.

Keyframe

A selected subset of sensor frames kept for matching and mapping, so the database grows with the environment rather than with time.

python3 -m venv slamlab && source slamlab/bin/activate
pip install numpy matplotlib

import numpy as np
import matplotlib.pyplot as plt

rng = np.random.default_rng(7)

# Ground truth: a 10 m square, 40 steps per side, 0.25 m per step
steps_per_side, step = 40, 0.25
headings = np.repeat([0.0, np.pi/2, np.pi, 3*np.pi/2], steps_per_side)
truth = np.vstack([[0, 0], np.cumsum(
    np.c_[step*np.cos(headings), step*np.sin(headings)], axis=0)])

# Odometry: each step's length and heading increment are slightly wrong
sigma_step, sigma_yaw = 0.01, np.deg2rad(0.6)     # 1 cm, 0.6 deg per step
dyaw = np.diff(headings, prepend=headings[0])
yaw_meas = np.cumsum(dyaw + rng.normal(0, sigma_yaw, len(headings)))
step_meas = step + rng.normal(0, sigma_step, len(headings))
odom = np.vstack([[0, 0], np.cumsum(
    np.c_[step_meas*np.cos(yaw_meas), step_meas*np.sin(yaw_meas)], axis=0)])

# Loop closure: the robot is back at the start; its belief says otherwise
err = odom[-1] - truth[0]
print(f"endpoint error before correction: {np.linalg.norm(err):.2f} m")

# Distribute the error along the path (poor man's pose graph relaxation)
w = np.linspace(0, 1, len(odom))[:, None]
corrected = odom - w * err

fig, ax = plt.subplots(figsize=(6, 6))
ax.plot(*truth.T,     'k-',  lw=2,   label='ground truth')
ax.plot(*odom.T,      'r--', lw=1.5, label='odometry (drifted)')
ax.plot(*corrected.T, 'g-',  lw=1.5, label='after loop closure')
ax.scatter(0, 0, s=80, c='teal', zorder=5, label='start = end')
ax.set_aspect('equal'); ax.legend(); ax.set_title('drift, then one loop closure')
plt.savefig('loop_closure.png', dpi=150); plt.show()

0.4Idea 3: The Map, or What We Are Actually Drawing

"Map" can mean several very different data structures, and choosing one is a real engineering decision:

Size intuition makes these concrete. A landmark map of a building is a few thousand to a few hundred thousand points with descriptors, tens of megabytes at most; this is what ORB-SLAM3 keeps and relocalizes against. An occupancy grid at 5 cm resolution covering 100×100 m is 2000×2000 cells, about 4 MB at a byte per cell; this is the product of slam_toolbox and the diet of every 2D planner, and OctoMap extends the idea to 3D with octree compression. A raw point cloud is the memory hog: a 16-beam lidar at 10 Hz produces roughly 300k points per second, so a ten-minute survey is nearly 200 million points and gigabytes on disk, which is why FAST-LIO2 style pipelines voxel-downsample aggressively. A TSDF stores dense voxels near surfaces; at 10 cm voxels a room is cheap, at 1 cm it is not, and systems like Voxblox exist to manage exactly that trade. Neural and Gaussian maps invert the trade-off: a NeRF-style network is only megabytes of weights but costs a GPU to query, while a Gaussian-splat map of a house runs to millions of primitives and hundreds of megabytes, in exchange for photorealistic rendering (Chapter 4).

Keep the word "unknown" in mind for occupancy grids. A good map distinguishes "I know this is empty" from "I have never looked there," and an exploring robot lives on that difference.

0.5Idea 4: Everything Is Uncertain, So Everything Is Probability

Every sensor lies a little. Wheels slip. Lidar ranges have noise. Cameras blur. So SLAM never says "I am at (3.20, 5.10)." It says "I believe I am near (3.20, 5.10), give or take this much": a probability distribution over poses. The math machinery of the field is about maintaining and updating that belief:

• Predict: use your motion (odometry) to shift the belief forward, and widen it, because motion adds uncertainty.
• Correct: use a sensor observation ("the wall is 2.1 m ahead") to narrow the belief, because observations remove uncertainty.

This predict/correct heartbeat is the Bayes filter, and its two famous children are the Kalman filter (belief is a single Gaussian blob: fast, but it can only believe in one place at a time) and the particle filter (belief is a swarm of thousands of guesses, which can believe in several places at once; that matters for the kidnapped-robot problem in Chapter 9). The bible for all of this is Probabilistic Robotics, Thrun, Burgard & Fox (MIT Press, 2005).

That is all the probability this chapter needs. The full machinery (Bayes rule to Kalman to particle filters, and why they are all the same idea wearing different clothes) now lives in Chapter 1, which is the toolbox every later chapter reaches into.

0.6Idea 5: Modern SLAM Is an Optimization Problem

Filters update a belief step by step and throw the past away. Modern SLAM keeps the past and solves it all at once. Picture a graph:

• Nodes are your pose at each moment ("where I was at t=1, t=2, ...").
• Edges are measurements connecting poses: "odometry says node 5 is 1 m ahead of node 4," or "loop closure says node 200 is the same place as node 12."

Each edge is a soft constraint, a spring. Odometry springs chain the nodes together; a loop closure adds a long-range spring from late in the trajectory back to early. Then you relax the spring network: find the set of poses that stretches all springs the least. That "least total squared stretch" is least-squares optimization, the formulation is called pose graph optimization, and its general form is a factor graph. Libraries that solve it: GTSAM, Ceres Solver, g2o.

Front-end

The part that processes raw sensor data into constraints: feature matching, scan matching, loop closure detection. Deals with pixels and points.

Back-end

The optimizer that takes those constraints and solves for the best poses and map. Deals with math. "Front-end proposes, back-end disposes."

Bundle adjustment (BA)

The camera version of graph optimization: jointly refine camera poses and 3D point positions so that points re-project onto the images where they were actually seen. The gold standard of accuracy in visual SLAM.

How the springs become sparse linear algebra, why the problem is solvable at all for millions of variables, and what "optimizing on a manifold" means when rotations enter the picture: that is Chapter 1. Here, keep the physical picture.

0.7Idea 6: Coordinate Frames, the Robot's Family Tree

A robot juggles several coordinate systems at once, and confusing them causes the most embarrassing bugs in robotics. The convention the whole industry converged on (written down as REP 105, but honored far beyond its birthplace): frames form a tree, every frame has exactly one parent, and a typical mobile robot's tree looks like this:

map                               <- map->odom maintained by SLAM / localization
└── odom                          <- odom->base maintained by the odometry source
    └── base_footprint            (robot projected onto the floor plane)
        └── base_link             (robot body; x forward, y left, z up)
            ├── laser_link            (static: measured extrinsic)
            ├── imu_link              (static: measured extrinsic)
            └── camera_link           (static: measured extrinsic)
                ├── camera_color_optical_frame   (z forward, x right, y down)
                └── camera_depth_optical_frame

• base_link: the robot's own body. "Forward" for the robot. Body-frame axes by convention: x forward, y left, z up.
• odom: where odometry thinks the robot has moved since power-on. Smooth and continuous, but drifts.
• map: the globally correct world frame. The SLAM system maintains the correction between map and odom, and that correction can jump when a loop closure fires.

Who owns what is a contract, and it is worth reciting. The wheel or VIO odometry code owns odom→base. The SLAM or localization code owns map→odom, computed so that the chain map→odom→base equals its best pose estimate. The static sensor transforms base_link→laser_link and friends are your measured extrinsics from Chapter 2, loaded from a config file at startup. Nobody writes map→base directly.

Why the odd split? Because the motor controller must never see the robot's position teleport 30 cm mid-motion. Smooth things (steering, velocity control) consume odom→base; correct things (global plans, "go to the charger") consume map→base through the full chain. Chapter 9 turns this contract into a superpower, and it remains one of the most senior-sounding things you can explain in an interview.

import numpy as np
rng = np.random.default_rng(7)

def T(x, y, th):                       # SE(2) as a 3x3 homogeneous matrix
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1]])

def params(M):                         # back to (x, y, theta)
    return M[0, 2], M[1, 2], np.arctan2(M[1, 0], M[0, 0])

# ground truth: drive a 10 m square, 1 m steps, left turn at corners
truth, odom = [T(0, 0, 0)], [T(0, 0, 0)]
for leg in range(4):
    for step in range(10):
        step_true = T(1.0, 0, np.pi/2 if step == 9 else 0)
        noise     = T(rng.normal(0, .02), rng.normal(0, .02), rng.normal(0, .01))
        truth.append(truth[-1] @ step_true)
        odom.append(odom[-1] @ step_true @ noise)   # odometry integrates noise

T_map_odom = T(0, 0, 0)                # the SLAM correction, identity at start
for k, (Tt, To) in enumerate(zip(truth, odom)):
    if k == len(truth) - 1:            # back at start: loop closure fires!
        T_map_odom = Tt @ np.linalg.inv(To)
    x, y, th   = params(T_map_odom)          # map->odom: the correction
    ox, oy, ot = params(To)                  # odom->base: smooth, drifting
    mx, my, mt = params(T_map_odom @ To)     # map->base: the composed truth-estimate
    print(f'{k:3d}  map->odom [{x:+.3f} {y:+.3f} {th:+.3f}]   '
          f'odom->base [{ox:+.3f} {oy:+.3f}]   map->base [{mx:+.3f} {my:+.3f}]')

0.8The Sensors, in One Honest Table

No sensor is sufficient alone, which is why the word fusion appears in every system name (LIO is Lidar-Inertial Odometry, VIO is Visual-Inertial Odometry). Sensors are chosen so that one's weakness is another's strength. The IMU carries you through the 0.1 s the camera is blurred; the camera cancels the IMU's drift. The physics behind each row, the noise models that describe them, and the calibration that makes fusion possible at all are Chapter 2.

0.9A Short History of SLAM

Knowing the eras is not trivia. Each one exists because the previous one broke somewhere specific, and interviewers love asking why.

1986, the problem gets its shape. Randall Smith and Peter Cheeseman publish "On the Estimation and Representation of Spatial Uncertainty," and with Matthew Self extend it to the stochastic map: robot pose and all landmark positions stacked into one Gaussian state with a full covariance matrix. The insight that matters is the off-diagonal terms. Landmark estimates are correlated through the robot that observed them, so seeing one landmark improves your estimate of all the others. What it fixed: SLAM became a well-posed estimation problem instead of a heuristic. What it broke: nothing yet, because computers of the era could not run it beyond toy sizes.

The 1990s, EKF-SLAM meets real robots. Leonard, Durrant-Whyte, and others put the stochastic map on sonar- and laser-equipped robots, and the acronym SLAM itself is coined mid-decade. Convergence proofs follow (Dissanayake et al., 2001). What it fixed: the first genuinely online systems that both mapped and localized. What it broke: the covariance update is O(n²) in the number of landmarks, capping honest maps at a few hundred features; linearizing around a wrong heading makes the filter quietly overconfident; and one bad data association is baked into the state forever, since a filter cannot revise the past.

2002, FastSLAM and the particle trick. Montemerlo, Thrun, Koller, and Wegbreit factor the problem: sample whole trajectories with a particle filter, and note that given a trajectory, landmarks become independent, so each particle carries N tiny per-landmark EKFs instead of one giant coupled one. Cost drops to O(M log N). What it fixed: tens of thousands of landmarks, plus per-particle data association, so a wrong match can die with its particle instead of poisoning everything. What it broke: long loops. Resampling collapses particle diversity, and the one particle that remembered the correct early trajectory is often gone by the time the loop closes.

The 2010s, the graph era. The idea dates to Lu and Milios in 1997, but sparse linear algebra made it practical: square-root SAM (Dellaert and Kaess, 2006), iSAM2, g2o (Kümmerle et al., ICRA 2011), GTSAM, and Ceres. The SLAM information matrix is naturally sparse, so re-optimizing an entire trajectory takes milliseconds, and loop closures can rewrite history. This is the substrate under ORB-SLAM, Cartographer, and the LOAM lineage. What it fixed: scale, accuracy, and retroactive correction. What it left open: the optimizer believes its front-end, so a single false loop closure still wrecks the map, which spawned robust kernels, switchable constraints, and a small industry of outlier-hardened solvers (Chapter 1).

The 2020s, the learning era. Neural networks replace the hand-tuned pieces one at a time: learned features (SuperPoint), learned matching (SuperGlue), learned dense bundle adjustment (DROID-SLAM, 2021), then maps themselves go neural with NeRF-style and Gaussian-splat representations, and feed-forward models like DUSt3R skip the pipeline entirely. What it fixed: robustness exactly where hand-crafted features die, in blur, low texture, and bad light. What it broke: compute budgets, predictability outside the training distribution, and the comfortable assumption that you can explain a failure by reading the code. Chapters 4 and 6 keep score honestly.

0.10Anatomy of a Modern SLAM System

Strip the branding off any modern system, LiDAR or visual, classical or learned, and the same four organs appear. The front-end eats raw sensor data and produces odometry, features, and keyframes at sensor rate. The back-end holds the factor graph and re-optimizes it when new constraints arrive. The loop detector watches the keyframe stream, searches the past, and injects the occasional long-range constraint. The map manager owns the growing map: it culls redundant keyframes, maintains the landmark database the loop detector searches, and serves whatever map product the rest of the robot consumes.

The names change per community but the org chart survives. In ORB-SLAM3 the organs are called tracking, local mapping, loop closing, and the atlas. In a LiDAR stack like FAST-LIO2 plus a pose-graph layer, the front-end is the tight IMU-lidar filter and the back-end lives in a separate node. RTAB-Map (Chapter 5) ships all four in one process with a memory manager deciding which keyframes stay eligible for loop search. When you read any system paper, your first job is to find the four organs; whatever is left over is the paper's contribution.

0.11Your Map of the Book

Ten chapters, each stated in a line:

• Ch. 1, The Estimation Toolbox. Bayes to Kalman to particle filters to least squares to factor graphs, plus manifolds for rotations. The math everything else leans on.
• Ch. 2, Sensors, Calibration & Time. Sensor physics, noise models, intrinsics and extrinsics, Kalibr, clock sync. Where accuracy is actually won or lost.
• Ch. 3, LiDAR SLAM & the Mapping Pipeline. Deskew, scan matching, LOAM to FAST-LIO2, and the map products people ship.
• Ch. 4, Visual SLAM: the State of the Art. Six eras of camera SLAM, from ORB-SLAM3 through DROID to neural rendering and feed-forward models.
• Ch. 5, RTAB-Map + RealSense D435i. The budget indoor rig, end to end, with the bruises documented.
• Ch. 6, ORB-SLAM: Every Version, Explained and Run. The four threads shared by every release, what changed from v1 to v2 to v3, and builds of both ORB-SLAM2 and ORB-SLAM3 you run yourself.
• Ch. 7, Learning-Based SLAM. Learned modules, end-to-end systems, learned maps, and an honest ledger of the trade-offs.
• Ch. 8, Evaluation, Datasets & Benchmarks. ATE and RPE, evo, KITTI, EuRoC, TUM, and how to run an eval you would stake a deployment on.
• Ch. 9, Planning, Navigation & Localization. The frame contract in anger: Monte Carlo localization, costmaps, and planners from A* to MPPI, built from scratch in Python.

Reading order: 0, 1, 2 in sequence, then 3 or 4 depending on whether your robot carries a laser or a camera, then wherever your project points. Chapter 8 pays off whenever you touch it.

0.12Check Yourself: Interview Warm-Ups