>
返回

一些常见的关于旋转的函数对旋转求导过程的推导

简介

主要进行了一些常见的关于旋转的函数对旋转求导过程的推导。

旋转矩阵函数对自变量求导

我们为什么要关注在李群或者李代数上叠加微小量的情况呢?因为在求导过程中,我们常见的做法是对自变量添加一个微小值来进行:

$$
f'(x) = \lim_{\Delta x\rightarrow0}\frac{f(x+\Delta x)}{\Delta x}
$$

对于旋转矩阵 $SO(3)$ 我们不能这么做,因为李群对加法不封闭,因此两个旋转矩阵相加不一定是旋转矩阵,但是利用李代数,根据上面两个方向的 BCH 近似不难看出我们有两种思路进行求导,分别是:

  • 用李代数(旋转向量)来表示姿态,然后利用李代数加法叠加微小量并对该微小量进行求导
  • 用李群(旋转矩阵)表示姿态,然后左/右乘上一个扰动,然后对该扰动求导,即左扰动模型和右扰动模型

旋转点的求导推导

下面举例,假设我们对空间一个点 $\boldsymbol{p}$ 使用旋转矩阵 $\boldsymbol{R}$ 进行旋转得到 $\boldsymbol{Rp}$,我们想要计算旋转后的点的坐标对于旋转的导数,记为:

$$
\boldsymbol{J}_R = \frac{\partial \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p})}{\partial \boldsymbol{R}} = \frac{\partial \boldsymbol{Rp}}{\partial \boldsymbol{R}}
$$

上述是一个记号(并不是对旋转矩阵本身求导!),在实际计算时我们通常会对优化变量施加微小扰动 $\boldsymbol{\phi}$,通过最小化扰动来对误差进行线性化,此时对函数线性化有:$\boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) = \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\delta\boldsymbol{\phi}$,其中 $\boldsymbol{J}_{\boldsymbol{R}_0}$ 是函数在 $\boldsymbol{R}_0$ 的雅可比矩阵。

这篇博客里面主要关注李群变量(具体来说是旋转变量),由于旋转矩阵 $\boldsymbol{R} \in SO(3)$ 本身是李群,没有向量空间的加法,因此添加扰动我们有三种方法:

  • 李代数求导:在李群对应的李代数的局部坐标上,即:($\boldsymbol{\phi}, \boldsymbol{\phi}^\wedge\in \mathfrak{so}(3)$) 上添加扰动,即:$\boldsymbol{\phi} \leftarrow \boldsymbol{\phi} + \delta\boldsymbol{\phi}$,由于李代数本身对应旋转向量,因此对旋转向量添加扰动相当于同时改变旋转轴和旋转角度。
  • 旋转矩阵右扰动求导:由于旋转矩阵没有加法,因此要对旋转矩阵本身添加扰动,需要先通过指数映射将李代数转化为李群,然后根据李群的运算来添加扰动,即:$\boldsymbol{R} \leftarrow \boldsymbol{R}\exp{(\boldsymbol{\phi}^\wedge)}$,由于是旋转矩阵右乘扰动,因此相当于是在局部坐标系下对旋转矩阵进行更新
  • 旋转矩阵左扰动求导:和右扰动同理,我们也可以将扰动添加在旋转矩阵左侧,即:$\boldsymbol{R} \leftarrow \exp{(\boldsymbol{\phi}^\wedge)}\boldsymbol{R}$,由于是旋转矩阵左乘扰动,因此相当于在全局坐标系下对旋转矩阵进行更新

首先将旋转矩阵(李群 $SO(3)$)转换为旋转向量(李代数 $\boldsymbol{\mathfrak{so}}(3)$),并对旋转向量求导,这里原先我用的是高博十四讲的推导流程,但是从个人直觉上不如直接更具雅可比矩阵的定义(对函数进行线性化)直观,因此改为这种求导顺序,本质上都是一样的,而且写法更简洁:)

$$
\begin{aligned}
    \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) &= \boldsymbol{Rp} = \exp{(\boldsymbol{\phi}^\wedge})\boldsymbol{p}\\
    &= \exp{((\boldsymbol{\phi}_0 + \delta\boldsymbol{\phi})^\wedge})\boldsymbol{p}\\
    &= \exp{(\boldsymbol{\phi}_0^\wedge + \delta\boldsymbol{\phi}^\wedge})\boldsymbol{p}\\
    \mathrm{BCH 近似}&\approx \exp{((\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge})\exp{(\boldsymbol{\phi}_0^\wedge)}\boldsymbol{p} \\
    \mathrm{泰勒展开并去除高阶项}&\approx (\boldsymbol{I} + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge)\exp{(\boldsymbol{\phi}_0^\wedge)}\boldsymbol{p} \\
    &= \exp{(\boldsymbol{\phi}_0^\wedge})\boldsymbol{p} + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge\exp{(\boldsymbol{\phi}_0^\wedge)}\boldsymbol{p} \\
    &= \boldsymbol{R}_0\boldsymbol{p} + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge\boldsymbol{R}_0\boldsymbol{p} \\
    &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + (\boldsymbol{J}_l\delta\boldsymbol{\phi})^\wedge\boldsymbol{R}_0\boldsymbol{p} \\
    \mathrm{利用a^\wedge b = -b^\wedge a 性质}&= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{J}_l\delta\boldsymbol{\phi} \\
    &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\delta\boldsymbol{\phi} \\
    \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{J}_l
\end{aligned}
$$

最后我们得到以下结果:

$$
\boldsymbol{J}_{\boldsymbol{R}_0} = - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{J}_l(\boldsymbol{\phi}_0)
$$

其中涉及到 $\boldsymbol{J}_l(\boldsymbol{\phi}_0)$ 的计算, 因此相对而言还是比较复杂。

  • 左扰动模型

在旋转矩阵上左乘一个扰动,并对函数进行线性化。

$$
\begin{aligned}
    \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) &= \boldsymbol{Rp}\\
    &= \exp{(\boldsymbol{\psi})^\wedge}\boldsymbol{R}_0\boldsymbol{p}\\
    \mathrm{泰勒展开并去除高阶项}&\approx (\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\boldsymbol{R}_0\boldsymbol{p}\\
    &= \boldsymbol{R}_0\boldsymbol{p} + \boldsymbol{\psi}^\wedge\boldsymbol{R}_0\boldsymbol{p}\\
    &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{\psi}^\wedge\boldsymbol{R}_0\boldsymbol{p}\\
    \mathrm{利用a^\wedge b = -b^\wedge a 性质}&= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{\psi} \\
    &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\psi} \\
        \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= - (\boldsymbol{R}_0\boldsymbol{p})^\wedge
\end{aligned}
$$

不难看出来,利用左扰动模型计算的导数比使用李代数直接求导省去了一个 $\boldsymbol{J}_l(\boldsymbol{\phi}_0)$ 的计算,因此更为实用,同时理论上精度也会更高(因为在计算该矩阵时需要近似)。

  • 右扰动模型

在旋转矩阵上右乘一个扰动,并对扰动进行求导。

$$
\begin{aligned}
    \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) &= \boldsymbol{Rp}\\
    &= \boldsymbol{R}_0\exp{(\boldsymbol{\psi})^\wedge}\boldsymbol{p}\\
    \mathrm{泰勒展开并去除高阶项}&\approx \boldsymbol{R}_0(\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\boldsymbol{p}\\
    &= \boldsymbol{R}_0\boldsymbol{p} + \boldsymbol{R}_0\boldsymbol{\psi}^\wedge\boldsymbol{p}\\
    &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{R}_0\boldsymbol{\psi}^\wedge\boldsymbol{p}\\
    \mathrm{利用a^\wedge b = -b^\wedge a 性质}&= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) - \boldsymbol{R}_0\boldsymbol{p}^\wedge\boldsymbol{\psi} \\
    &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\psi} \\
        \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= - \boldsymbol{R}_0\boldsymbol{p}^\wedge\\
\end{aligned}
$$

不难看出来,相比于左扰动的模型中计算 $\boldsymbol{Rp}$ 的反对称矩阵,右扰动模型计算的是 $\boldsymbol{p}$ 的反对称矩阵,因此有细微的区别,使用时注意区分。

旋转连乘的求导的推导

除了旋转点以外,我们还经常需要对两个旋转叠加的结果 $\boldsymbol{R}_1\boldsymbol{R}_2$ 对其中一个旋转进行求导,求导形式取决于我们对函数定义方式,在 GTSAM 学习记录(三): 常见李群李代数及其函数的导数和微分,我整理了使用李群对李群的映射的雅可比矩阵求导方式,通常来说函数需要将变量映射到向量空间,因此这里我们将旋转通过对数映射转化为对应李代数的坐标(3维向量),并对该函数进行线性化从而求解雅可比矩阵,即:

$$
\begin{aligned}
    \boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\
    &=\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}^0_2)}\boldsymbol{\phi}_1 + \boldsymbol{J}^2_{(\boldsymbol{R}^0_1, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_2\\
    \boldsymbol{J}_{(\boldsymbol{R}_1^0, \boldsymbol{R}^2_0)} &= \begin{bmatrix}
        \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}^0_2)} & \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}
    \end{bmatrix} \in \mathbb{R}^{3\times 6}
\end{aligned}
$$

首先,对 $\boldsymbol{R}_2$ 求导,此时固定 $\boldsymbol{R}_1$

  • 右扰动模型

如下所示:

$$
\begin{aligned}
    \boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\
    &= (\ln{(\boldsymbol{R}_1^0\boldsymbol{R}_2^0\exp{(\boldsymbol{\phi}_2^\wedge})})^\vee\\
    \mathrm{BCH 近似}&\approx (\ln{(\boldsymbol{R}_1^0\boldsymbol{R}_2^0)})^\vee + \boldsymbol{J}_r^{-1}\boldsymbol{\phi}_2\\
    &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_2\\
    \Rightarrow \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0))
\end{aligned}
$$
  • 左扰动模型

过程大同小异。

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\
&= (\ln{(\boldsymbol{R}_1^0\exp{(\boldsymbol{\phi}_2^\wedge})}\boldsymbol{R}_2^0)^\vee\\
\mathrm{SO(3) 的伴随性质}&= (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2\exp{(\boldsymbol{R}_2^{0T}\boldsymbol{\phi}_2)^\wedge}})^\vee\\
\mathrm{BCH 近似}&\approx (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2)})^\vee+\boldsymbol{J}^{-1}_r\boldsymbol{R}_{2}^{0T}\boldsymbol{\phi}_2\\
&= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^2_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_2\\
\Rightarrow \boldsymbol{J}^2_{(\boldsymbol{R}_1, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0))\boldsymbol{R}_2^T
\end{aligned}
$$

$\boldsymbol{R}_1$ 求导。

  • 右扰动模型

过程如下:

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\
        &= (\ln{(\boldsymbol{R}_1^0\exp{(\boldsymbol{\phi}_1^\wedge})}\boldsymbol{R}_2^0)^\vee\\
        \mathrm{SO(3) 的伴随性质}&= (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2\exp{(\boldsymbol{R}_2^{0T}\boldsymbol{\phi}_1)^\wedge}})^\vee\\
        \mathrm{BCH 近似}&\approx (\ln{(\boldsymbol{R}^0_1\boldsymbol{R}^0_2)})^\vee+\boldsymbol{J}^{-1}_r\boldsymbol{R}_{2}^{0T}\boldsymbol{\phi}_1\\
        &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_1\\
        \Rightarrow \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0))\boldsymbol{R}_2^T
\end{aligned}
$$
  • 左扰动模型

过程如下:

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}_1, \boldsymbol{R}_2) &= (\ln{(\boldsymbol{R}_1\boldsymbol{R}_2)})^\vee\\
        &= (\ln{(\exp{(\boldsymbol{\phi}_1^\wedge})}\boldsymbol{R}_1^0\boldsymbol{R}_2^0)^\vee\\
    \mathrm{BCH 近似}&\approx \ln{(\boldsymbol{R}_1^0\boldsymbol{R}_2^0)^\vee} + \boldsymbol{J}_l^{-1}\boldsymbol{\phi}_1\\
    &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}_l^{-1}\boldsymbol{\phi}_1 \\
    &= \boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0) + \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)}\boldsymbol{\phi}_1\\
    \Rightarrow \boldsymbol{J}^1_{(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0)} &= \boldsymbol{J}_l^{-1} = \boldsymbol{J}_l^{-1}(\boldsymbol{e}(\boldsymbol{R}_1^0, \boldsymbol{R}_2^0))
\end{aligned}
$$

可以发现,无论是对 $\boldsymbol{R}_1$ 还是 $\boldsymbol{R}_2$ 使用左扰动或者右扰动求导,我们总是想办法使用 BCH 或者 伴随性质将 $\boldsymbol{R}_1\boldsymbol{R}_2$ 凑到一起与第二项相减,将扰动单独分离出来获得结果,过程大同小异。

逆旋转点的求导

下面进行对逆旋转点的函数的求导,即对下列函数进行线性化,对旋转部分求导:

$$
\begin{aligned}
    \boldsymbol{e}(\boldsymbol{R}) &= \boldsymbol{R}^{-1}\boldsymbol{p}\\
    &= \boldsymbol{e}(\boldsymbol{R}_0) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\phi}
\end{aligned}
$$

式中,$\boldsymbol{J}_{R_0}$ 为函数 $\boldsymbol{e}(\boldsymbol{R})$$\boldsymbol{R}_0$ 的雅可比矩阵

  • 左扰动模型

过程如下:

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}) &= \boldsymbol{R}^{-1}\boldsymbol{p}\\
    &= (\exp(\boldsymbol{\phi}^\wedge)\boldsymbol{R}_0)^{-1}\boldsymbol{p}\\
    \mathrm{逆矩阵的性质}&= \boldsymbol{R}_0^{-1}\exp(\boldsymbol{\phi}^\wedge)^{-1}\boldsymbol{p}\\
    \mathrm{指数映射的性质}&= \boldsymbol{R}_0^{-1}\exp(-\boldsymbol{\phi}^\wedge)\boldsymbol{p}\\
    \mathrm{泰勒展开并去除高阶项}&= \boldsymbol{R}_0^{-1}(\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{p}\\
    &= \boldsymbol{R}_0^{-1}\boldsymbol{p} -\boldsymbol{R}_0^{-1}\boldsymbol{\phi}^\wedge\boldsymbol{p}\\
    &= \boldsymbol{R}_0^{-1}\boldsymbol{p} +\boldsymbol{R}_0^{-1}\boldsymbol{p}^\wedge\boldsymbol{\phi}\\
    &= \boldsymbol{e}(\boldsymbol{R}_0) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\phi}\\
    \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= \boldsymbol{R}_0^{-1}\boldsymbol{p}^\wedge
\end{aligned}
$$
  • 右扰动模型

过程如下:

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}) &= \boldsymbol{R}^{-1}\boldsymbol{p}\\
    &= (\boldsymbol{R}_0\exp(\boldsymbol{\phi}^\wedge))^{-1}\boldsymbol{p}\\
    \mathrm{逆矩阵的性质}&= \exp(\boldsymbol{\phi}^\wedge)^{-1}\boldsymbol{R}_0^{-1}\boldsymbol{p}\\
    \mathrm{指数映射的性质}&= \exp(-\boldsymbol{\phi}^\wedge)\boldsymbol{R}_0^{-1}\boldsymbol{p}\\
    \mathrm{泰勒展开并去除高阶项}&= (\boldsymbol{I} -\boldsymbol{\phi}^\wedge)\boldsymbol{R}_0^{-1}\boldsymbol{p}\\
    &= \boldsymbol{R}_0^{-1}\boldsymbol{p} -\boldsymbol{\phi}^\wedge\boldsymbol{R}_0^{-1}\boldsymbol{p}\\
    &= \boldsymbol{R}_0^{-1}\boldsymbol{p} +(\boldsymbol{R}_0^{-1}\boldsymbol{p})^\wedge\boldsymbol{\phi}\\
    &= \boldsymbol{e}(\boldsymbol{R}_0) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\phi}\\
    \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= (\boldsymbol{R}_0^{-1}\boldsymbol{p})^\wedge
\end{aligned}
$$

逆旋转函数的求导

在求解 SLAM 问题时,有时候需要最小化相对旋转,因此会引入有关逆旋转的函数,这里同样用逆旋转后的旋转对应的李代数的局部坐标作为误差:

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}) &= (\ln(\boldsymbol{R}_1\boldsymbol{R}^{-1}))^\vee\\
&= \boldsymbol{e}(\boldsymbol{R}_0) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\phi}
\end{aligned}
$$

注:如果将 $\boldsymbol{R}_1$ 也作为变量进行求导,只需要将 $\boldsymbol{R}^{-1}$ 视为一个整体应用上面推导连乘时的结论即可。

  • 左扰动模型

过程如下:

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}) &= (\ln(\boldsymbol{R}_1\boldsymbol{R}))^\vee\\
&=  (\ln(\boldsymbol{R}_1(\exp{(\boldsymbol{\phi}^\wedge)}\boldsymbol{R}_0)^{-1}))^\vee\\
&=  (\ln(\boldsymbol{R}_1\boldsymbol{R}_0^{-1}\exp{(\boldsymbol{\phi}^\wedge)}^{-1})^\vee\\
&= (\ln(\boldsymbol{R}_1\boldsymbol{R}_0^{-1}\exp{(-\boldsymbol{\phi}^\wedge)})^\vee\\
&= (\ln(\boldsymbol{R}_1\boldsymbol{R}_0^{-1}))^\vee + \boldsymbol{J}_r^{-1}(-\boldsymbol{\phi})\\
&= \boldsymbol{e}(\boldsymbol{R}_0) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\phi}\\ 
\Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0}&= -\boldsymbol{J}_r^{-1} = -\boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_0))
\end{aligned}
$$
  • 右扰动模型

过程如下:

$$
\begin{aligned}
\boldsymbol{e}(\boldsymbol{R}) &= (\ln(\boldsymbol{R}_1\boldsymbol{R}))^\vee\\
&=  (\ln(\boldsymbol{R}_1(\boldsymbol{R}_0\exp{(\boldsymbol{\phi}^\wedge)})^{-1}))^\vee\\
&=  (\ln(\boldsymbol{R}_1\exp{(\boldsymbol{\phi}^\wedge)}^{-1}\boldsymbol{R}_0^{-1})^\vee\\
&= (\ln(\boldsymbol{R}_1\exp{(-\boldsymbol{\phi}^\wedge)}\boldsymbol{R}_0^{-1})^\vee\\
&= (\ln(\boldsymbol{R}_1\boldsymbol{R}_0^{-1}\exp{(-\boldsymbol{R}_0\boldsymbol{\phi}^\wedge)})^\vee\\
&= (\ln(\boldsymbol{R}_1\boldsymbol{R}_0^{-1}))^\vee + \boldsymbol{J}_r^{-1}(-\boldsymbol{R}_0\boldsymbol{\phi})\\
&= \boldsymbol{e}(\boldsymbol{R}_0) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\phi}\\ 
\Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0}&= -\boldsymbol{J}_r^{-1}\boldsymbol{R}_0 = -\boldsymbol{J}_r^{-1}(\boldsymbol{e}(\boldsymbol{R}_0))\boldsymbol{R}_0
\end{aligned}
$$
Built with Hugo
Theme Stack designed by Jimmy