Derivative of squared geodesic distance w.r.t. a tangent vector on so(3)

67 Views Asked by At

I'm a programmer, I'm self-studying the Lie group theory by this reference: "A micro Lie theory for state estimation in robotics" and struggling to find the derivative of squared geodesic distance of $\mathcal{X}, \mathcal{Y}\in SO(3)$ w.r.t. the tangent vector of $\mathcal{Y}$, where $\mathcal{Y}=\mathcal{X}\circ \text{Exp}(z)$:

$$ \frac{d\|\text{Log}(\mathcal{X}^{-1}\circ \mathcal{Y})\|^2_2}{d\text{Log}(\mathcal{Y})} $$

I'm expecting the result to be $2(\text{Log}(\mathcal{X}^{-1}\mathcal{Y}))=2z$ because I have run several simulations on my computer with an auto differentiation package, where the results are very close to what I'm expecting. But I have no idea how to prove it theoretically. Here is my derivation following the aforementioned reference: $$ \begin{align} \frac{d\|\text{Log}(\mathcal{X}^{-1}\circ \mathcal{Y})\|^2_2}{d\text{Log}(\mathcal{Y})} &= \frac{d\text{Log}(\mathcal{X}^{-1}\circ\mathcal{Y})}{d\text{Log}(\mathcal{Y})} \cdot 2z \\ &= \frac{d\text{Log}(\mathcal{X}^{-1}\circ\mathcal{Y})}{d\mathcal{X}^{-1}\circ\mathcal{Y}}\cdot\frac{d\mathcal{X}^{-1}\circ\mathcal{Y}}{d\mathcal{Y}}\cdot\frac{d\mathcal{Y}}{d\text{Log}(\mathcal{Y})} \cdot 2z \\ &= \mathbf{J}_{r}^{-1}(z)\cdot I\cdot \mathbf{J}_{r}(\text{Log}(\mathcal{Y}))\cdot 2z \end{align} $$ I don't know how to deal with these two Jacobian matrices. Can someone help me? Many thanks.