In graph-based SLAM (Lesson 1), the robot maintains a set of keyframe poses \( \mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_n \) and a set of factors/edges encoding relative measurements (odometry, scan matching, visual odometry). Without loop closure, the graph is nearly a chain and drift accumulates. A loop closure adds a non-local constraint between two distant keyframes, e.g. \( \mathbf{x}_i \) and \( \mathbf{x}_j \) with \( |i-j| \) large, which (after optimization in Lesson 3) distributes the correction across the trajectory.

Operationally, loop closure detection is a two-stage pipeline: (i) place recognition proposes candidates using descriptors, and (ii) verification (geometric + statistical) accepts/rejects the candidate before inserting a new constraint into the graph.

flowchart TD
  A["Inputs: keyframes, descriptors, odom chain constraints"] --> B["Candidate generation: retrieve past keyframes similar to current"]
  B --> C["Temporal / spatial gating: ignore too-recent frames and implausible matches"]
  C --> D["Geometric verification: estimate relative transform and inlier statistics"]
  D --> E["Statistical decision: accept or reject loop closure hypothesis"]
  E --> F["If accepted: create new graph edge between pose i and pose j"]
  F --> G["Output: augmented pose/factor graph (optimized in next lesson)"]
        

The central difficulty is perceptual aliasing: distinct places produce similar measurements (e.g., long corridors, repeated shelves), causing false loop closures that can deform the map. Therefore, loop closure detection must be framed as risk-controlled inference, not merely nearest-neighbor matching.

Let the current keyframe index be \( i \). We consider a candidate past index \( j \) with \( j < i-\Delta \) (a minimum separation \( \Delta \) prevents trivial matches to near-neighbors). Define a binary loop-closure event: \( L_{ij} \in \{0,1\} \), where \( L_{ij}=1 \) means “keyframes \( i \) and \( j \) correspond to the same physical place.”

We observe (a) an appearance descriptor \( \mathbf{d}_i \) and (b) a geometric measurement attempt (scan/vision) that yields a relative-pose estimate \( \mathbf{z}_{ij} \) and a set of inlier statistics. Collect all evidence into \( \mathcal{E}_{ij} = \{\mathbf{d}_i,\mathbf{d}_j,\mathbf{z}_{ij},\text{inliers},\dots\} \). Loop closure detection is the decision: \( \delta(\mathcal{E}_{ij}) \in \{0,1\} \).

The Bayes posterior is:

\[ P(L_{ij}=1 \mid \mathcal{E}_{ij}) = \frac{p(\mathcal{E}_{ij}\mid L_{ij}=1)\,P(L_{ij}=1)} {p(\mathcal{E}_{ij}\mid L_{ij}=1)\,P(L_{ij}=1) + p(\mathcal{E}_{ij}\mid L_{ij}=0)\,P(L_{ij}=0)}. \]

A key modeling step is splitting evidence into retrieval and verification. A common approximation (useful in practice) is conditional independence: \( p(\mathcal{E}_{ij}\mid L_{ij}) \approx p(\mathbf{d}_i,\mathbf{d}_j\mid L_{ij})\;p(\mathbf{z}_{ij},\text{stats}\mid L_{ij}) \), which clarifies why both appearance and geometry are needed to keep false positives small.

Suppose the cost of a false loop closure (accept when \( L_{ij}=0 \)) is \( C_{10} \), and the cost of missing a true closure (reject when \( L_{ij}=1 \)) is \( C_{01} \). Bayes risk minimization yields the rule:

\[ \delta(\mathcal{E}_{ij}) = \begin{cases} 1 & \text{if } \log \Lambda(\mathcal{E}_{ij}) > \log \eta \\ 0 & \text{otherwise} \end{cases} \quad \text{where} \quad \Lambda(\mathcal{E}_{ij}) = \frac{p(\mathcal{E}_{ij}\mid L_{ij}=1)}{p(\mathcal{E}_{ij}\mid L_{ij}=0)}, \\ \eta = \frac{C_{10}}{C_{01}}\cdot\frac{P(L_{ij}=0)}{P(L_{ij}=1)}. \]

Interpretation. In large environments, the prior \( P(L_{ij}=1) \) for an arbitrary pair is small, so \( \eta \) is typically large, meaning we require strong evidence to accept loop closure. This is exactly what practitioners do: “high precision first,” because a single false closure can catastrophically bend the graph.

Proof sketch (Bayes decision rule). Choose \( \delta=1 \) when its conditional expected cost is smaller: \( C_{10}P(L_{ij}=0\mid\mathcal{E}_{ij}) < C_{01}P(L_{ij}=1\mid\mathcal{E}_{ij}) \). Using Bayes’ rule and rearranging yields the likelihood ratio test above.

Neyman–Pearson view. If you fix a maximum allowable false positive rate \( \alpha \), then among all tests with \( P(\delta=1\mid L_{ij}=0)\le \alpha \), the likelihood-ratio test is most powerful (maximizes detection probability). This connects loop closure to classical detection theory: we tune thresholds to meet false-alarm budgets.

Candidate generation maps each keyframe to a descriptor vector \( \mathbf{d}_i \in \mathbb{R}^m \). Examples (conceptual, not re-taught here): bag-of-visual-words for images, scan-context histograms for LiDAR, learned embeddings from CNN/transformer encoders. Retrieval produces a shortlist \( \mathcal{C}(i) \subset \{0,\dots,i-\Delta\} \).

A common similarity is cosine: \( s_{ij} = \frac{\mathbf{d}_i^\top \mathbf{d}_j}{\|\mathbf{d}_i\|\;\|\mathbf{d}_j\|} \). When using bag-of-words with TF–IDF weighting, define term-frequency \( \mathrm{tf}_{iw} \) and inverse-document-frequency \( \mathrm{idf}_w \):

\[ \mathrm{idf}_w = \log\!\left(\frac{N+1}{\mathrm{df}_w+1}\right) + 1, \qquad \mathbf{d}_i[w] = \frac{\mathrm{tf}_{iw}\,\mathrm{idf}_w}{\left\|\mathrm{tf}_i \odot \mathrm{idf}\right\|_2}, \]

where \( N \) is the number of keyframes so far and \( \mathrm{df}_w \) is the number of keyframes containing word \( w \). The normalization makes cosine similarity equal to the dot product.

Temporal gating. Always exclude the most recent window: \( j \notin \{i-\Delta,\dots,i-1\} \). Additionally, if you have odometry, reject candidates that violate feasibility (e.g., requiring implausible travel speed).

Statistical scoring model. To connect retrieval to the likelihood ratio in Section 3, model the similarity under two hypotheses: \( s_{ij}\mid L_{ij}=1 \sim \mathcal{N}(\mu_1,\sigma_1^2) \), \( s_{ij}\mid L_{ij}=0 \sim \mathcal{N}(\mu_0,\sigma_0^2) \). Then the retrieval contribution to \( \log \Lambda \) is the log-density difference. This is precisely what the lab code implements (a simple, inspectable proxy for complex learned models).

After a candidate \( (i,j) \) is proposed, we run a geometric alignment step to estimate a relative pose measurement \( \mathbf{z}_{ij} \) and (often) an inlier set. For 2D AMR, a minimal representation is \( \mathbf{z}_{ij} \in \mathbb{R}^3 \) (dx, dy, dθ). Define a residual against the predicted relative transform (from the current graph estimate): \( \mathbf{r}_{ij} = \mathbf{z}_{ij} - \hat{\mathbf{z}}_{ij} \).

Under a local linear-Gaussian approximation, if \( \mathbf{r}_{ij} \sim \mathcal{N}(\mathbf{0}, \mathbf{S}_{ij}) \), then the Mahalanobis distance: \( d_{ij}^2 = \mathbf{r}_{ij}^\top \mathbf{S}_{ij}^{-1}\mathbf{r}_{ij} \) follows a chi-square distribution with \( \nu \) degrees of freedom (here \( \nu=3 \)).

\[ \text{Accept geometry if}\quad \mathbf{r}_{ij}^\top \mathbf{S}_{ij}^{-1}\mathbf{r}_{ij} \;\le\; \chi^2_{\nu,\,1-\alpha}, \quad \nu=3, \]

where \( \chi^2_{\nu,\,1-\alpha} \) is the (1−α)-quantile. This is a principled false-alarm control mechanism: if the residual model is correct, the probability of wrongly rejecting a true loop closure is approximately \( \alpha \).

Why chi-square? (proof sketch) Let \( \mathbf{y} = \mathbf{S}_{ij}^{-1/2}\mathbf{r}_{ij} \). If \( \mathbf{r}_{ij} \sim \mathcal{N}(\mathbf{0},\mathbf{S}_{ij}) \), then \( \mathbf{y} \sim \mathcal{N}(\mathbf{0},\mathbf{I}) \). Therefore: \( d_{ij}^2 = \mathbf{r}_{ij}^\top \mathbf{S}_{ij}^{-1}\mathbf{r}_{ij} = \|\mathbf{y}\|_2^2 = \sum_{k=1}^{\nu} y_k^2 \), which is chi-square with \( \nu \) degrees of freedom.

flowchart TD
  A["Candidate (i,j) proposed by descriptor match"] --> B["Estimate relative transform z_ij via geometry"]
  B --> C["Compute residual r_ij = z_ij - zhat_ij"]
  C --> D["Compute d2 = r_ij^T * S^-1 * r_ij"]
  D --> E{"Compare d2 with \ngate threshold"}
  E -->|pass| F["Accept: add loop closure constraint edge"]
  E -->|fail| G["Reject: treat as perceptual aliasing / outlier"]
  

In real systems, \( \mathbf{S}_{ij} \) is derived from scan-matching covariance, vision reprojection Jacobians, or an information-form approximation. This lesson focuses on the concept: verification converts “looks similar” into “consistent with rigid-body geometry + uncertainty.”

When a loop closure is accepted, it becomes a new factor connecting \( \mathbf{x}_j \) to \( \mathbf{x}_i \). In a pose-graph least-squares form (Lesson 3), we will use an error function: \( \mathbf{e}_{ij}(\mathbf{x}_j,\mathbf{x}_i) \) and an information matrix \( \boldsymbol{\Omega}_{ij} \). Conceptually:

\[ \text{Loop factor:}\quad \phi_{ij}(\mathbf{x}_j,\mathbf{x}_i) \propto \exp\!\Big( -\tfrac{1}{2}\,\mathbf{e}_{ij}(\mathbf{x}_j,\mathbf{x}_i)^\top \boldsymbol{\Omega}_{ij}\, \mathbf{e}_{ij}(\mathbf{x}_j,\mathbf{x}_i) \Big). \]

The key message: detection quality determines whether adding this factor improves the estimate or corrupts it. That is why this lesson treats loop closure as an explicit hypothesis test with verification.

This lab simulates a square-loop trajectory, creates TF–IDF-like descriptors, retrieves candidates by cosine similarity, then applies (i) a Gaussian log-likelihood ratio threshold and (ii) a chi-square geometric gate. It reports precision/recall against a distance-based ground truth loop definition.

File: Chapter12_Lesson2.py

# Chapter12_Lesson2.py
# Loop Closure Detection Concepts (Graph-Based SLAM) — Toy but principled demo
# Author: Abolfazl Mohammadijoo (course scaffold)
#
# This script demonstrates:
# 1) Descriptor-based retrieval (TF–IDF-like vectors + cosine similarity).
# 2) A Bayesian / likelihood-ratio decision rule on similarity scores.
# 3) Geometric verification via chi-square gating on a (dx,dy,dtheta) residual.
#
# NOTE: This is a pedagogical simulator. In a real SLAM system:
# - descriptors come from vision/LiDAR place recognition modules,
# - geometric verification comes from scan matching or PnP + RANSAC,
# - accepted loop closures become graph edges optimized in the next lesson.

from __future__ import annotations
import math
import random
from dataclasses import dataclass
from typing import List, Tuple, Dict, Optional

import numpy as np

try:
    # SciPy is optional; we provide a fallback approximate chi-square threshold if absent.
    from scipy.stats import chi2, norm
    _HAVE_SCIPY = True
except Exception:
    _HAVE_SCIPY = False


@dataclass
class Params:
    seed: int = 7
    N: int = 520                   # number of keyframes
    min_separation: int = 40       # exclude recent frames from loop closure candidates
    topK: int = 5                  # number of retrieved candidates per query
    # Similarity score model (toy):
    mu1: float = 0.92              # mean cosine similarity for true loop closures
    sig1: float = 0.03             # std  cosine similarity for true loop closures
    mu0: float = 0.55              # mean cosine similarity for non-loop pairs
    sig0: float = 0.08             # std  cosine similarity for non-loop pairs
    # Decision costs (false accept vs false reject):
    C10: float = 50.0              # false accept cost (catastrophic)
    C01: float = 1.0               # false reject cost
    prior_loop: float = 0.01       # P(L=1) prior
    # Geometric residual covariance (dx, dy, dtheta):
    sigma_dx: float = 0.25
    sigma_dy: float = 0.25
    sigma_dth: float = 0.08
    alpha_geom: float = 0.005      # chi-square gate significance
    # Ground-truth loop definition (for evaluation only):
    loop_distance_thresh: float = 0.9
    # Descriptor dimensions:
    V: int = 500                   # "vocabulary size" or embedding dim


def square_loop_trajectory(N: int) -> np.ndarray:
    """
    Generate a 2D trajectory that runs around a square multiple times.
    Returns array of shape (N,2).
    """
    pts = []
    # Make 4 segments per lap; total laps ~ N / 4S where S points per segment
    S = max(20, N // 12)
    # A square of side 10
    side = 10.0
    for lap in range(max(1, N // (4 * S)) + 1):
        # (0,0) -> (side,0)
        for k in range(S):
            pts.append((k / S * side, 0.0))
        # (side,0) -> (side,side)
        for k in range(S):
            pts.append((side, k / S * side))
        # (side,side) -> (0,side)
        for k in range(S):
            pts.append((side - k / S * side, side))
        # (0,side) -> (0,0)
        for k in range(S):
            pts.append((0.0, side - k / S * side))
    pts = pts[:N]
    return np.array(pts, dtype=float)


def make_tfidf_like_descriptors(xy: np.ndarray, V: int, seed: int = 0) -> np.ndarray:
    """
    Create descriptors correlated with spatial location:
    - We map the (x,y) position into a soft assignment over V bins.
    - Then normalize to unit length.
    This imitates "place-specific" words while still allowing aliasing.
    """
    rng = np.random.default_rng(seed)
    N = xy.shape[0]
    # Random projection centers for "words"
    centers = rng.uniform(0.0, 10.0, size=(V, 2))
    # RBF bandwidth
    ell = 1.5
    D = np.zeros((N, V), dtype=float)
    for i in range(N):
        diff = centers - xy[i]
        r2 = np.sum(diff * diff, axis=1)
        # soft activation + noise to imitate sensor variation
        D[i] = np.exp(-0.5 * r2 / (ell * ell)) + 0.02 * rng.random(V)
    # TF–IDF style weighting: idf based on "document frequency"
    # Here df_w = number of frames where word activation exceeds a small threshold.
    thr = 0.05
    df = np.sum(D > thr, axis=0)
    idf = np.log((N + 1.0) / (df + 1.0)) + 1.0
    D = D * idf
    # L2 normalize
    norms = np.linalg.norm(D, axis=1, keepdims=True) + 1e-12
    D = D / norms
    return D


def cosine_similarity(a: np.ndarray, b: np.ndarray) -> float:
    return float(np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b) + 1e-12))


def normal_logpdf(x: float, mu: float, sigma: float) -> float:
    # log N(x; mu, sigma^2)
    return -0.5 * math.log(2.0 * math.pi * sigma * sigma) - 0.5 * ((x - mu) / sigma) ** 2


def llr_similarity(s: float, p: Params) -> float:
    """
    Log-likelihood ratio contributed by similarity score s under:
    H1: s ~ N(mu1,sig1^2), H0: s ~ N(mu0,sig0^2)
    """
    return normal_logpdf(s, p.mu1, p.sig1) - normal_logpdf(s, p.mu0, p.sig0)


def bayes_threshold_eta(p: Params) -> float:
    """
    eta = (C10/C01) * (P(L=0)/P(L=1))
    """
    prior1 = p.prior_loop
    prior0 = max(1e-12, 1.0 - prior1)
    return (p.C10 / p.C01) * (prior0 / prior1)


def chi2_threshold(nu: int, alpha: float) -> float:
    """
    Return chi-square quantile for df=nu at 1-alpha.
    If SciPy is missing, use Wilson-Hilferty approximation.
    """
    if _HAVE_SCIPY:
        return float(chi2.ppf(1.0 - alpha, df=nu))
    # Wilson-Hilferty transform:
    # chi2 ~ nu * (1 - 2/(9nu) + z*sqrt(2/(9nu)))^3
    # with z = Phi^{-1}(1-alpha)
    # We'll approximate z using a simple inverse-erf relation.
    # (works adequately for typical alpha like 0.01, 0.005, 0.001)
    # z = sqrt(2) * erfinv(2p-1); implement via math.erf inverse approximation
    p = 1.0 - alpha
    # Abramowitz-Stegun approximation for inverse error function
    # (good enough for this fallback path)
    def erfinv(y: float) -> float:
        a = 0.147
        sgn = 1.0 if y >= 0 else -1.0
        ln = math.log(1.0 - y * y)
        t = 2.0 / (math.pi * a) + ln / 2.0
        return sgn * math.sqrt(math.sqrt(t * t - ln / a) - t)
    z = math.sqrt(2.0) * erfinv(2.0 * p - 1.0)
    nu_f = float(nu)
    return nu_f * (1.0 - 2.0 / (9.0 * nu_f) + z * math.sqrt(2.0 / (9.0 * nu_f))) ** 3


def geometric_gate(residual: np.ndarray, p: Params) -> Tuple[bool, float, float]:
    """
    Chi-square gate for residual r in R^3 with diagonal covariance.
    Returns (ok, d2, threshold)
    """
    S = np.diag([p.sigma_dx ** 2, p.sigma_dy ** 2, p.sigma_dth ** 2])
    Sinv = np.linalg.inv(S)
    d2 = float(residual.T @ Sinv @ residual)
    thr = chi2_threshold(3, p.alpha_geom)
    return (d2 <= thr), d2, thr


def ground_truth_loop(i: int, j: int, xy: np.ndarray, p: Params) -> bool:
    # purely for evaluation: loop if physically close and not too recent
    if j >= i - p.min_separation:
        return False
    return float(np.linalg.norm(xy[i] - xy[j])) <= p.loop_distance_thresh


def simulate_measurement_residual(i: int, j: int, is_true_loop: bool, rng: np.random.Generator) -> np.ndarray:
    """
    Toy residual generator for geometric verification.
    True loop => small residual; false loop => large residual.
    """
    if is_true_loop:
        return rng.normal(0.0, [0.08, 0.08, 0.02], size=3)
    # perceptual aliasing often yields a plausible but wrong alignment; model as larger errors
    return rng.normal(0.0, [0.9, 0.9, 0.25], size=3)


def retrieve_candidates(i: int, D: np.ndarray, p: Params) -> List[Tuple[int, float]]:
    """
    Return topK candidates (j, similarity) for keyframe i among frames < i - min_separation.
    """
    if i <= p.min_separation:
        return []
    d_i = D[i]
    sims = []
    for j in range(0, i - p.min_separation):
        s = cosine_similarity(d_i, D[j])
        sims.append((j, s))
    sims.sort(key=lambda t: -t[1])
    return sims[: p.topK]


def main():
    p = Params()
    random.seed(p.seed)
    np.random.seed(p.seed)
    rng = np.random.default_rng(p.seed)

    # 1) Build trajectory and descriptors
    xy = square_loop_trajectory(p.N)
    D = make_tfidf_like_descriptors(xy, V=p.V, seed=p.seed)

    # 2) Bayesian threshold on similarity LLR
    eta = bayes_threshold_eta(p)
    log_eta = math.log(eta)

    # 3) Run loop closure detection
    accepted = []
    proposed = []
    gt_true = 0
    det_true = 0
    det_fp = 0

    for i in range(p.N):
        cands = retrieve_candidates(i, D, p)
        for (j, s) in cands:
            is_true = ground_truth_loop(i, j, xy, p)
            if is_true:
                gt_true += 1

            # Stage A: similarity test (LLR)
            llr = llr_similarity(s, p)
            sim_accept = (llr > log_eta)

            # Stage B: geometric gate (chi-square)
            if sim_accept:
                r = simulate_measurement_residual(i, j, is_true, rng)
                ok, d2, thr = geometric_gate(r, p)
                proposed.append((i, j, s, llr, d2, thr, is_true))
                if ok:
                    accepted.append((i, j, s, llr, d2, is_true))
                    if is_true:
                        det_true += 1
                    else:
                        det_fp += 1

    # 4) Metrics
    # Define "detections" as accepted, and "positives" as gt_true counted over examined candidates
    total_det = len(accepted)
    precision = det_true / total_det if total_det > 0 else 0.0
    recall = det_true / gt_true if gt_true > 0 else 0.0

    print("=== Loop Closure Detection (Toy) ===")
    print(f"N keyframes: {p.N}")
    print(f"min separation: {p.min_separation}, topK: {p.topK}")
    print(f"Bayes threshold eta = {eta:.3g}  (log eta = {log_eta:.3f})")
    print(f"Geometric gate alpha = {p.alpha_geom}, chi2 threshold (df=3) = {chi2_threshold(3, p.alpha_geom):.3f}")
    print("")
    print(f"Proposed (after similarity): {len(proposed)}")
    print(f"Accepted (after geometry):   {len(accepted)}")
    print(f"Detected true: {det_true}")
    print(f"False accepts: {det_fp}")
    print(f"Precision: {precision:.3f}")
    print(f"Recall:    {recall:.3f}")

    print("\nTop accepted loop closures (by similarity):")
    accepted_sorted = sorted(accepted, key=lambda t: -t[2])
    for row in accepted_sorted[:10]:
        i, j, s, llr, d2, is_true = row
        print(f"  i={i:4d}, j={j:4d}, sim={s:.3f}, llr={llr:.3f}, d2={d2:.3f}, true={is_true}")

    # Optional: show how you would convert to a graph edge (conceptual)
    if accepted_sorted:
        i, j, s, llr, d2, is_true = accepted_sorted[0]
        print("\nExample accepted pair interpreted as a graph constraint:")
        print(f"  Add edge between x_{j} and x_{i} with measurement z_ij (dx,dy,dtheta) from geometry.")
        print("  The optimizer in Lesson 3 will incorporate this edge into the pose graph.")


if __name__ == "__main__":
    main()
      

This C++ version mirrors the pipeline with brute-force cosine retrieval and a chi-square gate. For production, replace retrieval with an approximate nearest-neighbor index and replace the toy measurement with scan matching / vision geometry outputs.

File: Chapter12_Lesson2.cpp

// Chapter12_Lesson2.cpp
// Loop Closure Detection Concepts (Graph-Based SLAM) — Minimal C++ demo
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter12_Lesson2.cpp -I /usr/include/eigen3 -o ch12_l2
//
// This program demonstrates:
// - Descriptor retrieval by cosine similarity (brute-force).
// - Bayesian / likelihood-ratio threshold on similarity scores (Gaussian toy model).
// - Chi-square gating for a (dx,dy,dtheta) residual with diagonal covariance.
//
// NOTE: Replace toy residual generation with your scan matching / vision geometry residuals.

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <random>
#include <tuple>
#include <vector>

struct Params {
  int seed = 7;
  int N = 520;
  int min_separation = 40;
  int topK = 5;

  // Similarity score model (toy):
  double mu1 = 0.92, sig1 = 0.03;
  double mu0 = 0.55, sig0 = 0.08;

  // Bayes costs + prior:
  double C10 = 50.0;
  double C01 = 1.0;
  double prior_loop = 0.01;

  // Geometric gate (dx,dy,dtheta) covariance std:
  double sigma_dx = 0.25;
  double sigma_dy = 0.25;
  double sigma_dth = 0.08;

  // If you do not have a chi-square quantile function available, set a typical constant:
  // chi2_{df=3, 0.995} ≈ 12.838, chi2_{df=3, 0.999} ≈ 16.266
  double chi2_thr_df3_0p995 = 12.838;

  // Descriptor dimension:
  int V = 500;
};

static double normal_logpdf(double x, double mu, double sigma) {
  const double s2 = sigma * sigma;
  return -0.5 * std::log(2.0 * M_PI * s2) - 0.5 * (x - mu) * (x - mu) / s2;
}

static double llr_similarity(double s, const Params& p) {
  return normal_logpdf(s, p.mu1, p.sig1) - normal_logpdf(s, p.mu0, p.sig0);
}

static double bayes_eta(const Params& p) {
  const double prior1 = p.prior_loop;
  const double prior0 = std::max(1e-12, 1.0 - prior1);
  return (p.C10 / p.C01) * (prior0 / prior1);
}

static Eigen::MatrixXd square_loop_xy(int N) {
  std::vector<Eigen::Vector2d> pts;
  const int S = std::max(20, N / 12);
  const double side = 10.0;
  const int laps = std::max(1, N / (4 * S)) + 1;

  pts.reserve(laps * 4 * S);
  for (int lap = 0; lap < laps; ++lap) {
    for (int k = 0; k < S; ++k) pts.emplace_back((double)k / S * side, 0.0);
    for (int k = 0; k < S; ++k) pts.emplace_back(side, (double)k / S * side);
    for (int k = 0; k < S; ++k) pts.emplace_back(side - (double)k / S * side, side);
    for (int k = 0; k < S; ++k) pts.emplace_back(0.0, side - (double)k / S * side);
  }
  if ((int)pts.size() > N) pts.resize(N);

  Eigen::MatrixXd xy(N, 2);
  for (int i = 0; i < N; ++i) xy.row(i) = pts[i];
  return xy;
}

static Eigen::MatrixXd make_descriptors(const Eigen::MatrixXd& xy, int V, int seed) {
  std::mt19937_64 rng(seed);
  std::uniform_real_distribution<double> unif(0.0, 10.0);
  std::uniform_real_distribution<double> unif01(0.0, 1.0);

  const int N = (int)xy.rows();
  Eigen::MatrixXd centers(V, 2);
  for (int w = 0; w < V; ++w) centers(w, 0) = unif(rng), centers(w, 1) = unif(rng);

  const double ell = 1.5;
  Eigen::MatrixXd D(N, V);
  for (int i = 0; i < N; ++i) {
    for (int w = 0; w < V; ++w) {
      const double dx = centers(w, 0) - xy(i, 0);
      const double dy = centers(w, 1) - xy(i, 1);
      const double r2 = dx * dx + dy * dy;
      D(i, w) = std::exp(-0.5 * r2 / (ell * ell)) + 0.02 * unif01(rng);
    }
  }

  // TF–IDF-like idf
  const double thr = 0.05;
  Eigen::VectorXd df(V);
  for (int w = 0; w < V; ++w) {
    int count = 0;
    for (int i = 0; i < N; ++i) if (D(i, w) > thr) count++;
    df(w) = (double)count;
  }
  Eigen::VectorXd idf(V);
  for (int w = 0; w < V; ++w) {
    idf(w) = std::log((N + 1.0) / (df(w) + 1.0)) + 1.0;
  }
  for (int i = 0; i < N; ++i) {
    D.row(i) = D.row(i).array() * idf.transpose().array();
    const double nrm = D.row(i).norm() + 1e-12;
    D.row(i) /= nrm;
  }
  return D;
}

static double cosine_sim(const Eigen::VectorXd& a, const Eigen::VectorXd& b) {
  const double denom = a.norm() * b.norm() + 1e-12;
  return a.dot(b) / denom;
}

static bool ground_truth_loop(int i, int j, const Eigen::MatrixXd& xy, const Params& p) {
  if (j >= i - p.min_separation) return false;
  const double dist = (xy.row(i) - xy.row(j)).norm();
  return dist <= 0.9; // same as python example
}

static Eigen::Vector3d simulate_residual(bool is_true, std::mt19937_64& rng) {
  auto gauss = [&](double s) {
    std::normal_distribution<double> nd(0.0, s);
    return nd(rng);
  };
  if (is_true) return Eigen::Vector3d(gauss(0.08), gauss(0.08), gauss(0.02));
  return Eigen::Vector3d(gauss(0.9), gauss(0.9), gauss(0.25));
}

static std::vector<std::pair<int, double>> retrieve_topK(int i, const Eigen::MatrixXd& D, const Params& p) {
  std::vector<std::pair<int, double>> sims;
  if (i <= p.min_separation) return sims;
  sims.reserve(i - p.min_separation);
  const Eigen::VectorXd di = D.row(i);
  for (int j = 0; j < i - p.min_separation; ++j) {
    const double s = cosine_sim(di, D.row(j));
    sims.emplace_back(j, s);
  }
  std::partial_sort(
      sims.begin(), sims.begin() + std::min((int)sims.size(), p.topK), sims.end(),
      [](const auto& a, const auto& b) { return a.second > b.second; });
  if ((int)sims.size() > p.topK) sims.resize(p.topK);
  return sims;
}

int main() {
  Params p;
  std::mt19937_64 rng(p.seed);

  Eigen::MatrixXd xy = square_loop_xy(p.N);
  Eigen::MatrixXd D = make_descriptors(xy, p.V, p.seed);

  const double eta = bayes_eta(p);
  const double log_eta = std::log(eta);

  int gt_true = 0, det_true = 0, det_fp = 0;
  int proposed = 0, accepted = 0;

  for (int i = 0; i < p.N; ++i) {
    auto cands = retrieve_topK(i, D, p);
    for (const auto& cs : cands) {
      const int j = cs.first;
      const double s = cs.second;

      const bool is_true = ground_truth_loop(i, j, xy, p);
      if (is_true) gt_true++;

      const double llr = llr_similarity(s, p);
      const bool sim_accept = (llr > log_eta);

      if (!sim_accept) continue;
      proposed++;

      Eigen::Vector3d r = simulate_residual(is_true, rng);

      Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
      S(0, 0) = p.sigma_dx * p.sigma_dx;
      S(1, 1) = p.sigma_dy * p.sigma_dy;
      S(2, 2) = p.sigma_dth * p.sigma_dth;

      const double d2 = r.transpose() * S.inverse() * r;
      if (d2 <= p.chi2_thr_df3_0p995) {
        accepted++;
        if (is_true)
          det_true++;
        else
          det_fp++;
      }
    }
  }

  const double precision = (accepted > 0) ? (double)det_true / accepted : 0.0;
  const double recall = (gt_true > 0) ? (double)det_true / gt_true : 0.0;

  std::cout << "=== Loop Closure Detection (C++) ===\n";
  std::cout << "N keyframes: " << p.N << "\n";
  std::cout << "min separation: " << p.min_separation << ", topK: " << p.topK << "\n";
  std::cout << "Bayes eta: " << eta << " (log eta: " << log_eta << ")\n";
  std::cout << "Chi2 threshold df=3, 0.995: " << p.chi2_thr_df3_0p995 << "\n\n";
  std::cout << "Proposed (after similarity): " << proposed << "\n";
  std::cout << "Accepted (after geometry):   " << accepted << "\n";
  std::cout << "Detected true: " << det_true << "\n";
  std::cout << "False accepts: " << det_fp << "\n";
  std::cout << "Precision: " << precision << "\n";
  std::cout << "Recall:    " << recall << "\n";
  return 0;
}
      

This Java demo keeps dependencies minimal (no external libraries required) and demonstrates the core logic: cosine retrieval + chi-square gating. In robotics stacks, Java is commonly paired with middleware bindings or custom perception modules; the algorithmic structure remains identical.

File: Chapter12_Lesson2.java

// Chapter12_Lesson2.java
// Loop Closure Detection Concepts (Graph-Based SLAM) — Minimal Java demo
//
// This program demonstrates:
// - Descriptor retrieval by cosine similarity (brute-force).
// - Bayesian / likelihood-ratio threshold on similarity scores (Gaussian toy model).
// - Chi-square gating for a (dx,dy,dtheta) residual with diagonal covariance.
//
// NOTE: Replace toy residual generation with real geometric verification.

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.Random;

public class Chapter12_Lesson2 {

    static class Params {
        int seed = 7;
        int N = 520;
        int minSeparation = 40;
        int topK = 5;

        // Similarity model (toy)
        double mu1 = 0.92, sig1 = 0.03;
        double mu0 = 0.55, sig0 = 0.08;

        // Bayes costs + prior
        double C10 = 50.0;
        double C01 = 1.0;
        double priorLoop = 0.01;

        // Geometric gate (dx,dy,dtheta) std
        double sigmaDx = 0.25;
        double sigmaDy = 0.25;
        double sigmaDth = 0.08;

        // Chi-square threshold df=3 at 0.995 ~ 12.838
        double chi2Thr = 12.838;

        int V = 500;
        double loopDistThresh = 0.9;
    }

    static double normalLogPdf(double x, double mu, double sigma) {
        double s2 = sigma * sigma;
        return -0.5 * Math.log(2.0 * Math.PI * s2) - 0.5 * (x - mu) * (x - mu) / s2;
    }

    static double llrSimilarity(double s, Params p) {
        return normalLogPdf(s, p.mu1, p.sig1) - normalLogPdf(s, p.mu0, p.sig0);
    }

    static double bayesEta(Params p) {
        double prior1 = p.priorLoop;
        double prior0 = Math.max(1e-12, 1.0 - prior1);
        return (p.C10 / p.C01) * (prior0 / prior1);
    }

    static double[][] squareLoopXY(int N) {
        List<double[]> pts = new ArrayList<>();
        int S = Math.max(20, N / 12);
        double side = 10.0;
        int laps = Math.max(1, N / (4 * S)) + 1;
        for (int lap = 0; lap < laps; lap++) {
            for (int k = 0; k < S; k++) pts.add(new double[]{(double)k / S * side, 0.0});
            for (int k = 0; k < S; k++) pts.add(new double[]{side, (double)k / S * side});
            for (int k = 0; k < S; k++) pts.add(new double[]{side - (double)k / S * side, side});
            for (int k = 0; k < S; k++) pts.add(new double[]{0.0, side - (double)k / S * side});
        }
        if (pts.size() > N) pts = pts.subList(0, N);
        double[][] xy = new double[N][2];
        for (int i = 0; i < N; i++) xy[i] = pts.get(i);
        return xy;
    }

    static double dot(double[] a, double[] b) {
        double s = 0.0;
        for (int i = 0; i < a.length; i++) s += a[i] * b[i];
        return s;
    }

    static double norm(double[] a) {
        return Math.sqrt(dot(a, a));
    }

    static double cosine(double[] a, double[] b) {
        return dot(a, b) / (norm(a) * norm(b) + 1e-12);
    }

    static double[][] makeDescriptors(double[][] xy, int V, int seed) {
        Random rng = new Random(seed);
        int N = xy.length;

        // Random centers
        double[][] centers = new double[V][2];
        for (int w = 0; w < V; w++) {
            centers[w][0] = 10.0 * rng.nextDouble();
            centers[w][1] = 10.0 * rng.nextDouble();
        }

        double ell = 1.5;
        double[][] D = new double[N][V];
        for (int i = 0; i < N; i++) {
            for (int w = 0; w < V; w++) {
                double dx = centers[w][0] - xy[i][0];
                double dy = centers[w][1] - xy[i][1];
                double r2 = dx * dx + dy * dy;
                D[i][w] = Math.exp(-0.5 * r2 / (ell * ell)) + 0.02 * rng.nextDouble();
            }
        }

        // TF–IDF-like idf
        double thr = 0.05;
        int[] df = new int[V];
        for (int w = 0; w < V; w++) {
            int c = 0;
            for (int i = 0; i < N; i++) if (D[i][w] > thr) c++;
            df[w] = c;
        }
        double[] idf = new double[V];
        for (int w = 0; w < V; w++) {
            idf[w] = Math.log((N + 1.0) / (df[w] + 1.0)) + 1.0;
        }

        // Weight and normalize
        for (int i = 0; i < N; i++) {
            for (int w = 0; w < V; w++) D[i][w] *= idf[w];
            double nrm = norm(D[i]) + 1e-12;
            for (int w = 0; w < V; w++) D[i][w] /= nrm;
        }
        return D;
    }

    static boolean groundTruthLoop(int i, int j, double[][] xy, Params p) {
        if (j >= i - p.minSeparation) return false;
        double dx = xy[i][0] - xy[j][0];
        double dy = xy[i][1] - xy[j][1];
        double dist = Math.sqrt(dx * dx + dy * dy);
        return dist <= p.loopDistThresh;
    }

    static double[] simulateResidual(boolean isTrue, Random rng) {
        // Box-Muller gaussian
        java.util.function.DoubleUnaryOperator gauss = (sigma) -> {
            double u1 = Math.max(1e-12, rng.nextDouble());
            double u2 = rng.nextDouble();
            double z = Math.sqrt(-2.0 * Math.log(u1)) * Math.cos(2.0 * Math.PI * u2);
            return sigma * z;
        };
        if (isTrue) return new double[]{gauss.applyAsDouble(0.08), gauss.applyAsDouble(0.08), gauss.applyAsDouble(0.02)};
        return new double[]{gauss.applyAsDouble(0.9), gauss.applyAsDouble(0.9), gauss.applyAsDouble(0.25)};
    }

    static class Cand {
        int j;
        double sim;
        Cand(int j, double sim) { this.j = j; this.sim = sim; }
    }

    static List<Cand> retrieveTopK(int i, double[][] D, Params p) {
        List<Cand> list = new ArrayList<>();
        if (i <= p.minSeparation) return list;
        for (int j = 0; j < i - p.minSeparation; j++) {
            double s = cosine(D[i], D[j]);
            list.add(new Cand(j, s));
        }
        Collections.sort(list, Comparator.comparingDouble((Cand c) -> -c.sim));
        if (list.size() > p.topK) return list.subList(0, p.topK);
        return list;
    }

    static boolean chi2Gate(double[] r, Params p) {
        double d2 = (r[0] * r[0]) / (p.sigmaDx * p.sigmaDx)
                  + (r[1] * r[1]) / (p.sigmaDy * p.sigmaDy)
                  + (r[2] * r[2]) / (p.sigmaDth * p.sigmaDth);
        return d2 <= p.chi2Thr;
    }

    public static void main(String[] args) {
        Params p = new Params();
        Random rng = new Random(p.seed);

        double[][] xy = squareLoopXY(p.N);
        double[][] D = makeDescriptors(xy, p.V, p.seed);

        double eta = bayesEta(p);
        double logEta = Math.log(eta);

        int gtTrue = 0, detTrue = 0, detFp = 0;
        int proposed = 0, accepted = 0;

        for (int i = 0; i < p.N; i++) {
            List<Cand> cands = retrieveTopK(i, D, p);
            for (Cand c : cands) {
                int j = c.j;
                double s = c.sim;

                boolean isTrue = groundTruthLoop(i, j, xy, p);
                if (isTrue) gtTrue++;

                double llr = llrSimilarity(s, p);
                boolean simAccept = (llr > logEta);
                if (!simAccept) continue;
                proposed++;

                double[] r = simulateResidual(isTrue, rng);
                if (chi2Gate(r, p)) {
                    accepted++;
                    if (isTrue) detTrue++;
                    else detFp++;
                }
            }
        }

        double precision = (accepted > 0) ? (double)detTrue / accepted : 0.0;
        double recall = (gtTrue > 0) ? (double)detTrue / gtTrue : 0.0;

        System.out.println("=== Loop Closure Detection (Java) ===");
        System.out.println("N keyframes: " + p.N);
        System.out.println("min separation: " + p.minSeparation + ", topK: " + p.topK);
        System.out.println("Bayes eta: " + eta + " (log eta: " + logEta + ")");
        System.out.println("Chi2 threshold df=3, 0.995: " + p.chi2Thr);
        System.out.println();
        System.out.println("Proposed (after similarity): " + proposed);
        System.out.println("Accepted (after geometry):   " + accepted);
        System.out.println("Detected true: " + detTrue);
        System.out.println("False accepts: " + detFp);
        System.out.println("Precision: " + precision);
        System.out.println("Recall:    " + recall);
    }
}
      

The MATLAB script implements retrieval + gating and optionally generates a small Simulink model containing a MATLAB Function block to illustrate how loop-closure logic can be embedded in a streaming pipeline (descriptors arriving over time).

File: Chapter12_Lesson2.m

% Chapter12_Lesson2.m
% Loop Closure Detection Concepts (Graph-Based SLAM) — MATLAB demo
%
% Demonstrates:
% - Descriptor retrieval using cosine similarity (TF–IDF-like descriptors).
% - Bayesian / likelihood-ratio thresholding on similarity scores.
% - Chi-square gating on (dx, dy, dtheta) residual with diagonal covariance.
%
% Also optionally builds a tiny Simulink model with a MATLAB Function block
% showing how gating could be used in a streaming pipeline.
%
% NOTE: Replace toy residual generation with scan matching / vision geometry residuals.

function Chapter12_Lesson2()
    rng(7);

    % Parameters
    N = 520;
    minSep = 40;
    topK = 5;

    % Similarity score model (toy)
    mu1 = 0.92; sig1 = 0.03;
    mu0 = 0.55; sig0 = 0.08;

    % Bayes costs + prior
    C10 = 50.0;  % false accept
    C01 = 1.0;   % false reject
    priorLoop = 0.01;

    % Geometric gate covariance std
    sigma = diag([0.25^2, 0.25^2, 0.08^2]);

    % Chi-square threshold df=3 at 0.995
    chi2thr = 12.838;

    % Descriptor dimension
    V = 500;

    % Ground-truth loop definition (evaluation only)
    loopDistThr = 0.9;

    % Build toy square-loop trajectory and descriptors
    xy = square_loop_xy(N);
    D  = make_descriptors(xy, V);

    % Bayes threshold
    eta = (C10/C01) * ((1-priorLoop)/priorLoop);
    logEta = log(eta);

    gtTrue = 0; detTrue = 0; detFp = 0;
    proposed = 0; accepted = 0;

    for i = 1:N
        cands = retrieve_topK(i, D, minSep, topK);
        for c = 1:size(cands,1)
            j = cands(c,1);
            s = cands(c,2);

            isTrue = ground_truth_loop(i, j, xy, minSep, loopDistThr);
            if isTrue, gtTrue = gtTrue + 1; end

            llr = normal_logpdf(s, mu1, sig1) - normal_logpdf(s, mu0, sig0);
            simAccept = (llr > logEta);
            if ~simAccept
                continue
            end
            proposed = proposed + 1;

            r = simulate_residual(isTrue);
            d2 = r' * (sigma \ r);
            if d2 <= chi2thr
                accepted = accepted + 1;
                if isTrue, detTrue = detTrue + 1; else, detFp = detFp + 1; end
            end
        end
    end

    precision = 0; recall = 0;
    if accepted > 0, precision = detTrue / accepted; end
    if gtTrue > 0, recall = detTrue / gtTrue; end

    fprintf('=== Loop Closure Detection (MATLAB) ===\n');
    fprintf('N=%d, minSep=%d, topK=%d\n', N, minSep, topK);
    fprintf('Bayes eta=%.3g (log=%.3f)\n', eta, logEta);
    fprintf('Chi2 thr (df=3,0.995)=%.3f\n\n', chi2thr);
    fprintf('Proposed (after similarity): %d\n', proposed);
    fprintf('Accepted (after geometry):   %d\n', accepted);
    fprintf('Detected true: %d\n', detTrue);
    fprintf('False accepts: %d\n', detFp);
    fprintf('Precision: %.3f\n', precision);
    fprintf('Recall:    %.3f\n', recall);

    % Optional: generate tiny Simulink model
    % make_simulink_scaffold();
end

function xy = square_loop_xy(N)
    S = max(20, floor(N/12));
    side = 10.0;
    pts = [];
    laps = max(1, floor(N/(4*S))) + 1;
    for lap = 1:laps
        for k = 1:S, pts = [pts; (k-1)/S*side, 0.0]; end
        for k = 1:S, pts = [pts; side, (k-1)/S*side]; end
        for k = 1:S, pts = [pts; side-(k-1)/S*side, side]; end
        for k = 1:S, pts = [pts; 0.0, side-(k-1)/S*side]; end
    end
    xy = pts(1:N,:);
end

function D = make_descriptors(xy, V)
    N = size(xy,1);
    centers = 10.0 * rand(V,2);
    ell = 1.5;
    D = zeros(N,V);
    for i = 1:N
        diff = centers - xy(i,:);
        r2 = sum(diff.^2, 2);
        D(i,:) = exp(-0.5 * r2' / (ell*ell)) + 0.02 * rand(1,V);
    end

    thr = 0.05;
    df = sum(D > thr, 1);
    idf = log((N+1)./(df+1)) + 1;
    D = D .* idf;
    D = D ./ (sqrt(sum(D.^2,2)) + 1e-12);
end

function cands = retrieve_topK(i, D, minSep, topK)
    if i <= minSep
        cands = zeros(0,2);
        return
    end
    idx = 1:(i-minSep);
    di = D(i,:);
    sims = (D(idx,:) * di') ./ (sqrt(sum(D(idx,:).^2,2)) * norm(di) + 1e-12);
    [~,ord] = sort(sims,'descend');
    K = min(topK, numel(ord));
    jj = idx(ord(1:K))';
    ss = sims(ord(1:K));
    cands = [jj, ss];
end

function tf = ground_truth_loop(i, j, xy, minSep, distThr)
    if j >= i - minSep
        tf = false; return
    end
    tf = norm(xy(i,:) - xy(j,:)) <= distThr;
end

function lp = normal_logpdf(x, mu, sigma)
    lp = -0.5*log(2*pi*sigma*sigma) - 0.5*((x-mu)/sigma)^2;
end

function r = simulate_residual(isTrue)
    if isTrue
        r = [0.08*randn; 0.08*randn; 0.02*randn];
    else
        r = [0.9*randn; 0.9*randn; 0.25*randn];
    end
end

function make_simulink_scaffold()
    mdl = 'Ch12_L2_LoopClosureScaffold';
    if bdIsLoaded(mdl), close_system(mdl,0); end
    new_system(mdl);
    open_system(mdl);

    add_block('simulink/Sources/In1',[mdl '/sim_in']);
    add_block('simulink/User-Defined Functions/MATLAB Function',[mdl '/gate']);
    add_block('simulink/Sinks/Out1',[mdl '/sim_out']);

    set_param([mdl '/gate'], 'Position', [250 80 420 160]);
    set_param([mdl '/sim_in'], 'Position', [70 95 100 115]);
    set_param([mdl '/sim_out'], 'Position', [520 95 550 115]);

    add_line(mdl, 'sim_in/1', 'gate/1');
    add_line(mdl, 'gate/1', 'sim_out/1');

    % Insert code into MATLAB Function block
    code = [
        "function ok = gate(u)"
        "% u = [dx; dy; dth] residual"
        "S = diag([0.25^2, 0.25^2, 0.08^2]);"
        "chi2thr = 12.838;"
        "d2 = u'*(S\\u);"
        "ok = (d2 <= chi2thr);"
        "end"
    ];
    set_param([mdl '/gate'], 'Script', strjoin(code, newline));

    save_system(mdl);
    fprintf('Created Simulink scaffold: %s.slx\n', mdl);
end
      

This notebook demonstrates descriptor retrieval and a chi-square gate with built-in distributions. Replace the toy residual with your actual geometry residual \( \mathbf{r}_{ij} = \mathbf{z}_{ij} - \hat{\mathbf{z}}_{ij} \) to replicate the verification stage.

File: Chapter12_Lesson2.nb

(* Chapter12_Lesson2.nb
   Loop Closure Detection Concepts — Mathematica demo (compact notebook)
   Open in Wolfram Mathematica.
*)

Notebook[{
  Cell["Chapter 12 — Lesson 2: Loop Closure Detection Concepts", "Title"],
  Cell["Compact demo: descriptor retrieval + simple statistical gating.", "Text"],

  Cell["SeedRandom[0];", "Input"],
  Cell["N = 420; V = 300; minSep = 40;", "Input"],

  Cell["(* 1) Toy descriptors (unit-normalized vectors) *)", "Section"],
  Cell["X = RandomReal[1, {N, V}]; X = Normalize /@ X;", "Input"],
  Cell["cos[a_, b_] := (a.b)/(Norm[a] Norm[b] + 10^-12);", "Input"],

  Cell["(* 2) Candidate retrieval: for each i, find best j < i-minSep by cosine similarity *)", "Section"],
  Cell[
    "bestJ[i_] := If[i <= minSep, Missing[\"NA\"], First @ OrderingBy[Range[i - minSep], -cos[X[[i]], X[[#]]] & , 1]];",
    "Input"
  ],
  Cell[
    "detEdges = Reap[Do[j = bestJ[i]; If[j =!= Missing[\"NA\"], sim = cos[X[[i]], X[[j]]]; If[sim > 0.85, Sow[{i, j, sim}]]], {i, 1, N}]][[2, 1]];",
    "Input"
  ],
  Cell["Length[detEdges]", "Input"],
  Cell["Take[Reverse@SortBy[detEdges, Last], UpTo[5]]", "Input"],

  Cell["(* 3) Geometric gate: chi-square threshold for 3 DoF (x,y,theta) residual *)", "Section"],
  Cell["chi2thr = Quantile[ChiSquareDistribution[3], 0.995];", "Input"],
  Cell["Sigma = DiagonalMatrix[{0.2^2, 0.2^2, 0.07^2}];", "Input"],
  Cell["gateOK[r_] := r.Inverse[Sigma].r < chi2thr;", "Input"],
  Cell["gateOK[{0.05, -0.02, 0.01}]", "Input"],

  Cell["(* Replace the toy residual with r = z_ij - h(x_i,x_j) from scan matching / vision. *)", "Text"]
},
WindowSize -> {1000, 750},
StyleDefinitions -> \"Default.nb\"
]
      

Problem 1 (Bayes threshold for loop closure): Let \( L_{ij}\in\{0,1\} \). Costs are \( C_{10} \) (false accept) and \( C_{01} \) (false reject). Derive the optimal acceptance condition in terms of the likelihood ratio \( \Lambda(\mathcal{E}_{ij}) \).

Solution: Choose accept if \( C_{10}P(L_{ij}=0\mid\mathcal{E}_{ij}) < C_{01}P(L_{ij}=1\mid\mathcal{E}_{ij}) \). Using Bayes’ rule: \( \frac{P(L_{ij}=1\mid\mathcal{E}_{ij})}{P(L_{ij}=0\mid\mathcal{E}_{ij})} = \Lambda(\mathcal{E}_{ij})\frac{P(L_{ij}=1)}{P(L_{ij}=0)} \). Therefore accept if:

\[ \Lambda(\mathcal{E}_{ij}) > \frac{C_{10}}{C_{01}}\cdot\frac{P(L_{ij}=0)}{P(L_{ij}=1)}. \]

Problem 2 (Chi-square gate): Assume \( \mathbf{r} \sim \mathcal{N}(\mathbf{0},\mathbf{S}) \) with \( \mathbf{r}\in\mathbb{R}^{\nu} \). Show that \( d^2=\mathbf{r}^\top\mathbf{S}^{-1}\mathbf{r} \) is chi-square distributed.

Solution: Let \( \mathbf{y}=\mathbf{S}^{-1/2}\mathbf{r} \). Then \( \mathbf{y}\sim\mathcal{N}(\mathbf{0},\mathbf{I}) \) and \( d^2=\|\mathbf{y}\|_2^2=\sum_{k=1}^{\nu}y_k^2 \), hence \( d^2\sim\chi^2_{\nu} \). The acceptance probability under the model is \( P(d^2 \le \chi^2_{\nu,\,1-\alpha}) = 1-\alpha \).

Problem 3 (TF–IDF weighting effect): Consider two words: a frequent “stop-word” with large \( \mathrm{df}_w \) and a rare, informative word with small \( \mathrm{df}_w \). Show qualitatively and quantitatively how \( \mathrm{idf}_w \) changes their contribution to cosine similarity.

Solution: Since \( \mathrm{idf}_w = \log\left(\frac{N+1}{\mathrm{df}_w+1}\right)+1 \), if \( \mathrm{df}_w \approx N \) then \( \mathrm{idf}_w \approx 1 \) (minimal boost), but if \( \mathrm{df}_w \ll N \) then \( \mathrm{idf}_w \) is much larger. Thus rare words increase dot products primarily when both frames share them, improving discriminability and lowering false positives caused by common patterns.

Problem 4 (Prior odds and environment size): In a long run, assume there are \( N \) keyframes and only \( K \) true loop closures for the current frame among all past frames. Approximate \( P(L_{ij}=1) \) for a random candidate and explain how this changes the Bayes threshold.

Solution: If candidates are uniformly drawn from past frames, then roughly \( P(L_{ij}=1)\approx \frac{K}{N} \) and \( P(L_{ij}=0)\approx 1-\frac{K}{N} \). As \( N \) grows with \( K \) small, the prior odds \( \frac{P(L_{ij}=0)}{P(L_{ij}=1)} \) becomes large, so the acceptance threshold \( \eta \) increases. Hence large environments demand more conservative decisions.

Problem 5 (Two-stage test composition): Suppose the appearance stage has false positive rate \( \alpha_a \) and the geometric stage has false positive rate \( \alpha_g \) (conditional on reaching it). Under the approximation that false matches must pass both, upper-bound the overall false positive rate.

Solution: A conservative bound is: \( \alpha_{\text{overall}} \le \alpha_a \alpha_g \), because a false candidate must survive both filters. This explains the practical rule: keep appearance recall reasonably high, then use strict geometric verification to drive precision.

Loop closure detection is the revisitation inference module of graph-based SLAM. We formulated loop closure as hypothesis testing, derived an optimal likelihood-ratio decision rule under asymmetric costs, and showed how geometric verification becomes a chi-square consistency test. The output of this lesson is a vetted set of loop-closure constraints to be inserted into the graph. In Lesson 3, we will optimize the augmented graph; in Lesson 4, we will handle remaining outliers robustly.