Math from first principles
GTSAM and OMPL look like big codebases, but they rest on a handful of mathematical ideas used again and again. These six chapters build those ideas from scratch. Each one starts from the problem that forces the idea into existence, derives the result (no "it can be shown"), works a small example by hand, and then points at the exact file and function that implements it.
The chapters
| # | Chapter | You will be able to… |
|---|---|---|
| 1 | Linear algebra & least squares | derive the normal equations, explain why QR beats them numerically, do Cholesky by hand, see Cholesky as repeated Schur complements, and predict fill-in |
| 2 | Rotations & Lie groups | derive Rodrigues' formula and its Taylor branches, compute exp/log on SO(3)/SE(3)/SE(2), derive the Jacobians of compose/inverse/between/action, and know GTSAM's and OMPL's conventions cold |
| 3 | Probability & estimation | marginalize and condition Gaussians in both forms, go from Bayes' rule to nonlinear least squares, derive the Kalman filter as elimination, write down the SLAM posterior, use robust losses, and understand IMU preintegration |
| 4 | Nonlinear optimization | derive Gauss–Newton, Levenberg–Marquardt, and Dogleg; run them on manifolds; read GTSAM's λ schedule and trust-region rules; understand GNC and initialization |
| 5 | Factor graphs & sparse inference | eliminate a factor graph by hand, connect it to sparse QR/Cholesky, reason about orderings and fill, understand Bayes trees, iSAM2, marginals, and the BA Schur trick |
| 6 | Motion-planning mathematics | define C-space formally, sample uniformly on SO(3), reason about dispersion and collision-check resolution, prove (sketch) RRT/PRM completeness, derive the RRT*/PRM* radii, sample informed sets, and handle dynamics and constraints |
How the ideas depend on each other
┌───────────────────────────┐
│ 1. Linear algebra │ least squares, QR, Cholesky, Schur complement
└──────┬──────────────┬──────┘
│ │
┌───────────────▼───┐ ┌──────▼──────────────────┐
│ 2. Lie groups │ │ 3. Probability │ Gaussians ⇄ quadratic forms
│ exp/log, Jacobians │ │ MAP ⇒ least squares │ marginalize = Schur complement
└──────┬────────┬────┘ └──────┬──────────────────┘
│ │ │
│ ┌────▼───────────────▼───┐
│ │ 4. Nonlinear optimization│ GN / LM / Dogleg on manifolds
│ └────────────┬────────────┘
│ │
│ ┌────────────▼────────────┐
│ │ 5. Factor graphs │ elimination, Bayes tree, iSAM2 ⇒ GTSAM
│ └──────────────────────────┘
│
┌──────▼──────────────────────────────┐
│ 6. Motion planning │ C-space, sampling, RRT/PRM, RRT* ⇒ OMPL
└──────────────────────────────────────┘
If you only care about OMPL, read 1 (sections 1–4 and 11), 2 (sections 1–8 and 17–18), then 6. For GTSAM, read 1 through 5 in order.
Notation used throughout
| Symbol | Meaning |
|---|---|
| \(x, v, \omega\) (lowercase) | column vectors. \(x^\top\) is a transpose. \(\R^n\) is \(n\)-dimensional real space |
| \(A, R, \Sigma\) (uppercase) | matrices. \(I\) is the identity, \(I_n\) the \(n\times n\) identity |
| \(\lVert x\rVert\) | Euclidean norm \(\sqrt{x^\top x}\) |
| \(\lVert r\rVert^2_\Sigma\) | squared Mahalanobis norm \(r^\top\Sigma^{-1}r\) |
| \(\skew{\omega}\) | the 3×3 skew-symmetric matrix with \(\skew{\omega}v = \omega\times v\) (also written \(\omega^\wedge\)) |
| \(\exp, \log\) | matrix exponential/logarithm (algebra ⇄ group) |
| \(\Exp, \Log\) | the same, but taking/returning plain vectors: \(\Exp(\omega) = \exp(\skew{\omega})\) |
| \(x \oplus \delta\) | retraction: apply tangent step \(\delta\) to \(x\). In GTSAM, \(x\oplus\delta = x\,\Exp(\delta)\) |
| \(y \ominus x\) | local coordinates of \(y\) around \(x\). In GTSAM, \(\Log(x^{-1}y)\) = x.localCoordinates(y) |
| \(\Ad_g\) | the adjoint matrix of group element \(g\) |
| \(J_r, J_l\) | right/left Jacobians of \(\Exp\) |
| \(\mathcal{N}(\mu,\Sigma)\) | Gaussian with mean \(\mu\), covariance \(\Sigma\). \(\Lambda = \Sigma^{-1}\) is the information matrix |
| \(x_i\), \(l_j\), \(z_k\) | poses, landmarks, measurements |
| \(\mathcal{C}\), \(\mathcal{C}_{free}\), \(q\) | configuration space, its collision-free part, a configuration |
| \(d\) | dimension (of a C-space or tangent space) |
| \(\mu(\cdot)\), \(\zeta_d\) | volume (Lebesgue measure), volume of the unit \(d\)-ball |
How to study these
Work the hand examples yourself before reading the answers, and re-derive the boxed formulas on paper. Then open the referenced GTSAM/OMPL source file and find the formula in code. Most of them are only a few lines. When you port, you'll be translating these formulas, so you have to recognize them in any notation.