Mathematical Foundations & Optimization

1.1Frames and Rigid Transformations

Every quantity a robot cares about — a lidar return, a wheel tick, a goal, gravity — is a set of numbers measured with respect to something. That something is a coordinate frame: an origin plus axes. A mobile robot drags a small zoo of them around: a fixed world (or map) frame, a base frame bolted to the chassis, and one frame per sensor. The same physical point has different coordinates in each, and the entire discipline of not confusing them is one object: the rigid transformation.

A rigid transformation is a rotation R plus a translation t. Applied to a point it rotates first, then translates. Packing both into a single matrix using homogeneous coordinates turns "rotate then translate" into one multiplication, which is what makes chains of frames manageable:

p_w = R_wb · p_b + t_wb                      rotate, then translate

homogeneous form:   p̃ = [p; 1]

T_wb = [ R_wb   t_wb ]        p̃_w = T_wb · p̃_b
       [  0       1  ]

SE(2), planar pose, 3 DoF (x, y, θ):        SE(3), 3D pose, 6 DoF:

T = [ cos θ   −sin θ   x ]                  T = [ R₃ₓ₃   t₃ₓ₁ ]
    [ sin θ    cos θ   y ]                      [  0       1  ]
    [   0        0     1 ]

composition:   T_wl = T_wb · T_bl            adjacent subscripts cancel
inverse:       T_wb⁻¹ = [ Rᵀ  −Rᵀt ]  =  T_bw
                        [ 0     1  ]

The subscript convention carries more weight than it looks. Read T_wb as "the pose of frame b expressed in frame w", or equivalently "the matrix that maps coordinates from b into w". Written that way, correctness becomes typography: in T_wb · T_bl the two b’s touch and cancel, leaving T_wl. If the inner subscripts don’t match, the expression is wrong before you evaluate a single number. The diagram below is the whole idea:

Frame

An origin plus orthogonal axes. Coordinates mean nothing until you say which frame they are in.

SE(2), SE(3)

The special Euclidean groups: all rigid transformations of the plane (3 DoF) and of 3D space (6 DoF). "Group" mostly means composable, invertible, closed under both.

Extrinsics

The fixed transform between two sensors or a sensor and the body, like T_bl above. Measured once by calibration, then trusted by everything.

Homogeneous coordinates

Appending a 1 to a point so rotation-plus-translation becomes a single matrix product, and chains of transforms become chains of multiplications.

1.2Rotations Are the Troublemakers

Translation is a vector: you can add translations, average them, scale them, and put Gaussians on them. Rotation looks like it should behave the same way and does not, and the gap between those two sentences is why this chapter needs Lie theory at all.

Three facts break the vector illusion. First, rotations do not commute: roll-then-pitch is not pitch-then-roll, while addition never cares about order. Second, angles wrap around: the average of headings +179° and −179° is ±180°, but the arithmetic mean says 0° — a robot that averages raw angles will eventually turn to face exactly backwards. Third, the popular three-number parametrization, Euler angles, has a singularity: at pitch ±90° two axes align, a degree of freedom vanishes, and Jacobians divide by zero. That is gimbal lock, and it is a property of the parametrization, not of physics.

The set of all 3D rotations has a name and a definition worth memorizing:

SO(3) = { R ∈ ℝ³ˣ³  :  RᵀR = I,  det R = +1 }

3 degrees of freedom, stored in 9 numbers with 6 constraints —
or in a unit quaternion: 4 numbers, 1 constraint, and q ≡ −q (double cover)

Every row of that table is a bad place to do calculus. Euler angles are singular, quaternions and matrices are constrained, and an optimizer that takes an unconstrained step off the quaternion sphere or out of orthogonality produces a "rotation" that isn’t one. The resolution is not a better parametrization — it is to stop pretending rotations live in a vector space at all.

1.3Lie Theory Without Tears: so(3) and se(3)

SO(3) is a manifold: a curved 3-dimensional surface living inside the 9-dimensional space of matrices, the way a sphere is a curved 2D surface inside 3D. You cannot add points on a curved surface. But every manifold is locally flat: zoom in far enough at any point and it looks like a plain vector space. That local flat approximation is the tangent space, and for SO(3) it is an honest ℝ³ — three unconstrained numbers where addition, Gaussians, Jacobians, and least squares all work exactly as they should.

The tangent space of SO(3) at the identity is called so(3), and its elements have two equivalent forms: a plain vector ω ∈ ℝ³ (rotation axis times angle), or a skew-symmetric matrix. The hat operator converts vector to matrix, vee (ᵛ) converts back, and the exponential map — an honest matrix exponential, with a famous closed form — wraps a tangent vector onto the manifold:

hat:                      [  0   −ω₃   ω₂ ]
  ω = (ω₁, ω₂, ω₃)ᵀ  →    [  ω₃    0  −ω₁ ]  = ω^        note: ω^ v = ω × v
                          [ −ω₂   ω₁    0 ]

exponential map (Rodrigues' formula),  θ = ‖ω‖:

  R = exp(ω^) = I + (sin θ / θ) · ω^ + ((1 − cos θ) / θ²) · (ω^)²

log map (the inverse):

  θ = arccos( (tr R − 1) / 2 )         ω = (θ / (2 sin θ)) · (R − Rᵀ)ᵛ

Poses get the same treatment. SE(3) is a 6-dimensional manifold; its tangent space se(3) holds twists ξ = (ρ, ω) — three numbers of translation-like velocity and three of rotation. The exponential map couples them through a matrix V (rotation makes the translation curve, which is why a robot commanding constant (v, ω) drives an arc, not a line — the exact fact wheel odometry in Phase 2 integrates):

ξ = (ρ, ω) ∈ ℝ⁶ ≅ se(3)

  exp(ξ^) = [ exp(ω^)   V·ρ ]        V = I + ((1 − cos θ)/θ²) ω^
            [    0       1  ]              + ((θ − sin θ)/θ³) (ω^)²

boxplus / boxminus — the only two operators estimation ever needs:

  x ⊞ δ  =  x · exp(δ^)              perturb pose x by tangent step δ
  y ⊟ x  =  log(x⁻¹ · y)ᵛ            the tangent step carrying x to y

Two conventions exist for ⊞: multiply the perturbation on the right (a step in the body frame, used above) or on the left (a step in the world frame). Both are fine; mixing them is not, because Jacobians derived in one convention are wrong in the other. The adjoint matrix Adj_T is the bookkeeping device that converts tangent vectors between the two sides: T · exp(δ^) = exp((Adj_T δ)^) · T. You mostly need to know it exists and that your library documents its choice.

The cheat sheet for the four groups this book actually uses:

Manifold

A curved space that is locally flat. Sphere: 2D manifold. SO(3): 3D manifold. SE(3): 6D manifold.

Tangent space / Lie algebra

The flat vector space that best approximates the manifold at a point. At the identity it gets a special name: so(3), se(3).

hat (^) / vee (ᵛ)

Convert between the vector form of a tangent element and its matrix form. ω^ is also the cross-product matrix.

exp / log

Wrap a tangent vector onto the manifold; unwrap a manifold displacement into a tangent vector. Closed-form for all groups in this book.

Adjoint

The linear map that carries tangent vectors between right- and left-perturbation conventions, or between frames.

1.4Sophus and manif: Lie Groups You Can Compile

You should implement Rodrigues’ formula once in your life, then never again. Two header-only C++ libraries own this niche, and this book uses both. Sophus (Strasdat) is the veteran: Eigen-based SO(2)/SE(2)/SO(3)/SE(3)/Sim(3), battle-tested inside a large share of the SLAM systems in Phases 2 and 3, with a ready-made Ceres manifold adapter we will use in Setup B. manif (Deray & Solà) is the younger sibling written to accompany the micro Lie theory paper: same groups plus SE_2(3) (the extended pose used by modern IMU filters), and its defining feature is that every operation can return its analytic Jacobians — exactly the matrices an EKF or a factor graph needs, with no autodiff and no finite differences.

The core of Sophus in fifteen lines:

#include <sophus/se3.hpp>

// a pose: 30° about z, translated (1, 2, 0)
Sophus::SE3d T_wb(Sophus::SO3d::rotZ(M_PI / 6.0),
                  Eigen::Vector3d(1.0, 2.0, 0.0));

// log: pose → twist ξ = (ρ, ω), a plain 6-vector you can put a Gaussian on
Eigen::Matrix<double, 6, 1> xi = T_wb.log();

// exp: twist → pose. Round trip is identity up to machine precision
Sophus::SE3d T_rt = Sophus::SE3d::exp(xi);

// boxplus: perturb by a small tangent step (right convention)
Eigen::Matrix<double, 6, 1> delta;
delta << 0.01, 0, 0, 0, 0, 0.002;            // 1 cm forward, 2 mrad yaw
Sophus::SE3d T_pert = T_wb * Sophus::SE3d::exp(delta);

// frames compose and invert like the math says
Eigen::Vector3d p_w = T_wb * Eigen::Vector3d(0.5, 0.0, 0.0);

And the manif habit that makes Phase 2 pleasant — Jacobians on demand from every operator:

#include <manif/manif.h>

manif::SE2d X(1.0, 2.0, M_PI / 6.0);          // planar pose: x, y, θ
manif::SE2Tangentd w;
w << 0.05, 0.0, 0.01;                          // a small twist

manif::SE2d::Jacobian J_x, J_w;                // 3×3 analytic Jacobians
manif::SE2d X2 = X.plus(w, J_x, J_w);          // X2 = X ⊞ w,  ∂X2/∂X,  ∂X2/∂w

// every EKF predict step in Phase 2 is literally this call:
// state ⊞ (velocity · dt), with J_x propagating the covariance

sudo apt install build-essential cmake libeigen3-dev
git clone https://github.com/strasdat/Sophus.git
cmake -S Sophus -B Sophus/build -DSOPHUS_USE_BASIC_LOGGING=ON
sudo cmake --install Sophus/build

// hello_se3.cc — poses, exp/log, and why you never average angles.
#include <sophus/se3.hpp>
#include <iostream>

int main() {
  // 1. frame chain: lidar → base → world (Fig 1.1, in code)
  const Sophus::SE3d T_wb(Sophus::SO3d::rotZ(M_PI / 6.0),      // robot at (1,2), yaw 30°
                          Eigen::Vector3d(1.0, 2.0, 0.0));
  const Sophus::SE3d T_bl(Sophus::SO3d(),                      // lidar 20 cm fwd, 35 cm up
                          Eigen::Vector3d(0.20, 0.0, 0.35));
  const Eigen::Vector3d p_l(4.0, 0.0, 0.0);                    // return 4 m ahead of lidar
  std::cout << "p_w = " << (T_wb * T_bl * p_l).transpose() << "\n";

  // 2. exp/log round trip
  const auto xi = T_wb.log();
  const double err = (Sophus::SE3d::exp(xi).inverse() * T_wb).log().norm();
  std::cout << "round-trip error = " << err << "\n";

  // 3. the trap: average headings +170° and −170°
  const Sophus::SO3d Ra = Sophus::SO3d::rotZ( 170.0 * M_PI / 180.0);
  const Sophus::SO3d Rb = Sophus::SO3d::rotZ(-170.0 * M_PI / 180.0);
  const double naive_deg = (170.0 + (-170.0)) / 2.0;           // arithmetic mean
  const Sophus::SO3d mid =                                     // geodesic midpoint:
      Ra * Sophus::SO3d::exp(0.5 * (Ra.inverse() * Rb).log()); // walk halfway on SO(3)
  std::cout << "naive mean yaw    = " << naive_deg << " deg\n";
  std::cout << "manifold mean yaw = " << mid.log()(2) * 180.0 / M_PI << " deg\n";
}

1.5The Bayes Filter: The Master Recipe

Everything above describes a robot’s state; nothing yet estimates it. Estimation starts from an honest admission: sensors are noisy, motion is sloppy, so the robot’s knowledge is not a pose but a probability distribution over poses — a belief. Two models describe how the world moves and how it is sensed, each with its noise stated explicitly:

x_t = f(x_{t−1}, u_t) + w_t          motion model,       w_t ~ N(0, Q)
z_t = h(x_t) + v_t                   measurement model,  v_t ~ N(0, R)

bel(x_t) = p(x_t | z_{1:t}, u_{1:t})          the belief we maintain

predict:  bel⁻(x_t) = ∫ p(x_t | x_{t−1}, u_t) · bel(x_{t−1}) dx_{t−1}
correct:  bel(x_t)  = η · p(z_t | x_t) · bel⁻(x_t)

The two lines at the bottom are the Bayes filter, and they are a heartbeat. Predict pushes the belief through the motion model — a convolution that smears probability mass, so uncertainty grows. Correct multiplies by the measurement likelihood — a reweighting that concentrates mass where the sensor agrees, so uncertainty shrinks. Widen, narrow, widen, narrow, at sensor rate, forever. Its legality rests on the Markov assumption: the state summarizes all history, so the past influences the future only through x_t−1.

The integral is intractable over a continuous 6-DoF state, so every practical filter is a compromise about the belief’s shape. Constrain it to a Gaussian and linearize the models: the EKF. Sample the nonlinearity at sigma points: the UKF. Represent the belief with a swarm of weighted samples: the particle filter that powers global localization. Those are Phase 2 and Phase 4’s machinery; what this phase supplies is the recipe they all instantiate, plus the one Gaussian fact that explains every fusion result you will ever compute:

fusing N(μ₁, σ₁²) with N(μ₂, σ₂²):

  σ² = (1/σ₁² + 1/σ₂²)⁻¹             precisions ADD — fusion never hurts
  μ  = σ² · (μ₁/σ₁² + μ₂/σ₂²)        precision-weighted mean

example:  N(3.0, 4.0) × N(5.0, 1.0)  →  N(4.6, 0.8)

1.6Nonlinear Least Squares and Gauss-Newton

The Bayes filter runs forward in time and never revisits the past. The other pillar of this phase solves for everything at once: given a pile of measurements, find the states that explain them best. Under Gaussian noise, "best" stops being a matter of taste — maximizing the likelihood of the measurements is exactly minimizing a sum of squared, covariance-weighted residuals:

residual — what a measurement disagrees with a candidate state x by:
  rᵢ(x) = zᵢ − hᵢ(x)

Gaussian noise  ⇒  maximum likelihood = least squares:

  x* = argmax Πᵢ exp(−½ rᵢᵀ Σᵢ⁻¹ rᵢ)
     = argmin  Σᵢ  rᵢ(x)ᵀ Σᵢ⁻¹ rᵢ(x)

the covariance Σᵢ⁻¹ is the weight: confident measurements pull harder

The residuals are nonlinear in x (rotations guarantee that), so no closed-form minimum exists. Gauss-Newton is the classical fix: linearize each residual around the current guess, solve the resulting linear least-squares problem exactly, step, repeat:

linearize:   rᵢ(x ⊞ δ) ≈ rᵢ(x) + Jᵢ δ          Jᵢ = ∂rᵢ/∂δ at δ = 0

stack all residuals and solve the normal equations:

  (Jᵀ Σ⁻¹ J) δ = −Jᵀ Σ⁻¹ r          H = JᵀΣ⁻¹J is the Gauss-Newton Hessian

update:      x ← x ⊞ δ,   repeat until ‖δ‖ is tiny

Two remarks the compact notation hides. First, H = JᵀΣ⁻¹J is an approximation of the true Hessian — the dropped term involves second derivatives of the residuals scaled by the residuals themselves, so the approximation is excellent precisely when residuals are small (near the solution) or models are mildly curved. You get near-second-order convergence at first-order cost, which is the whole appeal. Second, H is sparse in every robotics problem that matters: each measurement touches only a couple of variables (one pose and one landmark; two consecutive poses), so J has a few nonzero blocks per row and H is nearly all zeros. Sparse Cholesky factorization exploits that ruthlessly — it is the only reason optimizing ten thousand poses at interactive rates is possible, and Phase 3 (pose graphs) and Phase 6 (calibration) both bank on it.

Gauss-Newton’s temper is the reason it never ships alone: it trusts its linearization completely. Near the minimum, where the quadratic model is honest, it converges in a handful of iterations. Far from the minimum, the model is fiction, the full step overshoots into worse territory, and the iteration can diverge outright.

1.7Levenberg-Marquardt and the Art of Not Overshooting

Levenberg-Marquardt is Gauss-Newton with an adjustable amount of humility. Add a damping term λ to the diagonal of H, and the same linear solve smoothly interpolates between two algorithms:

(H + λ · diag(H)) δ = −g              g = Jᵀ Σ⁻¹ r   (Marquardt's scaling;
                                       Levenberg's original used λI)

λ → 0      :  the step is pure Gauss-Newton — bold, quadratic model trusted
λ → large  :  δ ≈ −g / λ — a short, safe gradient-descent step

policy after each step:  cost ↓  →  accept, shrink λ (≈ ÷3)
                         cost ↑  →  reject, grow λ (≈ ×10), re-solve

The modern reading of λ is a trust region: the radius over which the quadratic model is believed. Good steps expand the region; bad steps shrink it. The algorithm is self-tuning, which is why LM has been the default engine of bundle adjustment, calibration, and pose-graph optimization for sixty years — and it is what actually runs when Phase 3’s SLAM backend "optimizes the graph".

1.8Optimizing on the Manifold: The Whole Phase in One Loop

Now snap the two halves of the chapter together. States live on manifolds (1.1–1.3); estimators minimize weighted squared residuals (1.6–1.7). The composite algorithm — nonlinear least squares with manifold updates — is four lines, and it is arguably the most-executed loop in robotics:

given residuals rᵢ, a state x on a manifold, weights Σᵢ⁻¹:

repeat:
    Jᵢ  = ∂ rᵢ(x ⊞ δ) / ∂δ   evaluated at δ = 0      (tangent-space Jacobian)
    solve   (Jᵀ Σ⁻¹ J + λ·diag) δ = −Jᵀ Σ⁻¹ r
    x  ← x ⊞ δ                                        (retract onto manifold)
until ‖δ‖ small

Rename the residuals and you have named half this book. Reprojection errors of 3D points into cameras: bundle adjustment. Relative-pose disagreements between odometry and loop closures: pose-graph optimization (Phase 3). Point-to-plane distances between two lidar scans: the inner loop of ICP (Phase 2). Checkerboard corners against a projection model: camera calibration (Phase 6). Tracking error against a dynamics model over a horizon: the QP heart of MPC (Phase 5). Different residuals, same loop.

1.9Ceres, g2o, GTSAM: The Three Back-Ends

Three mature C++ libraries implement section 1.8 so you never hand-roll it. They differ in abstraction, not in math:

A rule of thumb that survives contact with real projects: Ceres when your problem is "minimize this cost I just invented" (calibration, alignment, fitting); g2o when it is a textbook pose graph and you want the classic machinery; GTSAM when factors arrive continuously and you need incremental updates rather than batch re-solves. All three accept robust loss functions (Huber, Cauchy) that tame outlier measurements — a theme Phase 3 returns to when loop closures lie.

sudo apt install libceres-dev            # Ubuntu 22.04+/Debian: Ceres 2.x

cmake_minimum_required(VERSION 3.16)
project(align_se3 CXX)
set(CMAKE_CXX_STANDARD 17)
find_package(Ceres REQUIRED)
find_package(Sophus REQUIRED)
add_executable(align_se3 align_se3.cc)
target_link_libraries(align_se3 Ceres::ceres Sophus::Sophus)

// align_se3.cc — recover T_ba between two clouds by LM on the SE(3) manifold.
#include <ceres/ceres.h>
#include <sophus/se3.hpp>
#include <sophus/ceres_manifold.hpp>
#include <iostream>
#include <random>
#include <vector>

struct PointPairResidual {
  PointPairResidual(const Eigen::Vector3d& p_a, const Eigen::Vector3d& p_b)
      : p_a_(p_a), p_b_(p_b) {}

  template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    const Eigen::Map<const Sophus::SE3<T>> T_ba(pose);
    Eigen::Map<Eigen::Matrix<T, 3, 1>> r(residual);
    r = T_ba * p_a_.template cast<T>() - p_b_.template cast<T>();
    return true;
  }
  const Eigen::Vector3d p_a_, p_b_;
};

int main() {
  std::mt19937 rng(42);
  std::normal_distribution<double> noise(0.0, 0.01);        // 1 cm sensor noise
  std::uniform_real_distribution<double> coord(-2.0, 2.0);

  // ground truth: 20° about z, translated (0.5, −0.3, 0.2)
  const Sophus::SE3d T_ba_true(Sophus::SO3d::rotZ(20.0 * M_PI / 180.0),
                               Eigen::Vector3d(0.5, -0.3, 0.2));

  std::vector<Eigen::Vector3d> pts_a, pts_b;
  for (int i = 0; i < 200; ++i) {
    Eigen::Vector3d p(coord(rng), coord(rng), coord(rng));
    pts_a.push_back(p);
    pts_b.push_back(T_ba_true * p +
                    Eigen::Vector3d(noise(rng), noise(rng), noise(rng)));
  }

  Sophus::SE3d T_ba;                        // identity — a poor initial guess
  ceres::Problem problem;
  problem.AddParameterBlock(T_ba.data(), Sophus::SE3d::num_parameters,
                            new Sophus::Manifold<Sophus::SE3>());
  for (int i = 0; i < 200; ++i) {
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<PointPairResidual, 3,
                                        Sophus::SE3d::num_parameters>(
            new PointPairResidual(pts_a[i], pts_b[i])),
        nullptr,                  // experiment 2: new ceres::HuberLoss(0.05)
        T_ba.data());
  }

  ceres::Solver::Options opts;
  opts.linear_solver_type = ceres::DENSE_QR;
  opts.minimizer_progress_to_stdout = true;  // watch λ (tr_radius) breathe
  ceres::Solver::Summary summary;
  ceres::Solve(opts, &problem, &summary);

  const Sophus::SE3d err = T_ba_true.inverse() * T_ba;
  std::cout << summary.BriefReport() << "\n"
            << "translation error: " << err.translation().norm() << " m\n"
            << "rotation error   : "
            << err.so3().log().norm() * 180.0 / M_PI << " deg\n";
}

1.10Reading List

1.11Interview Questions