Optimizing over manifolds

Let \(\xi \in \mathbb{R}^6\) represent twist coordinate then \(\xi^{\wedge} \in \mathfrak{se}(3) \) an element of the tangent space at identity.

Let \( T = \left(R, t\right) \in SE(3)\) where \( R \in SO(3) \) and \( t \in \mathbb{R}^3 \). Group action is defined as \(\hat q = T \hat p\) where \(\hat x \) is the homogenous representation of \(x\). If we consider an incremental pose update parameterized by the twist \( \xi \), then the corresponding incremental change in \(\hat q \) is given by, \( \hat{q}(\xi) = T\exp( \xi^\wedge) \hat p \)

\[\begin{align} \frac{\partial \hat q(\xi)}{\partial \xi} &= T \frac{\partial}{\partial \xi} \exp(\xi^\wedge) \hat p \\ &= T \frac{\partial}{\partial \xi}(I + \xi^{\wedge}) \hat p \\ &= T \frac{\partial}{\partial \xi}\xi^{\wedge} \hat p \\ &= T \frac{\partial}{\partial \xi}\left(-p^{\wedge} \xi \right) \\ &= T \end{align}\]