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} \]