Optimising on the manifold

Gauss–Newton wants to compute an update $\Delta$ and apply it as $x \leftarrow x + \Delta$ . For a rotation that is immediately broken: add anything to a rotation matrix and it stops being one. The set of rotations is a curved surface in the space of $3\times3$ matrices — a manifold — and addition leaves it.

The fix is to keep the state on the manifold and the update in a flat space attached to it. At any rotation, the tangent space is three-dimensional; a vector in it is an angular velocity $\boldsymbol{\omega}$ , and the exponential map turns it into a rotation:

$$\exp: \mathfrak{so}(3) \to SO(3), \qquad \exp([\boldsymbol{\omega}]_\times) = I + \sin\theta\,[\mathbf{k}]_\times + (1-\cos\theta)[\mathbf{k}]_\times^2.$$

That is Rodrigues' formula again, now with a job title. Updates then compose multiplicatively:

$$R \leftarrow R \exp([\Delta\boldsymbol{\omega}]_\times).$$

Three numbers, no constraint, no drift, and a $3\times3$ Hessian block instead of a $9\times9$ one with six redundant directions.

The same construction extends to full poses. $SE(3)$ has a six-dimensional tangent space — a twist $\xi = (\mathbf{v}, \boldsymbol{\omega})$ — and

$$\exp(\xi) = \begin{bmatrix} \exp([\boldsymbol{\omega}]_\times) & V\mathbf{v} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \qquad V = I + \frac{1-\cos\theta}{\theta}[\mathbf{k}]_\times + \frac{\theta - \sin\theta}{\theta}[\mathbf{k}]_\times^2.$$

The matrix $V$ is worth a moment. $\mathbf{v}$ is not the resulting translation — it is a velocity that acts while the frame is simultaneously rotating, and $V$ integrates that coupling. Only when $\boldsymbol{\omega} = 0$ do the two coincide. This is why a "small pose update" is six numbers with a non-obvious relationship to the twelve numbers of the matrix it produces.