Chapter 12: SLAM II — Graph-Based SLAM
Lesson 1: Pose Graphs and Factor Graphs
This lesson introduces the graph-based formulation of SLAM as a maximum a posteriori (MAP) estimation problem over robot poses. We build the mathematical bridge from probabilistic models (Gaussian noise on relative constraints) to sparse nonlinear least squares, and we formalize \( \) pose graphs and factor graphs as two complementary views of the same estimation structure. Emphasis is placed on measurement models, residual construction, gauge freedom, and the conditional-independence structure that explains sparsity.
1. Motivation and High-Level Picture
In filter-based SLAM (Chapter 11), the state grows and covariance coupling becomes expensive and fragile under linearization. Graph-based SLAM instead stores a set of constraints (factors) and computes the configuration of poses that best satisfies them in a global sense.
A pose graph uses: nodes for robot poses \( \mathbf{x}_i \in \mathbb{R}^3 \) (2D: \( \mathbf{x}_i = [x_i, y_i, \theta_i]^T \)), and edges for relative measurements \( \mathbf{z}_{ij} \) between poses \( i \) and \( j \).
flowchart TD
A["Sensors and odometry stream"] --> B["Create nodes: pose0, pose1, ..."]
B --> C["Add sequential constraints: z_01, z_12, ..."]
C --> D["Detect loop closure candidate"]
D --> E["Validate and add loop constraint: z_ij"]
E --> F["Add one prior to fix reference frame"]
F --> G["Solve MAP estimate (least squares)"]
G --> H["Optimized trajectory and uncertainty summary"]
The key conceptual shift is: instead of updating a belief sequentially, we build a batch objective that incorporates all constraints (including loop closures) and optimize it.
2. Pose Graph Measurement Model (2D) and Likelihood
Let each pose be \( \mathbf{x}_i = [x_i, y_i, \theta_i]^T \). A relative-pose measurement (from scan matching, odometry integration, or feature tracking) is modeled as:
\[ \mathbf{z}_{ij} = \begin{bmatrix} \Delta x_{ij} \\ \Delta y_{ij} \\ \Delta \theta_{ij} \end{bmatrix} = h(\mathbf{x}_i,\mathbf{x}_j) + \boldsymbol{\varepsilon}_{ij}, \quad \boldsymbol{\varepsilon}_{ij} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{ij}) \]
For planar motion, a standard choice of \( h(\cdot) \) is the relative transform “pose \( j \) expressed in frame \( i \)”. Define the rotation matrix \( \mathbf{R}(\theta) \in \mathbb{R}^{2\times 2} \):
\[ \mathbf{R}(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \]
Let \( \mathbf{t}_i = [x_i, y_i]^T \). Then the predicted relative translation is \( \mathbf{R}(\theta_i)^T(\mathbf{t}_j - \mathbf{t}_i) \), and predicted relative heading is \( \theta_j - \theta_i \) (wrapped to \( (-\pi,\pi] \)).
\[ h(\mathbf{x}_i,\mathbf{x}_j) = \begin{bmatrix} \mathbf{R}(\theta_i)^T(\mathbf{t}_j - \mathbf{t}_i) \\ \operatorname{wrap}(\theta_j - \theta_i) \end{bmatrix} \]
With Gaussian noise, the likelihood factor is: \( p(\mathbf{z}_{ij}\mid \mathbf{x}_i,\mathbf{x}_j) \propto \exp\!\big(-\tfrac{1}{2}\mathbf{r}_{ij}^T \boldsymbol{\Omega}_{ij}\mathbf{r}_{ij}\big) \), where \( \boldsymbol{\Omega}_{ij}=\boldsymbol{\Sigma}_{ij}^{-1} \) is the information matrix and \( \mathbf{r}_{ij} \) is the residual:
\[ \mathbf{r}_{ij}(\mathbf{x}_i,\mathbf{x}_j) = \mathbf{z}_{ij} - h(\mathbf{x}_i,\mathbf{x}_j), \quad \text{with angle component wrapped to } (-\pi,\pi]. \]
3. MAP Estimation as Weighted Least Squares (Key Derivation)
Collect all poses in a stacked vector \( \mathbf{X} = [\mathbf{x}_0^T,\mathbf{x}_1^T,\dots,\mathbf{x}_n^T]^T \). Assuming conditional independence of measurements given poses (a standard modeling assumption), the joint likelihood factorizes:
\[ p(\mathbf{Z}\mid \mathbf{X}) = \prod_{(i,j)\in \mathcal{E} } p(\mathbf{z}_{ij}\mid \mathbf{x}_i,\mathbf{x}_j). \]
Add one prior (anchor) factor to remove global gauge ambiguity (see Section 4): \( p(\mathbf{x}_0) \). The posterior is proportional to:
\[ p(\mathbf{X}\mid \mathbf{Z}) \propto p(\mathbf{x}_0)\prod_{(i,j)\in \mathcal{E} } p(\mathbf{z}_{ij}\mid \mathbf{x}_i,\mathbf{x}_j). \]
Proposition (Gaussian factors → least squares): If each factor is Gaussian with residual \( \mathbf{r}_k(\mathbf{X}) \) and information \( \boldsymbol{\Omega}_k \), then the MAP estimate \( \mathbf{X}^\star = \arg\max_\mathbf{X} p(\mathbf{X}\mid \mathbf{Z}) \) is equivalent to minimizing a sum of squared Mahalanobis norms.
Proof: For a Gaussian factor, \( p_k \propto \exp\!\big(-\tfrac{1}{2}\mathbf{r}_k^T\boldsymbol{\Omega}_k\mathbf{r}_k\big) \). Taking negative log of the posterior (dropping constants independent of \( \mathbf{X} \)):
\[ -\log p(\mathbf{X}\mid \mathbf{Z}) = \text{const} + \tfrac{1}{2}\sum_k \mathbf{r}_k(\mathbf{X})^T \boldsymbol{\Omega}_k \mathbf{r}_k(\mathbf{X}). \]
Since \( -\log(\cdot) \) is strictly decreasing, maximizing the posterior equals minimizing the expression above. Therefore:
\[ \mathbf{X}^\star = \arg\min_{\mathbf{X} } \sum_{(i,j)\in\mathcal{E} } \left\| \mathbf{r}_{ij}(\mathbf{x}_i,\mathbf{x}_j) \right\|_{\boldsymbol{\Omega}_{ij} }^2 + \left\| \mathbf{r}_{0}(\mathbf{x}_0) \right\|_{\boldsymbol{\Omega}_{0} }^2, \quad \|\mathbf{v}\|_{\boldsymbol{\Omega} }^2 := \mathbf{v}^T\boldsymbol{\Omega}\mathbf{v}. \]
This establishes pose-graph SLAM as a (sparse) nonlinear least squares problem. (The numerical optimization details are expanded in Lesson 3.)
4. Gauge Freedom (Why We Must Fix a Reference)
A pose graph built only from relative constraints is invariant to a global rigid transform: if we translate and rotate all poses together, every relative measurement prediction remains unchanged. This implies the objective has directions with zero curvature (rank deficiency).
Claim: If the graph has no absolute reference (no prior, no fixed pose, no GPS-like absolute factor), then the Hessian of the linearized least squares system is singular (in 2D: at least 3 null directions corresponding to global \( x \), \( y \), and \( \theta \)).
Reasoning: For any constant translation \( \Delta \mathbf{t} \in \mathbb{R}^2 \) and rotation \( \Delta \theta \), define transformed poses \( \mathbf{t}'_i = \mathbf{R}(\Delta\theta)\mathbf{t}_i + \Delta\mathbf{t}, \quad \theta'_i = \theta_i + \Delta\theta \). Then for any edge \( (i,j) \), the relative term \( \mathbf{R}(\theta_i)^T(\mathbf{t}_j-\mathbf{t}_i) \) is unchanged under this global transform, and \( \theta_j-\theta_i \) is also unchanged. Hence every residual remains identical, so the optimum is not unique. To obtain a unique solution we impose a prior such as: \( \mathbf{x}_0 \approx \bar{\mathbf{x} }_0 \) with strong information \( \boldsymbol{\Omega}_0 \).
\[ \mathbf{r}_0(\mathbf{x}_0) = \bar{\mathbf{x} }_0 - \mathbf{x}_0, \quad \text{(wrap the angle component)}. \]
5. Factor Graphs: A Generalization of Pose Graphs
A factor graph explicitly represents the factorization of a function (typically a probability distribution) into local terms. It is a bipartite graph with:
- Variable nodes: unknowns (poses, landmarks, biases, etc.).
- Factor nodes: local likelihood/prior terms connecting only the variables they depend on.
For pose graphs, each relative measurement is a binary factor connecting \( \mathbf{x}_i \) and \( \mathbf{x}_j \), plus one unary prior factor.
flowchart LR
x0["x0"] --- f0["prior"]
x0 --- f01["factor z01"] --- x1["x1"]
x1 --- f12["factor z12"] --- x2["x2"]
x2 --- f23["factor z23"] --- x3["x3"]
x1 --- f13["factor z13"] --- x3
This view becomes essential when we later mix multiple variable types (e.g., visual-inertial factors, bias nodes), but the core idea remains: the posterior is a product of factors; MAP is minimizing the sum of their negative logs.
6. Linearization and Sparsity (What Makes Graph SLAM Efficient)
Each residual is nonlinear in angles, so we solve iteratively by linearizing around a current estimate \( \mathbf{X}^{(k)} \). Let \( \delta \mathbf{X} \) be a stacked perturbation vector. A first-order approximation is:
\[ \mathbf{r}_k(\mathbf{X}^{(k)} \oplus \delta\mathbf{X}) \approx \mathbf{r}_k(\mathbf{X}^{(k)}) + \mathbf{J}_k\,\delta\mathbf{X}, \]
where \( \mathbf{J}_k \) contains nonzero blocks only for the variables touched by factor \( k \). Stacking all factors gives the normal equations:
\[ \left(\mathbf{A}^T\mathbf{W}\mathbf{A}\right)\delta\mathbf{X} = -\mathbf{A}^T\mathbf{W}\mathbf{b}, \]
where \( \mathbf{A} \) is the global Jacobian, \( \mathbf{b} \) stacks residuals, and \( \mathbf{W} \) is block-diagonal with the information matrices. Because each factor touches only a few variables, \( \mathbf{A} \) is sparse, making \( \mathbf{A}^T\mathbf{W}\mathbf{A} \) sparse as well.
7. Python Lab — From-Scratch SE(2) Pose Graph (Gauss–Newton)
The script below constructs a tiny looped trajectory, generates noisy relative constraints, anchors node 0 with a strong prior, and runs Gauss–Newton. This is intentionally dense (not sparse) to keep the mechanics transparent.
Chapter12_Lesson1.py
# Chapter12_Lesson1.py
"""
Autonomous Mobile Robots (Control Engineering)
Chapter 12 — SLAM II: Graph-Based SLAM
Lesson 1 — Pose Graphs and Factor Graphs
A minimal, from-scratch 2D pose-graph optimizer (SE(2)) using Gauss–Newton.
- Variables: x_i = (x,w,EKF maps), control u_t, obs z_t with IDs
- Factors/edges: relative pose measurements z_ij between nodes i and j
- Objective: sum of weighted squared residuals (Mahalanobis)
No external robotics libraries required. Uses NumPy only.
"""
from __future__ import annotations
import numpy as np
def wrap_angle(a: float) -> float:
"""Wrap angle to (-pi, pi]."""
a = (a + np.pi) % (2.0 * np.pi) - np.pi
return a
def rot(theta: float) -> np.ndarray:
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s],
[s, c]], dtype=float)
def boxplus(pose: np.ndarray, delta: np.ndarray) -> np.ndarray:
"""
Apply a local perturbation delta = [dx, dy, dtheta] in the *local* frame.
pose = [x, y, theta].
"""
x, y, th = pose
d = delta[:2]
dth = float(delta[2])
t_new = np.array([x, y]) + rot(th) @ d
th_new = wrap_angle(th + dth)
return np.array([t_new[0], t_new[1], th_new], dtype=float)
def between(x_i: np.ndarray, x_j: np.ndarray) -> np.ndarray:
"""
Relative pose from i to j: zhat_ij = x_i^{-1} oplus x_j (SE(2) between).
Returns [dx, dy, dtheta], where dx,dy are in frame i.
"""
ti = x_i[:2]
tj = x_j[:2]
thi = float(x_i[2])
thj = float(x_j[2])
dt = tj - ti
dxy = rot(thi).T @ dt
dth = wrap_angle(thj - thi)
return np.array([dxy[0], dxy[1], dth], dtype=float)
def edge_residual_and_jacobians(x_i: np.ndarray, x_j: np.ndarray, z_ij: np.ndarray):
"""
Residual r_ij = z_ij - between(x_i, x_j), with angle wrapped.
Provides analytic Jacobians wrt x_i and x_j under a small-angle local update.
State increments are local (boxplus). The Jacobians below correspond to
the linearization in the *global* parameterization x=[x,y,theta], but they
are commonly used with boxplus for small deltas.
"""
ti = x_i[:2]
tj = x_j[:2]
thi = float(x_i[2])
thj = float(x_j[2])
dt = tj - ti
RiT = rot(thi).T
zhat = between(x_i, x_j)
r = z_ij - zhat
r[2] = wrap_angle(r[2])
# J = [[0, -1],[1, 0]] (90deg rotation)
J = np.array([[0.0, -1.0],
[1.0, 0.0]], dtype=float)
# Jacobians of residual r = z - zhat => Jr = -Jzhat
# zhat_xy = RiT * dt
# d zhat_xy / d t_i = -RiT => d r_xy / d t_i = +RiT
# d zhat_xy / d t_j = +RiT => d r_xy / d t_j = -RiT
# d zhat_xy / d theta_i = d(RiT)/dtheta_i * dt = -(RiT*J)*dt
# => d r_xy / d theta_i = + (RiT*J*dt)
dri_dti = RiT
drj_dtj = -RiT
dri_dthi = (RiT @ (J @ dt.reshape(2,1))).reshape(2,) # 2-vector
Ji = np.zeros((3,3), dtype=float)
Jj = np.zeros((3,3), dtype=float)
Ji[0:2, 0:2] = dri_dti
Ji[0:2, 2] = dri_dthi
Ji[2, 2] = 1.0 # d r_theta / d theta_i = +1 because r_theta = z_theta - (thj-thi)
Jj[0:2, 0:2] = drj_dtj
Jj[2, 2] = -1.0 # d r_theta / d theta_j = -1
return r, Ji, Jj
def build_normal_equations(poses: np.ndarray, edges: list[dict], priors: list[dict]):
"""
Assemble H, g for Gauss–Newton: H * dx = -g.
poses: (N,3)
edges: list of {"i":int,"j":int,"z":(3,), "Omega":(3,3)}
priors: list of {"i":int, "mu":(3,), "Omega":(3,3)}
"""
N = poses.shape[0]
dim = 3*N
H = np.zeros((dim, dim), dtype=float)
g = np.zeros((dim,), dtype=float)
def sl(i): # slice for node i in stacked vector
return slice(3*i, 3*i+3)
# Relative factors
for e in edges:
i, j = int(e["i"]), int(e["j"])
z = np.asarray(e["z"], dtype=float).reshape(3,)
Omega = np.asarray(e["Omega"], dtype=float).reshape(3,3)
r, Ji, Jj = edge_residual_and_jacobians(poses[i], poses[j], z)
# Contributions: H += J^T Ω J, g += J^T Ω r
Hi = Ji.T @ Omega @ Ji
Hj = Jj.T @ Omega @ Jj
Hij = Ji.T @ Omega @ Jj
gij = Ji.T @ Omega @ r
gj = Jj.T @ Omega @ r
si, sj = sl(i), sl(j)
H[si, si] += Hi
H[sj, sj] += Hj
H[si, sj] += Hij
H[sj, si] += Hij.T
g[si] += gij
g[sj] += gj
# Prior factors to fix gauge / anchor
for p in priors:
i = int(p["i"])
mu = np.asarray(p["mu"], dtype=float).reshape(3,)
Omega = np.asarray(p["Omega"], dtype=float).reshape(3,3)
# residual: r = mu - x_i (wrap angle)
r = mu - poses[i]
r[2] = wrap_angle(r[2])
J = np.eye(3, dtype=float) * (-1.0) # r = mu - x => dr/dx = -I
si = sl(i)
H[si, si] += J.T @ Omega @ J
g[si] += J.T @ Omega @ r
return H, g
def gauss_newton_optimize(poses0: np.ndarray, edges: list[dict], priors: list[dict],
iters: int = 10, damping: float = 1e-8):
poses = poses0.copy().astype(float)
N = poses.shape[0]
for k in range(iters):
H, g = build_normal_equations(poses, edges, priors)
# Levenberg-style tiny damping to stabilize
H = H + damping * np.eye(3*N)
dx = np.linalg.solve(H, -g)
max_step = 0.0
for i in range(N):
delta = dx[3*i:3*i+3]
poses[i] = boxplus(poses[i], delta)
max_step = max(max_step, float(np.linalg.norm(delta)))
print(f"iter {k:02d}: max|delta| = {max_step:.3e}")
if max_step < 1e-9:
break
return poses
def demo():
# Ground truth poses (a small loop)
gt = np.array([
[0.0, 0.0, 0.0],
[1.0, 0.0, 0.0],
[2.0, 0.0, 0.0],
[2.0, 1.0, np.pi/2],
[2.0, 2.0, np.pi/2],
[1.0, 2.0, np.pi],
[0.0, 2.0, np.pi],
[0.0, 1.0, -np.pi/2],
], dtype=float)
N = gt.shape[0]
# Build relative measurements with noise
rng = np.random.default_rng(4)
sigma_xy = 0.02
sigma_th = np.deg2rad(1.0)
Omega = np.diag([1/sigma_xy**2, 1/sigma_xy**2, 1/sigma_th**2])
edges = []
for i in range(N-1):
z = between(gt[i], gt[i+1])
z_noisy = z + rng.normal([0,0,0], [sigma_xy, sigma_xy, sigma_th], size=3)
z_noisy[2] = wrap_angle(z_noisy[2])
edges.append({"i": i, "j": i+1, "z": z_noisy, "Omega": Omega})
# Add loop closure (7 -> 0)
z_lc = between(gt[7], gt[0])
z_lc_noisy = z_lc + rng.normal([0,0,0], [sigma_xy, sigma_xy, sigma_th], size=3)
z_lc_noisy[2] = wrap_angle(z_lc_noisy[2])
edges.append({"i": 7, "j": 0, "z": z_lc_noisy, "Omega": Omega})
# Initial guess: drifted chain integration (simulate odom drift)
poses0 = gt.copy()
poses0[:, 0:2] += rng.normal(0.0, 0.15, size=(N,2))
poses0[:, 2] = np.array([wrap_angle(a) for a in (poses0[:,2] + rng.normal(0.0, np.deg2rad(5.0), size=N))])
# Prior on node 0 to fix gauge
prior_sigma = np.array([1e-3, 1e-3, 1e-3], dtype=float)
Omega0 = np.diag(1/prior_sigma**2)
priors = [{"i": 0, "mu": gt[0], "Omega": Omega0}]
print("Optimizing...")
est = gauss_newton_optimize(poses0, edges, priors, iters=15)
print("\nNode i | gt (x,y,th) | est (x,y,th)")
for i in range(N):
print(f"{i:4d} | {gt[i]} | {est[i]}")
return gt, poses0, est
if __name__ == "__main__":
demo()
Libraries (ecosystem note): For real robot-scale graphs, typical Python stacks use \( \) GTSAM (Python bindings), Ceres via wrappers, or custom sparse solvers. The code here is a teaching implementation of the core math: residuals, Jacobians, and normal equations.
8. C++ Lab — Minimal Pose Graph (Eigen) + Common Robotics Libraries
In C++, graph SLAM is commonly implemented with: g2o (graph optimization), GTSAM (factor graphs), and Ceres Solver (general nonlinear least squares). The following teaching example uses Eigen and a dense solver.
Chapter12_Lesson1.cpp
// Chapter12_Lesson1.cpp
/*
Autonomous Mobile Robots (Control Engineering)
Chapter 12 — SLAM II: Graph-Based SLAM
Lesson 1 — Pose Graphs and Factor Graphs
Minimal 2D pose graph Gauss–Newton optimizer (SE(2)) using Eigen.
Build (Linux/macOS):
g++ -O2 -std=c++17 Chapter12_Lesson1.cpp -I /usr/include/eigen3 -o pose_graph
Run:
./pose_graph
Note: This is dense and small-scale by design for teaching.
*/
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <cmath>
#include <random>
struct Pose2 {
double x{0}, y{0}, th{0};
};
static double WrapAngle(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 Eigen::Matrix2d Rot(double th) {
double c = std::cos(th), s = std::sin(th);
Eigen::Matrix2d R;
R << c, -s,
s, c;
return R;
}
// Local update: t ← t + R(th)*d, th ← th + dth
static Pose2 BoxPlus(const Pose2& p, const Eigen::Vector3d& d) {
Pose2 out = p;
Eigen::Vector2d t(p.x, p.y);
Eigen::Vector2d inc = Rot(p.th) * d.head<2>();
t += inc;
out.x = t.x();
out.y = t.y();
out.th = WrapAngle(p.th + d(2));
return out;
}
static Eigen::Vector3d Between(const Pose2& i, const Pose2& j) {
Eigen::Vector2d ti(i.x, i.y), tj(j.x, j.y);
Eigen::Vector2d dt = tj - ti;
Eigen::Vector2d dxy = Rot(i.th).transpose() * dt;
double dth = WrapAngle(j.th - i.th);
return Eigen::Vector3d(dxy.x(), dxy.y(), dth);
}
struct Edge {
int i{0}, j{0};
Eigen::Vector3d z;
Eigen::Matrix3d Omega;
};
static void ResidualAndJacobians(const Pose2& xi, const Pose2& xj, const Eigen::Vector3d& z,
Eigen::Vector3d& r, Eigen::Matrix3d& Ji, Eigen::Matrix3d& Jj)
{
Eigen::Vector2d ti(xi.x, xi.y), tj(xj.x, xj.y);
Eigen::Vector2d dt = tj - ti;
Eigen::Matrix2d RiT = Rot(xi.th).transpose();
Eigen::Vector3d zhat = Between(xi, xj);
r = z - zhat;
r(2) = WrapAngle(r(2));
// J = [[0,-1],[1,0]]
Eigen::Matrix2d J;
J << 0, -1,
1, 0;
Ji.setZero();
Jj.setZero();
// dr/dt_i = +RiT
Ji.block<2,2>(0,0) = RiT;
// dr/dtheta_i = RiT * J * dt
Eigen::Vector2d dri_dth = (RiT * (J * dt));
Ji(0,2) = dri_dth.x();
Ji(1,2) = dri_dth.y();
// dr_theta/dtheta_i = +1
Ji(2,2) = 1.0;
// dr/dt_j = -RiT
Jj.block<2,2>(0,0) = -RiT;
// dr_theta/dtheta_j = -1
Jj(2,2) = -1.0;
}
static void BuildNormalEquations(const std::vector<Pose2>& poses,
const std::vector<Edge>& edges,
const std::vector<Edge>& priors,
Eigen::MatrixXd& H, Eigen::VectorXd& g)
{
const int N = static_cast<int>(poses.size());
const int dim = 3*N;
H.setZero(dim, dim);
g.setZero(dim);
auto sl = [](int idx) { return Eigen::seqN(3*idx, 3); };
// Relative edges
for (const auto& e : edges) {
Eigen::Vector3d r;
Eigen::Matrix3d Ji, Jj;
ResidualAndJacobians(poses[e.i], poses[e.j], e.z, r, Ji, Jj);
Eigen::Matrix3d Hi = Ji.transpose()*e.Omega*Ji;
Eigen::Matrix3d Hj = Jj.transpose()*e.Omega*Jj;
Eigen::Matrix3d Hij = Ji.transpose()*e.Omega*Jj;
Eigen::Vector3d gi = Ji.transpose()*e.Omega*r;
Eigen::Vector3d gj = Jj.transpose()*e.Omega*r;
H(sl(e.i), sl(e.i)) += Hi;
H(sl(e.j), sl(e.j)) += Hj;
H(sl(e.i), sl(e.j)) += Hij;
H(sl(e.j), sl(e.i)) += Hij.transpose();
g(sl(e.i)) += gi;
g(sl(e.j)) += gj;
}
// Priors as unary factors stored in Edge (i==j ignored, uses z as "mu")
for (const auto& p : priors) {
int i = p.i;
Eigen::Vector3d mu = p.z;
Eigen::Vector3d xi(poses[i].x, poses[i].y, poses[i].th);
Eigen::Vector3d r = mu - xi;
r(2) = WrapAngle(r(2));
Eigen::Matrix3d J = -Eigen::Matrix3d::Identity();
H(sl(i), sl(i)) += J.transpose()*p.Omega*J;
g(sl(i)) += J.transpose()*p.Omega*r;
}
}
static std::vector<Pose2> GaussNewton(std::vector<Pose2> poses,
const std::vector<Edge>& edges,
const std::vector<Edge>& priors,
int iters = 15, double damping = 1e-8)
{
const int N = static_cast<int>(poses.size());
for (int k=0; k<iters; ++k) {
Eigen::MatrixXd H(3*N, 3*N);
Eigen::VectorXd g(3*N);
BuildNormalEquations(poses, edges, priors, H, g);
H += damping * Eigen::MatrixXd::Identity(3*N, 3*N);
Eigen::VectorXd dx = H.fullPivLu().solve(-g);
double maxStep = 0.0;
for (int i=0; i<N; ++i) {
Eigen::Vector3d d = dx.segment<3>(3*i);
poses[i] = BoxPlus(poses[i], d);
maxStep = std::max(maxStep, d.norm());
}
std::cout << "iter " << k << ": max|delta|=" << maxStep << "\n";
if (maxStep < 1e-9) break;
}
return poses;
}
int main() {
// A tiny looped trajectory
std::vector<Pose2> gt = {
{0,0,0},
{1,0,0},
{2,0,0},
{2,1,M_PI/2},
{2,2,M_PI/2},
{1,2,M_PI},
{0,2,M_PI},
{0,1,-M_PI/2}
};
const int N = static_cast<int>(gt.size());
std::mt19937 rng(4);
std::normal_distribution<double> nxy(0.0, 0.02);
std::normal_distribution<double> nth(0.0, M_PI/180.0);
Eigen::Matrix3d Omega = Eigen::Matrix3d::Zero();
Omega(0,0) = 1.0/(0.02*0.02);
Omega(1,1) = 1.0/(0.02*0.02);
Omega(2,2) = 1.0/( (M_PI/180.0)*(M_PI/180.0) );
std::vector<Edge> edges;
for (int i=0; i<N-1; ++i) {
Eigen::Vector3d z = Between(gt[i], gt[i+1]);
z(0) += nxy(rng); z(1) += nxy(rng); z(2) = WrapAngle(z(2) + nth(rng));
edges.push_back({i, i+1, z, Omega});
}
// loop closure 7->0
{
Eigen::Vector3d z = Between(gt[7], gt[0]);
z(0) += nxy(rng); z(1) += nxy(rng); z(2) = WrapAngle(z(2) + nth(rng));
edges.push_back({7, 0, z, Omega});
}
// Initial guess
std::normal_distribution<double> initxy(0.0, 0.15);
std::normal_distribution<double> initth(0.0, 5.0*M_PI/180.0);
std::vector<Pose2> x0 = gt;
for (auto& p : x0) {
p.x += initxy(rng);
p.y += initxy(rng);
p.th = WrapAngle(p.th + initth(rng));
}
// Prior on node 0
Eigen::Matrix3d Omega0 = Eigen::Matrix3d::Zero();
Omega0(0,0) = 1e6; Omega0(1,1) = 1e6; Omega0(2,2) = 1e6;
std::vector<Edge> priors;
priors.push_back({0, 0, Eigen::Vector3d(gt[0].x, gt[0].y, gt[0].th), Omega0});
std::cout << "Optimizing...\n";
auto est = GaussNewton(x0, edges, priors);
std::cout << "\n i | gt(x,y,th) | est(x,y,th)\n";
for (int i=0; i<N; ++i) {
std::cout << i << " | "
<< gt[i].x << " " << gt[i].y << " " << gt[i].th << " | "
<< est[i].x << " " << est[i].y << " " << est[i].th << "\n";
}
return 0;
}
9. Java Lab — Minimal Pose Graph (EJML) + Integration Options
Java robotics stacks often rely on linear algebra libraries such as EJML, and integrate advanced factor-graph solvers via: (i) Java bindings (e.g., GTSAM Java wrappers in some distributions), or (ii) calling native solvers through JNI. Below is a teaching Gauss–Newton implementation using EJML.
Chapter12_Lesson1.java
// Chapter12_Lesson1.java
/*
Autonomous Mobile Robots (Control Engineering)
Chapter 12 — SLAM II: Graph-Based SLAM
Lesson 1 — Pose Graphs and Factor Graphs
Minimal 2D pose graph Gauss–Newton optimizer (SE(2)) using EJML (dense).
Build (example with Maven/Gradle dependency on EJML):
org.ejml:ejml-all:0.43+
This file is written as a single-class demo for teaching.
*/
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import java.util.*;
public class Chapter12_Lesson1 {
static class Pose2 {
double x, y, th;
Pose2(double x, double y, double th) { this.x=x; this.y=y; this.th=th; }
Pose2 copy() { return new Pose2(x,y,th); }
}
static class Edge {
int i, j;
double[] z; // length 3
DMatrixRMaj Omega; // 3x3
Edge(int i, int j, double[] z, DMatrixRMaj Omega) { this.i=i; this.j=j; this.z=z; this.Omega=Omega; }
}
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[][] rot(double th) {
double c = Math.cos(th), s = Math.sin(th);
return new double[][]{
{c, -s},
{s, c}
};
}
static Pose2 boxPlus(Pose2 p, double[] d) {
double[][] R = rot(p.th);
double dx = R[0][0]*d[0] + R[0][1]*d[1];
double dy = R[1][0]*d[0] + R[1][1]*d[1];
return new Pose2(p.x + dx, p.y + dy, wrapAngle(p.th + d[2]));
}
static double[] between(Pose2 i, Pose2 j) {
double dx = j.x - i.x;
double dy = j.y - i.y;
double[][] RiT = rot(i.th); // we will use transpose explicitly
// transpose(R) * dt:
double dxi = RiT[0][0]*dx + RiT[1][0]*dy;
double dyi = RiT[0][1]*dx + RiT[1][1]*dy;
double dth = wrapAngle(j.th - i.th);
return new double[]{dxi, dyi, dth};
}
static class LinEdge {
double[] r; // 3
DMatrixRMaj Ji; // 3x3
DMatrixRMaj Jj; // 3x3
LinEdge(double[] r, DMatrixRMaj Ji, DMatrixRMaj Jj) { this.r=r; this.Ji=Ji; this.Jj=Jj; }
}
static LinEdge residualAndJacobians(Pose2 xi, Pose2 xj, double[] z) {
double[] zhat = between(xi, xj);
double[] r = new double[]{ z[0]-zhat[0], z[1]-zhat[1], wrapAngle(z[2]-zhat[2]) };
// Prepare matrices
DMatrixRMaj Ji = new DMatrixRMaj(3,3);
DMatrixRMaj Jj = new DMatrixRMaj(3,3);
// RiT
double c = Math.cos(xi.th), s = Math.sin(xi.th);
double r00 = c, r01 = s;
double r10 = -s, r11 = c;
// dt
double dtx = xj.x - xi.x;
double dty = xj.y - xi.y;
// J = [[0,-1],[1,0]]
double j00 = 0, j01 = -1;
double j10 = 1, j11 = 0;
// dri/dti = RiT
Ji.set(0,0, r00); Ji.set(0,1, r01);
Ji.set(1,0, r10); Ji.set(1,1, r11);
// dri/dtheta = RiT * J * dt
double Jdt_x = j00*dtx + j01*dty;
double Jdt_y = j10*dtx + j11*dty;
double dri_dth_x = r00*Jdt_x + r01*Jdt_y;
double dri_dth_y = r10*Jdt_x + r11*Jdt_y;
Ji.set(0,2, dri_dth_x);
Ji.set(1,2, dri_dth_y);
// dr_theta/dtheta_i = +1
Ji.set(2,2, 1.0);
// dr/dt_j = -RiT
Jj.set(0,0, -r00); Jj.set(0,1, -r01);
Jj.set(1,0, -r10); Jj.set(1,1, -r11);
// dr_theta/dtheta_j = -1
Jj.set(2,2, -1.0);
return new LinEdge(r, Ji, Jj);
}
static void addSubMatrix(DMatrixRMaj A, int r0, int c0, DMatrixRMaj B) {
for (int r=0; r<B.numRows; r++) {
for (int c=0; c<B.numCols; c++) {
A.add(r0+r, c0+c, B.get(r,c));
}
}
}
static void addSubVector(DMatrixRMaj v, int r0, double[] b) {
for (int r=0; r<b.length; r++) v.add(r0+r, 0, b[r]);
}
static void buildNormalEquations(List<Pose2> poses, List<Edge> edges, List<Edge> priors,
DMatrixRMaj H, DMatrixRMaj g)
{
int N = poses.size();
CommonOps_DDRM.fill(H, 0);
CommonOps_DDRM.fill(g, 0);
for (Edge e : edges) {
LinEdge lin = residualAndJacobians(poses.get(e.i), poses.get(e.j), e.z);
// Hi = Ji^T Omega Ji
DMatrixRMaj tmp = new DMatrixRMaj(3,3);
DMatrixRMaj Hi = new DMatrixRMaj(3,3);
DMatrixRMaj Hj = new DMatrixRMaj(3,3);
DMatrixRMaj Hij = new DMatrixRMaj(3,3);
// tmp = Omega * Ji
CommonOps_DDRM.mult(e.Omega, lin.Ji, tmp);
CommonOps_DDRM.multTransA(lin.Ji, tmp, Hi);
CommonOps_DDRM.mult(e.Omega, lin.Jj, tmp);
CommonOps_DDRM.multTransA(lin.Jj, tmp, Hj);
CommonOps_DDRM.mult(e.Omega, lin.Jj, tmp);
CommonOps_DDRM.multTransA(lin.Ji, tmp, Hij);
// gi = Ji^T Omega r
DMatrixRMaj rvec = new DMatrixRMaj(3,1, true, lin.r);
DMatrixRMaj Or = new DMatrixRMaj(3,1);
CommonOps_DDRM.mult(e.Omega, rvec, Or);
DMatrixRMaj gi = new DMatrixRMaj(3,1);
DMatrixRMaj gj = new DMatrixRMaj(3,1);
CommonOps_DDRM.multTransA(lin.Ji, Or, gi);
CommonOps_DDRM.multTransA(lin.Jj, Or, gj);
int si = 3*e.i, sj = 3*e.j;
addSubMatrix(H, si, si, Hi);
addSubMatrix(H, sj, sj, Hj);
addSubMatrix(H, si, sj, Hij);
// add Hij^T into (sj,si)
DMatrixRMaj HijT = new DMatrixRMaj(3,3);
CommonOps_DDRM.transpose(Hij, HijT);
addSubMatrix(H, sj, si, HijT);
// g
addSubVector(g, si, gi.getData());
addSubVector(g, sj, gj.getData());
}
// Priors: r = mu - x, J = -I
for (Edge p : priors) {
int i = p.i;
Pose2 xi = poses.get(i);
double[] mu = p.z;
double[] r = new double[]{ mu[0]-xi.x, mu[1]-xi.y, wrapAngle(mu[2]-xi.th) };
DMatrixRMaj J = CommonOps_DDRM.identity(3);
CommonOps_DDRM.scale(-1.0, J);
DMatrixRMaj tmp = new DMatrixRMaj(3,3);
DMatrixRMaj Hi = new DMatrixRMaj(3,3);
CommonOps_DDRM.mult(p.Omega, J, tmp);
CommonOps_DDRM.multTransA(J, tmp, Hi);
DMatrixRMaj rvec = new DMatrixRMaj(3,1, true, r);
DMatrixRMaj Or = new DMatrixRMaj(3,1);
CommonOps_DDRM.mult(p.Omega, rvec, Or);
DMatrixRMaj gi = new DMatrixRMaj(3,1);
CommonOps_DDRM.multTransA(J, Or, gi);
int si = 3*i;
addSubMatrix(H, si, si, Hi);
addSubVector(g, si, gi.getData());
}
}
static List<Pose2> gaussNewton(List<Pose2> x0, List<Edge> edges, List<Edge> priors, int iters) {
int N = x0.size();
List<Pose2> x = new ArrayList<>();
for (Pose2 p : x0) x.add(p.copy());
DMatrixRMaj H = new DMatrixRMaj(3*N, 3*N);
DMatrixRMaj g = new DMatrixRMaj(3*N, 1);
for (int k=0; k<iters; k++) {
buildNormalEquations(x, edges, priors, H, g);
// Damping
for (int d=0; d<3*N; d++) H.add(d,d, 1e-8);
// Solve H dx = -g
DMatrixRMaj minusG = g.copy();
CommonOps_DDRM.scale(-1.0, minusG);
DMatrixRMaj dx = new DMatrixRMaj(3*N, 1);
CommonOps_DDRM.solve(H, minusG, dx);
double maxStep = 0.0;
for (int i=0; i<N; i++) {
double[] d = new double[]{ dx.get(3*i,0), dx.get(3*i+1,0), dx.get(3*i+2,0) };
Pose2 upd = boxPlus(x.get(i), d);
x.set(i, upd);
double n = Math.sqrt(d[0]*d[0] + d[1]*d[1] + d[2]*d[2]);
if (n > maxStep) maxStep = n;
}
System.out.println("iter " + k + ": max|delta|=" + maxStep);
if (maxStep < 1e-9) break;
}
return x;
}
public static void main(String[] args) {
// Ground truth
List<Pose2> gt = Arrays.asList(
new Pose2(0,0,0),
new Pose2(1,0,0),
new Pose2(2,0,0),
new Pose2(2,1,Math.PI/2),
new Pose2(2,2,Math.PI/2),
new Pose2(1,2,Math.PI),
new Pose2(0,2,Math.PI),
new Pose2(0,1,-Math.PI/2)
);
int N = gt.size();
Random rng = new Random(4);
double sigmaXY = 0.02;
double sigmaTh = Math.toRadians(1.0);
DMatrixRMaj Omega = new DMatrixRMaj(3,3);
Omega.set(0,0, 1.0/(sigmaXY*sigmaXY));
Omega.set(1,1, 1.0/(sigmaXY*sigmaXY));
Omega.set(2,2, 1.0/(sigmaTh*sigmaTh));
List<Edge> edges = new ArrayList<>();
for (int i=0; i<N-1; i++) {
double[] z = between(gt.get(i), gt.get(i+1));
z[0] += rng.nextGaussian()*sigmaXY;
z[1] += rng.nextGaussian()*sigmaXY;
z[2] = wrapAngle(z[2] + rng.nextGaussian()*sigmaTh);
edges.add(new Edge(i, i+1, z, Omega));
}
// loop closure 7->0
{
double[] z = between(gt.get(7), gt.get(0));
z[0] += rng.nextGaussian()*sigmaXY;
z[1] += rng.nextGaussian()*sigmaXY;
z[2] = wrapAngle(z[2] + rng.nextGaussian()*sigmaTh);
edges.add(new Edge(7, 0, z, Omega));
}
// Initial guess
List<Pose2> x0 = new ArrayList<>();
for (Pose2 p : gt) {
x0.add(new Pose2(
p.x + rng.nextGaussian()*0.15,
p.y + rng.nextGaussian()*0.15,
wrapAngle(p.th + rng.nextGaussian()*Math.toRadians(5.0))
));
}
// Prior on node 0
DMatrixRMaj Omega0 = new DMatrixRMaj(3,3);
Omega0.set(0,0, 1e6); Omega0.set(1,1, 1e6); Omega0.set(2,2, 1e6);
List<Edge> priors = new ArrayList<>();
priors.add(new Edge(0, 0, new double[]{gt.get(0).x, gt.get(0).y, gt.get(0).th}, Omega0));
System.out.println("Optimizing...");
List<Pose2> est = gaussNewton(x0, edges, priors, 15);
System.out.println("\n i | gt(x,y,th) | est(x,y,th)");
for (int i=0; i<N; i++) {
Pose2 gtp = gt.get(i), ep = est.get(i);
System.out.printf("%d | (%.3f,%.3f,%.3f) | (%.3f,%.3f,%.3f)\n",
i, gtp.x, gtp.y, gtp.th, ep.x, ep.y, ep.th);
}
}
}
10. MATLAB/Simulink Lab — Pose Graph Optimization + Simulink Constraint Generation
MATLAB is convenient for prototyping factor definitions and testing solvers quickly. In practice, MATLAB/Simulink workflows often use Simulink for simulation (generate odometry/relative constraints) and MATLAB scripts to build and optimize the graph.
Chapter12_Lesson1.m
% Chapter12_Lesson1.m
% Autonomous Mobile Robots (Control Engineering)
% Chapter 12 — SLAM II: Graph-Based SLAM
% Lesson 1 — Pose Graphs and Factor Graphs
%
% A minimal 2D pose-graph optimizer (SE(2)) in MATLAB using Gauss–Newton.
% Also includes a small programmatic Simulink stub that generates a noisy
% odometry chain (optional) to create sequential constraints.
function Chapter12_Lesson1()
rng(4);
% --------
% Ground truth trajectory (tiny loop)
% --------
gt = [...
0 0 0;
1 0 0;
2 0 0;
2 1 pi/2;
2 2 pi/2;
1 2 pi;
0 2 pi;
0 1 -pi/2];
N = size(gt,1);
% --------
% Build noisy relative measurements z_ij
% --------
sigma_xy = 0.02;
sigma_th = deg2rad(1.0);
Omega = diag([1/sigma_xy^2, 1/sigma_xy^2, 1/sigma_th^2]);
edges = {};
for i=1:N-1
z = between(gt(i,:), gt(i+1,:));
z = z + [sigma_xy*randn, sigma_xy*randn, sigma_th*randn];
z(3) = wrapAngle(z(3));
edges{end+1} = struct('i', i, 'j', i+1, 'z', z, 'Omega', Omega);
end
% loop closure 8 -> 1
zlc = between(gt(8,:), gt(1,:));
zlc = zlc + [sigma_xy*randn, sigma_xy*randn, sigma_th*randn];
zlc(3) = wrapAngle(zlc(3));
edges{end+1} = struct('i', 8, 'j', 1, 'z', zlc, 'Omega', Omega);
% --------
% Initial guess
% --------
x0 = gt;
x0(:,1:2) = x0(:,1:2) + 0.15*randn(N,2);
x0(:,3) = arrayfun(@(a) wrapAngle(a), x0(:,3) + deg2rad(5)*randn(N,1));
% --------
% Prior on node 1 to fix gauge
% --------
Omega0 = diag([1e6, 1e6, 1e6]);
priors = { struct('i', 1, 'mu', gt(1,:), 'Omega', Omega0) };
fprintf('Optimizing...\n');
est = gaussNewton(x0, edges, priors, 15);
disp(' i | gt(x,y,th) | est(x,y,th)');
for i=1:N
fprintf('%2d | [% .3f % .3f % .3f] | [% .3f % .3f % .3f]\n', ...
i-1, gt(i,1), gt(i,2), gt(i,3), est(i,1), est(i,2), est(i,3));
end
% Optional: build a simple Simulink model that generates a noisy odometry chain
% (This is illustrative; it is not required for optimization above.)
buildSimulinkOdometryStub();
end
% --------------------------
% Core math helpers (SE(2))
% --------------------------
function a = wrapAngle(a)
a = mod(a + pi, 2*pi) - pi;
end
function R = rot(th)
c = cos(th); s = sin(th);
R = [c -s; s c];
end
function z = between(xi, xj)
ti = xi(1:2)'; tj = xj(1:2)';
thi = xi(3); thj = xj(3);
dt = tj - ti;
dxy = rot(thi)' * dt;
dth = wrapAngle(thj - thi);
z = [dxy' dth];
end
function xnew = boxplus(x, d)
% d is in local frame: [dx dy dth]
th = x(3);
t = x(1:2)' + rot(th) * d(1:2)';
xnew = [t' wrapAngle(th + d(3))];
end
function [r, Ji, Jj] = residualAndJacobians(xi, xj, z)
% residual r = z - between(xi,xj)
ti = xi(1:2)'; tj = xj(1:2)'; thi = xi(3);
dt = tj - ti;
RiT = rot(thi)';
zhat = between(xi, xj);
r = z - zhat; r(3) = wrapAngle(r(3));
J = [0 -1; 1 0];
Ji = zeros(3,3);
Jj = zeros(3,3);
Ji(1:2,1:2) = RiT;
dri_dth = RiT * (J * dt);
Ji(1:2,3) = dri_dth;
Ji(3,3) = 1;
Jj(1:2,1:2) = -RiT;
Jj(3,3) = -1;
end
function est = gaussNewton(x0, edges, priors, iters)
est = x0;
N = size(x0,1);
for k=1:iters
[H, g] = buildNormalEquations(est, edges, priors);
H = H + 1e-8*eye(3*N);
dx = -H \ g;
maxStep = 0;
for i=1:N
d = dx(3*i-2:3*i)';
est(i,:) = boxplus(est(i,:), d);
maxStep = max(maxStep, norm(d));
end
fprintf('iter %02d: max|delta| = %.3e\n', k-1, maxStep);
if maxStep < 1e-9, break; end
end
end
function [H, g] = buildNormalEquations(poses, edges, priors)
N = size(poses,1);
H = zeros(3*N, 3*N);
g = zeros(3*N, 1);
sl = @(i) (3*i-2):(3*i); % i is 1-based
% Relative edges
for eidx=1:numel(edges)
e = edges{eidx};
i = e.i; j = e.j;
[r, Ji, Jj] = residualAndJacobians(poses(i,:), poses(j,:), e.z);
Omega = e.Omega;
Hi = Ji' * Omega * Ji;
Hj = Jj' * Omega * Jj;
Hij = Ji' * Omega * Jj;
gi = Ji' * Omega * r(:);
gj = Jj' * Omega * r(:);
si = sl(i); sj = sl(j);
H(si,si) = H(si,si) + Hi;
H(sj,sj) = H(sj,sj) + Hj;
H(si,sj) = H(si,sj) + Hij;
H(sj,si) = H(sj,si) + Hij';
g(si) = g(si) + gi;
g(sj) = g(sj) + gj;
end
% Priors
for pidx=1:numel(priors)
p = priors{pidx};
i = p.i;
mu = p.mu(:);
xi = poses(i,:)'; xi(3) = wrapAngle(xi(3));
r = mu - xi; r(3) = wrapAngle(r(3));
J = -eye(3);
Omega = p.Omega;
si = sl(i);
H(si,si) = H(si,si) + J' * Omega * J;
g(si) = g(si) + J' * Omega * r;
end
end
% --------------------------
% Simulink stub (optional)
% --------------------------
function buildSimulinkOdometryStub()
% This stub constructs a simple Simulink model programmatically:
% inputs v,w, integrate to x,y,theta using Euler.
% It illustrates how a simulation can produce sequential constraints z_{k,k+1}.
%
% If Simulink is not installed/licensed, this function safely exits.
if ~license('test','Simulink')
fprintf('[Simulink] Not available. Skipping model creation.\n');
return;
end
model = 'Chapter12_Lesson1_OdomStub';
if bdIsLoaded(model), close_system(model, 0); end
if exist([model '.slx'],'file'), delete([model '.slx']); end
new_system(model);
open_system(model);
add_block('simulink/Sources/Constant', [model '/v'], 'Value', '1.0', 'Position',[30 40 80 70]);
add_block('simulink/Sources/Constant', [model '/w'], 'Value', '0.1', 'Position',[30 120 80 150]);
add_block('simulink/Sources/Constant', [model '/dt'], 'Value', '0.05', 'Position',[30 200 80 230]);
% MATLAB Function block for state update
add_block('simulink/User-Defined Functions/MATLAB Function', [model '/SE2 Integrator'], 'Position',[160 60 340 210]);
set_param([model '/SE2 Integrator'], 'Script', sprintf([ ...
'function [x,y,th]=f(v,w,dt)\n' ...
'%% Simple Euler integrator for (x,y,th)\n' ...
'persistent X Y TH;\n' ...
'if isempty(X), X=0; Y=0; TH=0; end\n' ...
'X = X + v*cos(TH)*dt;\n' ...
'Y = Y + v*sin(TH)*dt;\n' ...
'TH = TH + w*dt;\n' ...
'x=X; y=Y; th=TH;\n' ...
'end\n']));
add_block('simulink/Sinks/To Workspace', [model '/x_out'], 'VariableName','x_out','Position',[410 60 470 90]);
add_block('simulink/Sinks/To Workspace', [model '/y_out'], 'VariableName','y_out','Position',[410 110 470 140]);
add_block('simulink/Sinks/To Workspace', [model '/th_out'], 'VariableName','th_out','Position',[410 160 470 190]);
add_line(model, 'v/1', 'SE2 Integrator/1');
add_line(model, 'w/1', 'SE2 Integrator/2');
add_line(model, 'dt/1', 'SE2 Integrator/3');
add_line(model, 'SE2 Integrator/1', 'x_out/1');
add_line(model, 'SE2 Integrator/2', 'y_out/1');
add_line(model, 'SE2 Integrator/3', 'th_out/1');
save_system(model);
fprintf('[Simulink] Created model: %s.slx\n', model);
end
11. Wolfram Mathematica Lab — MAP Objective + FindMinimum
Mathematica can express the MAP objective symbolically and numerically. For small graphs, direct optimization (quasi-Newton) is instructive: it emphasizes that graph SLAM is simply MAP over a product of Gaussian factors.
Chapter12_Lesson1.nb
(* Chapter12_Lesson1.nb
Autonomous Mobile Robots (Control Engineering)
Chapter 12 — SLAM II: Graph-Based SLAM
Lesson 1 — Pose Graphs and Factor Graphs
A compact Wolfram Language notebook (as a Notebook[] expression) that:
- defines SE(2) "between" for 2D poses (x,y,theta),
- constructs a tiny pose graph objective,
- solves MAP via FindMinimum (quasi-Newton) on the sum of squared residuals.
Open this file in Wolfram Mathematica.
*)
Notebook[{
Cell["Chapter 12 — Lesson 1: Pose Graphs and Factor Graphs (SE(2) pose graph demo)", "Title"],
Cell["This notebook optimizes a tiny 2D pose graph using a direct MAP objective.", "Text"],
Cell[BoxData @ ToBoxes @
Row[{"1) Helpers: wrapAngle, rotation, between"}], "Section"],
Cell[BoxData @ ToBoxes @
HoldForm[
wrapAngle[a_] := Module[{x = Mod[a + Pi, 2 Pi] - Pi}, x];
rot[th_] := { {Cos[th], -Sin[th]}, {Sin[th], Cos[th]} };
between[xi_, xj_] := Module[{ti, tj, thi, thj, dt, dxy, dth},
ti = xi[[1 ;; 2]];
tj = xj[[1 ;; 2]];
thi = xi[[3]];
thj = xj[[3]];
dt = tj - ti;
dxy = Transpose[rot[thi]].dt;
dth = wrapAngle[thj - thi];
{dxy[[1]], dxy[[2]], dth}
];
]
, "Input"],
Cell["2) Build a tiny graph: sequential edges plus one loop closure", "Section"],
Cell[BoxData @ ToBoxes @
HoldForm[
gt = {
{0., 0., 0.},
{1., 0., 0.},
{2., 0., 0.},
{2., 1., Pi/2},
{2., 2., Pi/2},
{1., 2., Pi},
{0., 2., Pi},
{0., 1., -Pi/2}
};
n = Length[gt];
SeedRandom[4];
sigmaXY = 0.02; sigmaTh = 1 Degree;
Omega = DiagonalMatrix[{1/sigmaXY^2, 1/sigmaXY^2, 1/sigmaTh^2}];
noise[] := {RandomVariate[NormalDistribution[0, sigmaXY]],
RandomVariate[NormalDistribution[0, sigmaXY]],
RandomVariate[NormalDistribution[0, sigmaTh]]};
edges = Table[
Module[{z = between[gt[[i]], gt[[i+1]]] + noise[]},
z[[3]] = wrapAngle[z[[3]]];
<|"i" -> i, "j" -> (i+1), "z" -> z, "Omega" -> Omega|>
],
{i, 1, n-1}
];
(* loop closure: 8 -> 1 *)
zlc = between[gt[[8]], gt[[1]]] + noise[];
zlc[[3]] = wrapAngle[zlc[[3]]];
edges = Append[edges, <|"i" -> 8, "j" -> 1, "z" -> zlc, "Omega" -> Omega|>];
]
, "Input"],
Cell["3) MAP objective: sum over factors of r^T Omega r + prior on node 1", "Section"],
Cell[BoxData @ ToBoxes @
HoldForm[
(* decision variables: poses x[i] = {x,y,th} *)
vars = Flatten@Table[{x[i,1], x[i,2], x[i,3]}, {i, 1, n}];
(* initial guess: noisy gt *)
init = Flatten@Table[
With[{g = gt[[i]]},
{x[i,1] -> g[[1]] + RandomVariate[NormalDistribution[0, 0.15]],
x[i,2] -> g[[2]] + RandomVariate[NormalDistribution[0, 0.15]],
x[i,3] -> wrapAngle[g[[3]] + RandomVariate[NormalDistribution[0, 5 Degree]]]}
],
{i, 1, n}
];
rEdge[poseI_, poseJ_, z_] := Module[{zhat, r},
zhat = between[poseI, poseJ];
r = z - zhat;
r[[3]] = wrapAngle[r[[3]]];
r
];
obj =
Sum[
Module[{i = edges[[k]]["i"], j = edges[[k]]["j"], z = edges[[k]]["z"], Om = edges[[k]]["Omega"],
poseI, poseJ, r},
poseI = {x[i,1], x[i,2], x[i,3]};
poseJ = {x[j,1], x[j,2], x[j,3]};
r = rEdge[poseI, poseJ, z];
r.Om.r
],
{k, 1, Length[edges]}
];
(* strong prior on node 1 *)
priorOm = DiagonalMatrix[{10^6, 10^6, 10^6}];
priorMu = gt[[1]];
r0 = ({x[1,1], x[1,2], x[1,3]} - priorMu);
r0[[3]] = wrapAngle[r0[[3]]];
obj = obj + r0.priorOm.r0;
Print["Objective constructed. Variables: ", Length[vars]];
]
, "Input"],
Cell["4) Solve using FindMinimum (quasi-Newton)", "Section"],
Cell[BoxData @ ToBoxes @
HoldForm[
sol = FindMinimum[obj, init, Method -> "QuasiNewton", MaxIterations -> 200];
minVal = sol[[1]];
estRules = sol[[2]];
Print["Min objective: ", minVal];
]
, "Input"],
Cell["5) Extract estimates", "Section"],
Cell[BoxData @ ToBoxes @
HoldForm[
est = Table[{x[i,1], x[i,2], x[i,3]} /. estRules, {i, 1, n}];
Grid[
Prepend[
Table[{i-1, gt[[i]], est[[i]]}, {i, 1, n}],
{"i", "gt (x,y,th)", "est (x,y,th)"}
],
Frame -> All
]
]
, "Input"]
}]
12. Problems and Solutions
Problem 1 (Residual construction): For two poses \( \mathbf{x}_i=[x_i,y_i,\theta_i]^T \) and \( \mathbf{x}_j=[x_j,y_j,\theta_j]^T \), define \( h(\mathbf{x}_i,\mathbf{x}_j) \) as in Section 2. Show that the translational part can be written as \( \mathbf{R}(\theta_i)^T(\mathbf{t}_j-\mathbf{t}_i) \).
Solution: The world-frame displacement is \( \mathbf{t}_j-\mathbf{t}_i \). To express this vector in frame \( i \), rotate it by the inverse of frame \( i \) orientation. Since \( \mathbf{R}(\theta) \) is orthonormal, \( \mathbf{R}(\theta)^{-1}=\mathbf{R}(\theta)^T \). Therefore the coordinates of the displacement in frame \( i \) are exactly \( \mathbf{R}(\theta_i)^T(\mathbf{t}_j-\mathbf{t}_i) \).
Problem 2 (Gaussian factor → least squares): Let \( \boldsymbol{\varepsilon}\sim\mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}) \) and \( \mathbf{z}=h(\mathbf{x})+\boldsymbol{\varepsilon} \). Derive the negative log-likelihood and show it is proportional to \( \|\mathbf{z}-h(\mathbf{x})\|_{\boldsymbol{\Omega} }^2 \) where \( \boldsymbol{\Omega}=\boldsymbol{\Sigma}^{-1} \).
Solution: The likelihood is \( p(\mathbf{z}\mid \mathbf{x}) = \frac{1}{\sqrt{(2\pi)^m|\boldsymbol{\Sigma}|} } \exp\!\left(-\tfrac{1}{2}(\mathbf{z}-h(\mathbf{x}))^T\boldsymbol{\Sigma}^{-1}(\mathbf{z}-h(\mathbf{x}))\right) \). Taking \( -\log \) yields constants plus \( \tfrac{1}{2}(\mathbf{z}-h(\mathbf{x}))^T\boldsymbol{\Omega}(\mathbf{z}-h(\mathbf{x})) \), which is the squared Mahalanobis norm.
Problem 3 (Gauge freedom): Consider a pose graph with only relative factors \( \mathbf{z}_{ij} \) and no priors. Prove that if \( \{\mathbf{x}_i\} \) is a solution, then applying any global rigid transform produces another solution with identical objective value.
Solution: Define a transformed set: \( \mathbf{t}'_i = \mathbf{R}(\Delta\theta)\mathbf{t}_i + \Delta\mathbf{t}, \ \theta'_i = \theta_i + \Delta\theta \). For any edge \( (i,j) \), \( \mathbf{t}'_j-\mathbf{t}'_i = \mathbf{R}(\Delta\theta)(\mathbf{t}_j-\mathbf{t}_i) \). Also \( \mathbf{R}(\theta'_i)^T = \mathbf{R}(\theta_i+\Delta\theta)^T = \mathbf{R}(\Delta\theta)^T\mathbf{R}(\theta_i)^T \). Therefore:
\[ \mathbf{R}(\theta'_i)^T(\mathbf{t}'_j-\mathbf{t}'_i) = \mathbf{R}(\Delta\theta)^T\mathbf{R}(\theta_i)^T \mathbf{R}(\Delta\theta)(\mathbf{t}_j-\mathbf{t}_i) = \mathbf{R}(\theta_i)^T(\mathbf{t}_j-\mathbf{t}_i), \]
using \( \mathbf{R}(\Delta\theta)^T\mathbf{R}(\Delta\theta)=\mathbf{I} \). Also, \( \theta'_j-\theta'_i = (\theta_j+\Delta\theta)-(\theta_i+\Delta\theta)=\theta_j-\theta_i \). Hence every factor prediction is unchanged, so every residual is unchanged, so the objective is unchanged. Therefore the solution is non-unique and a prior is required to fix the reference.
Problem 4 (Sparsity): Explain why the normal-equation matrix \( \mathbf{H}=\mathbf{A}^T\mathbf{W}\mathbf{A} \) is sparse in pose graphs, and why this matters computationally for large trajectories.
Solution: Each factor depends on only a small subset of variables (binary for pose graphs: only \( \mathbf{x}_i \) and \( \mathbf{x}_j \)). Therefore each Jacobian row block has nonzeros only in those variable columns, making \( \mathbf{A} \) sparse. Multiplying by block-diagonal \( \mathbf{W} \) preserves sparsity, and the product \( \mathbf{A}^T\mathbf{W}\mathbf{A} \) produces nonzero blocks only where variables share a factor (graph adjacency). Sparse linear algebra can solve such systems far faster and with far less memory than dense methods as the number of poses grows.
Problem 5 (Unary and binary factors): Give one example of a unary factor and one example of a binary factor in mobile robotics, and write their residuals.
Solution: Unary example: an anchor prior on pose 0, \( \mathbf{r}_0(\mathbf{x}_0)=\bar{\mathbf{x} }_0-\mathbf{x}_0 \) (wrap the angle component). Binary example: a relative pose constraint from scan matching, \( \mathbf{r}_{ij}(\mathbf{x}_i,\mathbf{x}_j)=\mathbf{z}_{ij}-h(\mathbf{x}_i,\mathbf{x}_j) \).
13. Summary
We formulated graph-based SLAM as MAP estimation over poses with Gaussian factors, derived the weighted least-squares objective, explained gauge freedom and the need for an anchoring prior, and introduced factor graphs as the explicit representation of posterior factorization. This prepares the ground for loop closure concepts (Lesson 2) and for nonlinear least squares optimization mechanics (Lesson 3).
14. References
- Lu, F., & Milios, E. (1997). Globally consistent range scan alignment for environment mapping. Autonomous Robots, 4(4), 333–349.
- Kschischang, F.R., Frey, B.J., & Loeliger, H.-A. (2001). Factor graphs and the sum-product algorithm. IEEE Transactions on Information Theory, 47(2), 498–519.
- Dellaert, F., & Kaess, M. (2006). Square root SAM: Simultaneous localization and mapping via square root information smoothing. The International Journal of Robotics Research, 25(12), 1181–1203.
- Kaess, M., Ranganathan, A., & Dellaert, F. (2008). iSAM: Incremental smoothing and mapping. IEEE Transactions on Robotics, 24(6), 1365–1378.
- Olson, E., Leonard, J., & Teller, S. (2006). Fast iterative alignment of pose graphs with poor initial estimates. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2262–2269.
- Grisetti, G., Kümmerle, R., Stachniss, C., & Burgard, W. (2010). A tutorial on graph-based SLAM. IEEE Intelligent Transportation Systems Magazine, 2(4), 31–43.
- Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J., & Dellaert, F. (2012). iSAM2: Incremental smoothing and mapping using the Bayes tree. The International Journal of Robotics Research, 31(2), 216–235.
