Exploration and Active SLAM

Where should the robot go in order to learn? Map entropy, expected information gain, frontiers, and the moment the pose graph's information matrix stops being an output and becomes a decision variable.

The lawnmower fallacy

Here is the obvious way to map a room you have never seen: drive a boustrophedon sweep. Lanes up, lanes down, like mowing a lawn. Coverage path planning is a solved problem with a beautiful theory behind it (Choset's cellular decompositions), and if you already have the floorplan it is exactly what you should do.

Rusty does not have the floorplan. That is the entire point. So the lanes run into walls the script did not know about, they cross rooms that were finished three lanes ago, and every metre of re-scanned corridor is a metre of odometry drift bought for nothing. The widget below will run it for you: 93 metres of lawn-mowing leaves 4% of the apartment unmapped, and it does not know that, because a script has no notion of what it has already learned.

Switch it to Utility and watch what changes. The robot now has an opinion. Every few seconds it stops, looks at its own map, finds the places where mapped free space touches the unknown, asks how many bits a scan from each of them would be worth and how many metres each would cost, and drives to the argmax. Over ten seeded runs it clears 4000 of the map's 4800 bits in a median of 33.6 metres, against 46.0 for the lawnmower.

Three things in that sentence are doing real work, and the rest of the chapter is about them.

"How many bits." Not "how much unknown area" — bits, of the entropy of the same log-odds grid Chapter 13 built. The objective was already there; nobody had asked the map what it was worth.

"Would be worth." The robot has to score a measurement it has not taken. That is an expectation over a measurement it does not have, evaluated through the map it does have — and the map is wrong. Every honest word about exploration lives in that sentence.

"And how many metres each would cost." Information alone is not a policy. A robot that maximizes bits without pricing motion will happily cross the apartment for one extra doorway. The weights that trade the two are the difference between a good explorer and a busy one.

Building intuition

Before any algebra, three observations you can make from the widget above.

Nearest is not a different algorithm; it is one setting of a knob. Push $w_C$ — the cost weight — above about 0.9 and the utility policy stops crossing the apartment and starts creeping to whatever boundary is closest. It has not switched strategies; the information term has simply been priced out of the argmax. Yamauchi's original frontier explorer is the $w_I \to 0$ corner of the same family, and so is every "go to the nearest unexplored cell" implementation shipped since.

Greed is efficient per metre right up to the point where it stalls. Select Nearest frontier and watch the distance readout. It stops moving. The robot is not frozen: with $w_I = 0$ the utility is $-C(a)$, and the cheapest frontier to reach is the one directly under the wheels, which costs nothing. So Rusty stands there re-scanning until the map pushes the boundary far enough away to be worth walking to. Across ten seeds that policy is the most efficient of the three per metre travelled — 83 bits/m against the utility policy's 77 — and it reaches the 4000-bit mark in two runs out of ten. Efficiency per metre is the wrong statistic when the metres stop happening.

The score falls in steps, not smoothly. The bits-per-metre sparkline spikes every time Rusty crosses a threshold and a whole room resolves at once, then decays as the room fills in. Exploration is not a gradient descent on entropy; it is a sequence of decisions to go somewhere and be surprised. That shape is what makes stopping hard, and it is the last section of this chapter.

The mathematics

Entropy, from the map you already have

Chapter 13 stored the map as an array of log odds $\ell_i = \log \frac{p(m_i = 1)}{p(m_i = 0)}$, one per cell, and recovered probabilities with the logistic

Each cell is a Bernoulli variable, so its uncertainty is the binary entropy

which is $1$ at $p = \tfrac12$ (a cell you know nothing about) and $0$ at $p \in \{0, 1\}$. Under Chapter 13's standing approximation that cells are independent given the poses, the entropy of the whole map is a sum:

That is the objective. It has units, it is computable in one pass over the array, and it starts at exactly the number of cells: the apartment at 15 cm resolution is $80 \times 60 = 4800$ cells, so a virgin map holds 4800 bits of ignorance. Everything Rusty does for the rest of this chapter is an attempt to spend metres to remove bits.

Expected information gain

Fix a candidate action $a$ — say, "drive to that doorway and take one scan." Before acting, the robot's uncertainty about the map is $H(m)$. Afterwards it will be $H(m \mid z_a = z)$ for whichever reading $z$ arrives. The robot does not have $z$. What it has is a distribution over it, so the honest thing to score is the average:

The right-hand equality is the definition of mutual information, and it is the whole reason this quantity behaves. Two consequences follow immediately, and they are the two facts every practitioner needs.

The gain of a sensing pose, beam by beam

The mutual information above is defined; computing it for a real LiDAR pose is the work. Two factorizations make it cheap enough to evaluate for a dozen candidates several times a second.

Over cells. Chapter 13 already assumes the cells are independent, so the map entropy is a sum and the gain is a sum of per-cell gains. For one cell with prior $p$, observed through a binary symmetric channel that reports correctly with probability $q$, the gain is closed form:

Two limits are worth carrying in your head. At $p = \tfrac12$ this collapses to the channel capacity $1 - H_b(q)$ — for $q = 0.9$ that is $0.5310$ bits, and no scan of an unknown cell can ever be worth more. And as $p \to 0$ or $p \to 1$ it goes to zero: a cell you already know says nothing back.

Along a beam. Cells on a ray are not reached independently. A beam arrives at the $k$-th cell only if every cell before it was empty, which gives the reach probability

a running product computed as the ray is traced. The expected gain of one beam is then each cell's gain, discounted by the chance the beam gets there:

A worked example you can check by hand

Take one beam, five cells long, and two situations.

Into the unknown. Every cell sits at the prior, $p_k = 0.5$, and the sensor is right with $q = 0.9$. Then every cell has the same per-cell gain, the channel capacity:

and the reach probability halves at every step, $R_k = (1/2)^{k-1}$. The total is a five-term geometric sum:

Or in closed form, $0.5310 \times (2 - 2^{-4}) = 0.5310 \times 1.9375 = 1.0288$ bits.

Into space you have already mapped. Two or three sweeps of Chapter 13's inverse model drive a free cell to about $p = 0.01$. Now the per-cell gain collapses to $0.02486$ bits, but the beam sails through — $R_k = 0.99^{\,k-1}$, still 0.96 at the fifth cell — so the sum is

Eight and a half times less, from the same sensor over the same five cells. That ratio is the entire content of "point the sensor at the unknown," and now it is a number rather than a slogan.

Notice that the mapped ray is not worth zero, and it never will be. A log-odds map with a clamp never becomes certain, so re-scanning known space always scores something, and an explorer with a badly tuned $w_C$ will happily spend metres collecting it. The clamp that keeps Chapter 13's map revisable is the same clamp that keeps this number off the floor.

#[test]
fn worked_example_ch24_five_cell_ray() {
    // A fresh ray into the unknown: p = 0.5, sensor reliability q = 0.9.
    let unknown = ray_info_gain(&[0.5; 5], 0.9, 1e-3);
    assert_relative_eq!(unknown.cells[0].mutual_info, 1.0 - binary_entropy(0.9), epsilon = 1e-12);
    assert_relative_eq!(unknown.cells[3].reach, 0.125, epsilon = 1e-12);
    assert_relative_eq!(unknown.total, 1.0288, epsilon = 1e-4);

    // The same ray through space that two sweeps have already emptied.
    let mapped = ray_info_gain(&[0.01; 5], 0.9, 1e-3);
    assert_relative_eq!(mapped.total, 0.12183, epsilon = 1e-4);
    assert!(unknown.total / mapped.total > 8.0);

    // A cell that is already certain is worth nothing, whatever the sensor.
    assert_relative_eq!(cell_mutual_information(0.0, 0.99), 0.0, epsilon = 1e-12);
}

Frontiers, and why chasing them is enough

Information gain tells you what a pose is worth. It does not tell you which poses to bother scoring, and scoring every free cell in an 80 × 60 grid at every replan is both wasteful and pointless — most of them are worth nothing.

Yamauchi's 1997 answer is still the right one. Define:

• A frontier cell is a known-free cell with at least one unknown 4-neighbour.
• A frontier region is a connected component of frontier cells, summarized by its centroid and its size in cells.

That is all. Frontier regions are the only places in the map where a sensor can convert ignorance into evidence, they are found in one pass plus a flood fill, and grouping them into regions matters more than it looks: scoring individual cells produces hundreds of near-identical candidates and a robot that dithers between two of them.

The representative cell is not a detail. The centroid of a C-shaped frontier region can land inside a wall, and driving to a point inside a wall is not a plan; taking the member cell nearest the centroid guarantees a real, occupiable target.

Utility: bits are not metres

With a candidate set from detect_frontiers and a score from expected_info_gain, the selection stage is one line:

Three things about that expression are worth stating plainly, because they are where deployed systems go wrong.

$C(a)$ is not Euclidean distance. A frontier three metres away through a wall is not three metres away. The cost has to come from a planner over the known-free cells — in the implementation below, a Dijkstra expansion from the robot's cell, which doubles as the reachability test the completeness argument needs. Using straight-line distance produces a robot that repeatedly chooses a target it cannot reach.

The weights carry units, and pretending otherwise is how you get a magic constant. $w_I$ prices bits, $w_C$ prices metres, $w_G$ prices nats of graph information. There is no principled universal setting; there is a mission that says what a bit is worth in metres. Saying so out loud is better than hiding a 0.35 in a config file.

Every classical explorer is a corner of this expression. Set $w_I = w_G = 0$ and you have nearest-frontier. Set $w_C = w_G = 0$ and you have greedy maximum-information, which crosses the building for a big room. Set $w_G = 0$ and keep both others and you have the 2005 textbook's information-gain explorer. The interesting question was never which of these is right; it is what $w_G$ is for, and that is the next section.

Active localization: motion as a sensing action

Everything so far treated the pose as known. It is not, and the coupling runs both ways: a robot that is unsure where it is cannot integrate a scan into the right cells, so pose uncertainty corrupts the map that the exploration objective is defined on.

The narrower problem — choose motions that sharpen the pose belief — is active localization, and Fox, Burgard and Thrun formalized it in 1998 with a rule that is one line of decision theory:

where $\bel'$ is the belief after pushing $\bel$ through the motion model for $a$ and then correcting with a hypothetical $z_a$. Both halves of the Bayes filter appear, run forwards, on a measurement that has not happened.

The corridor in that widget is built to make one point unavoidable. Its doors repeat every 2.5 m, and every candidate motion except one is a whole number of door spacings — so each of them lands both hypotheses at the same offset inside a feature, and each of them scores an expected gain of exactly $0.000$ bits. The corridor ahead cannot tell those two futures apart, and the bars say so without being told. Only the alcove, 3.75 m behind, breaks the symmetry.

Over two hundred seeded trials at the default noise, the entropy-greedy policy takes the detour and ends in the right place 95% of the time. The goal-greedy policy heads for the goal, arrives confidently, and is in the wrong place almost exactly half the time — 48%, which is what a coin flip looks like when a filter has no evidence and still has to produce a maximum. Nothing in that filter ever complains, because nothing it saw was surprising.

The $w_T\,T(a)$ term is not decoration. Without a task cost, the argmax of expected entropy reduction is a robot that finds the most informative corner of the building and stays there. In the widget, $T(a)$ is the distance still owed to the goal under the MAP hypothesis, and it is what makes Rusty leave the alcove once the belief has collapsed.

Active SLAM: the information matrix becomes a decision variable

Map entropy scores what the robot will see. It says nothing about whether the robot will know where it was standing when it saw it — and the map is only as trustworthy as the trajectory it was painted along. Chapter 16's rubber-band problem is what an un-closed loop looks like: a corridor that arrives back at its own start two metres off, and a map that is locally crisp and globally wrong.

So the second objective is the pose graph itself. Chapter 15 built the information matrix $\Omega = \sum_e A_e\T \Omega_e A_e$ and used it to solve for the MAP trajectory. Its inverse is (to first order) the covariance of that trajectory, so a small $\Omega^{-1}$ — equivalently, a large $\Omega$ — is a well-determined map. "Large" needs a definition, and optimal experiment design has supplied three for seventy years.

Because $\log\det$ falls straight out of a Cholesky factorization — $\log\det\Omega = 2\sum_k \log L_{kk}$ — evaluating it for a candidate costs one sparse solve, roughly $O(n^{1.5})$ for a planar graph. That is what makes scoring a dozen candidate trajectories per decision affordable at all.

That widget hides a theorem, and the theorem is the reason active SLAM exists as a separate subject from exploration.

Numbers, so you can check it. Take a chain with $\sigma_x = \sigma_y = 0.05$ m and $\sigma_\theta = 0.03$ rad, giving $\log\det\Omega_e = 2\log(1/0.05^2) + \log(1/0.03^2) = 18.996$ nats per factor:

Three chains of wildly different length, one D-optimality. Add a single loop-closure factor with $\sigma_\theta = 0.02$ and $\log\det$ jumps by 8.68 nats. Note also that $\mathrm{Dopt} = (\det\Omega_e)^{1/3} = (400 \cdot 400 \cdot 1111.1)^{1/3} = 562.29$, which you can do on a calculator.

#[test]
fn worked_example_ch24_tree_logdet_is_geometry_free() {
    let omega = information_from_sigmas(0.05, 0.05, 0.03);
    let per_edge = omega.determinant().ln();          // 18.996 nats
    let mut rng = SmallRng::seed_from_u64(24);

    // Three chains with completely different shapes.
    for n in [2usize, 5, 12] {
        let g = random_odometry_chain(n, &omega, &mut rng);
        assert_relative_eq!(graph_log_det(&g), n as f64 * per_edge, epsilon = 1e-9);
        // …and therefore the same D-optimality, whatever the shape.
        assert_relative_eq!(d_optimality(&g), omega.determinant().cbrt(), epsilon = 1e-9);
    }

    // One loop closure, and only then does the criterion move.
    let mut g = random_odometry_chain(12, &omega, &mut rng);
    let before = graph_log_det(&g);
    g.add_edge(0, 12, g.relative_pose(0, 12), information_from_sigmas(0.05, 0.05, 0.02), Loop);
    assert_relative_eq!(graph_log_det(&g) - before, 8.675, epsilon = 1e-3);
}

Which is exactly what the widget shows: pushing on into room E adds 646 nats of $\log\det$ and moves D-optimality by zero, while closing the loop adds 659 nats and moves D-optimality from 1314 to 1423. The raw determinant puts the two branches within 2% of each other because it is mostly counting nodes; the normalized criterion separates them cleanly. And the trajectory error — the number neither criterion is allowed to see — falls from 0.84 m to 0.34 m.

When to stop

Every run in this chapter ends. None of them ends for a principled reason.

The candidates are all defensible and all flawed. An absolute entropy threshold ("stop below 200 bits") depends on the size of the map, so it does not transfer between buildings. An entropy plateau detector needs a window length that nobody can justify, and exploration's step-shaped progress means a genuine plateau and the pause before entering a new wing look identical. A coverage percentage requires knowing the area to be covered, which is what you were trying to find out.

The least-bad rule with units the mission can argue about is a gain-per-cost floor:

because "a bit is worth a metre" is a statement about the job rather than about information theory. The chart above shows what it buys on one real run: at $\epsilon = 15$ b/m Rusty stops after 15 metres with barely half the information; at $\epsilon = 4$ it stops at 42 metres holding 93% of it; and the remaining 57 metres — more than half the entire run — buy the last 7%.

Placed et al. list principled stopping among active SLAM's open problems, and this chapter has nothing better to offer. What it does offer is the discipline of plotting the curve before choosing the number.

Implementation in Rust

The crate is ch24_explore, and it is Part VI's integrator: it depends on ch13_occgrid for the map, ch16_slam2d for the graph, ch20_planning for the paths, and ch23_mppi for the execution. It adds four things of its own: frontier detection, the gain estimator, the utility, and the loop that ties them together.

Frontiers and the navigation field

use std::collections::HashMap;

use ch13_occgrid::{log_odds_to_prob, GridIdx, OccGrid};
use nalgebra::Point2;
use petgraph::unionfind::UnionFind;

#[derive(Clone, Copy, PartialEq, Eq, Debug)]
pub enum CellClass { Free, Occupied, Unknown }

/// Two thresholds, not one. The band in between is what "unknown" means, and
/// the width of that band is why an explorer with a timid free-space model
/// lags its own sensor by several scans.
#[derive(Clone, Copy)]
pub struct ClassThresholds { pub free_below: f64, pub occ_above: f64 }

impl Default for ClassThresholds {
    fn default() -> Self { Self { free_below: 0.3, occ_above: 0.7 } }
}

pub struct Frontier {
    pub cells: Vec<GridIdx>,
    pub centroid: Point2<f64>,
    /// The member cell nearest the centroid. The centroid of a C-shaped region
    /// can sit inside a wall; a member cell never does.
    pub representative: GridIdx,
    pub size: usize,
}

/// `detect_frontiers` — Yamauchi (1997), cell for cell.
///
/// One marking pass, then union-find over 8-connected marked pairs. Union-find
/// rather than BFS because `petgraph` already has it, it is a single pass in
/// scan order, and it makes the "which region is this cell in" query that the
/// renderer wants O(α(n)).
pub fn detect_frontiers(grid: &OccGrid, th: ClassThresholds, min_size: usize) -> Vec<Frontier> {
    let n = grid.len();
    let mut marked = vec![false; n];

    for (idx, &l) in grid.log_odds().iter().enumerate() {
        if classify(log_odds_to_prob(l), th) != CellClass::Free { continue; }
        // Free, and touching the unknown: that is the whole definition.
        marked[idx] = grid.neighbors4(idx).any(|j| {
            classify(log_odds_to_prob(grid.log_odds()[j]), th) == CellClass::Unknown
        });
    }

    let mut uf = UnionFind::<usize>::new(n);
    for idx in 0..n {
        if !marked[idx] { continue; }
        for j in grid.neighbors8(idx) {
            if marked[j] { uf.union(idx, j); }
        }
    }

    let mut regions: HashMap<usize, Vec<GridIdx>> = HashMap::new();
    for idx in 0..n {
        if marked[idx] { regions.entry(uf.find(idx)).or_default().push(grid.to_idx2(idx)); }
    }

    regions
        .into_values()
        .filter(|cells| cells.len() >= min_size)
        .map(|cells| {
            let centroid = mean_point(grid, &cells);
            let representative = *cells
                .iter()
                .min_by(|a, b| {
                    let da = (grid.center(**a) - centroid).norm_squared();
                    let db = (grid.center(**b) - centroid).norm_squared();
                    da.partial_cmp(&db).unwrap()
                })
                .expect("regions are non-empty by construction");
            Frontier { size: cells.len(), cells, centroid, representative }
        })
        .collect()
}

The navigation field is the other half, and it earns its own type because two different consumers need it: the utility needs a cost, and the completeness argument needs a reachability predicate. They are the same Dijkstra expansion.

use std::cmp::Reverse;
use std::collections::BinaryHeap;

use ordered_float::OrderedFloat;

/// Dijkstra over known-free cells, 8-connected, in metres.
///
/// `cost[i] = ∞` means "not reachable through what we have mapped so far",
/// which is exactly the predicate the completeness proof needs — and exactly
/// the predicate an over-inflated obstacle map quietly falsifies.
pub struct NavField<'a> {
    grid: &'a OccGrid,
    cost: Vec<f64>,
    parent: Vec<Option<usize>>,
    start: usize,
}

impl<'a> NavField<'a> {
    pub fn new(grid: &'a OccGrid, start: GridIdx, th: ClassThresholds, inflate: usize) -> Self {
        let blocked = inflate_obstacles(grid, th, inflate);
        let mut cost = vec![f64::INFINITY; grid.len()];
        let mut parent = vec![None; grid.len()];
        let start = grid.to_flat(start);

        // The robot is standing where it is standing: inflation may never
        // strand it, so the source is seeded regardless of `blocked`.
        cost[start] = 0.0;
        let mut heap = BinaryHeap::new();
        heap.push(Reverse((OrderedFloat(0.0), start)));

        while let Some(Reverse((OrderedFloat(c), i))) = heap.pop() {
            if c > cost[i] { continue; }
            for (j, step) in grid.neighbors8_with_step(i) {
                if blocked[j] { continue; }
                if classify(log_odds_to_prob(grid.log_odds()[j]), th) != CellClass::Free { continue; }
                let next = c + step;
                if next < cost[j] - 1e-12 {
                    cost[j] = next;
                    parent[j] = Some(i);
                    heap.push(Reverse((OrderedFloat(next), j)));
                }
            }
        }
        Self { grid, cost, parent, start }
    }

    /// The cheapest reachable cell within `radius` of a frontier target.
    ///
    /// Frontier cells sit against the unknown, so inflation often makes the
    /// exact cell unreachable while its neighbour two cells back is fine.
    /// Aiming at the neighbour is what keeps an explorer from declaring a
    /// doorway impossible.
    pub fn nearest_reachable(&self, goal: GridIdx, radius: i32) -> Option<(GridIdx, f64)> { /* … */ }

    pub fn path_to(&self, goal: GridIdx) -> Vec<Point2<f64>> { /* walk `parent` back */ }
}

Expected gain

use ch02_prob::binary_entropy;
use ch13_occgrid::{bresenham, log_odds_to_prob, OccGrid};
use nalgebra::{Isometry2, Vector2};
use rustc_hash::FxHashSet;

/// The sensor as the *planner* models it. Deliberately cruder than Chapter 10's
/// beam mixture: all the planner needs from a range finder is how many beams,
/// how far, and how much to trust one cell's verdict.
#[derive(Clone, Copy)]
pub struct SensingParams {
    pub n_beams: usize,
    pub fov: f64,
    pub max_range: f64,
    /// q: the probability the inverse model calls a cell correctly. The
    /// crossover of a binary symmetric channel, and the only sensor number
    /// this estimator ever sees.
    pub z_hit: f64,
}

/// I(mᵢ ; zᵢ) in bits, for one cell with prior `p` through a channel of
/// reliability `q`.
///
/// At p = ½ this is the channel capacity 1 − H_b(q); as p → 0 or 1 it is zero.
/// Both limits are worth remembering, because between them they explain every
/// decision an information-gain explorer makes.
pub fn cell_mutual_information(p: f64, q: f64) -> f64 {
    if !(0.0..=1.0).contains(&p) || p <= 0.0 || p >= 1.0 { return 0.0; }
    let p_occ = p * q + (1.0 - p) * (1.0 - q);
    let p_free = 1.0 - p_occ;
    let post_occ = if p_occ > 0.0 { p * q / p_occ } else { 0.0 };
    let post_free = if p_free > 0.0 { p * (1.0 - q) / p_free } else { 0.0 };
    let expected = p_occ * binary_entropy(post_occ) + p_free * binary_entropy(post_free);
    (binary_entropy(p) - expected).max(0.0)
}

pub struct InfoGainEstimator<'a> {
    pub grid: &'a OccGrid,
    pub sensor: SensingParams,
    /// Count each cell at most once per scan. Without it, the estimator happily
    /// rewards standing still with its nose against a wall.
    pub dedupe: bool,
}

impl InfoGainEstimator<'_> {
    /// `expected_info_gain` — bits of map entropy one scan from `pose` should
    /// remove. Equation (F.2); the reach probability is the running product.
    pub fn expected_gain(&self, pose: &Isometry2<f64>) -> f64 {
        let origin = self.grid.world_to_cell(pose.translation.vector.into());
        let mut visited = FxHashSet::default();
        let mut total = 0.0;

        for phi in beam_angles(self.sensor.n_beams, self.sensor.fov) {
            let theta = pose.rotation.angle() + phi;
            let end = self.grid.world_to_cell(
                pose.translation.vector + self.sensor.max_range * Vector2::new(theta.cos(), theta.sin()),
            );

            let mut reach = 1.0_f64;
            for cell in bresenham(origin, end) {
                if reach < 1e-3 { break; }
                let Some(flat) = self.grid.try_flat(cell) else { break };
                let p = log_odds_to_prob(self.grid.log_odds()[flat]);
                if !self.dedupe || visited.insert(flat) {
                    total += reach * cell_mutual_information(p, self.sensor.z_hit);
                }
                // The beam is stopped by this cell whether or not we already
                // scored it: occlusion belongs to the map, not the bookkeeping.
                reach *= 1.0 - p;
            }
        }
        total
    }
}

D-optimality over the pose graph

use ch13_occgrid::{GridIdx, OccGrid};
use ch15_graph::{BlockIndex, Ordering};
use ch16_slam2d::PoseGraph;
use faer::linalg::solvers::Llt;
use faer::sparse::SparseColMat;
use nalgebra::Isometry2;

use crate::frontier::{Frontier, NavField};
use crate::info_gain::InfoGainEstimator;

/// `graph_log_det` — log det Ω in nats, from a factorization we were going to
/// compute anyway.
///
/// The gauge node's rows and columns are *deleted*, not damped: with them in,
/// Ω is singular by construction and its determinant is zero however good the
/// map is. Cost is one sparse Cholesky — about O(n^1.5) for a planar graph,
/// which is what makes scoring a dozen candidates per decision affordable.
pub fn graph_log_det(graph: &PoseGraph) -> f64 {
    let (omega, _index): (SparseColMat<usize, f64>, BlockIndex) = graph.information_free();
    let symbolic = Ordering::Amd.symbolic(&omega);
    let llt = Llt::try_new_with_symbolic(symbolic, omega.as_ref())
        .expect("Ω is singular — is the gauge node fixed?");
    // log det Ω = 2 Σ log L_kk.
    2.0 * llt.L().diagonal().column_vector().iter().map(|d| d.ln()).sum::<f64>()
}

/// D-optimality in the form the field settled on (Carrillo et al. 2012;
/// Placed et al. 2023):  Dopt(Ω) = exp( (1/n) log det Ω ).
///
/// The 1/n is the whole point. Raw log det counts nodes, so it prefers the
/// longer trajectory for being longer; the geometric-mean form is what makes
/// two graphs of different sizes comparable at all.
pub fn d_optimality(graph: &PoseGraph) -> f64 {
    let n = 3 * graph.free_node_count();
    if n == 0 { return 0.0; }
    (graph_log_det(graph) / n as f64).exp()
}

#[derive(Clone, Copy)]
pub struct UtilityWeights { pub w_i: f64, pub w_g: f64, pub w_c: f64 }

pub struct Candidate {
    pub target: GridIdx,
    pub pose: Isometry2<f64>,
    /// I(a), bits.
    pub gain: f64,
    /// C(a), metres of known-free path.
    pub cost: f64,
    /// Δ_a = log Dopt(Ω₊ₐ) − log Dopt(Ω), nats per degree of freedom.
    pub graph_gain: f64,
    pub utility: f64,
}

/// `score_candidates` — one utility per frontier region.
///
/// The sensing pose faces from the reachable target toward the region's
/// centroid: a range finder pointed back down the corridor it just drove
/// learns nothing, and the estimator would happily tell you so if you asked it,
/// but only if you ask at the right heading.
pub fn score_candidates(
    grid: &OccGrid,
    frontiers: &[Frontier],
    nav: &NavField<'_>,
    gain: &InfoGainEstimator<'_>,
    w: UtilityWeights,
    graph_gain: Option<&dyn Fn(GridIdx, f64) -> f64>,
) -> Vec<Candidate> {
    let mut out: Vec<Candidate> = frontiers
        .iter()
        .filter_map(|f| {
            // Unreachable through known-free space: skip it, do not fail.
            let (target, cost) = nav.nearest_reachable(f.representative, 4)?;
            let t = grid.center(target);
            let heading = (f.centroid - t).y.atan2((f.centroid - t).x);
            let pose = Isometry2::new(t.coords, heading);
            let i = gain.expected_gain(&pose);
            let g = graph_gain.map_or(0.0, |f| f(target, cost));
            Some(Candidate {
                target, pose, gain: i, cost, graph_gain: g,
                utility: w.w_i * i + w.w_g * g - w.w_c * cost,
            })
        })
        .collect();
    out.sort_by(|a, b| b.utility.partial_cmp(&a.utility).unwrap());
    out
}

The loop

use ch16_slam2d::Slam2d;
use nalgebra::{Isometry2, Point2};

use crate::frontier::{detect_frontiers, ClassThresholds, NavField};
use crate::info_gain::{InfoGainEstimator, SensingParams};
use crate::utility::{d_optimality, score_candidates, Candidate, StopRule, UtilityWeights};

/// What one decision can be. `Done` carries *why*, because "the run ended" and
/// "the run ended for a good reason" are different claims and the ablation
/// table needs to tell them apart.
pub enum Decision {
    Goto { target: Isometry2<f64>, path: Vec<Point2<f64>>, candidates: Vec<Candidate> },
    CloseLoop { node: ch16_slam2d::NodeIx },
    Done { reason: StopReason },
}

pub enum StopReason { NoFrontiers, GainRate, Budget }

pub struct Explorer {
    pub weights: UtilityWeights,
    pub sensing: SensingParams,
    pub thresholds: ClassThresholds,
    pub stop: StopRule,
    pub min_frontier_size: usize,
    pub inflate: usize,
}

impl Explorer {
    /// `explore_step` — identify, select, execute. One decision.
    ///
    /// Placed et al. (2023) name the three stages every active-SLAM system has,
    /// whether or not its authors drew the boxes. Notice the proportions: the
    /// policy is the two `let` bindings in the middle, and everything else in
    /// this crate is keeping the estimate alive while it runs. That is the
    /// honest ratio for a real system too.
    pub fn decide(&mut self, slam: &Slam2d) -> Decision {
        let grid = slam.grid();
        let here = grid.world_to_cell(slam.pose().translation.vector.into());

        // ---- identify --------------------------------------------------
        let frontiers = detect_frontiers(grid, self.thresholds, self.min_frontier_size);
        let nav = NavField::new(grid, here, self.thresholds, self.inflate);
        let gain = InfoGainEstimator { grid, sensor: self.sensing, dedupe: true };

        // ---- select ----------------------------------------------------
        let base = d_optimality(slam.graph()).ln();
        let graph_gain = |target: GridIdx, cost: f64| {
            // Predict the factors this trajectory would add: one odometry
            // factor per step, plus a loop closure wherever it re-observes a
            // mapped region. Then score the hypothetical graph.
            let predicted = slam.graph().with_predicted_factors(target, cost, &self.sensing);
            d_optimality(&predicted).ln() - base
        };
        let candidates =
            score_candidates(grid, &frontiers, &nav, &gain, self.weights, Some(&graph_gain));

        if let Some(reason) = self.stop.triggered(&candidates) {
            return Decision::Done { reason };
        }

        // ---- execute ---------------------------------------------------
        let best = &candidates[0];
        Decision::Goto {
            target: best.pose,
            path: nav.path_to(best.target),   // handed to ch23_mppi to track
            candidates,
        }
    }
}

Putting it together

cargo run --release --example autonomous_explore drops Rusty into the apartment at the corridor midpoint with 4800 bits of ignorance and no instructions. The example prints one row per policy over ten seeds; the TypeScript port that drives the widgets on this page produces the same numbers, because it is the same algorithm.

Read it carefully, because the interesting result is not the one you expected.

The utility explorer wins the column that matters: it reaches a fixed information target in 27% less distance than the scripted sweep, in nine runs out of ten, and it finishes with the lowest final entropy of the three.

The lawnmower is not embarrassing. It is reliable — 10/10 — because it does not depend on the map being right. That is exactly the trade a scripted policy makes, and it is why coverage path planning is still the correct answer when you already have the floorplan.

Nearest-frontier is the most efficient policy per metre travelled, and it is the worst policy on this table. It clears 83 bits for every metre it drives; it just stops driving. Eight runs out of ten end below the information target, most of them with frontiers still visible on the map, because the argmax of $-C(a)$ is whatever boundary is nearest — including the one under the robot's own wheels. This is not a strawman built to lose. It is the reason every deployed frontier explorer carries a minimum-target-distance guard, a hysteresis term, or a blacklist, and the sixth exercise asks you to add one and re-run the table.

Everything the capstone needs now exists. Chapter 26 adds no new estimator, no new planner, and no new controller: what remains is orchestration and the failure modes that appear only when all of it runs at once.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Non-negative in expectation, and the case that is not

   Prove $I(a) = I(m; z_a) \ge 0$ from the KL form, stating exactly where Gibbs' inequality is used and what equality would mean physically. Then construct an explicit two-cell, one-beam example in which a specific measurement increases the map entropy, and compute both the increase and the probability of that reading. Finally, confirm your example numerically with cell_mutual_information and explain in two sentences why an entropy log that never rises would be evidence of a bug rather than of a good filter.

2. Foundation exerciseFDifficulty 2 of 3A worse sensor, sublinearly

   Redo the five-cell worked example with $q = 0.7$ instead of $0.9$. You should get 0.2300 bits against the chapter's 1.0288 — a factor of 4.5 for a sensor that is only 22% less reliable. Explain the nonlinearity in terms of the channel capacity $1 - H_b(q)$, and predict (before computing) whether the ratio between the unknown ray and the mapped ray grows or shrinks as $q \to 0.5$.

3. Foundation exerciseFDifficulty 3 of 3Where the tree identity breaks

   The derivation shows $\det\Omega = \prod_e \det\Omega_e$ for a tree. Two of its hypotheses can fail in practice. (a) Show what happens to $\det A$ when a node is constrained by two odometry factors — for instance because the front-end fused wheel odometry and IMU into separate factors — and say whether D-optimality then depends on the trajectory. (b) The derivation drops the right-Jacobian factor $J_r^{-1}(e)$ that Chapter 16 also drops. Argue why that is harmless for the ranking of two candidates while being wrong for the absolute value of $\log\det\Omega$.

4. Conceptual exerciseCDifficulty 2 of 3Predict the flip, then check it

   In the Disambiguation Detour, read the candidate bars — do not run the policy — and predict the sensor-noise level at which the scored argmax moves from the detour to the direct route. Write down your prediction, then find it with the slider. Explain the flip in terms of $I(a)$ against $C(a) + T(a)$, and say what would have to change about the corridor (not the sensor) to move the flip point by 0.05.

5. Conceptual exerciseCDifficulty 2 of 3Make greed win

   Using the Frontier Chaser, find a seed and a cost weight $w_C$ for which nearest-frontier reaches a higher cells-resolved percentage than the utility policy at the same distance travelled. What structural property of that particular run makes greed the right answer? Then state the property of the apartment as a whole that makes greed usually wrong, and connect it to the shape of the bits-per-metre sparkline.

6. Practical exercisePDifficulty 2 of 3A guard against the stall

   Reproduce the stall: run the nearest-frontier policy and log, per decision, the chosen candidate's cost. You will find long stretches at $C(a) \approx 0$. Add a guard on the minimum target distance — reject candidates whose reachable target is within $d_{\min}$ of the robot — and re-run the ten-seed ablation table. Report every column for $d_{\min} \in \{0, 0.5, 1.5\}$ m, and say which column moves the most; then argue whether the guard is a fix or a hyperparameter hiding a modelling error. Extension: a frontier region that wraps around a large room gets one candidate at its centroid, which can be a bad place to stand — implement centroid-splitting along the region's principal axis and run the table a third time.

7. Practical exercisePDifficulty 3 of 3Lookahead, and whether it pays

   Replace the depth-1 active_localize argmax with a POMCP search (Chapter 22) over a three-action macro-space: detour, direct, and wait-and-scan. Run both on the Disambiguation Detour world across 50 seeds and report the success rate, the mean decisions per trial, and the wall-clock time per decision. Then build the world where the lookahead is necessary — a corridor whose disambiguating feature lies two macro-actions away, so that the first step toward it scores exactly zero — and show the depth-1 policy failing on it.

References

1. yamauchi1997Yamauchi, B. (1997) A Frontier-Based Approach for Autonomous Exploration. Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation (CIRA), Monterey, CA, 146–151.doi:10.1109/CIRA.1997.613851 (opens in a new tab)

   The origin of the frontier, and the source of this chapter's epigraph. Older than the 2005 textbook baseline and still the structural backbone of every exploration stack in this chapter.

2. fox1998Fox, D., Burgard, W., and Thrun, S. (1998) Active Markov Localization for Mobile Robots. Robotics and Autonomous Systems 25(3–4), 195–207.doi:10.1016/S0921-8890(98)00049-9 (opens in a new tab)

   The expected-entropy-reduction rule that widget w24.2 implements, derived over a grid belief. Reading it beside Chapter 12 shows how little the idea needs beyond a Bayes filter you can run forwards.

3. thrun2005Thrun, S., Burgard, W., and Fox, D. (2005) Probabilistic Robotics. MIT Press.link to Probabilistic Robotics (opens in a new tab)

   Chapter 17 is this chapter's conceptual ancestor: greedy information-gain exploration, active localization, and multi-robot coordination over occupancy grids. Note that the 1999–2000 draft this book otherwise works from has no exploration chapter at all.

4. stachniss2005Stachniss, C., Grisetti, G., and Burgard, W. (2005) Information Gain-based Exploration Using Rao-Blackwellized Particle Filters. Proceedings of Robotics: Science and Systems (RSS), Cambridge, MA.doi:10.15607/RSS.2005.I.009 (opens in a new tab)

   The first system to score exploration actions by their effect on the joint map-and-trajectory posterior rather than the map alone — the idea this chapter re-expresses over a pose graph instead of a particle set.

5. carrillo2012Carrillo, H., Reid, I., and Castellanos, J. A. (2012) On the Comparison of Uncertainty Criteria for Active SLAM. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), St. Paul, MN, 2080–2087.doi:10.1109/ICRA.2012.6224890 (opens in a new tab)

   The paper that settled A- versus D- versus E-optimality for robotics, and the source of the normalized D-opt form used in this chapter's utility.

6. placed2023surveyPlaced, J. A., Strader, J., Carrillo, H., Atanasov, N., Indelman, V., Carlone, L., and Castellanos, J. A. (2023) A Survey on Active Simultaneous Localization and Mapping: State of the Art and New Frontiers. IEEE Transactions on Robotics 39(3), 1686–1705.doi:10.1109/TRO.2023.3248510 (opens in a new tab)

   The chapter's map of the field: active SLAM as an intractable POMDP approached by an identify–select–execute pipeline, with a unified treatment of optimality criteria and an open-problems section that includes stopping.

7. placed2023doptPlaced, J. A. and Castellanos, J. A. (2023) A General Relationship Between Optimality Criteria and Connectivity Indices for Active Graph-SLAM. IEEE Robotics and Automation Letters 8(2), 816–823.doi:10.1109/LRA.2022.3233230 (opens in a new tab)

   Relates the graph Laplacian's connectivity indices to the optimality criteria of the information matrix, which is what makes the tree-versus-cycle argument of this chapter's derivation into a usable surrogate on graphs too large to factor per candidate.

8. zhou2021fuelZhou, B., Zhang, Y., Chen, X., and Shen, S. (2021) FUEL: Fast UAV Exploration Using Incremental Frontier Structure and Hierarchical Planning. IEEE Robotics and Automation Letters 6(2), 779–786.doi:10.1109/LRA.2021.3051563 (opens in a new tab)

   Frontier detection maintained incrementally rather than recomputed, plus a hierarchical tour over frontier clusters — the two engineering moves that take this chapter's O(#cells) rescan to real-time on a drone.

9. cao2021tareCao, C., Zhu, H., Choset, H., and Zhang, J. (2021) TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments. Proceedings of Robotics: Science and Systems (RSS), Virtual.doi:10.15607/RSS.2021.XVII.018 (opens in a new tab)

   The strongest counterargument to greedy selection: plan a detailed local path and a coarse global tour, and the myopia that produces this chapter's nearest-frontier stall disappears. Best paper at RSS 2021.

10. asgharivaskasi2025Asgharivaskasi, A., Girke, F., and Atanasov, N. (2025) Riemannian Optimization for Active Mapping With Robot Teams. IEEE Transactions on Robotics 41, 1077–1097.doi:10.1109/TRO.2025.3526295 (opens in a new tab)

    The modern multi-robot form of this chapter's utility: distributed optimization of a mutual-information objective over trajectories on a manifold, with consensus and optimality guarantees. The pointer for the multi-robot material this chapter drops.