视觉slam-----第一篇刚体和李群

最新推荐文章于 2023-01-12 12:02:57 发布

loricon_test

最新推荐文章于 2023-01-12 12:02:57 发布

阅读量472

点赞数

分类专栏：视觉slam学习文章标签： slam学习

本文链接：https://blog.youkuaiyun.com/jialing678/article/details/86022624

版权

视觉slam学习专栏收录该内容

5 篇文章

订阅专栏

刚体运动

为什么学习slam

本人早期从事硬件和嵌入式开发，之后又对无人机产生兴趣，从事了一段时间的农业无人机开发工作，主要是算法方面的。现在又从事导航算法的开发，开发过程中接触了一些视觉导航方面的技术，但属于很特殊的视觉导航，和现在的主流格格不入，因此打算从零开始学习视觉slam。从最起初开始，从硬件到软件，从软件到算法，从算法到数学，视觉slam系列就是用来记录本人的学习过程，即是对自己学习过程的记录，也希望自己的收获能与各位网友分享。

视觉slam系列更多偏向对本人的记录而非讲解，因此叙述部分会占多数，推导过程几乎没有，文章主要内容摘自《视觉slam十四讲》《计算机视觉中的多视图几何》《机器人学中的状态估计》，博客内容为读书笔记，所有内容均摘自以上几本书。

slam与刚体

slam需要解决的重要问题就是相机本身的运动，这个时候需要把相机简化为一个刚体，这个时候研究的重点就是刚体在三维空间中的转动和平动问题。

向量描述

三维空间中的某个向量的坐标可以用 $\mathbb { R } ^ { 3 }$ 当中的三个数来描述。某个点的坐标也可以用 $\mathbb { R } ^ { 3 }$ 来描述。确定一个坐标系，也就是一个线性空间的基 $\left( e _ { 1 } , e _ { 2 } , e _ { 3 } \right)$ ，向量a 在这组基下的坐标为：

$\left[ e _ { 1 } , e _ { 2 } , e _ { 3 } \right] \left[ \begin{array} { c } { a _ { 1 } } \\ { a _ { 2 } } \\ { a _ { 3 } } \end{array} \right] = a _ { 1 } e _ { 1 } + a _ { 2 } e _ { 2 } + a _ { 3 } e _ { 3 }$

向量运算

对于 $\in \mathbb { R } ^ { 3 }$ ，内积可以写成：

$\cdot b = a ^ { T } b = \sum _ { i = 1 } ^ { 3 } a _ { i } b _ { i } = | a | | b | \cos < a , b>$

内积可以描述向量间的投影关系，向量外积为：

$\times b = \left[ \begin{array} { c c c } { i } & { j } & { k } \\ { a _ { 1 } } & { a _ { 2 } } & { a _ { 3 } } \\ { b _ { 1 } } & { b _ { 2 } } & { b _ { 3 } } \end{array} \right] = \left[ \begin{array} { c } { a _ { 2 } b _ { 3 } - a _ { 3 } b _ { 2 } } \\ { a _ { 3 } b _ { 1 } - a _ { 1 } b _ { 3 } } \\ { a _ { 1 } b _ { 2 } - a _ { 2 } b _ { 1 } } \end{array} \right] = \left[ \begin{array} { c c c } { 0 } & { - a _ { 3 } } & { a _ { 2 } } \\ { a _ { 3 } } & { 0 } & { - a _ { 1 } } \\ { - a _ { 2 } } & { a _ { 1 } } & { 0 } \end{array} \right] b \triangleq a ^ { \wedge } b$

外积只对三维向量存在定义，外积可以表示向量的旋转。

于外积，引入^ 符号，把a 写成一个矩阵。事实上是一个反对称矩阵，可以将^ 记成一个反对称符号。这样就把外积 $\times b$ ，写成了矩阵与向量的乘法 a^b ，把它变成了线性运算。

向量外积遵从右手定则

旋转和平移

相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧氏变换。一个欧氏变换由一个旋转和一个平移两部分组成。

设某个单位正交基 $\left( e _ { 1 } , e _ { 2 } , e _ { 3 } \right)$ 经过一次旋转，变成了 $\left( e _ { 1 } ^ { \prime } , e _ { 2 } ^ { \prime } , e _ { 3 } ^ { \prime } \right)$ 。那么，对于同一个向量a（注意该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 $\left[ a _ { 1 } , a _ { 2 } , a _ { 3 } \right] ^ { T }$ 和 $\left[ a _ { 1 } ^ { \prime } , a _ { 2 } ^ { \prime } , a _ { 3 } ^ { \prime } \right] ^ { T }$ 。根据坐标的定义，有：

$\left[ e _ { 1 } , e _ { 2 } , e _ { 3 } \right] \left[ \begin{array} { c } { a _ { 1 } } \\ { a _ { 2 } } \\ { a _ { 3 } } \end{array} \right] = \left[ e _ { 1 } ^ { \prime } , e _ { 2 } ^ { \prime } , e _ { 3 } ^ { \prime } \right] \left[ \begin{array} { c } { a _ { 1 } ^ { \prime } } \\ { a _ { 2 } ^ { \prime } } \\ { a _ { 3 } ^ { \prime } } \end{array} \right]$

为了描述两个坐标之间的关系，我们对上面等式左右同时左乘 $\left[ \begin{array} { c } { e _ { 1 } ^ { T } } \\ { e _ { 2 } ^ { T } } \\ { e _ { 3 } ^ { T } } \end{array} \right]$ ，那么左边的系数变成了单位矩阵，所以：

$\left[ \begin{array} { l } { a _ { 1 } } \\ { a _ { 2 } } \\ { a _ { 3 } } \end{array} \right] = \left[ \begin{array} { c c c } { e _ { 1 } ^ { T } e _ { 1 } ^ { \prime } } & { e _ { 1 } ^ { T } e _ { 2 } ^ { \prime } } & { e _ { 1 } ^ { T } e _ { 3 } ^ { \prime } } \\ { e _ { 2 } ^ { T } e _ { 1 } ^ { \prime } } & { e _ { 2 } ^ { T } e _ { 2 } ^ { \prime } } & { e _ { 2 } ^ { T } e _ { 3 } ^ { \prime } } \\ { e _ { 3 } ^ { T } e _ { 1 } ^ { \prime } } & { e _ { 3 } ^ { T } e _ { 2 } ^ { \prime } } & { e _ { 3 } ^ { T } e _ { 3 } ^ { \prime } } \end{array} \right] \left[ \begin{array} { c } { a _ { 1 } ^ { \prime } } \\ { a _ { 2 } ^ { \prime } } \\ { a _ { 3 } ^ { \prime } } \end{array} \right] \triangleq R a ^ { \prime }$

把中间的阵拿出来，定义成一个矩阵 R，这个矩阵由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。矩阵R 描述了旋转本身。因此它又称为旋转矩阵。
旋转矩阵有一些特别的性质。事实上，它是一个行列式为1 的正交矩阵。反之，行列式为1 的正交矩阵也是一个旋转矩阵。所以，我们可以把旋转矩阵的集合定义如下：

$\left\{ R \in \mathbb { R } ^ { n \times n } | R R ^ { T } = I , \operatorname { det } ( R ) = 1 \right\}$

在欧氏变换中，除了旋转之外还有一个平移。考虑世界坐标系中的向量a，经过一次旋转（用R 描述）和一次平移t 后，得到了 $\prime }$ ，那么把旋转和平移合到一起，有：

$\prime } = R a + t$

t 称为平移向量。我们用一个旋转矩阵R 和一个平移向量t 完整地描述了一个欧氏空间的坐标变换关系。

为了方便变换，需要引入齐次坐标和变换矩阵重写上式：

$\left[ \begin{array} { l } { a ^ { \prime } } \\ { 1 } \end{array} \right] = \left[ \begin{array} { l l } { R } & { t } \\ { 0 ^ { T } } & { 1 } \end{array} \right] \left[ \begin{array} { l } { a } \\ { 1 } \end{array} \right] \triangleq T \left[ \begin{array} { l } { a } \\ { 1 } \end{array} \right]$

变换矩阵与齐次坐标

矩阵T 称为变换矩阵（Transform Matrix）。我们暂时用 $\tilde { \boldsymbol { a } }$ 表示a 的齐次坐标，。依靠齐次坐标和变换矩阵，两次变换的累加就可以有很好的形式：

$\tilde { b } = T _ { 1 } \tilde { a } , \tilde { c } = T _ { 2 } \tilde { b } \quad \Rightarrow \tilde { c } = T _ { 2 } T _ { 1 } \tilde { a }$

直接把它写成 $b = T a$ 的样子，默认其就是齐次坐标的写法，方便书写。

关于变换矩阵T ，它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为0 向量，右下角为1。这种矩阵又称为特殊欧氏群（Special Euclidean Group）：

$\left\{ T = \left[ \begin{array} { l l } { R } & { t } \\ { 0 ^ { T } } & { 1 } \end{array} \right] \in \mathbb { R } ^ { 4 \times 4 } | R \in S O ( 3 ) , t \in \mathbb { R } ^ { 3 } \right\}$

与SO(3) 一样，求解该矩阵的逆表示一个反向的变换：

$\left[ \begin{array} { c c } { \boldsymbol { R } ^ { T } } & { - \boldsymbol { R } ^ { T } \boldsymbol { t } } \\ { \mathbf { 0 } ^ { T } } & { 1 } \end{array} \right]$

李群

SO(3)指数映射

我们说三维旋转矩阵构成了特殊正交群 SO(3)，而变换矩阵构成了特殊欧氏群 SE(3)：
$O(3)=\left\{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} | \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}$

$E(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \boldsymbol{0}^{T} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} | \boldsymbol{R} \in S O(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\}$

于 $\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}$ 是一个反对称矩阵，我们可以找到一个三维向量 $\phi(t) \in \mathbb{R}^{3}$ 与之对应。于是有：

$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{\phi}(t)^{\wedge}$

最终得到：
$\boldsymbol{R}(t)=\exp \left(\boldsymbol{\phi}_{0}^{\wedge} t\right)$
我们看到，旋转矩阵 R 与另一个反对称矩阵 $\phi_{0}$ 通过指数关系发生了联系。也就是说，当我们知道某个时刻的 R 时，存在一个向量 $\phi$ ，它们满足这个矩阵指数关系。

如果上式成立，那么给定某时刻的 R ，我们就能求得一个 $\phi$ ，它描述了 R 在局部的导数关系。与 R 对应的 ϕ 有什么含义呢？后面会看到， $\phi$ 正是对应到 SO(3) 上的李代数 so(3)。我们已清楚了 so(3) 的内容，它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。它与 SO(3) 的关系由指数映射给定：
$\boldsymbol{R}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)$

由于 $\phi$ 是三维向量，我们可以定义它的模长和它的方向，分别记作 $\theta$ 和 $a$ ，于是有 $\phi=\theta a$ 。这里 a 是一个长度为 1 的方向向量。最后我们得到了一个式子：罗德里格斯公式
$\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge}$
这表明，so(3) 实际上就是由所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。通过它们，我们把so(3) 中任意一个向量对应到了一个位于 SO(3) 中的旋转矩阵。

SE(3)指数映射

se(3) 上的指数映射形式如下：
$\begin{aligned} \exp \left(\boldsymbol{\xi}^{\wedge}\right) &=\left[\begin{array}{cc} \sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} & \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\phi^{\wedge}\right)^{n} \boldsymbol{\rho} \\ \mathbf{0}^{T} & 1 \end{array}\right] \\ & \triangleq\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{J} \rho \\ \mathbf{0}^{T} & 1 \end{array}\right]=\boldsymbol{T} \end{aligned}$
从结果上看，ξ 的指数映射左上角的 R 是我们熟知的 SO(3) 中的元素，与 se(3) 当中的旋转部分 $\phi$ 对应。而右上角的 $J$ 则可整理为（设 $\phi=\theta a$ ）：
$\boldsymbol{J}=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}$
该式与罗德里格斯有些相似，但不完全一样。我们看到，平移部分经过指数映射之后，
发生了一次以 $J$ 为系数矩阵的线性变换。
在这里插入图片描述

SO(3) 李代数上的求导

李群可能比较难以理解，毕竟一般专业是不会学习的，这里只记下李群在视觉slam的作用，不详细描述，毕竟本人物理专业出身，刚体运动本来就滚瓜烂熟，李群实在力不从心。

在SLAM 中，要估计一个相机的位置和姿态，该位姿是由SO(3) 上的旋转矩阵或SE(3) 上的变换矩阵描述的。设某个时刻相机的位姿为T 。它观察到了一个世界坐标位于p 的点，产生了一个观测数据z。那么，由坐标变换关知：

$z = T p + w$

由于观测噪声w 的存在，z 往往不可能精确地满足 $z = T p$ 的关系。所以通常会计算理想的观测与实际数据的误差：

$e = z - T p$

假设一共有N 个这样的路标点和观测，于是就有N 个上式。那么，对相机的位姿估计，相当于是寻找一个最优的T ，使得整体误差最小化：

$\min _ { T } J ( T ) = \sum _ { i = 1 } ^ { N } \left\| z _ { i } - T p _ { i } \right\| _ { 2 } ^ { 2 }$

求解此问题，需要计算目标函数J 关于变换矩阵T 的导数。重点要说的是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而，SO(3)， SE(3) 上并没有良好定义的加法，它们只是群。如果我们把T 当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：