SLAM——Geometry

Posted on Edited on In

回顾一下SLAM中的几何基础。

李群李代数（$\text{SO(3) } & ~\mathfrak{so}(3)$）：

  • 李群李代数对应的都是三维矩阵，在SLAM中，李群$\text{SO(3) }$ 对应旋转矩阵（方向余弦阵,3×3 正交阵），而李代数$\mathfrak{so}(3)$对应的为三维向量的反对称矩阵，只不过任何一个反对称矩阵都和一个三维向量是一一对应的，因此有时候也会直接用三维向量来指代$\mathfrak {so}(3)$的元素。
  • 一般来说反对称矩阵与向量之间的转换用$\wedge$与$\vee$运算符表示。有三维向量$\mathbf w$，则它与其对应的反对称矩阵$\mathbf W$转化关系为：

$$
\mathbf w^{\wedge}=\left[\begin{array}{l}w_ {1} \w_ {2} \w_ {3}\end{array}\right]^{^{\wedge}}=\left[\begin{array}{ccc}0 & -w_ {3} & w_ {2} \w_ {3} & 0 & -w_ {1} \-w_ {2} & w_ {1} & 0\end{array}\right]=\mathbf{W}\\mathbf{W}^{\vee}=\mathbf w
$$

关于反对称矩阵的一个性质：

$$
\mathbf{a}^{\wedge} \cdot \mathbf{b}=-\mathbf{b}^{\wedge} \cdot \mathbf{a}, \quad \forall \mathbf{a}, \mathbf{b} \in \mathbb R^{3}
$$

  • 指数映射$\exp(\cdot)$将$\mathfrak{so}(3)$映射到$\text{SO(3)}$：

$$
\exp \left(\vec{\phi}^{\wedge}\right)=\mathbf{I}+\frac{\sin (|\vec{\phi}|)}{|\vec{\phi}|} \vec{\phi}^{\wedge}+\frac{1-\cos (|\vec{\phi}|)}{|\vec{\phi}|^{2}}\left(\vec{\phi}^{\wedge}\right)^{2}
$$

特别，当$\phi$很小的时，有一阶近似：

$$
\exp \left(\vec{\phi}^{\wedge}\right) \approx \mathbf{I}+\vec{\phi}^{\wedge}
$$

  • 对数映射$\log(\cdot)$将$\text{SO}(3)$映射到$\mathfrak{so}(3)$：

$$
\log (\mathbf{R})=\frac{\varphi \cdot\left(\mathbf{R}-\mathbf{R}^{T}\right)}{2 \sin (\varphi)}
$$

其中：$\varphi=\cos ^{-1}\left(\frac{\operatorname{tr}(R)-1}{2}\right)$。有$\log (\mathbf{R})^{\vee}=\varphi \cdot \mathbf{a}$，其中$\varphi$是旋转角，而$\mathbf a$为旋转轴的单位向量，且$\mathbf{a}=\left(\frac{\mathbf{R}-\mathbf{R}^{T}}{2 \sin (\varphi)}\right)^{\vee}$。因此，每一个旋转矩阵对应一个旋转向量。很奇妙的事情是一个旋转向量对应的旋转矩阵，也就是罗德里格斯公式求得的旋转矩阵（从几何关系上推导出来并不难），正是该旋转向量的反对称矩阵$\left(\mathfrak{so}(3)\right)$在$\text{SO(3)}$上的映射。

有意思的事情，任何一个三维向量实际上都可以看作是一个旋转向量（方向为旋转轴，而大小为旋转角度），因此任何一个三维向量都可以对应到一个旋转矩阵。但是不是所有的三维矩阵都是旋转矩阵，因此尽管三维向量与三维矩阵都是无穷多个，但是三维矩阵的个数就是比三维向量“多”，这个也是非常直观的，因为三维矩阵有9个变量，同时，旋转矩阵所在的这个空间实际上就“摊”满了三维向量的空间。

  • 当$\left\Vert \vec{\phi}\right\Vert<\pi$时（或者$\left\Vert\vec{\phi} \right\Vert$被限制在一个连续的$2\pi$范围内），指数映射与对数映射是双射，而$\left\Vert\vec{\phi} \right\Vert$也就是旋转角度$\varphi$。
  • 从上面的内容可以得知，实际上任何一个旋转矩阵都和一个三维向量是对应的，因此为了方便，我们规定$\text{Exp}(\cdot)$把$\mathbb R^3$映射到$\text{SO(3)}$，而$\text{Log}(\cdot)$把$\text{SO(3)}$映射到$\mathbb R^3$，也就是：

$$
\text{Exp}(\vec\phi) = \exp(\vec\phi^{\wedge}) , ~\text{Log}(\mathbf R) = \log^\vee(\mathbf R)
$$

  • 对于$\vec\phi$以及一个微小量$\delta\vec\phi$，有：

$$
\begin{array}{l}\operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_ {r}(\vec{\phi}) \cdot \delta \vec{\phi}\right) \\text{Log} (\operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}(\delta \vec{\phi}))=\vec{\phi}+\mathbf{J}_ {r}^{-1}(\vec{\phi}) \cdot \delta \vec{\phi}\end{array}
$$

也就是三维向量上加一个微小量，对应了$\text{SO(3)}$上乘一个$\operatorname{Exp}\left(\mathbf{J}_ {r}(\vec{\phi}) \cdot \delta \vec{\phi}\right)$，而在$\text{SO(3)}$上乘一个$\operatorname{Exp}(\delta \vec{\phi})$也就对应了在$\mathbb R^3$上加一个$\mathbf{J}_ {r}^{-1}(\vec{\phi}) \cdot \delta \vec{\phi}$。其中，$\mathbf{J}_ {r}(\vec{\phi})$ 是属于$\text{SO(3)}$空间的右Jacobian，有：

$$
\begin{array}{l}\mathbf{J}_ {r}(\vec{\phi})=\mathbf{I}-\frac{1-\cos (|\vec{\phi}|)}{|\vec{\phi}|^{2}} \vec{\phi}^{\wedge}+\frac{|\vec{\phi}|-\sin (|\vec{\phi}|)}{|\vec{\phi}|^{3}}\left(\vec{\phi}^{\wedge}\right)^{2} \\mathbf{J}_ {r}^{-1}(\vec{\phi})=\mathbf{I}+\frac{1}{2} \vec{\phi}^{\wedge}+\left(\frac{1}{|\vec{\phi}|^{2}}-\frac{1+\cos (|\vec{\phi}|)}{2 \cdot|\vec{\phi}| \cdot \sin (|\vec{\phi}|)}\right)\left(\vec{\phi}^{\wedge}\right)^{2}\end{array}
$$

当$\delta\vec\phi$很小时，有

$$
\mathbf{J}_ {r}(\delta\vec{\phi}) \approx\mathbf{J}^{-1}_ {r}(\delta\vec{\phi})\approx \mathbf I\\operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right) \approx \mathbf{I}+(\delta\vec\phi_i)^{\wedge}
$$

  • 指数映射的伴随（Adjoint）性质：

$$
\begin{array}{l}\mathbf{R} \cdot \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}^{T}=\exp \left(\mathbf{R} \vec{\phi}^{\wedge} \mathbf{R}^{T}\right)=\operatorname{Exp}(\mathbf{R} \vec{\phi}) \\Leftrightarrow \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}=\mathbf{R} \cdot \operatorname{Exp}\left(\mathbf{R}^{T} \vec{\phi}\right)\end{array}
$$

为了证明伴随性质，我们首先要知道一个前提：$(\mathbf{R} \vec{\phi})^{\wedge}=\mathbf{R} \vec{\phi}^{\wedge} \mathbf{R}^{T}$。令$\vec \phi = \theta \mathbf a$，根据罗德里格斯公式，可得：

$$
\begin{array}{l}\operatorname{Exp}(\vec{\phi})=\exp \left(\vec{\phi}^{\wedge}\right)=\exp \left(\theta \mathbf{a}^{\wedge}\right) \=\cos \theta \cdot \mathbf{I}+(1-\cos \theta) \mathbf{a} \mathbf{a}^{T}+\sin \theta \cdot \mathbf{a}^{\wedge}\end{array}
$$

那么有（伴随性质第一行的证明）：

$$
\begin{array}{l}\mathbf{R} \cdot \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}^{T} \=\mathbf{R} \cdot\left[\cos \theta \cdot \mathbf{I}+(1-\cos \theta) \mathbf{a} \mathbf{a}^{T}+\sin \theta \cdot \mathbf{a}^{\wedge}\right] \cdot \mathbf{R}^{T} \=\cos \theta \cdot \mathbf{R} \mathbf{R}^{T}+(1-\cos \theta) \cdot \mathbf{R a a}^{T} \mathbf{R}^{T}+\sin \theta \cdot \mathbf{R} \mathbf{a}^{\wedge} \mathbf{R}^{T} \=\cos \theta \cdot \mathbf{I}+(1-\cos \theta) \cdot(\mathbf{R a}) \cdot(\mathbf{R} \mathbf{a})^{T}+\sin \theta \cdot(\mathbf{R} \mathbf{a})^{\wedge} \=\exp \left(\theta(\mathbf{R} \mathbf{a})^{\wedge}\right)=\exp \left((\mathbf{R} \vec{\phi})^{\wedge}\right)\=\exp \left(\mathbf{R} \vec{\phi}^{\wedge} \mathbf{R}^{T}\right)\ = \text{Exp}(\mathbf{R\vec\phi})\end{array}
$$

对于第二行：

$$
\begin{aligned}&\mathbf{R} \cdot \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}^{T}=\operatorname{Exp}(\mathbf{R} \vec{\phi})\\Leftrightarrow&\mathbf{R}^T \cdot \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}=\operatorname{Exp}(\mathbf{R}^T \vec{\phi})\\Leftrightarrow&\operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}=\mathbf R\operatorname{Exp}(\mathbf{R}^T \vec{\phi})\end{aligned}
$$