Mapping & SLAM Systems

3.1From Odometry to SLAM

Odometry answers "how did I move"; a map answers "what is here"; SLAM — simultaneous localization and mapping — is the admission that on unknown ground each requires the other. You cannot place sensor readings into a map without knowing the pose that took them; you cannot correct the pose against the map until the map exists. Every SLAM system breaks that circle the same way: trust odometry locally (it is excellent over ten metres), build the map incrementally from it, and when the robot recognizes a previously mapped place, use the reunion to repair everything at once.

Architecturally the field converged on a two-part design. The front-end is perception: Phase 2’s odometry, plus the machinery that decides "this scan matches that old place" (a Phase 4 topic previewed here as loop closure detection). The back-end is inference: an optimization over the whole trajectory that reconciles what the front-end reported. The split matters because the two halves fail differently — front-ends lie occasionally (a wrong match), back-ends amplify whatever they are fed — and because the back-end is, to the letter, the Phase 1 loop.

Full vs online SLAM

Estimating the entire trajectory plus map (full, what graph back-ends do) versus only the current pose plus map (online, what filters did). Modern systems are full-SLAM with real-time incremental solvers.

Submap / keyframe

The unit of map building: a small locally-consistent chunk (Cartographer’s submaps) or a selected sensor frame with pose (ORB-SLAM’s keyframes). Local consistency comes from odometry; global consistency from the graph.

Loop closure

Recognizing a previously visited place and computing the relative pose to it — the only measurement that ties "now" to "long ago", and therefore the only cure for drift.

Front-end / back-end

Perception (make constraints) versus inference (optimize over them). The dividing line of every architecture diagram in this field.

3.2Occupancy Grids: The Workhorse Memory

The oldest map representation is still the one your planner probably eats: carve space into cells, store per cell the probability it is occupied. Each lidar ray is evidence — the cells it traverses are probably free, the cell it ends in is probably occupied — and per-cell Bayes updates have a famous closed form once you work in log-odds, where fusing evidence becomes addition:

odds = p/(1−p),   l = log(odds)                l₀ = prior (usually 0)

per ray, per cell:   l ← l + l_sensor − l₀     additive! clamp to [l_min, l_max]

  cells along the ray  →  l_free = log(0.3/0.7) ≈ −0.85     "probably empty"
  cell at the endpoint →  l_occ  = log(0.7/0.3) ≈ +0.85     "probably a hit"

recover probability:  p = 1 − 1/(1 + eˡ)
cells never touched stay at p = 0.5 — unknown, and "unknown" is real
information a planner must respect (Phase 5's frontier exploration lives on it)

Why log-odds instead of raw probabilities? Multiplying probabilities underflows and needs renormalizing; adding bounded integers is fast, stable, and trivially undoable. The clamp matters too: without bounds, a cell seen free ten thousand times takes ten thousand hits of evidence to flip when a pallet is parked there — clamping keeps the map adaptable. Rays are traced with Bresenham stepping; resolution is the knob that trades fidelity against memory and update cost, and in 2D a few centimetres is cheap. In 3D it is not — which is the next two sections.

3.3Octrees and OctoMap: 3D Without the Memory Bill

A dense 3D grid of a warehouse at 5 cm dies fast: 100 m × 50 m × 10 m is 400 million cells, nearly all of them air, most never observed. The octree fixes this with recursive subdivision: a cube splits into eight children only where the world has detail; huge uniform regions — free space, unknown space — stay as single fat nodes. OctoMap is the standard C++ implementation: probabilistic (each leaf carries the log-odds machinery of 3.2), lossless at the chosen max depth, queryable at any coarser resolution for free (ancestors summarize descendants), and serialized compactly enough to ship over a network. Setup B below builds one from simulated rays and measures the savings.

3.4TSDF and ESDF: Maps Made of Distances (Voxblox, NVBlox)

Occupancy answers "is this cell full?" — a question about cells. Planners and surface reconstruction keep asking better questions: "how far is the nearest obstacle?" and "where exactly is the surface?". Signed distance fields answer both natively. Each voxel stores its distance to the nearest surface, signed: positive in free space, negative inside objects; the surface is the zero crossing, recoverable at sub-voxel precision. The truncated SDF (TSDF) only maintains this within a band ±τ around surfaces (distance far from walls is expensive and useless for reconstruction), and fuses observations with a weighted running average that averages away sensor noise instead of just voting like occupancy does:

per observed ray/pixel, for voxels near the endpoint:
  d  = depth_measured − depth_of_voxel_along_ray       signed: + in front of wall
  d  ← clamp(d, −τ, +τ)                                the truncation band

fuse (weighted running average — this is why TSDF surfaces come out smooth):
  D(v) ← ( W(v)·D(v) + w·d ) / ( W(v) + w )
  W(v) ← W(v) + w

surface  = zero crossing of D  →  marching cubes  →  mesh
ESDF     = untruncated distance field, propagated outward from the TSDF —
           d(x) and ∇d(x) are exactly the collision cost and gradient
           Phase 5's trajectory optimizers want

Voxblox (ETH Zurich) made this practical on CPUs for planning: projective TSDF integration, then a wavefront that propagates the TSDF band outward into a full ESDF, incrementally, as the map grows — built explicitly so that Phase 5’s trajectory optimizers could query distance-and-gradient in microseconds. NVBlox is the same idea rebuilt for GPUs — TSDF, color, ESDF layers integrated in CUDA, order-of-magnitude faster, with a decay mechanism for dynamic scenes — usable as a plain C++/CUDA library. The representation menu, summarized:

3.5Loop Closure: The Drift Eraser

Everything so far still inherits Phase 2’s drift — a map built on drifting poses is a drifting map. The repair comes from one event: the robot re-enters known territory, recognizes it, and measures its pose relative to a place it saw long ago. That single relative measurement — pose 840 to pose 12 — carries information no amount of odometry can: it bounds the error of everything between.

A loop closure is manufactured in three steps, each owned by a different phase of this book. Propose: place recognition (Phase 4’s descriptors — DBoW, Scan Context) nominates "current scan resembles old submap k". Verify & measure: Phase 2’s registration (ICP/NDT, or PnP for cameras) aligns the two, producing the relative transform and — critically — a fitness score. Gate: accept only if the alignment is geometrically strong and statistically consistent with current drift estimates (a chi-squared test on the innovation). The gate exists because the downstream optimizer treats every accepted constraint as truth, and one false loop closure folds a building in half.

3.6Pose Graph Optimization: The Back-End

Now assemble the estimation problem. Nodes: the trajectory’s poses X_i (one per submap or keyframe — not every sensor frame). Edges: relative-pose measurements — consecutive ones from odometry, long-range ones from loop closures — each with its information matrix. The residual of an edge asks one question: does the trajectory disagree with what was measured between i and j?

edge (i, j):  measured Z_ij, information Ω_ij = Σ_ij⁻¹

residual:     r_ij(X) = log( Z_ij⁻¹ · X_i⁻¹ · X_j )ᵛ        a tangent vector!
                        (Phase 1's ⊟: "the step separating prediction from
                         measurement", so ‖r‖ = 0 means perfect agreement)

solve:        X* = argmin Σ_(i,j)  r_ijᵀ Ω_ij r_ij           + pin X₀ (gauge!)

— sparse damped Gauss-Newton on the manifold: Phase 1.8, line for line.
robust variant: wrap loop-closure terms in Huber/Cauchy ρ(·) so a false
edge saturates instead of bending the whole trajectory to appease it.

Sparsity does the heavy lifting: each edge touches two poses, so the Hessian is nearly empty, and 10,000-pose graphs solve in tens of milliseconds — which is why systems can afford to re-optimize on every accepted loop. The optimization redistributes the accumulated drift around the loop according to each edge’s stiffness Ω: confident odometry bends little, sloppy stretches absorb more. Before and after, in one picture:

// posegraph2d.cc — SE(2) pose graph: drifting square + loop closure(s).
#include <ceres/ceres.h>
#include <sophus/se2.hpp>
#include <sophus/ceres_manifold.hpp>
#include <iostream>
#include <vector>

struct EdgeResidual {          // r = log( Z⁻¹ · Xa⁻¹ · Xb )
  explicit EdgeResidual(const Sophus::SE2d& Z) : Z_inv_(Z.inverse()) {}
  template <typename T>
  bool operator()(const T* const a, const T* const b, T* residual) const {
    const Eigen::Map<const Sophus::SE2<T>> Xa(a), Xb(b);
    Eigen::Map<Eigen::Matrix<T, 3, 1>> r(residual);
    r = (Z_inv_.cast<T>() * (Xa.inverse() * Xb)).log();
    return true;
  }
  const Sophus::SE2d Z_inv_;
};

ceres::CostFunction* make_edge(const Sophus::SE2d& Z) {
  return new ceres::AutoDiffCostFunction<EdgeResidual, 3,
      Sophus::SE2d::num_parameters, Sophus::SE2d::num_parameters>(
      new EdgeResidual(Z));
}

int main() {
  const bool inject_false_loop = false;     // experiment 1: flip me
  const bool use_robust        = false;     // experiment 2: then flip me

  // truth: a 2 m square. odometry measures 2.1 m legs and 84.8° turns.
  const Sophus::SE2d odom_meas(1.48, Eigen::Vector2d(2.1, 0.0));

  std::vector<Sophus::SE2d> X(5);           // initial guess: integrate odometry
  for (int i = 1; i < 5; ++i) X[i] = X[i - 1] * odom_meas;

  ceres::Problem problem;
  for (auto& x : X)
    problem.AddParameterBlock(x.data(), Sophus::SE2d::num_parameters,
                              new Sophus::Manifold<Sophus::SE2>());
  problem.SetParameterBlockConstant(X[0].data());    // pin the gauge!

  for (int i = 0; i < 4; ++i)                        // odometry edges
    problem.AddResidualBlock(make_edge(odom_meas), nullptr,
                             X[i].data(), X[i + 1].data());

  // the true loop closure: pose 4 re-observes pose 0 at identity
  ceres::LossFunction* loss =
      use_robust ? new ceres::HuberLoss(0.5) : nullptr;
  problem.AddResidualBlock(make_edge(Sophus::SE2d()), loss,
                           X[4].data(), X[0].data());

  if (inject_false_loop)                    // a place-recognition mistake:
    problem.AddResidualBlock(make_edge(Sophus::SE2d()), loss,
                             X[2].data(), X[0].data());   // "pose2 = pose0" — lie

  ceres::Solver::Options opts;
  ceres::Solver::Summary summary;
  ceres::Solve(opts, &problem, &summary);

  for (int i = 0; i < 5; ++i)
    std::cout << "x" << i << "  (" << X[i].translation().x() << ", "
              << X[i].translation().y() << ", "
              << X[i].so2().log() * 180.0 / M_PI << "°)\n";
  std::cout << summary.BriefReport() << "\n";
}

3.7The Systems: Cartographer, GLIM, ORB-SLAM3

Three mature, ROS-free-usable systems cover the classical SLAM design space — one lidar-first with submaps, one LIO-first with a factor graph and a GPU, one camera-first with keyframes:

Two architectural contrasts worth internalizing. Submaps vs keyframes: Cartographer trusts odometry for the few seconds a submap takes to build, freezes the submap, and does all global reasoning submap-to-submap; ORB-SLAM keeps individual keyframes live forever and lets bundle adjustment re-shape everything. Submaps bound compute and suit dense grids; keyframes maximize accuracy and suit sparse features. Detected loops vs global matching: Cartographer and ORB-SLAM3 close loops when a detector fires; GLIM instead keeps registration-error factors between all overlapping submap pairs and lets the optimizer continuously grind global consistency — more compute, fewer dramatic jumps, no single make-or-break detection.

And the ecosystem note that keeps this book honest: the Phase 2 LIO systems (FAST-LIO2, DLIO) are odometry — superb, drifting. Add a loop-closure layer and a pose graph over their output and you have built a SLAM system; GLIM is what that looks like done properly, and Phase 4’s place recognition is the proposer it needs.

3.8Living With the Jump

When a loop closes, thousands of poses move at once — and the map, rebuilt from corrected poses, moves with them. Something on the robot is mid-motion when that happens. The survival pattern is a two-frame contract: keep a continuous odometry frame (Phase 2’s output, smooth, drifting, what the controller consumes) and a map frame (globally correct, jumping), and express SLAM’s output as the slowly-updated correction transform between them. Controllers and local planners live entirely in the odometry frame and never feel a jump; global planners re-plan in the map frame after each correction. The same contract handles map re-rendering: Cartographer re-anchors frozen submaps (cheap — the grid content never changes, only its pose); ORB-SLAM re-projects landmarks; TSDF systems either re-integrate or accept local warping.

// room_octree.cc — insert simulated lidar rays into an OctoMap octree.
#include <octomap/OcTree.h>
#include <cmath>
#include <iostream>

// room interior: |x|<3, |y|<2, 0<z<2.5. pillar: box 1.2<x<1.8, 0.5<y<1.1.
double exit_t(double o, double d, double lo, double hi) {   // slab exit
  if (std::abs(d) < 1e-9) return 1e18;
  return (d > 0 ? hi - o : lo - o) / d;
}
bool pillar_entry(const octomap::point3d& o, const octomap::point3d& d,
                  double& t_hit) {                          // slab entry test
  double t0 = 0.0, t1 = 1e18;
  const double lo[3] = {1.2, 0.5, 0.0}, hi[3] = {1.8, 1.1, 2.5};
  const double oo[3] = {o.x(), o.y(), o.z()}, dd[3] = {d.x(), d.y(), d.z()};
  for (int a = 0; a < 3; ++a) {
    if (std::abs(dd[a]) < 1e-9) { if (oo[a] < lo[a] || oo[a] > hi[a]) return false; continue; }
    double ta = (lo[a] - oo[a]) / dd[a], tb = (hi[a] - oo[a]) / dd[a];
    if (ta > tb) std::swap(ta, tb);
    t0 = std::max(t0, ta); t1 = std::min(t1, tb);
  }
  t_hit = t0;  return t0 < t1 && t0 > 0.0;
}

int main() {
  octomap::OcTree tree(0.05);                               // 5 cm leaves
  const octomap::point3d origin(0.0f, 0.0f, 1.2f);          // sensor pose

  for (int yaw_deg = 0; yaw_deg < 360; ++yaw_deg)           // a full sweep
    for (int pitch_deg = -30; pitch_deg <= 30; pitch_deg += 2) {
      const double yaw = yaw_deg * M_PI / 180.0, pit = pitch_deg * M_PI / 180.0;
      const octomap::point3d dir(std::cos(pit) * std::cos(yaw),
                                 std::cos(pit) * std::sin(yaw), std::sin(pit));
      double t = std::min({exit_t(origin.x(), dir.x(), -3.0, 3.0),
                           exit_t(origin.y(), dir.y(), -2.0, 2.0),
                           exit_t(origin.z(), dir.z(),  0.0, 2.5)});
      double tp;                                             // pillar occludes?
      if (pillar_entry(origin, dir, tp) && tp < t) t = tp;
      tree.insertRay(origin, origin + dir * (float)t);       // free + endpoint
    }

  tree.updateInnerOccupancy();
  const auto* hit  = tree.search(octomap::point3d(3.0f, 0.0f, 1.2f));  // wall
  const auto* mid  = tree.search(octomap::point3d(0.5f, 0.0f, 1.2f));  // air
  const auto* shad = tree.search(octomap::point3d(2.4f, 0.8f, 1.2f));  // shadow
  std::cout << "wall   P(occ) = " << (hit  ? hit->getOccupancy() : -1) << "\n"
            << "air    P(occ) = " << (mid  ? mid->getOccupancy() : -1) << "\n"
            << "shadow          " << (shad ? "observed?!" : "unknown — correct!") << "\n";
  std::cout << "leaf nodes : " << tree.getNumLeafNodes() << "\n"
            << "memory     : " << tree.memoryUsage() / 1024.0 << " kB  (dense 5 cm grid of this room: "
            << 120L * 80 * 50 * 8 / 1024.0 << " kB)\n";
  tree.writeBinary("room.bt");                               // octovis room.bt
}

3.9Reading List

3.10Interview Questions