A pose graph has unknown robot poses \( \mathbf{x}_i \in \mathbb{R}^3 \) (2D position + heading) at keyframes \( i=0,1,\dots,N-1 \), and relative-pose measurements on edges \( (i,j) \in \mathcal{E} \). Each edge stores a measurement \( \mathbf{z}_{ij} \) and uncertainty.

We adopt the standard conditional independence structure: given all poses, measurements are independent, so the likelihood factors as \( p(\mathcal{Z}\mid\mathcal{X})=\prod_{(i,j)\in\mathcal{E} } p(\mathbf{z}_{ij}\mid \mathbf{x}_i,\mathbf{x}_j) \). Under a zero-mean Gaussian noise model on a residual vector \( \mathbf{e}_{ij} \),

\[ \mathbf{e}_{ij}(\mathcal{X}) \sim \mathcal{N}(\mathbf{0},\,\boldsymbol{\Sigma}_{ij}), \quad p(\mathbf{z}_{ij}\mid \mathbf{x}_i,\mathbf{x}_j) \propto \exp\Big(-\tfrac{1}{2}\,\mathbf{e}_{ij}(\mathcal{X})^\top \boldsymbol{\Omega}_{ij}\, \mathbf{e}_{ij}(\mathcal{X})\Big), \quad \boldsymbol{\Omega}_{ij} = \boldsymbol{\Sigma}_{ij}^{-1}. \]

If we assume a (possibly weak) prior on one pose to fix the global reference frame, the MAP estimate is

\[ \mathcal{X}^\star \;\in\; \arg\max_{\mathcal{X} } \; p(\mathcal{X})\, p(\mathcal{Z}\mid\mathcal{X}) \;\equiv\; \arg\min_{\mathcal{X} } \; \sum_{(i,j)\in\mathcal{E} } \mathbf{e}_{ij}(\mathcal{X})^\top\boldsymbol{\Omega}_{ij}\mathbf{e}_{ij}(\mathcal{X}) \; + \; \text{(prior terms)}. \]

Key consequence: Gaussian measurement noise turns the MAP objective into a weighted nonlinear least squares problem with one residual per edge. This is the computational core of graph-based SLAM.

Each pose is parameterized as \( \mathbf{x}_i = [x_i,\,y_i,\,\theta_i]^\top \). Let \( \mathbf{t}_i=[x_i,y_i]^\top \) and rotation \( \mathbf{R}(\theta)=\begin{bmatrix}\cos\theta & -\sin\theta\\ \sin\theta & \cos\theta\end{bmatrix} \). The predicted relative transform from pose \( i \) to pose \( j \) is

\[ \hat{\mathbf{z} }_{ij}(\mathbf{x}_i,\mathbf{x}_j) = \begin{bmatrix} \mathbf{R}(\theta_i)^\top\,(\mathbf{t}_j-\mathbf{t}_i)\\ \mathrm{wrap}(\theta_j-\theta_i) \end{bmatrix}. \]

A common “between-factor” error compares the predicted relative pose to the measured one \( \mathbf{z}_{ij}=[\Delta x,\Delta y,\Delta\theta]^\top \). Using a minimal translation+angle approximation (sufficient for teaching Gauss–Newton),

\[ \mathbf{e}_{ij}(\mathbf{x}_i,\mathbf{x}_j) = \begin{bmatrix} \mathbf{R}(\Delta\theta)^\top\Big( \mathbf{R}(\theta_i)^\top(\mathbf{t}_j-\mathbf{t}_i) - \begin{bmatrix}\Delta x\\\Delta y\end{bmatrix} \Big)\\ \mathrm{wrap}\big((\theta_j-\theta_i) - \Delta\theta\big) \end{bmatrix}. \]

The information matrix \( \boldsymbol{\Omega}_{ij} \) weights translation and rotation according to sensor confidence (wheel odometry vs scan matching vs visual loop closure).

Reminder (next lesson): in real data, some loop closures are wrong (outliers). In Lesson 4 we replace the quadratic penalty with robust kernels, but here we keep the pure least-squares form.

flowchart TD
  A["Inputs: nodes (poses) and edges (relative constraints)"] --> B["Choose initial guess (integrate odometry)"]
  B --> C["Define residual per edge: compare predicted relative pose to measured"]
  C --> D["Build weighted sum of squared residuals"]
  D --> E["Iterative optimizer updates poses to reduce total error"]
  E --> F["Output: globally consistent trajectory (and covariances if computed)"]
        

Stack all pose parameters in a vector \( \mathbf{x} \in \mathbb{R}^{3N} \). The objective is \( F(\mathbf{x})=\sum_{(i,j)\in\mathcal{E} } \|\mathbf{e}_{ij}(\mathbf{x})\|_{\boldsymbol{\Omega}_{ij} }^2 \), where \( \|\mathbf{v}\|_{\Omega}^2 = \mathbf{v}^\top\Omega\mathbf{v} \). At an iterate \( \mathbf{x} \), consider an increment \( \delta\mathbf{x} \). First-order Taylor expansion gives

\[ \mathbf{e}_{ij}(\mathbf{x}+\delta\mathbf{x}) \approx \mathbf{e}_{ij}(\mathbf{x}) + \mathbf{J}_{ij}\,\delta\mathbf{x}, \quad \mathbf{J}_{ij} = \frac{\partial \mathbf{e}_{ij} }{\partial \mathbf{x} }\Big|_{\mathbf{x} }. \]

Plugging into the cost and dropping higher-order terms yields a quadratic model:

\[ \min_{\delta\mathbf{x} } \;\sum_{(i,j)\in\mathcal{E} } \big\|\mathbf{e}_{ij} + \mathbf{J}_{ij}\,\delta\mathbf{x}\big\|_{\boldsymbol{\Omega}_{ij} }^2 \;\equiv\; \min_{\delta\mathbf{x} } \; \tfrac{1}{2}\delta\mathbf{x}^\top\mathbf{H}\,\delta\mathbf{x} + \delta\mathbf{x}^\top\mathbf{g} + \text{const}. \]

where the Gauss–Newton Hessian approximation and gradient are sums of edge contributions:

\[ \mathbf{H} = \sum_{(i,j)} \mathbf{J}_{ij}^\top\boldsymbol{\Omega}_{ij}\mathbf{J}_{ij}, \quad \mathbf{g} = \sum_{(i,j)} \mathbf{J}_{ij}^\top\boldsymbol{\Omega}_{ij}\mathbf{e}_{ij}. \]

Setting the derivative to zero yields the normal equations:

\[ \mathbf{H}\,\delta\mathbf{x} = -\mathbf{g}. \]

Sparsity: each residual depends only on two poses \( (i,j) \), so \( \mathbf{J}_{ij} \) has nonzeros only in the blocks for those poses. Therefore \( \mathbf{H} \) is sparse with a block pattern that matches the graph adjacency. This is why sparse Cholesky (or conjugate gradients with preconditioning) scales well to large maps.

Gauge freedom (important): if there is no prior, shifting and rotating all poses together leaves every relative measurement unchanged, so \( F(\mathbf{x}) \) is invariant. In 2D, this creates a 3-dimensional nullspace (global x, y, yaw), so \( \mathbf{H} \) is singular. Fix it by anchoring one pose, or adding a weak prior.

flowchart TD
  S["Start with x0 (initial poses)"] --> L["Linearize residuals around current x"]
  L --> N["Assemble sparse H and g (sum of edge blocks)"]
  N --> SOL["Solve: H * dx = -g (sparse Cholesky or CG)"]
  SOL --> UPD["Update poses: x = x + dx"]
  UPD --> CHK["Stop if norm(dx) small or cost decreases slowly"]
  CHK -->|continue| L
  CHK -->|stop| OUT["Return optimized poses"]
  

Gauss–Newton can diverge if the initial guess is poor or if some constraints are very nonlinear. A classic stabilization is Levenberg–Marquardt (LM), which interpolates between gradient descent and Gauss–Newton by adding a damping term:

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

When \( \lambda \) is large, the step resembles gradient descent; when small, it approaches Gauss–Newton. In practice, \( \lambda \) is adapted using a ratio between actual and predicted cost decrease (trust-region interpretation).

Stopping criteria (examples) use absolute and relative thresholds: \( \|\delta\mathbf{x}\| < \varepsilon_{dx} \), or \( |F_{k+1}-F_k|/F_k < \varepsilon_F \), or a maximum iteration cap.

Numerical issues: (i) normalize angles with \( \mathrm{wrap}(\cdot) \); (ii) use consistent units (meters vs radians); (iii) avoid ill-conditioned information matrices; (iv) ensure a gauge fix (anchor/prior) so the linear system is solvable.

Connection to software stacks: in ROS/SLAM systems, back-end pose-graph solvers appear in packages such as Ceres Solver, g2o, and GTSAM; they implement sparse linear algebra, robust kernels, and incremental updates. Our implementations below show the core math “from scratch”.

This script builds a synthetic pose graph (odometry chain + one loop closure), anchors pose 0, assembles sparse normal equations, and runs Gauss–Newton iterations. It uses \( \texttt{scipy.sparse} \) for sparse solves.

Chapter12_Lesson3.py

#!/usr/bin/env python3
# Chapter12_Lesson3.py
# Pose Graph Optimization in SE(2) via Gauss-Newton (from scratch, educational)
#
# Dependencies:
#   numpy
#   scipy  (sparse linear algebra)
#
# Run:
#   python Chapter12_Lesson3.py

import numpy as np
from dataclasses import dataclass
from typing import List, Tuple
from scipy.sparse import lil_matrix, csr_matrix
from scipy.sparse.linalg import spsolve

def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return (a + np.pi) % (2.0 * np.pi) - np.pi

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

@dataclass
class EdgeSE2:
    i: int
    j: int
    z: np.ndarray      # measurement [dx, dy, dtheta] from i to j (in i-frame)
    Omega: np.ndarray  # information matrix (3x3), inverse of covariance

def predict_relative(xi: np.ndarray, xj: np.ndarray) -> np.ndarray:
    """
    Predicted relative pose from i to j:
      zhat = inv(xi) ⊕ xj
    where pose x = [x, y, theta].
    """
    ti = xi[:2]
    tj = xj[:2]
    Ri = rot2(xi[2])
    dt = Ri.T @ (tj - ti)
    dtheta = wrap_angle(xj[2] - xi[2])
    return np.array([dt[0], dt[1], dtheta], dtype=float)

def se2_error(xi: np.ndarray, xj: np.ndarray, z: np.ndarray) -> np.ndarray:
    """
    Simple SE(2) 'between' error:
      e = inv(z) ⊕ (inv(xi) ⊕ xj)
    implemented using a minimal (translation + angle) approximation.
    """
    zhat = predict_relative(xi, xj)
    Rz = rot2(z[2])
    terr = Rz.T @ (zhat[:2] - z[:2])
    therr = wrap_angle(zhat[2] - z[2])
    return np.array([terr[0], terr[1], therr], dtype=float)

def se2_jacobians(xi: np.ndarray, xj: np.ndarray, z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    """
    Analytic Jacobians for the simple between error used in se2_error.

    e_t = Rz^T ( Ri^T (t_j - t_i) - t_z )
    e_th = (theta_j - theta_i) - theta_z  (wrapped)

    Returns (A, B) where:
      A = de/dxi  (3x3), B = de/dxj  (3x3)
    """
    ti = xi[:2]
    tj = xj[:2]
    Ri = rot2(xi[2])
    Rz = rot2(z[2])

    # A2 = Rz^T Ri^T
    A2 = Rz.T @ Ri.T

    # skew-symmetric matrix for 2D cross product
    S = np.array([[0.0, -1.0],
                  [1.0,  0.0]])

    dt = (tj - ti)

    A = np.zeros((3, 3), dtype=float)
    B = np.zeros((3, 3), dtype=float)

    # translation derivatives
    A[0:2, 0:2] = -A2
    B[0:2, 0:2] =  A2

    # derivative wrt theta_i affects translation part via Ri^T
    # d(Ri^T dt)/dtheta_i = -Ri^T S dt
    dti_dtheta = Rz.T @ (-Ri.T @ (S @ dt))
    A[0:2, 2] = dti_dtheta

    # theta derivatives
    A[2, 2] = -1.0
    B[2, 2] =  1.0

    return A, B

def build_normal_equations(
    X: np.ndarray,
    edges: List[EdgeSE2],
    anchor_first_pose: bool = True
) -> Tuple[csr_matrix, np.ndarray, float]:
    """
    Build sparse normal equations H dx = -b for Gauss-Newton.

    If anchor_first_pose=True, pose 0 is held fixed (gauge fixing) and variables
    are only for poses 1..N-1.
    """
    N = X.shape[0]
    dim = 3 * (N - 1) if anchor_first_pose else 3 * N

    H = lil_matrix((dim, dim), dtype=float)
    b = np.zeros(dim, dtype=float)
    chi2 = 0.0

    def idx(pose_id: int) -> int:
        if anchor_first_pose:
            return 3 * (pose_id - 1)  # pose 1 -> 0
        return 3 * pose_id

    for e in edges:
        xi = X[e.i]
        xj = X[e.j]
        r = se2_error(xi, xj, e.z)
        A, B = se2_jacobians(xi, xj, e.z)

        Omega = e.Omega
        chi2 += float(r.T @ Omega @ r)

        # local blocks
        AtOmega = A.T @ Omega
        BtOmega = B.T @ Omega
        Hii = AtOmega @ A
        Hij = AtOmega @ B
        Hji = BtOmega @ A
        Hjj = BtOmega @ B
        bi = AtOmega @ r
        bj = BtOmega @ r

        # scatter-add into global H and b, skipping anchored pose if needed
        for (p, q, Hpq) in [(e.i, e.i, Hii), (e.i, e.j, Hij), (e.j, e.i, Hji), (e.j, e.j, Hjj)]:
            if anchor_first_pose and (p == 0 or q == 0):
                continue
            ip = idx(p)
            iq = idx(q)
            for a in range(3):
                for bb in range(3):
                    H[ip + a, iq + bb] += Hpq[a, bb]

        # b terms
        if not (anchor_first_pose and e.i == 0):
            ii = idx(e.i)
            b[ii:ii+3] += bi
        if not (anchor_first_pose and e.j == 0):
            jj = idx(e.j)
            b[jj:jj+3] += bj

    return H.tocsr(), b, chi2

def apply_increment(X: np.ndarray, dx: np.ndarray, anchor_first_pose: bool = True) -> np.ndarray:
    """Apply increments to poses; theta updated additively and wrapped."""
    X_new = X.copy()
    if anchor_first_pose:
        for k in range(1, X.shape[0]):
            d = dx[3*(k-1):3*(k-1)+3]
            X_new[k, 0] += d[0]
            X_new[k, 1] += d[1]
            X_new[k, 2] = wrap_angle(X_new[k, 2] + d[2])
    else:
        for k in range(X.shape[0]):
            d = dx[3*k:3*k+3]
            X_new[k, 0] += d[0]
            X_new[k, 1] += d[1]
            X_new[k, 2] = wrap_angle(X_new[k, 2] + d[2])
    return X_new

def gauss_newton(
    X0: np.ndarray,
    edges: List[EdgeSE2],
    iters: int = 10,
    damping: float = 0.0,
    anchor_first_pose: bool = True
) -> np.ndarray:
    X = X0.copy()
    for it in range(iters):
        H, b, chi2 = build_normal_equations(X, edges, anchor_first_pose=anchor_first_pose)
        if damping > 0.0:
            H = H + damping * csr_matrix(np.eye(H.shape[0]))

        dx = spsolve(H, -b)
        X = apply_increment(X, dx, anchor_first_pose=anchor_first_pose)

        print(f"iter={it:02d}  chi2={chi2:.6f}  |dx|={np.linalg.norm(dx):.3e}")

        if np.linalg.norm(dx) < 1e-8:
            break
    return X

def make_synthetic_pose_graph(N: int = 20, noise_xy: float = 0.02, noise_th: float = 0.01):
    """
    Build a simple chain with one loop-closure edge.
    Ground truth: gentle arc.
    Measurements: relative odometry edges (i,i+1) + loop edge (0,N-1).
    """
    X_true = np.zeros((N, 3), dtype=float)
    for k in range(1, N):
        X_true[k, 0] = X_true[k-1, 0] + 0.5 * np.cos(0.1 * k)
        X_true[k, 1] = X_true[k-1, 1] + 0.5 * np.sin(0.1 * k)
        X_true[k, 2] = wrap_angle(0.05 * k)

    edges = []
    Sigma = np.diag([noise_xy**2, noise_xy**2, noise_th**2])
    Omega = np.linalg.inv(Sigma)

    rng = np.random.default_rng(7)

    for k in range(N-1):
        z = predict_relative(X_true[k], X_true[k+1])
        z_noisy = z + rng.normal(0.0, [noise_xy, noise_xy, noise_th])
        z_noisy[2] = wrap_angle(z_noisy[2])
        edges.append(EdgeSE2(i=k, j=k+1, z=z_noisy, Omega=Omega))

    zlc = predict_relative(X_true[0], X_true[N-1])
    zlc_noisy = zlc + rng.normal(0.0, [noise_xy, noise_xy, noise_th])
    zlc_noisy[2] = wrap_angle(zlc_noisy[2])
    edges.append(EdgeSE2(i=0, j=N-1, z=zlc_noisy, Omega=Omega))

    X0 = np.zeros_like(X_true)
    for k in range(N-1):
        z = edges[k].z
        Rk = rot2(X0[k,2])
        X0[k+1,:2] = X0[k,:2] + (Rk @ z[:2])
        X0[k+1,2] = wrap_angle(X0[k,2] + z[2])

    return X0, edges

def main():
    X0, edges = make_synthetic_pose_graph(N=30)
    print("Optimizing pose graph (Gauss-Newton, anchored pose 0)...")
    X_opt = gauss_newton(X0, edges, iters=15, damping=0.0, anchor_first_pose=True)

    print("\nFirst 5 poses (x,y,theta) initial vs optimized:")
    for k in range(5):
        print(f"k={k:02d}  X0={X0[k]}  Xopt={X_opt[k]}")

if __name__ == "__main__":
    main()
      

Related robotics libraries (Python): GTSAM (Python wrapper) provides factor graphs and efficient optimization; SciPy is used here for clarity.

Below is a compact C++ implementation using Eigen sparse matrices and sparse Cholesky. For production, consider Ceres Solver (LM + robust loss) or g2o/GTSAM.

Chapter12_Lesson3.cpp

// Chapter12_Lesson3.cpp
// Pose Graph Optimization in SE(2) via Gauss-Newton (Eigen, sparse)
// Build (Linux/Mac):
//   g++ -O2 -std=c++17 Chapter12_Lesson3.cpp -I /path/to/eigen -o pg
// Run:
//   ./pg
//
// Notes:
// - This is an educational, minimal implementation (dense edge loop, sparse H).
// - For production, use Ceres/g2o/GTSAM and robust loss (next lesson).

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

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

using Vec3 = Eigen::Matrix<double, 3, 1>;
using Mat3 = Eigen::Matrix<double, 3, 3>;
using Vec2 = Eigen::Matrix<double, 2, 1>;
using Mat2 = Eigen::Matrix<double, 2, 2>;

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

static Mat2 R2(double th) {
  double c = std::cos(th), s = std::sin(th);
  Mat2 R;
  R << c, -s,
       s,  c;
  return R;
}

struct Edge {
  int i, j;
  Vec3 z;     // [dx, dy, dtheta] in i-frame
  Mat3 Omega; // information
};

static Vec3 predict_relative(const Vec3& xi, const Vec3& xj) {
  Vec2 ti(xi(0), xi(1));
  Vec2 tj(xj(0), xj(1));
  Mat2 Ri = R2(xi(2));
  Vec2 dt = Ri.transpose() * (tj - ti);
  double dth = wrap_angle(xj(2) - xi(2));
  return Vec3(dt(0), dt(1), dth);
}

static Vec3 se2_error(const Vec3& xi, const Vec3& xj, const Vec3& z) {
  Vec3 zhat = predict_relative(xi, xj);
  Mat2 Rz = R2(z(2));
  Vec2 terr = Rz.transpose() * (Vec2(zhat(0), zhat(1)) - Vec2(z(0), z(1)));
  double therr = wrap_angle(zhat(2) - z(2));
  return Vec3(terr(0), terr(1), therr);
}

static void jacobians(const Vec3& xi, const Vec3& xj, const Vec3& z, Mat3& A, Mat3& B) {
  Vec2 ti(xi(0), xi(1));
  Vec2 tj(xj(0), xj(1));
  Mat2 Ri = R2(xi(2));
  Mat2 Rz = R2(z(2));

  Mat2 A2 = Rz.transpose() * Ri.transpose();

  Mat2 S;
  S << 0.0, -1.0,
       1.0,  0.0;

  Vec2 dt = (tj - ti);

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

  A.block<2,2>(0,0) = -A2;
  B.block<2,2>(0,0) =  A2;

  Vec2 dti_dtheta = Rz.transpose() * (-Ri.transpose() * (S * dt));
  A(0,2) = dti_dtheta(0);
  A(1,2) = dti_dtheta(1);

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

static void make_synthetic(int N, std::vector<Vec3>& X0, std::vector<Edge>& edges) {
  std::vector<Vec3> Xtrue(N, Vec3::Zero());

  for (int k = 1; k < N; ++k) {
    Xtrue = Xtrue + 0.5 * std::cos(0.1 * k);
    Xtrue = Xtrue + 0.5 * std::sin(0.1 * k);
    Xtrue = wrap_angle(0.05 * k);
  }

  double noise_xy = 0.02;
  double noise_th = 0.01;

  Mat3 Sigma = Mat3::Zero();
  Sigma(0,0) = noise_xy * noise_xy;
  Sigma(1,1) = noise_xy * noise_xy;
  Sigma(2,2) = noise_th * noise_th;
  Mat3 Omega = Sigma.inverse();

  std::mt19937 rng(7);
  std::normal_distribution<double> n01(0.0, 1.0);

  edges.clear();
  edges.reserve(N);

  for (int k = 0; k < N-1; ++k) {
    Vec3 z = predict_relative(Xtrue[k], Xtrue[k+1]);
    Vec3 zn;
    zn(0) = z(0) + noise_xy * n01(rng);
    zn(1) = z(1) + noise_xy * n01(rng);
    zn(2) = wrap_angle(z(2) + noise_th * n01(rng));
    edges.push_back(Edge{k, k+1, zn, Omega});
  }

  // loop closure
  Vec3 zlc = predict_relative(Xtrue[0], Xtrue[N-1]);
  Vec3 zlc_n;
  zlc_n(0) = zlc(0) + noise_xy * n01(rng);
  zlc_n(1) = zlc(1) + noise_xy * n01(rng);
  zlc_n(2) = wrap_angle(zlc(2) + noise_th * n01(rng));
  edges.push_back(Edge{0, N-1, zlc_n, Omega});

  // Initial guess by chaining odometry
  X0.assign(N, Vec3::Zero());
  for (int k = 0; k < N-1; ++k) {
    const Vec3& z = edges[k].z;
    Mat2 Rk = R2(X0);
    Vec2 next_t = Vec2(X0, X0) + Rk * Vec2(z(0), z(1));
    X0 = next_t(0);
    X0 = next_t(1);
    X0 = wrap_angle(X0 + z(2));
  }
}

static double build_normal_equations(
  const std::vector<Vec3>& X,
  const std::vector<Edge>& edges,
  Eigen::SparseMatrix<double>& H,
  Eigen::VectorXd& b,
  bool anchor_first_pose = true
) {
  int N = (int)X.size();
  int dim = anchor_first_pose ? 3*(N-1) : 3*N;

  std::vector<Eigen::Triplet<double>> trips;
  trips.reserve(edges.size() * 3 * 3 * 4);

  b.setZero(dim);
  double chi2 = 0.0;

  auto idx = [anchor_first_pose](int pose_id) -> int {
    if (anchor_first_pose) return 3*(pose_id - 1);
    return 3*pose_id;
  };

  for (const auto& e : edges) {
    const Vec3& xi = X[e.i];
    const Vec3& xj = X[e.j];

    Vec3 r = se2_error(xi, xj, e.z);
    Mat3 A, Bm;
    jacobians(xi, xj, e.z, A, Bm);

    chi2 += r.transpose() * e.Omega * r;

    Mat3 AtOmega = A.transpose() * e.Omega;
    Mat3 BtOmega = Bm.transpose() * e.Omega;

    Mat3 Hii = AtOmega * A;
    Mat3 Hij = AtOmega * Bm;
    Mat3 Hji = BtOmega * A;
    Mat3 Hjj = BtOmega * Bm;

    Vec3 bi = AtOmega * r;
    Vec3 bj = BtOmega * r;

    auto addBlock = [&trips](int rp, int cp, const Mat3& M) {
      for (int r = 0; r < 3; ++r)
        for (int c = 0; c < 3; ++c)
          trips.emplace_back(rp + r, cp + c, M(r,c));
    };

    // scatter H blocks (skip anchored pose 0)
    if (!(anchor_first_pose && e.i == 0)) {
      int ii = idx(e.i);
      // Hii
      if (!(anchor_first_pose && e.i == 0)) addBlock(ii, ii, Hii);
      // bi
      b.segment<3>(ii) += bi;
    }
    if (!(anchor_first_pose && e.j == 0)) {
      int jj = idx(e.j);
      // Hjj
      addBlock(jj, jj, Hjj);
      // bj
      b.segment<3>(jj) += bj;
    }

    // cross terms
    if (!(anchor_first_pose && (e.i == 0 || e.j == 0))) {
      int ii = idx(e.i);
      int jj = idx(e.j);
      addBlock(ii, jj, Hij);
      addBlock(jj, ii, Hji);
    }
  }

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

static void apply_increment(std::vector<Vec3>& X, const Eigen::VectorXd& dx, bool anchor_first_pose=true) {
  int N = (int)X.size();
  if (anchor_first_pose) {
    for (int k = 1; k < N; ++k) {
      Vec3 d = dx.segment<3>(3*(k-1));
      X += d(0);
      X += d(1);
      X = wrap_angle(X + d(2));
    }
  } else {
    for (int k = 0; k < N; ++k) {
      Vec3 d = dx.segment<3>(3*k);
      X += d(0);
      X += d(1);
      X = wrap_angle(X + d(2));
    }
  }
}

static std::vector<Vec3> gauss_newton(std::vector<Vec3> X, const std::vector<Edge>& edges, int iters=10, double damping=0.0) {
  for (int it = 0; it < iters; ++it) {
    Eigen::SparseMatrix<double> H;
    Eigen::VectorXd b;
    b.setZero(3*((int)X.size()-1));

    double chi2 = build_normal_equations(X, edges, H, b, true);

    if (damping > 0.0) {
      Eigen::SparseMatrix<double> I(H.rows(), H.cols());
      I.setIdentity();
      H = H + damping * I;
    }

    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    solver.compute(H);
    if (solver.info() != Eigen::Success) {
      std::cerr << "Factorization failed" << std::endl;
      break;
    }

    Eigen::VectorXd dx = solver.solve(-b);
    if (solver.info() != Eigen::Success) {
      std::cerr << "Solve failed" << std::endl;
      break;
    }

    double ndx = dx.norm();
    std::cout << "iter=" << it << "  chi2=" << chi2 << "  |dx|=" << ndx << std::endl;

    apply_increment(X, dx, true);
    if (ndx < 1e-8) break;
  }
  return X;
}

int main() {
  std::vector<Vec3> X0;
  std::vector<Edge> edges;

  make_synthetic(30, X0, edges);

  std::cout << "Optimizing pose graph (Gauss-Newton, anchored pose 0)..." << std::endl;
  auto Xopt = gauss_newton(X0, edges, 15, 0.0);

  std::cout << "\nFirst 5 poses (x,y,theta) initial vs optimized:" << std::endl;
  for (int k = 0; k < 5; ++k) {
    std::cout << "k=" << k
              << "  X0=[" << X0[k].transpose() << "]"
              << "  Xopt=[" << Xopt[k].transpose() << "]"
              << std::endl;
  }
  return 0;
}
      

Related robotics libraries (C++): Ceres Solver, g2o, and GTSAM are widely used back-ends for pose-graph optimization.

Java ecosystems commonly use EJML for linear algebra. For large sparse graphs, a JVM solution often relies on native bindings or specialized sparse packages. The example below is intentionally small and dense for readability.

Chapter12_Lesson3.java

/* Chapter12_Lesson3.java
 * Pose Graph Optimization in SE(2) via Gauss-Newton (dense, educational)
 *
 * Dependencies:
 *   EJML (Efficient Java Matrix Library)
 *
 * Gradle (example):
 *   implementation 'org.ejml:ejml-all:0.43'
 *
 * Run:
 *   javac -cp ejml-all.jar Chapter12_Lesson3.java
 *   java  -cp .:ejml-all.jar Chapter12_Lesson3
 */

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

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

public class Chapter12_Lesson3 {

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

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

    static class Edge {
        int i, j;
        double[] z;         // [dx, dy, dtheta]
        double[][] Omega;   // 3x3 information matrix
        Edge(int i, int j, double[] z, double[][] Omega) {
            this.i = i; this.j = j; this.z = z; this.Omega = Omega;
        }
    }

    static double[] predictRelative(double[] xi, double[] xj) {
        double[] ti = new double[]{xi[0], xi[1]};
        double[] tj = new double[]{xj[0], xj[1]};
        double[][] Ri = R2(xi[2]);
        // dt = Ri^T (tj - ti)
        double dx = tj[0] - ti[0];
        double dy = tj[1] - ti[1];
        double dt0 = Ri[0][0]*dx + Ri[1][0]*dy;
        double dt1 = Ri[0][1]*dx + Ri[1][1]*dy;
        double dth = wrapAngle(xj[2] - xi[2]);
        return new double[]{dt0, dt1, dth};
    }

    static double[] se2Error(double[] xi, double[] xj, double[] z) {
        double[] zhat = predictRelative(xi, xj);
        double[][] Rz = R2(z[2]);

        double ex = zhat[0] - z[0];
        double ey = zhat[1] - z[1];

        // terr = Rz^T (zhat_t - z_t)
        double terr0 = Rz[0][0]*ex + Rz[1][0]*ey;
        double terr1 = Rz[0][1]*ex + Rz[1][1]*ey;

        double therr = wrapAngle(zhat[2] - z[2]);
        return new double[]{terr0, terr1, therr};
    }

    static void jacobians(double[] xi, double[] xj, double[] z, double[][] A, double[][] B) {
        // A,B are 3x3
        double[] ti = new double[]{xi[0], xi[1]};
        double[] tj = new double[]{xj[0], xj[1]};
        double[][] Ri = R2(xi[2]);
        double[][] Rz = R2(z[2]);

        // A2 = Rz^T Ri^T
        double[][] A2 = new double[2][2];
        // Ri^T
        double[][] Rit = new double[][]{ {Ri[0][0], Ri[1][0]}, {Ri[0][1], Ri[1][1]} };
        // Rz^T
        double[][] Rzt = new double[][]{ {Rz[0][0], Rz[1][0]}, {Rz[0][1], Rz[1][1]} };
        // A2 = Rzt * Rit
        A2[0][0] = Rzt[0][0]*Rit[0][0] + Rzt[0][1]*Rit[1][0];
        A2[0][1] = Rzt[0][0]*Rit[0][1] + Rzt[0][1]*Rit[1][1];
        A2[1][0] = Rzt[1][0]*Rit[0][0] + Rzt[1][1]*Rit[1][0];
        A2[1][1] = Rzt[1][0]*Rit[0][1] + Rzt[1][1]*Rit[1][1];

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

        // -Ri^T S dt
        double s0 = S[0][0]*dx + S[0][1]*dy; // -dy
        double s1 = S[1][0]*dx + S[1][1]*dy; // dx
        // Ri^T * (S dt)
        double r0 = Rit[0][0]*s0 + Rit[0][1]*s1;
        double r1 = Rit[1][0]*s0 + Rit[1][1]*s1;
        // negate
        r0 = -r0; r1 = -r1;

        // dti_dtheta = Rz^T * r
        double d0 = Rzt[0][0]*r0 + Rzt[0][1]*r1;
        double d1 = Rzt[1][0]*r0 + Rzt[1][1]*r1;

        // zero A,B
        for (int r = 0; r < 3; r++) {
            for (int c = 0; c < 3; c++) { A[r][c] = 0.0; B[r][c] = 0.0; }
        }

        // translation blocks
        A[0][0] = -A2[0][0]; A[0][1] = -A2[0][1];
        A[1][0] = -A2[1][0]; A[1][1] = -A2[1][1];

        B[0][0] =  A2[0][0]; B[0][1] =  A2[0][1];
        B[1][0] =  A2[1][0]; B[1][1] =  A2[1][1];

        // theta_i effect on translation
        A[0][2] = d0;
        A[1][2] = d1;

        // theta derivatives
        A[2][2] = -1.0;
        B[2][2] =  1.0;
    }

    static class SyntheticResult {
        double[][] X0;
        List<Edge> edges;
        SyntheticResult(double[][] X0, List<Edge> edges) { this.X0 = X0; this.edges = edges; }
    }

    static SyntheticResult makeSynthetic(int N) {
        double[][] Xtrue = new double[N][3];
        for (int k = 1; k < N; k++) {
            Xtrue[k][0] = Xtrue[k-1][0] + 0.5 * Math.cos(0.1 * k);
            Xtrue[k][1] = Xtrue[k-1][1] + 0.5 * Math.sin(0.1 * k);
            Xtrue[k][2] = wrapAngle(0.05 * k);
        }

        double noiseXY = 0.02;
        double noiseTh = 0.01;

        double[][] Sigma = new double[][]{
                {noiseXY*noiseXY, 0, 0},
                {0, noiseXY*noiseXY, 0},
                {0, 0, noiseTh*noiseTh}
        };
        // Omega = inv(Sigma) (diagonal)
        double[][] Omega = new double[][]{
                {1.0/Sigma[0][0], 0, 0},
                {0, 1.0/Sigma[1][1], 0},
                {0, 0, 1.0/Sigma[2][2]}
        };

        Random rng = new Random(7);
        List<Edge> edges = new ArrayList<>();

        for (int k = 0; k < N-1; k++) {
            double[] z = predictRelative(Xtrue[k], Xtrue[k+1]);
            double[] zn = new double[]{
                    z[0] + noiseXY * rng.nextGaussian(),
                    z[1] + noiseXY * rng.nextGaussian(),
                    wrapAngle(z[2] + noiseTh * rng.nextGaussian())
            };
            edges.add(new Edge(k, k+1, zn, Omega));
        }

        double[] zlc = predictRelative(Xtrue[0], Xtrue[N-1]);
        double[] zlcN = new double[]{
                zlc[0] + noiseXY * rng.nextGaussian(),
                zlc[1] + noiseXY * rng.nextGaussian(),
                wrapAngle(zlc[2] + noiseTh * rng.nextGaussian())
        };
        edges.add(new Edge(0, N-1, zlcN, Omega));

        // Initial guess by chaining odometry edges
        double[][] X0 = new double[N][3];
        for (int k = 0; k < N-1; k++) {
            double[] z = edges.get(k).z;
            double[][] Rk = R2(X0[k][2]);
            double nx = X0[k][0] + (Rk[0][0]*z[0] + Rk[0][1]*z[1]);
            double ny = X0[k][1] + (Rk[1][0]*z[0] + Rk[1][1]*z[1]);
            X0[k+1][0] = nx;
            X0[k+1][1] = ny;
            X0[k+1][2] = wrapAngle(X0[k][2] + z[2]);
        }
        return new SyntheticResult(X0, edges);
    }

    static double buildNormalEquationsDense(double[][] X, List<Edge> edges, DMatrixRMaj H, DMatrixRMaj b) {
        int N = X.length;
        int dim = 3*(N-1); // anchor pose 0
        CommonOps_DDRM.fill(H, 0.0);
        CommonOps_DDRM.fill(b, 0.0);

        double chi2 = 0.0;

        for (Edge e : edges) {
            double[] xi = X[e.i];
            double[] xj = X[e.j];

            double[] r = se2Error(xi, xj, e.z);
            chi2 += r[0]*(e.Omega[0][0]*r[0]) + r[1]*(e.Omega[1][1]*r[1]) + r[2]*(e.Omega[2][2]*r[2]);

            double[][] A = new double[3][3];
            double[][] Bm = new double[3][3];
            jacobians(xi, xj, e.z, A, Bm);

            // H blocks and b blocks: J^T Omega J, J^T Omega r
            // Since Omega is diagonal here (synthetic), simplify multiplications.
            double o0 = e.Omega[0][0], o1 = e.Omega[1][1], o2 = e.Omega[2][2];

            // helper: M = J^T Omega J
            double[][] Hii = new double[3][3];
            double[][] Hij = new double[3][3];
            double[][] Hji = new double[3][3];
            double[][] Hjj = new double[3][3];

            double[] bi = new double[3];
            double[] bj = new double[3];

            for (int a = 0; a < 3; a++) {
                // b = J^T Omega r
                bi[a] = A[0][a]*o0*r[0] + A[1][a]*o1*r[1] + A[2][a]*o2*r[2];
                bj[a] = Bm[0][a]*o0*r[0] + Bm[1][a]*o1*r[1] + Bm[2][a]*o2*r[2];
                for (int c = 0; c < 3; c++) {
                    Hii[a][c] = A[0][a]*o0*A[0][c] + A[1][a]*o1*A[1][c] + A[2][a]*o2*A[2][c];
                    Hij[a][c] = A[0][a]*o0*Bm[0][c] + A[1][a]*o1*Bm[1][c] + A[2][a]*o2*Bm[2][c];
                    Hji[a][c] = Bm[0][a]*o0*A[0][c] + Bm[1][a]*o1*A[1][c] + Bm[2][a]*o2*A[2][c];
                    Hjj[a][c] = Bm[0][a]*o0*Bm[0][c] + Bm[1][a]*o1*Bm[1][c] + Bm[2][a]*o2*Bm[2][c];
                }
            }

            // scatter-add, skipping anchored pose 0
            if (e.i != 0) {
                int ii = 3*(e.i - 1);
                for (int r0i = 0; r0i < 3; r0i++) {
                    b.add(ii + r0i, 0, bi[r0i]);
                    for (int c0i = 0; c0i < 3; c0i++) {
                        H.add(ii + r0i, ii + c0i, Hii[r0i][c0i]);
                    }
                }
            }
            if (e.j != 0) {
                int jj = 3*(e.j - 1);
                for (int r0j = 0; r0j < 3; r0j++) {
                    b.add(jj + r0j, 0, bj[r0j]);
                    for (int c0j = 0; c0j < 3; c0j++) {
                        H.add(jj + r0j, jj + c0j, Hjj[r0j][c0j]);
                    }
                }
            }
            if (e.i != 0 && e.j != 0) {
                int ii = 3*(e.i - 1);
                int jj = 3*(e.j - 1);
                for (int r0 = 0; r0 < 3; r0++) {
                    for (int c0 = 0; c0 < 3; c0++) {
                        H.add(ii + r0, jj + c0, Hij[r0][c0]);
                        H.add(jj + r0, ii + c0, Hji[r0][c0]);
                    }
                }
            }
        }
        return chi2;
    }

    static void applyIncrement(double[][] X, DMatrixRMaj dx) {
        int N = X.length;
        for (int k = 1; k < N; k++) {
            int off = 3*(k-1);
            X[k][0] += dx.get(off, 0);
            X[k][1] += dx.get(off+1, 0);
            X[k][2] = wrapAngle(X[k][2] + dx.get(off+2, 0));
        }
    }

    static double[][] gaussNewton(double[][] X0, List<Edge> edges, int iters) {
        int N = X0.length;
        int dim = 3*(N-1);
        double[][] X = new double[N][3];
        for (int i = 0; i < N; i++) System.arraycopy(X0[i], 0, X[i], 0, 3);

        DMatrixRMaj H = new DMatrixRMaj(dim, dim);
        DMatrixRMaj b = new DMatrixRMaj(dim, 1);
        DMatrixRMaj dx = new DMatrixRMaj(dim, 1);

        for (int it = 0; it < iters; it++) {
            double chi2 = buildNormalEquationsDense(X, edges, H, b);

            // Solve H dx = -b (dense solve)
            DMatrixRMaj minusB = b.copy();
            CommonOps_DDRM.scale(-1.0, minusB);

            // dx = inv(H)*(-b) using linear solver
            // (EJML will choose a decomposition internally via CommonOps solve)
            CommonOps_DDRM.solve(H, minusB, dx);

            double ndx = 0.0;
            for (int k = 0; k < dim; k++) ndx += dx.get(k,0)*dx.get(k,0);
            ndx = Math.sqrt(ndx);

            System.out.printf("iter=%02d  chi2=%.6f  |dx|=%.3e%n", it, chi2, ndx);

            applyIncrement(X, dx);
            if (ndx < 1e-8) break;
        }
        return X;
    }

    public static void main(String[] args) {
        SyntheticResult syn = makeSynthetic(20);
        double[][] X0 = syn.X0;
        List<Edge> edges = syn.edges;

        System.out.println("Optimizing pose graph (dense GN, anchored pose 0)...");
        double[][] Xopt = gaussNewton(X0, edges, 12);

        System.out.println("\nFirst 5 poses (x,y,theta) initial vs optimized:");
        for (int k = 0; k < 5; k++) {
            System.out.printf("k=%02d  X0=[%.4f %.4f %.4f]  Xopt=[%.4f %.4f %.4f]%n",
                    k,
                    X0[k][0], X0[k][1], X0[k][2],
                    Xopt[k][0], Xopt[k][1], Xopt[k][2]
            );
        }
    }
}
      

MATLAB is convenient for prototyping sparse least squares. The script below uses sparse assembly and the backslash operator for the linear solve. A small Simulink skeleton is optionally created to show how a GN “step” could be wrapped into a MATLAB Function block inside a larger SLAM pipeline.

Chapter12_Lesson3.m

% Chapter12_Lesson3.m
% Pose Graph Optimization in SE(2) via Gauss-Newton (sparse MATLAB prototype)
%
% Run:
%   Chapter12_Lesson3
%
% Notes:
% - Educational implementation: sparse normal equations, anchored pose 1 (MATLAB indexing).
% - For production: use g2o/ceres/gtsam style solvers (C++), or MATLAB toolboxes if available.

function Chapter12_Lesson3()
  rng(7);

  % Build synthetic pose graph
  N = 30;
  [X0, edges] = make_synthetic(N);

  fprintf('Optimizing pose graph (Gauss-Newton, anchored pose 1)...\n');
  Xopt = gauss_newton(X0, edges, 15);

  fprintf('\nFirst 5 poses initial vs optimized:\n');
  for k = 1:5
    fprintf('k=%02d  X0=[%.4f %.4f %.4f]  Xopt=[%.4f %.4f %.4f]\n', ...
      k-1, X0(k,1), X0(k,2), X0(k,3), Xopt(k,1), Xopt(k,2), Xopt(k,3));
  end

  % Optional: create a tiny Simulink skeleton (safe to comment out if undesired)
  % build_simulink_skeleton();
end

function a = wrap_angle(a)
  a = mod(a + pi, 2*pi);
  a(a < 0) = a(a < 0) + 2*pi;
  a = a - pi;
end

function R = rot2(th)
  c = cos(th); s = sin(th);
  R = [c -s; s c];
end

function zhat = predict_relative(xi, xj)
  ti = xi(1:2);
  tj = xj(1:2);
  Ri = rot2(xi(3));
  dt = Ri'*(tj-ti);
  dth = wrap_angle(xj(3)-xi(3));
  zhat = [dt; dth];
end

function e = se2_error(xi, xj, z)
  zhat = predict_relative(xi, xj);
  Rz = rot2(z(3));
  terr = Rz'*(zhat(1:2)-z(1:2));
  therr = wrap_angle(zhat(3)-z(3));
  e = [terr; therr];
end

function [A,B] = se2_jacobians(xi, xj, z)
  ti = xi(1:2);
  tj = xj(1:2);
  Ri = rot2(xi(3));
  Rz = rot2(z(3));
  A2 = Rz'*Ri';
  S = [0 -1; 1 0];
  dt = (tj-ti);

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

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

  dti_dtheta = Rz'*(-Ri'*(S*dt));
  A(1:2,3) = dti_dtheta;

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

function [X0, edges] = make_synthetic(N)
  Xtrue = zeros(N,3);
  for k = 2:N
    kk = k-1;
    Xtrue(k,1) = Xtrue(k-1,1) + 0.5*cos(0.1*kk);
    Xtrue(k,2) = Xtrue(k-1,2) + 0.5*sin(0.1*kk);
    Xtrue(k,3) = wrap_angle(0.05*kk);
  end

  noise_xy = 0.02;
  noise_th = 0.01;

  Sigma = diag([noise_xy^2, noise_xy^2, noise_th^2]);
  Omega = inv(Sigma);

  edges = struct('i',{},'j',{},'z',{},'Omega',{});
  idx = 1;

  for k = 1:(N-1)
    z = predict_relative(Xtrue(k,:), Xtrue(k+1,:));
    zn = z + [noise_xy*randn; noise_xy*randn; noise_th*randn];
    zn(3) = wrap_angle(zn(3));
    edges(idx).i = k; edges(idx).j = k+1; edges(idx).z = zn; edges(idx).Omega = Omega;
    idx = idx + 1;
  end

  zlc = predict_relative(Xtrue(1,:), Xtrue(N,:));
  zlc_n = zlc + [noise_xy*randn; noise_xy*randn; noise_th*randn];
  zlc_n(3) = wrap_angle(zlc_n(3));
  edges(idx).i = 1; edges(idx).j = N; edges(idx).z = zlc_n; edges(idx).Omega = Omega;

  % Initial guess by chaining odometry
  X0 = zeros(N,3);
  for k = 1:(N-1)
    z = edges(k).z;
    Rk = rot2(X0(k,3));
    X0(k+1,1:2) = X0(k,1:2) + (Rk*z(1:2))';
    X0(k+1,3) = wrap_angle(X0(k,3) + z(3));
  end
end

function X = gauss_newton(X0, edges, iters)
  X = X0;
  N = size(X,1);
  dim = 3*(N-1); % anchor pose 1
  for it = 1:iters
    [H,b,chi2] = build_normal_equations(X, edges);
    dx = - (H \ b); %#ok<MINV>

    X = apply_increment(X, dx);

    fprintf('iter=%02d  chi2=%.6f  |dx|=%.3e\n', it-1, chi2, norm(dx));
    if norm(dx) < 1e-8
      break;
    end
  end
end

function [H,b,chi2] = build_normal_equations(X, edges)
  N = size(X,1);
  dim = 3*(N-1);
  H = spalloc(dim, dim, 200*dim);
  b = zeros(dim,1);
  chi2 = 0;

  for k = 1:numel(edges)
    ei = edges(k).i;
    ej = edges(k).j;

    xi = X(ei,:)';
    xj = X(ej,:)';

    r = se2_error(xi, xj, edges(k).z);
    [A,B] = se2_jacobians(xi, xj, edges(k).z);
    Omega = edges(k).Omega;

    chi2 = chi2 + r'*Omega*r;

    Hii = A'*Omega*A;
    Hij = A'*Omega*B;
    Hji = B'*Omega*A;
    Hjj = B'*Omega*B;
    bi  = A'*Omega*r;
    bj  = B'*Omega*r;

    if ei ~= 1
      ii = 3*(ei-2) + (1:3);
      H(ii,ii) = H(ii,ii) + Hii;
      b(ii) = b(ii) + bi;
    end
    if ej ~= 1
      jj = 3*(ej-2) + (1:3);
      H(jj,jj) = H(jj,jj) + Hjj;
      b(jj) = b(jj) + bj;
    end
    if (ei ~= 1) && (ej ~= 1)
      ii = 3*(ei-2) + (1:3);
      jj = 3*(ej-2) + (1:3);
      H(ii,jj) = H(ii,jj) + Hij;
      H(jj,ii) = H(jj,ii) + Hji;
    end
  end
end

function X = apply_increment(X, dx)
  N = size(X,1);
  for k = 2:N
    off = 3*(k-2);
    X(k,1) = X(k,1) + dx(off+1);
    X(k,2) = X(k,2) + dx(off+2);
    X(k,3) = wrap_angle(X(k,3) + dx(off+3));
  end
end

function build_simulink_skeleton()
  % Creates a minimal Simulink model with a MATLAB Function block placeholder.
  mdl = 'PoseGraphGN_Skeleton';
  if bdIsLoaded(mdl); close_system(mdl,0); end
  new_system(mdl); open_system(mdl);

  add_block('simulink/Sources/Constant', [mdl '/x0'], 'Position', [30 30 90 60]);
  add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/GN_Step'], 'Position', [150 20 280 80]);
  add_block('simulink/Sinks/Display', [mdl '/x_new'], 'Position', [340 30 400 60]);

  add_line(mdl, 'x0/1', 'GN_Step/1');
  add_line(mdl, 'GN_Step/1', 'x_new/1');

  save_system(mdl);
end
      

Mathematica is useful to verify Jacobians symbolically and to prototype the Gauss–Newton structure. The notebook below derives \( \partial \mathbf{e}/\partial \mathbf{x}_i \) and \( \partial \mathbf{e}/\partial \mathbf{x}_j \), then runs a small dense GN solver.

Chapter12_Lesson3.nb

(* Chapter12_Lesson3.nb (text export)
   Pose Graph Optimization in SE(2) via Gauss-Newton (Mathematica notebook-style)
   Goal: verify Jacobians symbolically for the simplified between-error used in the lesson,
         then run a small dense GN solver on a toy graph.

   Usage: paste into a Mathematica notebook and evaluate cells.
*)

ClearAll[wrapAngle, R2, predictRelative, se2Error, jacobians, gnSolve];

wrapAngle[a_] := Module[{x = Mod[a + Pi, 2 Pi]},
  If[x < 0, x = x + 2 Pi];
  x - Pi
];

R2[th_] := { {Cos[th], -Sin[th]}, {Sin[th], Cos[th]} };

predictRelative[xi_, xj_] := Module[{ti, tj, Ri, dt, dth},
  ti = xi[[1 ;; 2]];
  tj = xj[[1 ;; 2]];
  Ri = R2[xi[[3]]];
  dt = Transpose[Ri].(tj - ti);
  dth = wrapAngle[xj[[3]] - xi[[3]]];
  {dt[[1]], dt[[2]], dth}
];

se2Error[xi_, xj_, z_] := Module[{zhat, Rz, terr, therr},
  zhat = predictRelative[xi, xj];
  Rz = R2[z[[3]]];
  terr = Transpose[Rz].({zhat[[1]], zhat[[2]]} - {z[[1]], z[[2]]});
  therr = wrapAngle[zhat[[3]] - z[[3]]];
  {terr[[1]], terr[[2]], therr}
];

(* Symbolic Jacobians for the simplified error:
   e_t = Rz^T ( Ri^T (t_j - t_i) - t_z )
   e_th = (th_j - th_i) - th_z (wrapped later)
*)
jacobians[] := Module[{xi, xj, z, e, A, B},
  xi = {xi1, xi2, xi3};
  xj = {xj1, xj2, xj3};
  z  = {zx, zy, zth};

  e = se2Error[xi, xj, z];

  A = D[e, { {xi1, xi2, xi3} }] // Simplify;
  B = D[e, { {xj1, xj2, xj3} }] // Simplify;

  {A, B}
];

Print["Symbolic Jacobians (A=d e / d xi, B=d e / d xj):"];
{Aexpr, Bexpr} = jacobians[];
Aexpr // MatrixForm
Bexpr // MatrixForm

(* Build a tiny pose graph: chain + loop closure, then run dense GN.
   This is for demonstration only (dense H). For larger graphs, use sparse methods.
*)

gnSolve[X0_, edges_, iters_: 10, damping_: 0] := Module[
  {X = X0, N, dim, it, H, b, chi2, dx, e, A, B, Omega, ii, jj, idx},
  N = Length[X];
  dim = 3 (N - 1); (* anchor pose 1 (Mathematica indexing) *)
  idx[p_] := 3 (p - 2) + {1, 2, 3};

  For[it = 1, it <= iters, it++,
    H = ConstantArray[0, {dim, dim}];
    b = ConstantArray[0, dim];
    chi2 = 0;

    Do[
      (* edge = {i,j,z,Omega} with i,j in 1..N *)
      With[{i = ed[[1]], j = ed[[2]], z = ed[[3]], OmegaEd = ed[[4]]},
        e = se2Error[X[[i]], X[[j]], z];
        (* numeric Jacobians using analytic formulas would be faster; here we use finite differences for brevity *)
        (* For teaching: keep it clear. *)
        A = N[D[se2Error[{xi1, xi2, xi3}, X[[j]], z], { {xi1, xi2, xi3} }] /. {xi1 -> X[[i, 1]], xi2 -> X[[i, 2]], xi3 -> X[[i, 3]]}];
        B = N[D[se2Error[X[[i]], {xj1, xj2, xj3}, z], { {xj1, xj2, xj3} }] /. {xj1 -> X[[j, 1]], xj2 -> X[[j, 2]], xj3 -> X[[j, 3]]}];

        chi2 += e . (OmegaEd . e);

        (* assemble blocks, skipping anchor i=1 *)
        If[i != 1,
          ii = idx[i];
          H[[ii, ii]] += Transpose[A].OmegaEd.A;
          b[[ii]] += Transpose[A].OmegaEd.e;
        ];
        If[j != 1,
          jj = idx[j];
          H[[jj, jj]] += Transpose[B].OmegaEd.B;
          b[[jj]] += Transpose[B].OmegaEd.e;
        ];
        If[i != 1 && j != 1,
          ii = idx[i]; jj = idx[j];
          H[[ii, jj]] += Transpose[A].OmegaEd.B;
          H[[jj, ii]] += Transpose[B].OmegaEd.A;
        ];
      ],
      {ed, edges}
    ];

    If[damping > 0,
      H = H + damping IdentityMatrix[dim];
    ];

    dx = LinearSolve[H, -b];
    (* apply increment *)
    Do[
      If[k != 1,
        With[{off = idx[k]},
          X[[k, 1]] += dx[[off[[1]]]];
          X[[k, 2]] += dx[[off[[2]]]];
          X[[k, 3]] = wrapAngle[X[[k, 3]] + dx[[off[[3]]]]];
        ];
      ],
      {k, 1, N}
    ];

    Print[
      "iter=", IntegerString[it - 1, 10, 2],
      "  chi2=", NumberForm[chi2, {10, 6}],
      "  |dx|=", ScientificForm[Norm[dx], 3]
    ];

    If[Norm[dx] < 10^-8, Break[]];
  ];
  X
];

(* Construct a synthetic graph similar to Python/C++/MATLAB demos *)
makeSynthetic[N_] := Module[{Xtrue, edges, noiseXY, noiseTh, Sigma, Omega, z, zn, zlc, zlcN, X0, k},
  Xtrue = ConstantArray[{0., 0., 0.}, N];
  For[k = 2, k <= N, k++,
    Xtrue[[k, 1]] = Xtrue[[k - 1, 1]] + 0.5 Cos[0.1 (k - 1)];
    Xtrue[[k, 2]] = Xtrue[[k - 1, 2]] + 0.5 Sin[0.1 (k - 1)];
    Xtrue[[k, 3]] = wrapAngle[0.05 (k - 1)];
  ];

  noiseXY = 0.02; noiseTh = 0.01;
  Sigma = DiagonalMatrix[{noiseXY^2, noiseXY^2, noiseTh^2}];
  Omega = Inverse[Sigma];

  edges = {};
  For[k = 1, k < N, k++,
    z = predictRelative[Xtrue[[k]], Xtrue[[k + 1]]];
    zn = {z[[1]] + noiseXY RandomVariate[NormalDistribution[]],
          z[[2]] + noiseXY RandomVariate[NormalDistribution[]],
          wrapAngle[z[[3]] + noiseTh RandomVariate[NormalDistribution[]]]};
    edges = Append[edges, {k, k + 1, zn, Omega}];
  ];

  zlc = predictRelative[Xtrue[[1]], Xtrue[[N]]];
  zlcN = {zlc[[1]] + noiseXY RandomVariate[NormalDistribution[]],
          zlc[[2]] + noiseXY RandomVariate[NormalDistribution[]],
          wrapAngle[zlc[[3]] + noiseTh RandomVariate[NormalDistribution[]]]};
  edges = Append[edges, {1, N, zlcN, Omega}];

  X0 = ConstantArray[{0., 0., 0.}, N];
  For[k = 1, k < N, k++,
    z = edges[[k, 3]];
    X0[[k + 1, 1 ;; 2]] = X0[[k, 1 ;; 2]] + (R2[X0[[k, 3]]].z[[1 ;; 2]]);
    X0[[k + 1, 3]] = wrapAngle[X0[[k, 3]] + z[[3]]];
  ];
  {X0, edges}
];

{X0, edges} = makeSynthetic[15];
Print["Optimizing pose graph (dense GN, anchored pose 1)..."];
Xopt = gnSolve[X0, edges, 10, 0];

Print["First 5 poses initial vs optimized:"];
Do[
  Print[
    "k=", k - 1,
    "  X0=", NumberForm[X0[[k]], {6, 4}],
    "  Xopt=", NumberForm[Xopt[[k]], {6, 4}]
  ],
  {k, 1, 5}
];
      

Proposition 1 (Gaussian likelihood → weighted least squares). Suppose for each edge \( (i,j) \) the measurement model induces residual \( \mathbf{e}_{ij} \) with distribution \( \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{ij}) \). Then maximizing the likelihood is equivalent to minimizing \( \sum \mathbf{e}_{ij}^\top\boldsymbol{\Omega}_{ij}\mathbf{e}_{ij} \).

Sketch. The Gaussian density is \( p(\mathbf{e})=c\exp(-\tfrac{1}{2}\mathbf{e}^\top\boldsymbol{\Omega}\mathbf{e}) \). Using independence over edges, the log-likelihood is a sum; dropping constants gives a negative log-likelihood proportional to the weighted sum of squared residuals. □

Proposition 2 (Gauge freedom makes the Hessian singular without a prior). If the only constraints are relative poses, then for any global rigid transform applied to all poses, every predicted relative measurement is unchanged; therefore the cost is invariant and \( \mathbf{H} \) has a nontrivial nullspace.

Sketch. Relative prediction uses differences and rotations. Apply the same global translation and rotation to both poses; their difference in the local frame remains the same, hence each residual is unchanged. In 2D the symmetry group has 3 DOF (global x, y, yaw), giving at least a 3D nullspace; thus \( \mathbf{H} \) is singular unless we anchor or add a prior. □

Problem 1 (MAP objective): Assume independent Gaussian residuals for all edges with covariances \( \boldsymbol{\Sigma}_{ij} \). Derive the negative log-likelihood objective and show it has weighted least-squares form.

Solution: For a single edge, the Gaussian likelihood is proportional to \( \exp(-\tfrac{1}{2}\mathbf{e}_{ij}^\top\boldsymbol{\Omega}_{ij}\mathbf{e}_{ij}) \). Taking log of the product turns it into a sum, and negating drops constants:

\[ -\log p(\mathcal{Z}\mid\mathcal{X}) = \text{const} + \tfrac{1}{2}\sum_{(i,j)} \mathbf{e}_{ij}(\mathcal{X})^\top\boldsymbol{\Omega}_{ij}\mathbf{e}_{ij}(\mathcal{X}), \]

so minimizing the negative log-likelihood equals minimizing the weighted sum of squared residuals.

Problem 2 (Linearization): Let \( F(\mathbf{x})=\sum_k \|\mathbf{e}_k(\mathbf{x})\|_{\Omega_k}^2 \). Show linearization yields normal equations with \( \mathbf{H}=\sum \mathbf{J}_k^\top\Omega_k\mathbf{J}_k \) and \( \mathbf{g}=\sum \mathbf{J}_k^\top\Omega_k\mathbf{e}_k \).

Solution: Use \( \mathbf{e}_k(\mathbf{x}+\delta\mathbf{x})\approx \mathbf{e}_k+\mathbf{J}_k\delta\mathbf{x} \), expand the quadratic, collect terms, then set derivative to zero to obtain \( \mathbf{H}\delta\mathbf{x}=-\mathbf{g} \).

Problem 3 (Gauge freedom in 2D): Show that adding a constant translation and heading offset to every pose leaves all predicted relative measurements unchanged. Conclude the Hessian has a nullspace of dimension at least 3.

Solution: Differences \( \mathbf{t}_j-\mathbf{t}_i \) are unchanged by global translation. Global rotation affects both frames equally, leaving the relative expression in the local frame unchanged; likewise \( \theta_j-\theta_i \) is invariant under a common offset. Therefore the objective is constant along 3 independent directions (x, y, yaw), giving a 3D nullspace unless anchored.

Problem 4 (One Gauss–Newton step, anchored node): For a single edge between node 0 (anchored) and node 1, write the normal equation for \( \delta\mathbf{x}_1 \).

Solution:

\[ (\mathbf{J}_1^\top\Omega\mathbf{J}_1)\,\delta\mathbf{x}_1 = -\mathbf{J}_1^\top\Omega\mathbf{e}. \]

Problem 5 (LM damping intuition): Explain what happens when \( \lambda \) in LM becomes very large.

Solution: If \( \lambda \) dominates, then \( \mathbf{H}+\lambda\mathbf{I}\approx\lambda\mathbf{I} \), so \( \delta\mathbf{x}\approx-(1/\lambda)\mathbf{g} \), a small gradient step (stable but slow).

We derived pose-graph SLAM as a MAP problem that becomes weighted nonlinear least squares under Gaussian noise. Using SE(2) relative-pose residuals, we developed the Gauss–Newton linearization, showed how sparse normal equations arise from graph structure, and explained why gauge freedom requires anchoring or priors. We also introduced Levenberg–Marquardt damping as a stability mechanism. In the next lesson, we will address incorrect constraints using robust kernels and outlier handling.