Global Localization & Relocalization

4.1The Kidnapped Robot Problem

Localization against a known map comes in three regimes of increasing cruelty. Tracking: the pose is approximately known and each update is a small correction — Phase 2’s Gaussian filters handle this natively. Global localization: the robot wakes with no pose prior; the belief must start spread over the entire map, and in any building with repeated structure it will be genuinely multimodal — "I am in one of these four identical aisles" is the honest answer for a while. Kidnapped robot: hardest of all, the robot is confidently wrong — it was carried, the wheels slipped on a dolly, the elevator opened on a different floor — and it must first discover that its tight, self-assured belief is fiction before it can rebuild one. A unimodal Gaussian is structurally incapable of the last two: it has exactly one bump and no mechanism to say "or, entirely elsewhere".

4.2Monte Carlo Localization

MCL represents the belief as N weighted samples — particles, each a full pose hypothesis — and runs the Bayes filter on the swarm. Multimodality costs nothing: four plausible aisles simply means four clusters of particles. The loop is Phase 1’s heartbeat with a population:

predict:   move every particle through the motion model + sampled noise
weight:    wᵢ ∝ p(z | xᵢ, map)          how well does the scan fit HERE?
resample:  draw N new particles ∝ wᵢ    (low-variance/systematic sampler)
           … but only when N_eff = 1 / Σ wᵢ² drops below ~N/2

global init: particles uniform over free space
recovery (Augmented MCL): track fast/slow averages of the mean weight —
  w_fast ← w_fast + α_fast(w̄ − w_fast)        α_fast ≈ 0.1
  w_slow ← w_slow + α_slow(w̄ − w_slow)        α_slow ≈ 0.01
  inject uniform random particles with probability max(0, 1 − w_fast/w_slow)
  → when observations suddenly stop fitting (kidnap!), w_fast dives below
    w_slow and the filter automatically re-seeds hypotheses everywhere

The measurement likelihood p(z | x, map) is where lidar MCL earns its keep, and two models dominate. The beam model raycasts the map from the hypothesis pose and compares each predicted range to the measured one — physically faithful (it models max-range misses and unexpected obstacles explicitly) but expensive and brittle: a hypothesis one cell off a doorway predicts a wildly different ray. The likelihood field model precomputes a distance transform of the map and scores each scan endpoint by its distance to the nearest obstacle — smoother, faster, forgiving of small pose errors, blind to the free-space information along the ray. Production 2D stacks almost universally run likelihood fields, with the smoothing that makes the posterior well-behaved being exactly why.

// mcl1d.cc — Monte Carlo localization on a 20 m circular corridor.
#include <algorithm>
#include <cmath>
#include <iostream>
#include <random>
#include <vector>

const double L = 20.0;                          // corridor length (wraps)
bool at_door(double x) {                        // doors at 3, 9, 15 (1 m wide)
  for (double d : {3.0, 9.0, 15.0})
    if (std::abs(std::fmod(x - d + L, L)) < 0.5) return true;
  return false;
}

int main() {
  std::mt19937 rng(3);
  std::uniform_real_distribution<double> U(0.0, 1.0);
  auto gauss = [&](double s) {
    return std::normal_distribution<double>(0.0, s)(rng); };

  const int N = 1000;
  std::vector<double> p(N), w(N, 1.0 / N);
  for (auto& x : p) x = U(rng) * L;             // global init: uniform

  double robot = 6.2;                            // true pose (unknown to filter)
  double w_slow = 0, w_fast = 0;                 // augmented-MCL averages

  for (int step = 1; step <= 160; ++step) {
    if (step == 90) robot = std::fmod(robot + 7.0, L);   // KIDNAP!

    robot = std::fmod(robot + 0.25 + gauss(0.02), L);    // truth moves
    const bool z = (U(rng) < 0.9) == at_door(robot);     // noisy door sensor

    double w_sum = 0.0;
    for (int i = 0; i < N; ++i) {                        // predict + weight
      p[i] = std::fmod(p[i] + 0.25 + gauss(0.05) + L, L);
      w[i] *= (z == at_door(p[i])) ? 0.9 : 0.1;
      w_sum += w[i];
    }
    double w_avg = w_sum / N, neff = 0.0;
    for (auto& wi : w) { wi /= w_sum; neff += wi * wi; }
    neff = 1.0 / neff;

    w_fast += 0.10 * (w_avg - w_fast);                   // recovery bookkeeping
    w_slow += 0.01 * (w_avg - w_slow);
    const double p_inject = std::max(0.0, 1.0 - w_fast / std::max(w_slow, 1e-12));

    if (neff < N / 2.0) {                                // systematic resample
      std::vector<double> np(N);
      double c = w[0], r = U(rng) / N;
      for (int i = 0, j = 0; i < N; ++i) {
        const double u = r + double(i) / N;
        while (u > c) c += w[++j];
        np[i] = (U(rng) < p_inject) ? U(rng) * L : p[j]; // inject when lost
      }
      p = np;  std::fill(w.begin(), w.end(), 1.0 / N);
    }

    if (step % 10 == 0) {                                // ASCII belief map
      std::vector<int> hist(40, 0);
      for (double x : p) hist[int(x / L * 40)]++;
      std::cout << "step " << step << (step == 90 ? " [KIDNAPPED]" : "")
                << "  inject=" << p_inject << "\n  ";
      for (int h : hist) std::cout << " .:-=+*#@"[std::min(8, h / 12)];
      std::cout << "\n  robot at column " << int(robot / L * 40) << "\n";
    }
  }
}

4.3Scan Matching Against a Known Map

Particles get you a coarse, possibly multimodal fix; finishing the job — and staying localized cheaply afterwards — is registration against the map, which is Phase 2’s machinery pointed at a prior map instead of the previous scan. Two regimes matter. With a decent initial guess (a converged particle cluster, or last frame’s pose), ICP/NDT refine to centimetres in a few iterations. With no guess, gradient methods fall into the nearest local minimum, and the honest tool is exhaustive: correlative scan matching scores every pose in a search window against the map’s likelihood field, made real-time by branch and bound — evaluate coarse map resolutions first, and prune whole branches whose upper bound already loses to the best fine-resolution candidate. This is precisely Cartographer’s loop-closure searcher from Phase 3, reused here as a global localizer: deterministic, no learning, and guaranteed to find the window’s best pose.

correlative search:  score(x, y, θ) = Σᵢ likelihood_field( map, endpointᵢ(x,y,θ) )
                     over a window, e.g. ±5 m × ±5 m × 360°

branch & bound: coarse-to-fine map pyramid; a coarse cell's score upper-bounds
all its children → prune losing branches wholesale. Exhaustive answer,
logarithmic-ish cost. (Hess et al., Phase 3 reading list.)

4.4Place Recognition: Localization as Retrieval

Correlative search saturates at building scale, and cameras don’t raycast. At scale, global localization becomes a retrieval problem: during mapping, compress every place into a compact descriptor and index it; at wake-up (or at every frame, hunting loop closures for Phase 3), compress the current view the same way and search the index. The pipeline is always the same four boxes, and the last one is not optional:

Descriptor

A compact vector (or small matrix) summarizing a whole image or scan, built to be near its twins and far from everything else under some cheap distance.

Perceptual aliasing

Different places that produce near-identical descriptors — office corridors, warehouse aisles, tree lines. The central failure mode of this entire phase.

Inverted index

Word → list of images containing it. Makes BoW search cost proportional to shared vocabulary, not database size.

Geometric verification

Proving a candidate with geometry: enough RANSAC-consistent feature matches (PnP) or a high ICP fitness. The gate between "similar" and "same".

4.5Visual Place Recognition: DBoW3, NetVLAD, CosPlace

The classical engine is bag of visual words. Offline, cluster millions of local feature descriptors (ORB) into a vocabulary tree — words are cluster centers, the tree makes lookup logarithmic. Online, quantize each image’s features into words and represent the image as a sparse TF-IDF-weighted histogram; two images are compared by histogram similarity, and the inverted index finds sharers without scanning the database:

TF-IDF weight of word i in image:   tf(i) · log( N_images / N_images_containing_i )
   → words that are common everywhere (floor texture) count for nearly nothing;
     rare words (that one exit sign) dominate the vote

score(v₁, v₂) = 1 − ½‖ v₁/‖v₁‖ − v₂/‖v₂‖ ‖₁      then: candidates → verify

DBoW3 (successor of DBoW2, which sits inside ORB-SLAM3) is the standalone C++ library for exactly this: binary-descriptor vocabularies, inverted index, direct index for later feature matching — CPU-only, milliseconds per query, and still the default relocalizer of the classical stack. Its weakness is inherited from its features: ORB words describe texture, and texture changes with light, weather, and season.

The learned generation attacks that. NetVLAD (CVPR 2016) bolts a differentiable VLAD-pooling layer onto a CNN and trains end-to-end on street-view imagery with a weakly-supervised triplet loss ("same place under different conditions must be closer than different places"), producing a single global vector per image whose nearest neighbors survive day/night and seasonal change that kills BoW. CosPlace (CVPR 2022) got the same robustness cheaper by reframing training as classification over geographic cells on a city-scale dataset — no triplet mining — yielding compact descriptors (as small as 512-D) that set retrieval benchmarks with a fraction of the compute. The practical stack in 2026: CosPlace-style global descriptors for candidates, local-feature verification (ORB, or Phase 8’s SuperPoint+LightGlue) for the geometry gate.

4.6LiDAR Place Recognition: Scan Context++, OverlapNet, LiDAR-Iris

Lidar descriptors get one enormous gift: geometry doesn’t care about lighting. Their enemy is viewpoint — revisit a place facing the other way and a naive descriptor sees nothing familiar — so every serious design builds in rotation invariance explicitly.

Scan Context++ is the classical reference. Flatten the scan into a polar bird’s-eye grid — rings (range bins) × sectors (azimuth bins), each cell keeping the max point height — giving a small 2D matrix that reads like a fingerprint of the surrounding structure. A yaw change of the robot is a pure column shift of that matrix, so matching scores the minimum distance over all shifts — and the best shift itself is a free yaw estimate, exactly the initial guess registration wants. Candidate search stays sub-linear via a ring key (a rotation-invariant per-ring summary) in a kd-tree; the ++ version adds lateral-shift robustness for revisits in adjacent lanes. Setup B builds the 2D miniature of this.

scan context:  M[r][s] = max height of points in ring r, sector s
distance(A, B) = min over column shifts k of  Σ_cols cosine_dist(A_col, B_col(+k))
best k · (360°/N_sectors)  =  relative yaw  →  free initial guess for ICP

OverlapNet is the learned counterpoint: render scans as range images, feed pairs through a siamese network that predicts both their overlap fraction (how much they see the same structure — a direct loop-closure confidence) and their relative yaw. LiDAR-Iris borrows iris recognition from biometrics: expand the bird’s-eye view into a binary "iris" image whose Fourier-aligned Hamming distance is fast and rotation-invariant. The selection logic mirrors vision: Scan Context++ as the CPU-cheap default with a free yaw, OverlapNet where training data exists and confidence calibration matters, LiDAR-Iris where binary compactness and speed dominate.

4.7Relocalization Inside Real Systems

Assembled, the production relocalization pipeline is a funnel of cheap-to-expensive filters, and it is worth knowing how the reference systems walk it. ORB-SLAM3, on tracking loss: query DBoW2 over keyframes → for each candidate, PnP-RANSAC on ORB matches → guided re-matching and pose refinement → resume tracking; the same funnel with stricter thresholds proposes loop closures, and Atlas decides "known map, merge" versus "new map, explore". Lidar stacks: Scan Context++ (or correlative search at modest scale) proposes pose + yaw → point-to-plane ICP against the map refines → fitness gate (inlier fraction, residual RMS) accepts or rejects → the accepted pose seeds MCL or the Phase 2 filter for continuous tracking. Note the division of labor: descriptors handle the combinatorial explosion, geometry handles the truth, and the probabilistic filter handles time. No single stage is trusted alone.

// minisc.cc — a 2D miniature of Scan Context: polar signature + shift match.
#include <algorithm>
#include <array>
#include <cmath>
#include <iostream>
#include <random>
#include <vector>

// world: 20×10 outer walls, two IDENTICAL rooms + one distinctive pillar room
bool occupied(double x, double y) {
  auto box = [&](double x0, double y0, double x1, double y1) {
    return x >= x0 && x <= x1 && y >= y0 && y <= y1; };
  if (x < 0 || x > 20 || y < 0 || y > 10) return true;         // outer walls
  if (box(6.0, 0.0, 6.2, 7.0) || box(13.0, 3.0, 13.2, 10.0)) return true;
  if (box(16.5, 1.5, 17.5, 2.5)) return true;                   // the pillar
  return false;
}

using Sig = std::array<double, 72>;                             // 5° sectors
Sig signature(double x, double y, double yaw) {                 // range per sector
  Sig s{};
  for (int k = 0; k < 72; ++k) {
    const double a = yaw + k * (2 * M_PI / 72);
    double r = 0.0;
    while (r < 12.0 && !occupied(x + r * std::cos(a), y + r * std::sin(a)))
      r += 0.02;
    s[k] = r;
  }
  return s;
}
double dist_shift(const Sig& a, const Sig& b, int& best_k) {    // min over shifts
  double best = 1e18;
  for (int k = 0; k < 72; ++k) {
    double d = 0.0;
    for (int i = 0; i < 72; ++i) d += std::abs(a[i] - b[(i + k) % 72]);
    if (d < best) { best = d; best_k = k; }
  }
  return best / 72.0;
}

int main() {
  std::mt19937 rng(5);
  std::uniform_real_distribution<double> ux(0.5, 19.5), uy(0.5, 9.5);

  std::vector<std::array<double, 2>> poses;                     // database
  std::vector<Sig> db;
  while (poses.size() < 200) {
    const double x = ux(rng), y = uy(rng);
    if (occupied(x, y)) continue;
    poses.push_back({x, y});
    db.push_back(signature(x, y, 0.0));                         // canonical yaw
  }

  // query: revisit database place #17, heading rotated by 137°
  const double qyaw = 137.0 * M_PI / 180.0;
  const Sig q = signature(poses[17][0], poses[17][1], qyaw);

  int best = -1, k_at_best = 0;
  double d1 = 1e18, d2 = 1e18;
  for (int i = 0; i < 200; ++i) {
    int k;  const double d = dist_shift(q, db[i], k);
    if (d < d1) { d2 = d1; d1 = d; best = i; k_at_best = k; }
    else d2 = std::min(d2, d);
  }
  std::cout << "retrieved place " << best << " (truth: 17), "
            << "yaw from shift: " << k_at_best * 5 << "° (truth: 137°)\n"
            << "best distance " << d1 << "  runner-up " << d2
            << "  margin " << d2 / d1 << "×\n";
}

4.8Reading List

4.9Interview Questions