A pose graph consists of nodes (robot poses) and edges (relative-pose constraints). Each node \( i \) stores a planar pose \( \mathbf{x}_i = (x_i, y_i, \theta_i) \). An edge \( (i,j) \) carries a measured relative motion \( \mathbf{z}_{ij} = (\Delta x, \Delta y, \Delta\theta) \) (odometry or loop closure), plus an information matrix \( \mathbf{\Omega}_{ij} \).

What you will implement:

• Synthesize a ground-truth trajectory and generate noisy odometry edges.
• Add loop-closure edges (including a controllable fraction of outliers).
• Initialize by dead reckoning, then optimize the graph via sparse GN/LM.
• Evaluate convergence and trajectory quality.

flowchart TD
  A["Generate ground-truth poses (x,y,theta)"] --> B["Create noisy odometry edges (i,i+1)"]
  B --> C["Add loop-closure candidates (i,j)"]
  C --> D["Corrupt some loop closures (outliers)"]
  D --> E["Build pose graph: nodes + edges + information"]
  E --> F["Initialize: dead-reckoning chain"]
  F --> G["Optimize: GN/LM + sparse solve"]
  G --> H["Evaluate: cost and error metrics"]
        

The optimization objective (least squares) is:

\[ F(\mathbf{x}) = \sum_{(i,j)\in\mathcal{E} } \mathbf{e}_{ij}(\mathbf{x})^\top \mathbf{\Omega}_{ij}\,\mathbf{e}_{ij}(\mathbf{x}). \]

Associate each pose with a homogeneous transform \( \mathbf{T}_i \in SE(2) \):

\[ \mathbf{T}_i = \begin{bmatrix} \mathbf{R}(\theta_i) & \mathbf{t}_i \\ \mathbf{0}^\top & 1 \end{bmatrix},\quad \mathbf{R}(\theta)= \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix},\quad \mathbf{t}_i=\begin{bmatrix}x_i\\y_i\end{bmatrix}. \]

The predicted relative transform is \( \hat{\mathbf{Z} }_{ij}(\mathbf{x}) = \mathbf{T}_i^{-1}\mathbf{T}_j \). Using a minimal coordinate residual:

\[ \mathbf{e}_{ij}(\mathbf{x}) = \begin{bmatrix} \mathbf{R}(\theta_i)^\top(\mathbf{t}_j-\mathbf{t}_i) - \mathbf{t}_{ij} \\ \mathrm{wrap}(\theta_j-\theta_i-\theta_{ij}) \end{bmatrix}\in\mathbb{R}^3, \]

where \( \mathbf{t}_{ij}=[\Delta x,\Delta y]^\top \) and \( \theta_{ij}=\Delta\theta \) come from the measurement. The wrap operator enforces consistent angle residuals.

If you are given measurement covariance \( \mathbf{\Sigma}_{ij} \), you set \( \mathbf{\Omega}_{ij}=\mathbf{\Sigma}_{ij}^{-1} \).

At iteration k, linearize each residual around \( \mathbf{x}^{(k)} \): \( \mathbf{e}_{ij}(\mathbf{x}^{(k)}+\delta\mathbf{x}) \approx \mathbf{e}_{ij}^{(k)} + \mathbf{J}_{ij}\delta\mathbf{x} \). Summing quadratic models yields sparse normal equations.

\[ \mathbf{H}\,\delta\mathbf{x} = -\mathbf{g},\quad \mathbf{H}=\sum_{(i,j)\in\mathcal{E} }\mathbf{J}_{ij}^\top\mathbf{\Omega}_{ij}\mathbf{J}_{ij},\quad \mathbf{g}=\sum_{(i,j)\in\mathcal{E} }\mathbf{J}_{ij}^\top\mathbf{\Omega}_{ij}\mathbf{e}_{ij}. \]

Each edge touches only nodes i and j, so \( \mathbf{J}_{ij} \) has two 3×3 blocks \( \mathbf{A}_{ij}=\partial\mathbf{e}_{ij}/\partial\mathbf{x}_i \) and \( \mathbf{B}_{ij}=\partial\mathbf{e}_{ij}/\partial\mathbf{x}_j \). This is the key to sparse scalability.

Let \( \mathbf{d}=\mathbf{t}_j-\mathbf{t}_i \) and \( \mathbf{R}_i=\mathbf{R}(\theta_i) \). With \( \mathbf{S}=\begin{bmatrix}0&-1\\1&0\end{bmatrix} \), the translational part satisfies:

\[ \frac{\partial}{\partial\mathbf{t}_i}\Big(\mathbf{R}_i^\top\mathbf{d}\Big)=-\mathbf{R}_i^\top,\quad \frac{\partial}{\partial\mathbf{t}_j}\Big(\mathbf{R}_i^\top\mathbf{d}\Big)=\mathbf{R}_i^\top,\quad \frac{\partial}{\partial\theta_i}\Big(\mathbf{R}_i^\top\mathbf{d}\Big)=-\mathbf{R}_i^\top\mathbf{S}\mathbf{d}. \]

The angular residual \( r_\theta=\mathrm{wrap}(\theta_j-\theta_i-\theta_{ij}) \) has local derivatives \( \partial r_\theta/\partial\theta_i=-1 \), \( \partial r_\theta/\partial\theta_j=1 \) away from wrap discontinuities.

Gauge freedom: with only relative constraints, the cost is invariant to a global SE(2) transform, so \( \mathbf{H} \) is singular. Anchor pose 0 by adding a strong prior or removing its variables.

\[ \min_{\mathbf{x} }\; \sum_{(i,j)\in\mathcal{E} }\|\mathbf{e}_{ij}(\mathbf{x})\|^2_{\mathbf{\Omega}_{ij} } + \|\mathbf{x}_0-\bar{\mathbf{x} }_0\|^2_{\mathbf{\Omega}_0},\quad \mathbf{\Omega}_0 \text{ large}. \]

LM solves a damped system:

\[ (\mathbf{H}+\lambda\,\mathbf{D})\,\delta\mathbf{x}=-\mathbf{g},\quad \lambda > 0, \]

typically with \( \mathbf{D}=\mathrm{diag}(\mathbf{H}) \). If the new cost decreases sufficiently, reduce \( \lambda \); otherwise increase it.

flowchart TD
  S["Initialize x (dead-reckoning)"] --> L["Linearize edges at current x"]
  L --> A["Assemble sparse H and g"]
  A --> SOL["Solve: (H + lambda*D) dx = -g"]
  SOL --> U["Update x := x + dx (wrap angles)"]
  U --> C["Stop test: cost decrease and norm(dx)"]
  C -->|continue| L
  C -->|stop| OUT["Return optimized x"]
        

For loop closures, monitor per-edge residual magnitudes; extreme values are candidates for robust downweighting (Lesson 4) or gating.

Below are complete reference implementations.

Chapter12_Lesson5.py

# Chapter12_Lesson5.py
# Lab: Build and Optimize a 2D Pose Graph (SE(2)) using Gauss-Newton / Levenberg-Marquardt
# Dependencies: numpy, scipy
#
# Run:
#   python Chapter12_Lesson5.py
#
# This script:
#   1) Generates a synthetic SE(2) trajectory (ground truth)
#   2) Creates noisy odometry edges + loop closure edges (optionally with outliers)
#   3) Builds a pose graph and optimizes it with GN/LM and a sparse solver
#   4) Prints convergence diagnostics and returns optimized poses
#
# NOTE: All angles are in radians. wrap() maps angles to (-pi, pi].

from __future__ import annotations
import math
import numpy as np
from dataclasses import dataclass
from typing import List, Tuple, Optional

try:
    from scipy.sparse import lil_matrix, csr_matrix
    from scipy.sparse.linalg import spsolve
except Exception as e:
    raise RuntimeError("This script requires scipy (sparse). Install via: pip install scipy") from e


# ----------------------------
# SE(2) utilities
# ----------------------------
def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    a = (a + math.pi) % (2.0 * math.pi) - math.pi
    # map -pi to +pi (optional convention)
    if a <= -math.pi:
        a += 2.0 * math.pi
    return a


def R(theta: float) -> np.ndarray:
    c, s = math.cos(theta), math.sin(theta)
    return np.array([[c, -s], [s, c]], dtype=float)


def se2_compose(x1: np.ndarray, x2: np.ndarray) -> np.ndarray:
    """
    Compose two SE(2) poses in minimal coordinates:
      x = (t, theta) with t in R^2.
    """
    t1, th1 = x1[:2], x1[2]
    t2, th2 = x2[:2], x2[2]
    t = t1 + R(th1) @ t2
    th = wrap_angle(th1 + th2)
    return np.array([t[0], t[1], th], dtype=float)


def se2_inv(x: np.ndarray) -> np.ndarray:
    """Inverse of SE(2) pose in minimal coords."""
    t, th = x[:2], x[2]
    Rt = R(th).T
    tinv = -Rt @ t
    return np.array([tinv[0], tinv[1], wrap_angle(-th)], dtype=float)


def se2_between(xi: np.ndarray, xj: np.ndarray) -> np.ndarray:
    """Relative transform z = xi^{-1} ⊕ xj in minimal coords."""
    return se2_compose(se2_inv(xi), xj)


# ----------------------------
# Pose graph data structures
# ----------------------------
@dataclass
class EdgeSE2:
    i: int
    j: int
    z: np.ndarray      # measurement (3,)
    Omega: np.ndarray  # information (3,3)
    is_loop: bool = False


# ----------------------------
# Residual + Jacobians
# ----------------------------
def residual_and_jacobian(xi: np.ndarray, xj: np.ndarray, z_ij: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    Residual e_ij(x) and Jacobians A=de/dxi, B=de/dxj using minimal residual:
      e_t = R(theta_i)^T (t_j - t_i) - t_ij
      e_r = wrap(theta_j - theta_i - theta_ij)
    """
    ti, thi = xi[:2], xi[2]
    tj, thj = xj[:2], xj[2]
    RijT = R(thi).T
    d = tj - ti
    et = RijT @ d - z_ij[:2]
    er = wrap_angle(thj - thi - z_ij[2])
    e = np.array([et[0], et[1], er], dtype=float)

    # Jacobians
    # S = [[0,-1],[1,0]]
    S = np.array([[0.0, -1.0], [1.0, 0.0]])
    # d(et)/d(ti) = -R_i^T
    A_t_ti = -RijT
    # d(et)/d(tj) = +R_i^T
    B_t_tj = RijT
    # d(et)/d(theta_i) = -R_i^T S (t_j - t_i)
    A_t_thi = -(RijT @ (S @ d)).reshape(2, 1)

    # Assemble A (3x3), B (3x3)
    A = np.zeros((3, 3), dtype=float)
    B = np.zeros((3, 3), dtype=float)
    A[:2, :2] = A_t_ti
    A[:2, 2:3] = A_t_thi
    A[2, 2] = -1.0

    B[:2, :2] = B_t_tj
    B[2, 2] = 1.0

    return e, A, B


# ----------------------------
# Graph optimization (GN/LM)
# ----------------------------
def build_normal_equations(
    x: np.ndarray,
    edges: List[EdgeSE2],
    N: int,
    anchor_idx: int = 0,
    anchor_Omega: Optional[np.ndarray] = None,
    anchor_pose: Optional[np.ndarray] = None,
) -> Tuple[csr_matrix, np.ndarray, float]:
    """
    Build sparse H and dense g for all poses (3N variables).
    Optionally add an anchor prior on pose 'anchor_idx' to remove gauge.
    """
    dim = 3 * N
    H = lil_matrix((dim, dim), dtype=float)
    g = np.zeros(dim, dtype=float)
    cost = 0.0

    # Helper: slice for pose k
    def sl(k: int) -> slice:
        return slice(3 * k, 3 * k + 3)

    for ed in edges:
        i, j = ed.i, ed.j
        xi = x[sl(i)]
        xj = x[sl(j)]
        e, A, B = residual_and_jacobian(xi, xj, ed.z)
        Omega = ed.Omega

        cost += float(e.T @ Omega @ e)

        # Edge contributions to H and g (block form)
        AtO = A.T @ Omega
        BtO = B.T @ Omega

        H_ii = AtO @ A
        H_ij = AtO @ B
        H_ji = BtO @ A
        H_jj = BtO @ B

        g_i = AtO @ e
        g_j = BtO @ e

        si, sj = sl(i), sl(j)

        H[si, si] += H_ii
        H[si, sj] += H_ij
        H[sj, si] += H_ji
        H[sj, sj] += H_jj

        g[si] += g_i
        g[sj] += g_j

    # Anchor prior: (x_anchor - anchor_pose) with information anchor_Omega
    if anchor_Omega is None:
        anchor_Omega = np.diag([1e6, 1e6, 1e6])
    if anchor_pose is None:
        anchor_pose = x[sl(anchor_idx)].copy()

    ea = x[sl(anchor_idx)] - anchor_pose
    ea[2] = wrap_angle(ea[2])
    cost += float(ea.T @ anchor_Omega @ ea)

    H_a = anchor_Omega
    g_a = anchor_Omega @ ea
    sa = sl(anchor_idx)
    H[sa, sa] += H_a
    g[sa] += g_a

    return H.tocsr(), g, cost


def levenberg_marquardt(
    x0: np.ndarray,
    edges: List[EdgeSE2],
    N: int,
    max_iters: int = 30,
    lambda0: float = 1e-3,
    tol: float = 1e-6,
) -> Tuple[np.ndarray, List[float]]:
    """
    Sparse LM on pose graph:
      (H + lambda * diag(H)) dx = -g
    """
    x = x0.copy()
    lam = lambda0
    costs = []

    for it in range(1, max_iters + 1):
        H, g, cost = build_normal_equations(x, edges, N, anchor_idx=0)
        costs.append(cost)

        # Damping matrix: diag(H)
        diagH = H.diagonal()
        D = csr_matrix((diagH, (np.arange(3 * N), np.arange(3 * N))), shape=(3 * N, 3 * N))
        H_lm = H + lam * D

        # Solve linear system
        dx = spsolve(H_lm, -g)

        # Candidate update
        x_new = x + dx
        # wrap all angles
        for k in range(N):
            x_new[3 * k + 2] = wrap_angle(x_new[3 * k + 2])

        # Evaluate new cost
        _, _, cost_new = build_normal_equations(x_new, edges, N, anchor_idx=0)

        # Accept / reject
        if cost_new < cost:
            x = x_new
            lam = max(lam / 5.0, 1e-12)
        else:
            lam = min(lam * 10.0, 1e12)

        step_norm = float(np.linalg.norm(dx))
        print(f"iter={it:02d} cost={cost:.6e} cost_new={cost_new:.6e} lam={lam:.2e} ||dx||={step_norm:.3e}")

        if step_norm < tol:
            print("Converged: step norm below tolerance.")
            break

    return x, costs


# ----------------------------
# Synthetic data generation
# ----------------------------
def generate_ground_truth(N: int, step: float = 1.0, turn_rate: float = 0.03) -> np.ndarray:
    """
    Create a smooth curving trajectory.
    x_k+1 = x_k ⊕ u_k, with u_k = (step, 0, turn_rate)
    """
    x = np.zeros((N, 3), dtype=float)
    for k in range(1, N):
        u = np.array([step, 0.0, turn_rate], dtype=float)
        x[k] = se2_compose(x[k - 1], u)
    return x


def add_noise_to_measurement(z: np.ndarray, sig_t: float, sig_th: float, rng: np.random.Generator) -> np.ndarray:
    zn = z.copy()
    zn[0] += rng.normal(0.0, sig_t)
    zn[1] += rng.normal(0.0, sig_t)
    zn[2] = wrap_angle(zn[2] + rng.normal(0.0, sig_th))
    return zn


def info_from_sigmas(sig_t: float, sig_th: float) -> np.ndarray:
    # diagonal covariance
    Sigma = np.diag([sig_t**2, sig_t**2, sig_th**2])
    return np.linalg.inv(Sigma)


def build_synthetic_graph(
    x_gt: np.ndarray,
    sig_odom_t: float = 0.05,
    sig_odom_th: float = 0.02,
    sig_loop_t: float = 0.10,
    sig_loop_th: float = 0.05,
    num_loops: int = 15,
    outlier_frac: float = 0.20,
    outlier_scale: float = 5.0,
    seed: int = 7
) -> Tuple[List[EdgeSE2], np.ndarray]:
    """
    Build edges + an initial guess (dead reckoning from noisy odometry).
    """
    rng = np.random.default_rng(seed)
    N = x_gt.shape[0]
    edges: List[EdgeSE2] = []

    # Odometry edges (i,i+1)
    Omega_odom = info_from_sigmas(sig_odom_t, sig_odom_th)
    for i in range(N - 1):
        z = se2_between(x_gt[i], x_gt[i + 1])
        z_noisy = add_noise_to_measurement(z, sig_odom_t, sig_odom_th, rng)
        edges.append(EdgeSE2(i=i, j=i + 1, z=z_noisy, Omega=Omega_odom, is_loop=False))

    # Dead-reckoning init
    x0 = np.zeros_like(x_gt)
    for i in range(N - 1):
        x0[i + 1] = se2_compose(x0[i], edges[i].z)

    # Loop closures
    Omega_loop = info_from_sigmas(sig_loop_t, sig_loop_th)
    # Choose random pairs (i,j) with j > i+5
    candidates = []
    for i in range(N):
        for j in range(i + 6, N):
            candidates.append((i, j))
    rng.shuffle(candidates)
    pairs = candidates[:num_loops]

    for (i, j) in pairs:
        z = se2_between(x_gt[i], x_gt[j])
        z_noisy = add_noise_to_measurement(z, sig_loop_t, sig_loop_th, rng)
        # Inject outliers by corrupting measurement
        if rng.random() < outlier_frac:
            z_noisy[:2] += rng.normal(0.0, outlier_scale * sig_loop_t, size=2)
            z_noisy[2] = wrap_angle(z_noisy[2] + rng.normal(0.0, outlier_scale * sig_loop_th))
        edges.append(EdgeSE2(i=i, j=j, z=z_noisy, Omega=Omega_loop, is_loop=True))

    return edges, x0


def trajectory_rmse(x_est: np.ndarray, x_gt: np.ndarray) -> float:
    """RMSE in translation only (simple ATE proxy without alignment)."""
    d = x_est[:, :2] - x_gt[:, :2]
    return float(np.sqrt(np.mean(np.sum(d*d, axis=1))))


# ----------------------------
# Main
# ----------------------------
def main():
    N = 80
    x_gt = generate_ground_truth(N=N, step=1.0, turn_rate=0.03)
    edges, x0 = build_synthetic_graph(
        x_gt,
        sig_odom_t=0.05, sig_odom_th=0.02,
        sig_loop_t=0.10, sig_loop_th=0.05,
        num_loops=20,
        outlier_frac=0.25,
        outlier_scale=8.0,
        seed=3
    )

    print(f"Graph: N={N}, edges={len(edges)} (odom={N-1}, loops={len(edges)-(N-1)})")
    print("Initial RMSE (translation):", trajectory_rmse(x0, x_gt))

    x_opt, costs = levenberg_marquardt(x0.reshape(-1), edges, N, max_iters=30, lambda0=1e-3, tol=1e-6)
    x_opt = x_opt.reshape(N, 3)

    print("Final RMSE (translation):", trajectory_rmse(x_opt, x_gt))
    print("Cost sequence (first 10):", costs[:10])

if __name__ == "__main__":
    main()
      

Chapter12_Lesson5.cpp

// Chapter12_Lesson5.cpp
// Lab: Build and Optimize a 2D Pose Graph (SE(2)) using Gauss-Newton / LM
//
// Dependencies: Eigen (header-only for dense), Eigen Sparse for sparse linear solve.
// Build (example):
//   g++ -O2 -std=c++17 Chapter12_Lesson5.cpp -I /usr/include/eigen3 -o pose_graph
//
// Run:
//   ./pose_graph
//
// This program generates a synthetic trajectory, adds noisy odometry + loop constraints,
// anchors node 0, and optimizes using LM with sparse normal equations.

#include <iostream>
#include <vector>
#include <random>
#include <cmath>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

using Eigen::Matrix2d;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::MatrixXd;
using Eigen::VectorXd;

struct EdgeSE2 {
  int i;
  int j;
  Vector3d z;        // measurement (dx, dy, dtheta)
  Matrix3d Omega;    // information
  bool is_loop;
};

static double wrap_angle(double a) {
  a = std::fmod(a + M_PI, 2.0 * M_PI);
  if (a < 0) a += 2.0 * M_PI;
  a -= M_PI;
  if (a <= -M_PI) a += 2.0 * M_PI;
  return a;
}

static Matrix2d R(double theta) {
  double c = std::cos(theta), s = std::sin(theta);
  Matrix2d M;
  M << c, -s,
       s,  c;
  return M;
}

static Vector3d se2_compose(const Vector3d& x1, const Vector3d& x2) {
  Vector2d t1(x1(0), x1(1));
  double th1 = x1(2);
  Vector2d t2(x2(0), x2(1));
  double th2 = x2(2);

  Vector2d t = t1 + R(th1) * t2;
  double th = wrap_angle(th1 + th2);
  return Vector3d(t(0), t(1), th);
}

static Vector3d se2_inv(const Vector3d& x) {
  Vector2d t(x(0), x(1));
  double th = x(2);
  Matrix2d Rt = R(th).transpose();
  Vector2d tinv = -Rt * t;
  return Vector3d(tinv(0), tinv(1), wrap_angle(-th));
}

static Vector3d se2_between(const Vector3d& xi, const Vector3d& xj) {
  return se2_compose(se2_inv(xi), xj);
}

static void residual_and_jacobian(
    const Vector3d& xi, const Vector3d& xj, const Vector3d& z,
    Vector3d& e, Matrix3d& A, Matrix3d& B)
{
  Vector2d ti(xi(0), xi(1));
  double thi = xi(2);
  Vector2d tj(xj(0), xj(1));
  double thj = xj(2);

  Matrix2d RiT = R(thi).transpose();
  Vector2d d = tj - ti;

  Vector2d et = RiT * d - Vector2d(z(0), z(1));
  double er = wrap_angle(thj - thi - z(2));
  e << et(0), et(1), er;

  // Jacobians
  Matrix2d S;
  S << 0.0, -1.0,
       1.0,  0.0;

  A.setZero();
  B.setZero();

  // d(et)/d(ti) = -Ri^T
  A.block<2,2>(0,0) = -RiT;
  // d(et)/d(tj) = +Ri^T
  B.block<2,2>(0,0) = RiT;
  // d(et)/d(theta_i) = -Ri^T S (tj-ti)
  Vector2d A_t_th = -(RiT * (S * d));
  A(0,2) = A_t_th(0);
  A(1,2) = A_t_th(1);

  // angular
  A(2,2) = -1.0;
  B(2,2) =  1.0;
}

static Matrix3d info_from_sigmas(double sig_t, double sig_th) {
  Matrix3d Sigma = Matrix3d::Zero();
  Sigma(0,0) = sig_t * sig_t;
  Sigma(1,1) = sig_t * sig_t;
  Sigma(2,2) = sig_th * sig_th;
  return Sigma.inverse();
}

static void build_normal_equations(
    const Eigen::VectorXd& x, int N, const std::vector<EdgeSE2>& edges,
    Eigen::SparseMatrix<double>& H, Eigen::VectorXd& g, double& cost,
    int anchor_idx = 0,
    const Matrix3d& anchor_Omega = (Matrix3d() << 1e6,0,0, 0,1e6,0, 0,0,1e6).finished(),
    const Vector3d& anchor_pose = Vector3d::Zero()
) {
  int dim = 3 * N;
  std::vector<Eigen::Triplet<double>> trips;
  trips.reserve(edges.size() * 4 * 9 + 9);

  g.setZero(dim);
  cost = 0.0;

  auto sl = [](int k) { return 3*k; };

  for (const auto& ed : edges) {
    int i = ed.i, j = ed.j;
    Vector3d xi = x.segment(sl(i), 3);
    Vector3d xj = x.segment(sl(j), 3);

    Vector3d e;
    Matrix3d A, B;
    residual_and_jacobian(xi, xj, ed.z, e, A, B);

    cost += e.transpose() * ed.Omega * e;

    Matrix3d AtO = A.transpose() * ed.Omega;
    Matrix3d BtO = B.transpose() * ed.Omega;

    Matrix3d Hii = AtO * A;
    Matrix3d Hij = AtO * B;
    Matrix3d Hji = BtO * A;
    Matrix3d Hjj = BtO * B;

    Vector3d gi = AtO * e;
    Vector3d gj = BtO * e;

    int si = sl(i), sj = sl(j);

    // Add block triplets
    for (int r = 0; r < 3; ++r) {
      for (int c = 0; c < 3; ++c) {
        trips.emplace_back(si+r, si+c, Hii(r,c));
        trips.emplace_back(si+r, sj+c, Hij(r,c));
        trips.emplace_back(sj+r, si+c, Hji(r,c));
        trips.emplace_back(sj+r, sj+c, Hjj(r,c));
      }
    }

    g.segment(si,3) += gi;
    g.segment(sj,3) += gj;
  }

  // Anchor prior on node 0 (x0 - anchor_pose)
  {
    int s0 = sl(anchor_idx);
    Vector3d e0 = x.segment(s0,3) - anchor_pose;
    e0(2) = wrap_angle(e0(2));
    cost += e0.transpose() * anchor_Omega * e0;

    Matrix3d H0 = anchor_Omega;
    Vector3d g0 = anchor_Omega * e0;

    for (int r = 0; r < 3; ++r) {
      for (int c = 0; c < 3; ++c) {
        trips.emplace_back(s0+r, s0+c, H0(r,c));
      }
    }
    g.segment(s0,3) += g0;
  }

  H.resize(dim, dim);
  H.setFromTriplets(trips.begin(), trips.end());
}

static double traj_rmse_translation(const Eigen::VectorXd& x_est, const std::vector<Vector3d>& x_gt) {
  int N = (int)x_gt.size();
  double s = 0.0;
  for (int k = 0; k < N; ++k) {
    double dx = x_est(3*k+0) - x_gt;
    double dy = x_est(3*k+1) - x_gt;
    s += dx*dx + dy*dy;
  }
  return std::sqrt(s / N);
}

static std::vector<Vector3d> generate_ground_truth(int N, double step=1.0, double turn_rate=0.03) {
  std::vector<Vector3d> x(N, Vector3d::Zero());
  for (int k = 1; k < N; ++k) {
    Vector3d u(step, 0.0, turn_rate);
    x[k] = se2_compose(x[k-1], u);
  }
  return x;
}

static Vector3d add_noise(const Vector3d& z, double sig_t, double sig_th, std::mt19937& gen) {
  std::normal_distribution<double> nt(0.0, sig_t);
  std::normal_distribution<double> nth(0.0, sig_th);
  Vector3d zn = z;
  zn(0) += nt(gen);
  zn(1) += nt(gen);
  zn(2) = wrap_angle(zn(2) + nth(gen));
  return zn;
}

static void build_synthetic_graph(
    const std::vector<Vector3d>& x_gt,
    std::vector<EdgeSE2>& edges,
    Eigen::VectorXd& x0,
    int num_loops=20,
    double outlier_frac=0.25,
    double outlier_scale=8.0,
    unsigned seed=3)
{
  std::mt19937 gen(seed);
  std::uniform_real_distribution<double> uni(0.0, 1.0);

  int N = (int)x_gt.size();
  edges.clear();

  double sig_odom_t = 0.05, sig_odom_th = 0.02;
  double sig_loop_t = 0.10, sig_loop_th = 0.05;

  Matrix3d Omega_odom = info_from_sigmas(sig_odom_t, sig_odom_th);
  Matrix3d Omega_loop = info_from_sigmas(sig_loop_t, sig_loop_th);

  // Odometry edges
  for (int i = 0; i < N-1; ++i) {
    Vector3d z = se2_between(x_gt[i], x_gt[i+1]);
    Vector3d zn = add_noise(z, sig_odom_t, sig_odom_th, gen);
    edges.push_back(EdgeSE2{i, i+1, zn, Omega_odom, false});
  }

  // Initial guess via dead reckoning
  x0.setZero(3*N);
  for (int i = 0; i < N-1; ++i) {
    Vector3d xi = x0.segment(3*i, 3);
    Vector3d xip1 = se2_compose(xi, edges[i].z);
    x0.segment(3*(i+1), 3) = xip1;
  }

  // Candidate loop pairs
  std::vector<std::pair<int,int>> cand;
  cand.reserve(N*N);
  for (int i = 0; i < N; ++i) {
    for (int j = i+6; j < N; ++j) {
      cand.emplace_back(i,j);
    }
  }
  std::shuffle(cand.begin(), cand.end(), gen);

  for (int k = 0; k < num_loops && k < (int)cand.size(); ++k) {
    int i = cand[k].first;
    int j = cand[k].second;
    Vector3d z = se2_between(x_gt[i], x_gt[j]);
    Vector3d zn = add_noise(z, sig_loop_t, sig_loop_th, gen);

    if (uni(gen) < outlier_frac) {
      std::normal_distribution<double> nt(0.0, outlier_scale * sig_loop_t);
      std::normal_distribution<double> nth(0.0, outlier_scale * sig_loop_th);
      zn(0) += nt(gen);
      zn(1) += nt(gen);
      zn(2) = wrap_angle(zn(2) + nth(gen));
    }

    edges.push_back(EdgeSE2{i, j, zn, Omega_loop, true});
  }
}

static Eigen::VectorXd levenberg_marquardt(
    const Eigen::VectorXd& x_init,
    int N,
    const std::vector<EdgeSE2>& edges,
    int maxIters=30,
    double lambda0=1e-3,
    double tol=1e-6)
{
  Eigen::VectorXd x = x_init;
  double lam = lambda0;

  Eigen::SparseMatrix<double> H;
  Eigen::VectorXd g(3*N);
  double cost = 0.0;

  for (int it = 1; it <= maxIters; ++it) {
    // anchor_pose = current pose0 (so the prior is around current? better: anchor at init/gt; here we anchor at x0=0)
    Vector3d anchor_pose(0.0, 0.0, 0.0);
    Matrix3d anchor_Omega = (Matrix3d() << 1e6,0,0, 0,1e6,0, 0,0,1e6).finished();

    build_normal_equations(x, N, edges, H, g, cost, 0, anchor_Omega, anchor_pose);

    // Damping: diag(H)
    Eigen::VectorXd diag = H.diagonal();
    Eigen::SparseMatrix<double> D(3*N, 3*N);
    std::vector<Eigen::Triplet<double>> dtr;
    dtr.reserve(3*N);
    for (int k = 0; k < 3*N; ++k) dtr.emplace_back(k,k,diag(k));
    D.setFromTriplets(dtr.begin(), dtr.end());

    Eigen::SparseMatrix<double> Hlm = H + lam * D;

    // Solve sparse SPD system via SimplicialLDLT
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    solver.compute(Hlm);
    if (solver.info() != Eigen::Success) {
      std::cerr << "Decomposition failed at iter " << it << "\n";
      break;
    }

    Eigen::VectorXd dx = solver.solve(-g);
    if (solver.info() != Eigen::Success) {
      std::cerr << "Solve failed at iter " << it << "\n";
      break;
    }

    Eigen::VectorXd x_new = x + dx;
    // wrap angles
    for (int k = 0; k < N; ++k) {
      x_new(3*k+2) = wrap_angle(x_new(3*k+2));
    }

    // new cost
    Eigen::SparseMatrix<double> H2;
    Eigen::VectorXd g2(3*N);
    double cost_new = 0.0;
    build_normal_equations(x_new, N, edges, H2, g2, cost_new, 0, anchor_Omega, anchor_pose);

    if (cost_new < cost) {
      x = x_new;
      lam = std::max(lam / 5.0, 1e-12);
    } else {
      lam = std::min(lam * 10.0, 1e12);
    }

    double step_norm = dx.norm();
    std::cout << "iter=" << it
              << " cost=" << cost
              << " cost_new=" << cost_new
              << " lam=" << lam
              << " ||dx||=" << step_norm
              << "\n";

    if (step_norm < tol) {
      std::cout << "Converged: step norm below tolerance.\n";
      break;
    }
  }

  return x;
}

int main() {
  int N = 80;
  auto x_gt = generate_ground_truth(N, 1.0, 0.03);

  std::vector<EdgeSE2> edges;
  Eigen::VectorXd x0;
  x0.resize(3*N);

  build_synthetic_graph(x_gt, edges, x0, 20, 0.25, 8.0, 3);

  std::cout << "Graph: N=" << N << " edges=" << edges.size()
            << " (odom=" << (N-1) << ", loops=" << (edges.size() - (N-1)) << ")\n";

  std::cout << "Initial RMSE (translation): " << traj_rmse_translation(x0, x_gt) << "\n";

  Eigen::VectorXd x_opt = levenberg_marquardt(x0, N, edges, 30, 1e-3, 1e-6);

  std::cout << "Final RMSE (translation): " << traj_rmse_translation(x_opt, x_gt) << "\n";
  return 0;
}
      

Chapter12_Lesson5.java

// Chapter12_Lesson5.java
// Lab: Build and Optimize a 2D Pose Graph (SE(2)) using Gauss-Newton / LM
//
// Dependencies: EJML (Efficient Java Matrix Library)
//
// Maven (example):
//   <dependency>
//     <groupId>org.ejml</groupId>
//     <artifactId>ejml-simple</artifactId>
//     <version>0.43</version>
//   </dependency>
//
// Run:
//   javac -cp .:ejml-simple-0.43.jar Chapter12_Lesson5.java
//   java  -cp .:ejml-simple-0.43.jar Chapter12_Lesson5
//
// This program generates a synthetic trajectory, adds noisy odometry + loop constraints,
// anchors node 0, and optimizes using LM. For clarity and portability, we assemble dense
// normal equations (OK for N~80). For large graphs use sparse libraries.

import java.util.*;
import java.util.stream.Collectors;
import org.ejml.simple.SimpleMatrix;

public class Chapter12_Lesson5 {

    static class EdgeSE2 {
        int i, j;
        double[] z;     // (dx, dy, dtheta)
        double[][] Omega; // 3x3 information
        boolean isLoop;

        EdgeSE2(int i, int j, double[] z, double[][] Omega, boolean isLoop) {
            this.i = i; this.j = j; this.z = z; this.Omega = Omega; this.isLoop = isLoop;
        }
    }

    static double wrapAngle(double a) {
        double twoPi = 2.0*Math.PI;
        a = (a + Math.PI) % twoPi;
        if (a < 0) a += twoPi;
        a -= Math.PI;
        if (a <= -Math.PI) a += twoPi;
        return a;
    }

    static double[][] R(double theta) {
        double c = Math.cos(theta), s = Math.sin(theta);
        return new double[][]{
                {c, -s},
                {s,  c}
        };
    }

    static double[] matVec2(double[][] M, double[] v) {
        return new double[] {
                M[0][0]*v[0] + M[0][1]*v[1],
                M[1][0]*v[0] + M[1][1]*v[1]
        };
    }

    static double[] se2Compose(double[] x1, double[] x2) {
        double[] t1 = new double[]{x1[0], x1[1]};
        double th1 = x1[2];
        double[] t2 = new double[]{x2[0], x2[1]};
        double th2 = x2[2];

        double[][] R1 = R(th1);
        double[] Rt2 = matVec2(R1, t2);
        double[] t = new double[]{t1[0] + Rt2[0], t1[1] + Rt2[1]};
        double th = wrapAngle(th1 + th2);
        return new double[]{t[0], t[1], th};
    }

    static double[] se2Inv(double[] x) {
        double[] t = new double[]{x[0], x[1]};
        double th = x[2];
        double[][] Rth = R(th);
        // transpose
        double[][] Rt = new double[][]{
                {Rth[0][0], Rth[1][0]},
                {Rth[0][1], Rth[1][1]}
        };
        double[] tinv = matVec2(Rt, new double[]{-t[0], -t[1]});
        return new double[]{tinv[0], tinv[1], wrapAngle(-th)};
    }

    static double[] se2Between(double[] xi, double[] xj) {
        return se2Compose(se2Inv(xi), xj);
    }

    static class ResJac {
        double[] e;       // 3
        double[][] A;     // 3x3
        double[][] B;     // 3x3
        ResJac(double[] e, double[][] A, double[][] B) {
            this.e = e; this.A = A; this.B = B;
        }
    }

    static ResJac residualAndJacobian(double[] xi, double[] xj, double[] z) {
        double[] ti = new double[]{xi[0], xi[1]};
        double thi = xi[2];
        double[] tj = new double[]{xj[0], xj[1]};
        double thj = xj[2];

        double[][] Ri = R(thi);
        double[][] RiT = new double[][]{
                {Ri[0][0], Ri[1][0]},
                {Ri[0][1], Ri[1][1]}
        };

        double[] d = new double[]{tj[0]-ti[0], tj[1]-ti[1]};
        double[] et = matVec2(RiT, d);
        et[0] -= z[0];
        et[1] -= z[1];
        double er = wrapAngle(thj - thi - z[2]);

        double[] e = new double[]{et[0], et[1], er};

        // S = [[0,-1],[1,0]]
        double[][] S = new double[][]{
                {0.0, -1.0},
                {1.0,  0.0}
        };

        // A and B
        double[][] A = new double[3][3];
        double[][] B = new double[3][3];

        // d(et)/d(ti) = -Ri^T
        A[0][0] = -RiT[0][0]; A[0][1] = -RiT[0][1];
        A[1][0] = -RiT[1][0]; A[1][1] = -RiT[1][1];

        // d(et)/d(tj) = Ri^T
        B[0][0] = RiT[0][0]; B[0][1] = RiT[0][1];
        B[1][0] = RiT[1][0]; B[1][1] = RiT[1][1];

        // d(et)/d(theta_i) = -Ri^T S d
        double[] Sd = matVec2(S, d);
        double[] RiTSd = matVec2(RiT, Sd);
        A[0][2] = -RiTSd[0];
        A[1][2] = -RiTSd[1];

        // angle part
        A[2][2] = -1.0;
        B[2][2] =  1.0;

        return new ResJac(e, A, B);
    }

    static double[][] infoFromSigmas(double sigT, double sigTh) {
        double[][] Omega = new double[3][3];
        Omega[0][0] = 1.0/(sigT*sigT);
        Omega[1][1] = 1.0/(sigT*sigT);
        Omega[2][2] = 1.0/(sigTh*sigTh);
        return Omega;
    }

    static class NormalEq {
        SimpleMatrix H;
        SimpleMatrix g;
        double cost;
        NormalEq(SimpleMatrix H, SimpleMatrix g, double cost) {
            this.H = H; this.g = g; this.cost = cost;
        }
    }

    static NormalEq buildNormalEquations(double[] x, int N, List<EdgeSE2> edges) {
        int dim = 3*N;
        SimpleMatrix H = new SimpleMatrix(dim, dim);
        SimpleMatrix g = new SimpleMatrix(dim, 1);
        double cost = 0.0;

        for (EdgeSE2 ed : edges) {
            int i = ed.i, j = ed.j;
            double[] xi = new double[]{x[3*i], x[3*i+1], x[3*i+2]};
            double[] xj = new double[]{x[3*j], x[3*j+1], x[3*j+2]};

            ResJac rj = residualAndJacobian(xi, xj, ed.z);
            double[] e = rj.e;
            double[][] A = rj.A;
            double[][] B = rj.B;
            double[][] Omega = ed.Omega;

            // cost += e^T Omega e
            double s = 0.0;
            for(int r=0;r<3;r++){
                for(int c=0;c<3;c++){
                    s += e[r] * Omega[r][c] * e[c];
                }
            }
            cost += s;

            // Compute AtO = A^T Omega, BtO = B^T Omega
            double[][] AtO = new double[3][3];
            double[][] BtO = new double[3][3];

            for(int r=0;r<3;r++){
                for(int c=0;c<3;c++){
                    double sumA = 0.0;
                    double sumB = 0.0;
                    for(int k=0;k<3;k++){
                        sumA += A[k][r] * Omega[k][c];
                        sumB += B[k][r] * Omega[k][c];
                    }
                    AtO[r][c] = sumA;
                    BtO[r][c] = sumB;
                }
            }

            // Blocks Hii = AtO*A, Hij=AtO*B, Hji=BtO*A, Hjj=BtO*B
            double[][] Hii = new double[3][3];
            double[][] Hij = new double[3][3];
            double[][] Hji = new double[3][3];
            double[][] Hjj = new double[3][3];

            for(int r=0;r<3;r++){
                for(int c=0;c<3;c++){
                    double sHii=0, sHij=0, sHji=0, sHjj=0;
                    for(int k=0;k<3;k++){
                        sHii += AtO[r][k]*A[k][c];
                        sHij += AtO[r][k]*B[k][c];
                        sHji += BtO[r][k]*A[k][c];
                        sHjj += BtO[r][k]*B[k][c];
                    }
                    Hii[r][c]=sHii; Hij[r][c]=sHij; Hji[r][c]=sHji; Hjj[r][c]=sHjj;
                }
            }

            // gi = AtO*e, gj = BtO*e
            double[] gi = new double[3];
            double[] gj = new double[3];
            for(int r=0;r<3;r++){
                double sgi=0, sgj=0;
                for(int k=0;k<3;k++){
                    sgi += AtO[r][k]*e[k];
                    sgj += BtO[r][k]*e[k];
                }
                gi[r]=sgi; gj[r]=sgj;
            }

            int si = 3*i;
            int sj = 3*j;

            // Add blocks into H
            for(int r=0;r<3;r++){
                for(int c=0;c<3;c++){
                    H.set(si+r, si+c, H.get(si+r, si+c) + Hii[r][c]);
                    H.set(si+r, sj+c, H.get(si+r, sj+c) + Hij[r][c]);
                    H.set(sj+r, si+c, H.get(sj+r, si+c) + Hji[r][c]);
                    H.set(sj+r, sj+c, H.get(sj+r, sj+c) + Hjj[r][c]);
                }
            }

            // Add g
            for(int r=0;r<3;r++){
                g.set(si+r, 0, g.get(si+r, 0) + gi[r]);
                g.set(sj+r, 0, g.get(sj+r, 0) + gj[r]);
            }
        }

        // Anchor prior on node 0 at (0,0,0)
        double[][] Omega0 = new double[][]{
                {1e6,0,0},
                {0,1e6,0},
                {0,0,1e6}
        };
        double[] e0 = new double[]{x[0]-0.0, x[1]-0.0, wrapAngle(x[2]-0.0)};
        double s0 = 0.0;
        for(int r=0;r<3;r++){
            for(int c=0;c<3;c++){
                s0 += e0[r]*Omega0[r][c]*e0[c];
            }
        }
        cost += s0;

        for(int r=0;r<3;r++){
            for(int c=0;c<3;c++){
                H.set(r, c, H.get(r,c) + Omega0[r][c]);
            }
        }
        for(int r=0;r<3;r++){
            double gr=0.0;
            for(int k=0;k<3;k++) gr += Omega0[r][k]*e0[k];
            g.set(r,0, g.get(r,0) + gr);
        }

        return new NormalEq(H, g, cost);
    }

    static double rmseTranslation(double[] xEst, double[][] xGt) {
        int N = xGt.length;
        double s = 0.0;
        for (int k = 0; k < N; k++) {
            double dx = xEst[3*k]   - xGt[k][0];
            double dy = xEst[3*k+1] - xGt[k][1];
            s += dx*dx + dy*dy;
        }
        return Math.sqrt(s / N);
    }

    static double[][] generateGroundTruth(int N, double step, double turnRate) {
        double[][] x = new double[N][3];
        for(int k=1;k<N;k++){
            double[] u = new double[]{step, 0.0, turnRate};
            x[k] = se2Compose(x[k-1], u);
        }
        return x;
    }

    static double[] addNoise(double[] z, double sigT, double sigTh, Random rng) {
        double[] zn = z.clone();
        zn[0] += rng.nextGaussian()*sigT;
        zn[1] += rng.nextGaussian()*sigT;
        zn[2] = wrapAngle(zn[2] + rng.nextGaussian()*sigTh);
        return zn;
    }

    static class GraphInit {
        List<EdgeSE2> edges;
        double[] x0;
        GraphInit(List<EdgeSE2> edges, double[] x0) { this.edges=edges; this.x0=x0; }
    }

    static GraphInit buildSyntheticGraph(double[][] xGt, int numLoops, double outlierFrac, double outlierScale, long seed) {
        Random rng = new Random(seed);
        int N = xGt.length;

        double sigOdomT = 0.05, sigOdomTh = 0.02;
        double sigLoopT = 0.10, sigLoopTh = 0.05;

        double[][] OmegaOdom = infoFromSigmas(sigOdomT, sigOdomTh);
        double[][] OmegaLoop = infoFromSigmas(sigLoopT, sigLoopTh);

        List<EdgeSE2> edges = new ArrayList<>();

        // Odometry
        for(int i=0;i<N-1;i++){
            double[] xi = xGt[i];
            double[] xj = xGt[i+1];
            double[] z = se2Between(xi, xj);
            double[] zn = addNoise(z, sigOdomT, sigOdomTh, rng);
            edges.add(new EdgeSE2(i, i+1, zn, OmegaOdom, false));
        }

        // init by dead reckoning
        double[] x0 = new double[3*N];
        for(int i=0;i<N-1;i++){
            double[] xi = new double[]{x0[3*i], x0[3*i+1], x0[3*i+2]};
            double[] xip1 = se2Compose(xi, edges.get(i).z);
            x0[3*(i+1)]   = xip1[0];
            x0[3*(i+1)+1] = xip1[1];
            x0[3*(i+1)+2] = xip1[2];
        }

        // Loop candidates (i, j) with j > i+5
        List<int[]> cand = new ArrayList<>();
        for(int i=0;i<N;i++){
            for(int j=i+6;j<N;j++){
                cand.add(new int[]{i,j});
            }
        }
        Collections.shuffle(cand, rng);
        int K = Math.min(numLoops, cand.size());

        for(int k=0;k<K;k++){
            int i = cand.get(k)[0];
            int j = cand.get(k)[1];
            double[] z = se2Between(xGt[i], xGt[j]);
            double[] zn = addNoise(z, sigLoopT, sigLoopTh, rng);

            if (rng.nextDouble() < outlierFrac) {
                zn[0] += rng.nextGaussian() * outlierScale * sigLoopT;
                zn[1] += rng.nextGaussian() * outlierScale * sigLoopT;
                zn[2] = wrapAngle(zn[2] + rng.nextGaussian() * outlierScale * sigLoopTh);
            }

            edges.add(new EdgeSE2(i, j, zn, OmegaLoop, true));
        }

        return new GraphInit(edges, x0);
    }

    static double[] levenbergMarquardt(double[] xInit, int N, List<EdgeSE2> edges, int maxIters, double lambda0, double tol) {
        double[] x = xInit.clone();
        double lam = lambda0;

        for(int it=1; it<=maxIters; it++){
            NormalEq ne = buildNormalEquations(x, N, edges);
            SimpleMatrix H = ne.H;
            SimpleMatrix g = ne.g;
            double cost = ne.cost;

            // D = diag(H)
            SimpleMatrix D = new SimpleMatrix(3*N, 3*N);
            for(int k=0;k<3*N;k++){
                D.set(k,k, H.get(k,k));
            }

            SimpleMatrix Hlm = H.plus(D.scale(lam));

            // Solve Hlm dx = -g
            SimpleMatrix dx = Hlm.solve(g.negative());

            double[] xNew = x.clone();
            for(int k=0;k<3*N;k++){
                xNew[k] += dx.get(k,0);
            }
            for(int k=0;k<N;k++){
                xNew[3*k+2] = wrapAngle(xNew[3*k+2]);
            }

            double costNew = buildNormalEquations(xNew, N, edges).cost;

            if (costNew < cost) {
                x = xNew;
                lam = Math.max(lam/5.0, 1e-12);
            } else {
                lam = Math.min(lam*10.0, 1e12);
            }

            double stepNorm = dx.normF();
            System.out.printf("iter=%02d cost=%.6e costNew=%.6e lam=%.2e ||dx||=%.3e%n", it, cost, costNew, lam, stepNorm);

            if (stepNorm < tol) {
                System.out.println("Converged: step norm below tolerance.");
                break;
            }
        }

        return x;
    }

    public static void main(String[] args) {
        int N = 80;
        double[][] xGt = generateGroundTruth(N, 1.0, 0.03);

        GraphInit gi = buildSyntheticGraph(xGt, 20, 0.25, 8.0, 3L);
        List<EdgeSE2> edges = gi.edges;
        double[] x0 = gi.x0;

        long loops = edges.stream().filter(e -> e.isLoop).count();
        System.out.println("Graph: N=" + N + " edges=" + edges.size() + " (odom=" + (N-1) + ", loops=" + loops + ")");
        System.out.println("Initial RMSE (translation): " + rmseTranslation(x0, xGt));

        double[] xOpt = levenbergMarquardt(x0, N, edges, 30, 1e-3, 1e-6);

        System.out.println("Final RMSE (translation): " + rmseTranslation(xOpt, xGt));
    }
}
      

Chapter12_Lesson5.m

% Chapter12_Lesson5.m
% Lab: Build and Optimize a 2D Pose Graph (SE(2)) using Gauss-Newton / LM
%
% Requirements:
%   - MATLAB base (sparse matrices)
%
% Run:
%   Chapter12_Lesson5
%
% This script generates a synthetic trajectory, adds noisy odometry and loop closures (with outliers),
% anchors node 0, and optimizes using LM with sparse normal equations.

function Chapter12_Lesson5()

  N = 80;
  x_gt = generate_ground_truth(N, 1.0, 0.03);

  params.sig_odom_t = 0.05;
  params.sig_odom_th = 0.02;
  params.sig_loop_t = 0.10;
  params.sig_loop_th = 0.05;
  params.num_loops = 20;
  params.outlier_frac = 0.25;
  params.outlier_scale = 8.0;
  params.seed = 3;

  [edges, x0] = build_synthetic_graph(x_gt, params);

  fprintf('Graph: N=%d edges=%d (odom=%d, loops=%d)\\n', N, numel(edges), N-1, numel(edges)-(N-1));
  fprintf('Initial RMSE (translation): %.6f\\n', rmse_translation(x0, x_gt));

  [x_opt, costs] = levenberg_marquardt(x0, edges, N, 30, 1e-3, 1e-6);

  fprintf('Final RMSE (translation): %.6f\\n', rmse_translation(x_opt, x_gt));
  fprintf('First 10 costs: ');
  fprintf('%.3e ', costs(1:min(10,end)));
  fprintf('\\n');
end

% ----------------------------
% SE(2) utilities
% ----------------------------
function a = wrap_angle(a)
  a = mod(a + pi, 2*pi) - pi;
  if a <= -pi
    a = a + 2*pi;
  end
end

function M = R(theta)
  c = cos(theta); s = sin(theta);
  M = [c -s; s c];
end

function x = se2_compose(x1, x2)
  t1 = x1(1:2); th1 = x1(3);
  t2 = x2(1:2); th2 = x2(3);
  t = t1 + R(th1) * t2;
  th = wrap_angle(th1 + th2);
  x = [t; th];
end

function xinv = se2_inv(x)
  t = x(1:2); th = x(3);
  Rt = R(th)';
  tinv = -Rt * t;
  xinv = [tinv; wrap_angle(-th)];
end

function z = se2_between(xi, xj)
  z = se2_compose(se2_inv(xi), xj);
end

% ----------------------------
% Edge struct
% ----------------------------
function ed = make_edge(i, j, z, Omega, is_loop)
  ed.i = i;
  ed.j = j;
  ed.z = z;
  ed.Omega = Omega;
  ed.is_loop = is_loop;
end

function Omega = info_from_sigmas(sig_t, sig_th)
  Sigma = diag([sig_t^2, sig_t^2, sig_th^2]);
  Omega = inv(Sigma);
end

% ----------------------------
% Residual and Jacobians
% ----------------------------
function [e, A, B] = residual_and_jacobian(xi, xj, z_ij)
  ti = xi(1:2); thi = xi(3);
  tj = xj(1:2); thj = xj(3);

  RiT = R(thi)';
  d = tj - ti;

  et = RiT * d - z_ij(1:2);
  er = wrap_angle(thj - thi - z_ij(3));
  e = [et; er];

  S = [0 -1; 1 0];

  A = zeros(3,3);
  B = zeros(3,3);

  A(1:2,1:2) = -RiT;
  B(1:2,1:2) =  RiT;

  A_t_th = -(RiT * (S * d));
  A(1:2,3) = A_t_th;

  A(3,3) = -1;
  B(3,3) =  1;
end

% ----------------------------
% Normal equations
% ----------------------------
function [H, g, cost] = build_normal_equations(x, edges, N)
  dim = 3*N;
  H = spalloc(dim, dim, 36*numel(edges) + 9);
  g = zeros(dim, 1);
  cost = 0.0;

  for idx = 1:numel(edges)
    ed = edges(idx);
    i = ed.i; j = ed.j;

    xi = x(3*i+1:3*i+3);
    xj = x(3*j+1:3*j+3);

    [e, A, B] = residual_and_jacobian(xi, xj, ed.z);
    Omega = ed.Omega;

    cost = cost + e' * Omega * e;

    AtO = A' * Omega;
    BtO = B' * Omega;

    Hii = AtO * A;
    Hij = AtO * B;
    Hji = BtO * A;
    Hjj = BtO * B;

    gi = AtO * e;
    gj = BtO * e;

    si = 3*i+1;
    sj = 3*j+1;

    H(si:si+2, si:si+2) = H(si:si+2, si:si+2) + Hii;
    H(si:si+2, sj:sj+2) = H(si:si+2, sj:sj+2) + Hij;
    H(sj:sj+2, si:si+2) = H(sj:sj+2, si:si+2) + Hji;
    H(sj:sj+2, sj:sj+2) = H(sj:sj+2, sj:sj+2) + Hjj;

    g(si:si+2) = g(si:si+2) + gi;
    g(sj:sj+2) = g(sj:sj+2) + gj;
  end

  % Anchor prior on node 0 to remove gauge
  Omega0 = diag([1e6, 1e6, 1e6]);
  e0 = x(1:3) - [0;0;0];
  e0(3) = wrap_angle(e0(3));
  cost = cost + e0' * Omega0 * e0;

  H(1:3, 1:3) = H(1:3, 1:3) + Omega0;
  g(1:3) = g(1:3) + Omega0 * e0;
end

% ----------------------------
% LM
% ----------------------------
function [x, costs] = levenberg_marquardt(x0, edges, N, max_iters, lambda0, tol)
  x = x0;
  lam = lambda0;
  costs = zeros(max_iters,1);

  for it = 1:max_iters
    [H, g, cost] = build_normal_equations(x, edges, N);
    costs(it) = cost;

    D = spdiags(diag(H), 0, 3*N, 3*N);
    Hlm = H + lam * D;

    dx = - (Hlm \ g);

    x_new = x + dx;
    for k = 0:N-1
      x_new(3*k+3) = wrap_angle(x_new(3*k+3));
    end

    [~, ~, cost_new] = build_normal_equations(x_new, edges, N);

    if cost_new < cost
      x = x_new;
      lam = max(lam / 5.0, 1e-12);
    else
      lam = min(lam * 10.0, 1e12);
    end

    step_norm = norm(dx);
    fprintf('iter=%02d cost=%.6e cost_new=%.6e lam=%.2e ||dx||=%.3e\\n', it, cost, cost_new, lam, step_norm);

    if step_norm < tol
      fprintf('Converged: step norm below tolerance.\\n');
      costs = costs(1:it);
      return;
    end
  end
end

% ----------------------------
% Synthetic graph
% ----------------------------
function x_gt = generate_ground_truth(N, step, turn_rate)
  x_gt = zeros(N, 3);
  for k = 2:N
    u = [step; 0; turn_rate];
    x_gt(k,:) = se2_compose(x_gt(k-1,:)', u)';
  end
end

function zn = add_noise(z, sig_t, sig_th, rng)
  zn = z;
  zn(1) = zn(1) + randn(rng)*sig_t;
  zn(2) = zn(2) + randn(rng)*sig_t;
  zn(3) = wrap_angle(zn(3) + randn(rng)*sig_th);
end

function [edges, x0] = build_synthetic_graph(x_gt, params)
  rng = RandStream('mt19937ar','Seed', params.seed);
  N = size(x_gt,1);

  Omega_odom = info_from_sigmas(params.sig_odom_t, params.sig_odom_th);
  Omega_loop = info_from_sigmas(params.sig_loop_t, params.sig_loop_th);

  edges = repmat(make_edge(0,0,zeros(3,1), eye(3), false), 0, 1);

  % Odometry edges
  for i = 0:N-2
    z = se2_between(x_gt(i+1,:)', x_gt(i+2,:)');
    zn = add_noise(z, params.sig_odom_t, params.sig_odom_th, rng);
    edges(end+1) = make_edge(i, i+1, zn, Omega_odom, false);
  end

  % Dead reckoning init
  x0 = zeros(3*N, 1);
  for i = 0:N-2
    xi = x0(3*i+1:3*i+3);
    xip1 = se2_compose(xi, edges(i+1).z);
    x0(3*(i+1)+1:3*(i+1)+3) = xip1;
  end

  % Loop candidates
  cand = [];
  for i = 0:N-1
    for j = i+6:N-1
      cand = [cand; i j]; %#ok<AGROW>
    end
  end

  cand = cand(randperm(rng, size(cand,1)), :);
  K = min(params.num_loops, size(cand,1));

  for k = 1:K
    i = cand(k,1);
    j = cand(k,2);
    z = se2_between(x_gt(i+1,:)', x_gt(j+1,:)');
    zn = add_noise(z, params.sig_loop_t, params.sig_loop_th, rng);

    if rand(rng) < params.outlier_frac
      zn(1:2) = zn(1:2) + randn(rng,2,1) * (params.outlier_scale * params.sig_loop_t);
      zn(3) = wrap_angle(zn(3) + randn(rng) * (params.outlier_scale * params.sig_loop_th));
    end

    edges(end+1) = make_edge(i, j, zn, Omega_loop, true);
  end
end

function r = rmse_translation(x_est, x_gt)
  N = size(x_gt,1);
  xe = reshape(x_est, 3, N)';
  d = xe(:,1:2) - x_gt(:,1:2);
  r = sqrt(mean(sum(d.^2,2)));
end
      

Chapter12_Lesson5.nb

(* Chapter12_Lesson5.nb *)
(* Lab: Build and Optimize a 2D Pose Graph (SE(2)) in Wolfram Language.
   This is a plain-text Notebook expression. Open in Mathematica, then evaluate cells. *)

Notebook[{
 Cell[BoxData[
  RowBox[{"(*", " ", 
   RowBox[{"Chapter12_Lesson5", ".", "nb"}], " ", "*)"}]], "Input"],
 Cell[BoxData[
  RowBox[{"ClearAll", "[", "\"Global`*\"", "]"}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"wrapAngle", "[", "a_", "]"}], ":=", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", "x", "}"}], ",", 
     RowBox[{
      RowBox[{"x", "=", 
       RowBox[{"Mod", "[", 
        RowBox[{
         RowBox[{"a", "+", "Pi"}], ",", 
         RowBox[{"2", "Pi"}]}], "]"}]}], ";", 
      RowBox[{"x", "=", 
       RowBox[{"x", "-", "Pi"}]}], ";", 
      RowBox[{"If", "[", 
       RowBox[{
        RowBox[{"x", "\[LessEqual]", 
         RowBox[{"-", "Pi"}]}], ",", 
        RowBox[{"x", "=", 
         RowBox[{"x", "+", 
          RowBox[{"2", "Pi"}]}]}]}], "]"}], ";", "x"}]}], "]"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"R2", "[", "\[Theta]_", "]"}], ":=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Cos", "[", "\[Theta]", "]"}], ",", 
       RowBox[{"-", 
        RowBox[{"Sin", "[", "\[Theta]", "]"}]}]}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Sin", "[", "\[Theta]", "]"}], ",", 
       RowBox[{"Cos", "[", "\[Theta]", "]"}]}], "}"}]}], "}"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"se2Compose", "[", 
    RowBox[{"x1_", ",", "x2_"}], "]"}], ":=", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"t1", ",", "th1", ",", "t2", ",", "th2", ",", "t", ",", "th"}], 
      "}"}], ",", 
     RowBox[{
      RowBox[{"t1", "=", 
       RowBox[{"x1", "[", 
        RowBox[{"[", 
         RowBox[{"1", ";;", "2"}], "]"}], "]"}]}], ";", 
      RowBox[{"th1", "=", 
       RowBox[{"x1", "[", 
        RowBox[{"[", "3", "]"}], "]"}]}], ";", 
      RowBox[{"t2", "=", 
       RowBox[{"x2", "[", 
        RowBox[{"[", 
         RowBox[{"1", ";;", "2"}], "]"}], "]"}]}], ";", 
      RowBox[{"th2", "=", 
       RowBox[{"x2", "[", 
        RowBox[{"[", "3", "]"}], "]"}]}], ";", 
      RowBox[{"t", "=", 
       RowBox[{"t1", "+", 
        RowBox[{
         RowBox[{"R2", "[", "th1", "]"}], ".", "t2"}]}]}], ";", 
      RowBox[{"th", "=", 
       RowBox[{"wrapAngle", "[", 
        RowBox[{"th1", "+", "th2"}], "]"}]}], ";", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"t", "[", 
         RowBox[{"[", "1", "]"}], "]"}], ",", 
        RowBox[{"t", "[", 
         RowBox[{"[", "2", "]"}], "]"}], ",", "th"}], "}"}]}]}], "]"}]}]], 
  "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"se2Inv", "[", "x_", "]"}], ":=", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"t", ",", "th", ",", "Rt", ",", "tinv"}], "}"}], ",", 
     RowBox[{
      RowBox[{"t", "=", 
       RowBox[{"x", "[", 
        RowBox[{"[", 
         RowBox[{"1", ";;", "2"}], "]"}], "]"}]}], ";", 
      RowBox[{"th", "=", 
       RowBox[{"x", "[", 
        RowBox[{"[", "3", "]"}], "]"}]}], ";", 
      RowBox[{"Rt", "=", 
       RowBox[{"Transpose", "[", 
        RowBox[{"R2", "[", "th", "]"}], "]"}]}], ";", 
      RowBox[{"tinv", "=", 
       RowBox[{"-", 
        RowBox[{"Rt", ".", "t"}]}]}], ";", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"tinv", "[", 
         RowBox[{"[", "1", "]"}], "]"}], ",", 
        RowBox[{"tinv", "[", 
         RowBox[{"[", "2", "]"}], "]"}], ",", 
        RowBox[{"wrapAngle", "[", 
         RowBox[{"-", "th"}], "]"}]}], "}"}]}]}], "]"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"se2Between", "[", 
    RowBox[{"xi_", ",", "xj_"}], "]"}], ":=", 
   RowBox[{"se2Compose", "[", 
    RowBox[{
     RowBox[{"se2Inv", "[", "xi", "]"}], ",", "xj"}], "]"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"infoFromSigmas", "[", 
    RowBox[{"sigT_", ",", "sigTh_"}], "]"}], ":=", 
   RowBox[{"Inverse", "[", 
    RowBox[{"DiagonalMatrix", "[", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"sigT", "^", "2"}], ",", 
       RowBox[{"sigT", "^", "2"}], ",", 
       RowBox[{"sigTh", "^", "2"}]}], "}"}], "]"}], "]"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"resJac", "[", 
    RowBox[{"xi_", ",", "xj_", ",", "zij_"}], "]"}], ":=", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
      "ti", ",", "thi", ",", "tj", ",", "thj", ",", "RiT", ",", "d", ",", 
       "et", ",", "er", ",", "e", ",", "S", ",", "A", ",", "B"}], "}"}], ",", 
     RowBox[{
      RowBox[{"ti", "=", 
       RowBox[{"xi", "[", 
        RowBox[{"[", 
         RowBox[{"1", ";;", "2"}], "]"}], "]"}]}], ";", 
      RowBox[{"thi", "=", 
       RowBox[{"xi", "[", 
        RowBox[{"[", "3", "]"}], "]"}]}], ";", 
      RowBox[{"tj", "=", 
       RowBox[{"xj", "[", 
        RowBox[{"[", 
         RowBox[{"1", ";;", "2"}], "]"}], "]"}]}], ";", 
      RowBox[{"thj", "=", 
       RowBox[{"xj", "[", 
        RowBox[{"[", "3", "]"}], "]"}]}], ";", 
      RowBox[{"RiT", "=", 
       RowBox[{"Transpose", "[", 
        RowBox[{"R2", "[", "thi", "]"}], "]"}]}], ";", 
      RowBox[{"d", "=", 
       RowBox[{"tj", "-", "ti"}]}], ";", 
      RowBox[{"et", "=", 
       RowBox[{
        RowBox[{"RiT", ".", "d"}], "-", 
        RowBox[{"zij", "[", 
         RowBox[{"[", 
          RowBox[{"1", ";;", "2"}], "]"}], "]"}]}]}], ";", 
      RowBox[{"er", "=", 
       RowBox[{"wrapAngle", "[", 
        RowBox[{"thj", "-", "thi", "-", 
         RowBox[{"zij", "[", 
          RowBox[{"[", "3", "]"}], "]"}]}], "]"}]}], ";", 
      RowBox[{"e", "=", 
       RowBox[{"Join", "[", 
        RowBox[{"et", ",", 
         RowBox[{"{", "er", "}"}]}], "]"}]}], ";", 
      RowBox[{"S", "=", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0"}], "}"}]}], "}"}]}], ";", 
      RowBox[{"A", "=", 
       RowBox[{"ConstantArray", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"3", ",", "3"}], "}"}]}], "]"}]}], ";", 
      RowBox[{"B", "=", 
       RowBox[{"ConstantArray", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"3", ",", "3"}], "}"}]}], "]"}]}], ";", 
      RowBox[{"A", "[", 
       RowBox[{"[", 
        RowBox[{
         RowBox[{"1", ";;", "2"}], ",", 
         RowBox[{"1", ";;", "2"}]}], "]"}], "]"}], "=", 
      RowBox[{"-", "RiT"}], ";", 
      RowBox[{"B", "[", 
       RowBox[{"[", 
        RowBox[{
         RowBox[{"1", ";;", "2"}], ",", 
         RowBox[{"1", ";;", "2"}]}], "]"}], "]"}], "=", "RiT", ";", 
      RowBox[{"A", "[", 
       RowBox[{"[", 
        RowBox[{
         RowBox[{"1", ";;", "2"}], ",", "3"}], "]"}], "]"}], "=", 
      RowBox[{"-", 
       RowBox[{"(", 
        RowBox[{"RiT", ".", 
         RowBox[{"S", ".", "d"}]}], ")"}]}], ";", 
      RowBox[{"A", "[", 
       RowBox[{"[", 
        RowBox[{"3", ",", "3"}], "]"}], "]"}], "=", 
      RowBox[{"-", "1"}], ";", 
      RowBox[{"B", "[", 
       RowBox[{"[", 
        RowBox[{"3", ",", "3"}], "]"}], "]"}], "=", "1", ";", 
      RowBox[{"{", 
       RowBox[{"e", ",", "A", ",", "B"}], "}"}]}]}], "]"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"generateGroundTruth", "[", 
    RowBox[{"N_", ",", "step_", ",", "turnRate_"}], "]"}], ":=", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", "x", "}"}], ",", 
     RowBox[{
      RowBox[{"x", "=", 
       RowBox[{"ConstantArray", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.", ",", "0.", ",", "0."}], "}"}], ",", "N"}], "]"}]}], 
      ";", 
      RowBox[{"Do", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"x", "[", 
          RowBox[{"[", "k", "]"}], "]"}], "=", 
         RowBox[{"se2Compose", "[", 
          RowBox[{
           RowBox[{"x", "[", 
            RowBox[{"[", 
             RowBox[{"k", "-", "1"}], "]"}], "]"}], ",", 
           RowBox[{"{", 
            RowBox[{"step", ",", "0.", ",", "turnRate"}], "}"}]}], "]"}]}], 
        ",", 
        RowBox[{"{", 
         RowBox[{"k", ",", "2", ",", "N"}], "}"}]}], "]"}], ";", "x"}]}], 
    "]"}]}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"(*", " ", 
    RowBox[{
     RowBox[{"A", " ", "compact", " ", "LM", " ", "loop", " ", "is", " ", 
      "included", " ", "in", " ", "the", " ", "Python"}], "/", "C", "++", "/", 
     RowBox[{"MATLAB", " ", "versions"}]}], " ", "*)"}]], "Input"],
 Cell[BoxData[
  RowBox[{
   RowBox[{"N", "=", "80"}], ";", 
   RowBox[{"xgt", "=", 
    RowBox[{"generateGroundTruth", "[", 
     RowBox[{"N", ",", "1.0", ",", "0.03"}], "]"}]}], ";", 
   RowBox[{"Print", "[", 
    RowBox[{"\"N=\",", "N"}], "]"}], ";"}]], "Input"]
},
WindowSize->{1200, 800},
WindowMargins->{ {Automatic, 0}, {Automatic, 0} },
FrontEndVersion->"13.3 for Microsoft Windows (64-bit) (June 3, 2023)",
StyleDefinitions->"Default.nb"
]
      

Problem 1 (Gauge and Singularity): Consider a pose graph with only relative constraints and no absolute prior. Prove that the Hessian \( \mathbf{H} \) is singular (has at least 3 zero modes) in 2D.

Solution:

Let a fixed global transform \( \mathbf{T}_\Delta \in SE(2) \) act on every pose as \( \mathbf{T}'_i=\mathbf{T}_\Delta\mathbf{T}_i \). Then each predicted relative transform is invariant: \( (\mathbf{T}'_i)^{-1}\mathbf{T}'_j = \mathbf{T}_i^{-1}\mathbf{T}_j \). Hence every residual \( \mathbf{e}_{ij}(\mathbf{x}) \) and the total objective \( F(\mathbf{x}) \) are unchanged. Therefore the cost has a 3-dimensional flat direction (2 translations + 1 rotation). Differentiating at any point along those infinitesimal directions yields \( \mathbf{H}\mathbf{v}=\mathbf{0} \) for three independent stacked vectors \( \mathbf{v} \). Thus \( \mathbf{H} \) is singular with nullity at least 3.

Problem 2 (Jacobian Blocks): Derive the Jacobian blocks \( \mathbf{A}_{ij} \) and \( \mathbf{B}_{ij} \) for the residual used in this lesson.

Solution:

Let \( \mathbf{d}=\mathbf{t}_j-\mathbf{t}_i \), \( \mathbf{R}_i=\mathbf{R}(\theta_i) \), and \( \mathbf{r}_t=\mathbf{R}_i^\top\mathbf{d}-\mathbf{t}_{ij} \). Then \( \partial\mathbf{r}_t/\partial\mathbf{t}_i=-\mathbf{R}_i^\top \) and \( \partial\mathbf{r}_t/\partial\mathbf{t}_j=\mathbf{R}_i^\top \). With \( \mathbf{S}=\begin{bmatrix}0&-1\\1&0\end{bmatrix} \), \( \partial(\mathbf{R}_i^\top\mathbf{d})/\partial\theta_i = -\mathbf{R}_i^\top\mathbf{S}\mathbf{d} \). The angle residual \( r_\theta=\mathrm{wrap}(\theta_j-\theta_i-\theta_{ij}) \) has local derivatives \( \partial r_\theta/\partial\theta_i=-1 \), \( \partial r_\theta/\partial\theta_j=1 \). Combining gives:

\[ \mathbf{A}_{ij} = \begin{bmatrix} -\mathbf{R}_i^\top & -\mathbf{R}_i^\top\mathbf{S}(\mathbf{t}_j-\mathbf{t}_i) \\ \mathbf{0}^\top & -1 \end{bmatrix},\quad \mathbf{B}_{ij} = \begin{bmatrix} \mathbf{R}_i^\top & 0 \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

Problem 3 (LM as Regularization): Show that solving \( (\mathbf{H}+\lambda\mathbf{I})\delta\mathbf{x}=-\mathbf{g} \) minimizes a regularized quadratic model.

Solution:

Consider the GN quadratic model \( Q(\delta\mathbf{x})=\tfrac{1}{2}\delta\mathbf{x}^\top\mathbf{H}\delta\mathbf{x}+\mathbf{g}^\top\delta\mathbf{x}+c \). Add Tikhonov regularization: \( Q_\lambda(\delta\mathbf{x})=Q(\delta\mathbf{x})+\tfrac{\lambda}{2}\|\delta\mathbf{x}\|^2 \). Setting the gradient to zero yields \( (\mathbf{H}+\lambda\mathbf{I})\delta\mathbf{x}=-\mathbf{g} \). Thus damping penalizes large steps when the linearization is unreliable.

Problem 4 (Outlier Dominance in Least Squares): Suppose one loop closure has residual magnitude \( k \) times larger than typical edges (with similar information scaling). Quantify how it can dominate the objective and explain why robust kernels reduce its influence.

Solution:

In least squares, each edge contributes roughly \( s=\mathbf{e}^\top\mathbf{\Omega}\mathbf{e} \), which scales quadratically with residual magnitude. If typical residual scale is \( \sigma \) and the outlier scale is \( k\sigma \), then its contribution is about \( k^2 \) times a typical edge. For large \( k \), the optimizer can shift the solution to reduce the outlier, even if that worsens many inliers.

Robust kernels use \( \rho(s) \) instead of \( s \). The effective weight in the normal equations becomes \( w(s)=\rho'(s) \). For kernels where \( \rho'(s) \) decreases when \( s \) is large (e.g., Huber transitions to linear growth), the outlier gets downweighted, preventing domination of the objective.

You constructed a full pose-graph lab pipeline: generate edges, assemble a sparse GN/LM system, handle gauge freedom via anchoring, iterate to convergence, and evaluate performance. The provided implementations are explicit about residuals and Jacobians so you can extend them with robust kernels and real front-ends.