Optimization on manifolds
Everything built so far comes together here. A least-squares problem over rotations or poses is solved exactly like one over vectors, with a single change. The step is computed in the tangent space and applied with $\oplus$ instead of $+$. This part runs that recipe on three problems that together cover much of robotics and vision: aligning two point clouds, averaging noisy rotations, and correcting a drifting trajectory with a loop closure. The companion guide Nonlinear Optimization develops the solvers themselves in depth.
The recipe
Linearize in the tangent space, retract with ⊕
To minimize $F(X) = \tfrac12\sum_i\lVert r_i(X)\rVert^2$ over a group element $X$:
With $\lambda = 0$ this is Gauss–Newton; with $\lambda > 0$ adapted per iteration it is Levenberg–Marquardt. The linear system always has the dimension of the group (3 for a rotation, 6 for a pose), never the 9 or 12 entries of a matrix, and $\oplus$ keeps every iterate a valid rotation. The alternatives each break something. Optimizing Euler angles inherits gimbal lock: the Jacobian loses rank near $\pm90^\circ$ pitch, and the solver crawls or diverges there. Optimizing nine matrix entries and re-orthonormalizing afterwards takes steps the constraint immediately undoes. It wastes iterations, and it has no clean covariance at the end. Solvers expose the recipe as an interface: Ceres calls it a Manifold (Plus and Minus), GTSAM calls it retract and localCoordinates, and g2o calls it oplusImpl.
Aligning two point clouds on SE(3)
The inner loop of ICP
Given matched points $p_i$ (source) and $q_i$ (target), find the pose $T$ minimizing $\sum_i\lVert Tp_i - q_i\rVert^2$. The residual is $r_i = Tp_i - q_i$, and its right Jacobian comes straight from the table in Part 9: $J_i = [\,R\ \ -R[p_i]_\times\,]$. This is the inner loop of ICP (iterative closest point), which re-matches points between solves. With known matches there is also a closed-form answer, Kabsch's SVD method (here computed with Horn's quaternion form). Gauss–Newton on $SE(3)$ reaches it in a handful of iterations from almost any start.
Switch the solver to "Euler angles" to see what a parametrization costs. The true pose is pitched to $85^\circ$, close to gimbal lock, and the same Gauss–Newton steps on $(\psi,\theta,\phi,t)$ with a numerical Jacobian converge far more slowly, or stall.
Magenta: target points $q_i$. Blue: source points moved by the current estimate $T p_i$, with a thin line to their match. Right: $\log_{10}$ cost per iteration. Drag to orbit.
Averaging rotations
Geodesic mean, chordal mean, geodesic median
Many sensors measure one orientation, or many relative measurements imply one: the best single rotation is some kind of average. The geodesic (Karcher) mean minimizes $\sum_i\lVert\operatorname{Log}(M^\top R_i)\rVert^2$. Its Gauss–Newton step is just the average of the residuals, $M\leftarrow M\oplus\tfrac1N\sum_i\operatorname{Log}(M^\top R_i)$, which is the circle's intrinsic mean from Part 1 in 3D. The chordal mean averages the matrices and projects back to $SO(3)$ (Part 1's "average then snap"). It is closed-form, and because chordal distance saturates at large angles (Part 5), a far-away outlier pulls on it only weakly. The geodesic median minimizes $\sum_i\lVert\operatorname{Log}(M^\top R_i)\rVert$ (not squared) with Weiszfeld's reweighting. Being an L1 estimator, it also shrugs off gross outliers. Add outliers and compare: the geodesic mean, whose pull grows with an outlier's distance, is dragged by more than ten degrees, while the other two stay within a few degrees.
Every rotation drawn as $\operatorname{Log}(R_{\text{true}}^\top R)$, so the truth is the centre of the ball of radius π. Gray: inliers. Magenta: outliers. Colored crosses: the three estimates. Drag to orbit.
A pose graph closed by one loop
Gauss–Newton on 20 poses in SE(2)
A robot drives a loop of 20 poses, integrating noisy odometry $Z_{i,i+1}$, and at the end recognizes its starting place, which gives one extra edge $Z_{19,0}$. Each edge has the residual $r_{ij} = \operatorname{Log}(Z_{ij}^{-1}X_i^{-1}X_j)\in\mathbb{R}^3$. The state is 19 free poses (the first is fixed), so each Gauss–Newton step solves a $57\times57$ system and updates every pose by $X_k\leftarrow X_k\oplus\delta_k$. The Jacobians here are numerical, for brevity. A real solver uses the analytic ones from Part 9 and exploits the sparsity: each edge touches only two poses. Dead reckoning (magenta) drifts; one step bends the whole loop back toward the truth.