SfM和SLAM项目总结

发表于 | 更新于 | 分类于 | 评论数： | 阅读次数：
2018.5至2019.5大学生创新创业训练项目经验总结。

前言：2019年6月27日，我进行了网易伏羲实验室虚拟人方向的电话面试。在电话中，我支支吾吾连自己的项目都讲不清楚。不管最后面试结果如何，自己做过的东西起码自己要懂。

基础知识

内积

$$ \bf{a} \cdot \bf{b}=\bf{a}^T\bf{b}=\sum_{i=1}^3a_ib_i=|\bf{a}||\bf{b}|cos<\bf{a,b}> $$

外积

$$ \bf{a} \times \bf{b} = \left[ \begin{array}{ccc} {\bf{i}} & {\bf{j}} & {\bf{k}}\ {a_1} & {a_2} & {a_3}\{b_1} & {b_2} & {b_3} \end{array} \right] = \left[ \begin{array}{c} {a_2b_3-a_3b_2}\ {a_3b_1-a_1b_3}\ {a_1b_2-a_2b_1} \end{array} \right] =\left[ \begin{array}{ccc} {0} & {-a_3} & {a_2}\ {a_3} & {0} & {-a_1}\ {-a_2} & {a_1} & {0} \end{array} \right] \bf{b} \triangleq \bf{a} ^\wedge \bf{b} $$

相应的，有 $$ \boldsymbol{a}=[a_1\boldsymbol{i},a_2\boldsymbol{j},a_3\boldsymbol{k}] $$

$$ \boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[\begin{matrix}0&-a_3&a_2\a_3&0&-a_1\-a_2&a_1&0\\end{matrix}\right] $$

$$ \boldsymbol{A}^{\vee}=\boldsymbol{a} $$

特殊正交群 $SO(3)$

$$ SO(3)={R\in\mathbb{R}^{3\times3}|RR^T=I,\det(R)=1} $$

（det为行列式）

特殊欧式群$SE(3)$

$$ SE(3)={T=\left[ \begin{matrix} R&t\ 0^T&1\ \end{matrix} \right]\in\mathbb{R}^{4\times4}|R\in SO(3),t\in \mathbb{R}^3} $$

李代数$\mathfrak{so}(3)$

在对$RR^T=I$进行求导及整理之后，可以得到$R(t)^{'}R(t)^T$是一个反对称矩阵，那么就用一个$\phi(t)\in\mathbb{R}^3$与之对应。有：

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

之后求解微分方程，可以得到：

$$ R(t)=exp(\phi^{\wedge}t) $$

实际求解时利用到指数映射和对数映射。

双目系统

01

02

左右相机只有在 x 轴上有位移，因此 P 的像也只在 x 轴（对应图像的 u 轴）上有差异。

$$ \frac{z-f}{z}=\frac{b-(u_{L}+u_{R})}{b} $$

整理得:

$$ z=\frac{f b}{d}, \quad d=u_{L}-u_{R} $$

对极几何

03

两帧图像$I_{1},I_{2}$，两个相机中心分别为 $O_{1},O_{2}$。$p_{1},p_{2}$为互相匹配的特征点。

极平面：连线$\overrightarrow{O_{1} p_{1}}$和连线$\overrightarrow{O_{2} p_{2}}$在三维空间中会相交于点 P。这时候点$O_{1}, O_{2}, P$三个点可以确定一个平面，该平面称极平面。

极点：$O_{1} O_{2}$连线与像平面$I_{1},I_{2}$的交点被称为极点。

基线：$O_{1},O_{2}$的连线。

极线：极平面与两个像平面$I_{1},I_{2}$之间的相交线$l_{1},l_{2}$。

用途：双目匹配中，给定左图一个点，能将右图中对应的点的搜索空间缩减到一条线。

条件：需要八个或八个以上的点对

推导：设在第一帧的坐标系下，P坐标为：$\boldsymbol{P}=[X, Y, Z]^{T}​$，两像素点 $p_{1},p_{2}​$ **（齐次化坐标）**的像素位置为：

$$ s_{1} \boldsymbol{p}{1}=\boldsymbol{K} \boldsymbol{P}, \quad s{2} \boldsymbol{p}_{2}=\boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t}) $$

因为是齐次坐标，所以认为等式任意一边乘以非零常数，等式仍然成立。即：

$$ \boldsymbol{p}{1}=\boldsymbol{K} \boldsymbol{P}, \quad \boldsymbol{p}{2}=\boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t}) $$

取$\boldsymbol{x}{1}, \boldsymbol{x}{2}$是两个像素点的归一化平面上的坐标：

$$ \boldsymbol{x}{1}=\boldsymbol{K}^{-1} \boldsymbol{p}{1}, \quad \boldsymbol{x}{2}=\boldsymbol{K}^{-1} \boldsymbol{p}{2} $$

有：

$$ \boldsymbol{x}{2}=\boldsymbol{R} \boldsymbol{x}{1}+\boldsymbol{t} $$

同时左乘$t^{\wedge}$，相当于同时与$t$做外积。

$$ \boldsymbol{t}^{\wedge} \boldsymbol{x}{2}=\boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}{1} $$

同时左乘$\boldsymbol{x}_{2}^{T}$

$$ \boldsymbol{x}{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{x}{2}=\boldsymbol{x}{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}{1} $$

因$t^{\wedge} x_{2}$垂直于$\boldsymbol{x}_{2}^{T}$或$t$，因此左侧为0。极线约束即：

$$ \boldsymbol{x}{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}{1}=0 $$

将$\boldsymbol{x}{1}=\boldsymbol{K}^{-1} \boldsymbol{p}{1}, \quad \boldsymbol{x}{2}=\boldsymbol{K}^{-1} \boldsymbol{p}{2}$代回，可得极线约束的另一种形式：

$$ \boldsymbol{p}{2}^{T} \boldsymbol{K}^{-T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{K}^{-1} \boldsymbol{p}{1}=0 $$

意义：

$O_{1}, P, O_{2}$三点共面，$\boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}{1}$表示极线，且极线一定通过$\boldsymbol{x}{2}$。

本质矩阵

本质矩阵为：

$$ \boldsymbol{E}=\boldsymbol{t}^{\wedge} \boldsymbol{R} $$

特点：

  1. 秩为2
  2. 仅依赖于外参
  3. 采用摄像机坐标系

基本矩阵

基本矩阵为：

$$ \boldsymbol{F}=\boldsymbol{K}^{-T} \boldsymbol{E} \boldsymbol{K}^{-1} $$

特点：

  1. 秩为2
  2. 仅依赖于外参和内参
  3. 采用像素坐标系
  4. F是从左图到右图，有方向，从右图到左图就是F的转置
  5. 7个自由度。（$3\times3$矩阵本有9个自由度，因行列式值为零，相差常数因子，即$kF,k\neq 0$,也是基本矩阵，减去两个自由度。）

单应矩阵

单应矩阵是齐次矩阵，有8个未知数，需要四个对应点。

张正友标定法

A Flexible New Technique for Camera Calibration

摘要：张正友标定法只需要相机观察几个（至少两个）不同方向上显示的平面图案。相机或平面图案都可以自由移动。且相机或标定板的运动不需要知道。张正友标定法对径向透镜畸变进行了建模。该方法由一个封闭形式的解组成，然后根据最大似然准则进行非线性改进。

数学推导

针孔相机模型三维点到二维点投影矩阵

符号 含义
$m=[u,v]^T$ 二维点
$M=[X,Y,Z]^T$ 三维点
$\widetilde{m}=[u,v,1]^T$ 二维点齐次坐标
$\widetilde{M}=[X,Y,Z,1]^T$ 三维点齐次坐标
s 随机尺度因子
$\bf{(R,t)}$ 外参，即将世界坐标系和相机坐标系联系起来的旋转矩阵和位移向量
$\bf{A}$ 相机内参矩阵
$\alpha,\beta$ 分别为图像u轴和v轴的尺度因子
$u_0,v_0$ 光心的图像坐标系坐标
$\gamma$ 描述图像轴u,v之间的倾斜

$$ s \widetilde{\bf{m}} = \bf{A[R\quad t]\widetilde{M}} $$

其中，

$$ A = \left[ \begin{array}{ccc} {\alpha} & {\gamma} & {u_0}\ {0} & {\beta} & {v_0}\ {0} & {0} & {1}\end{array}\right] $$

标定板平面与图像之间的单应性矩阵

这里假设标定板所在的平面为空间坐标系下的$Z=0$。

符号 含义
$\bf{r_i}$ 旋转矩阵$\bf{R}$的第i列

$$ s\left[\begin{array}{c}{u}\{v}\{1}\end{array}\right]=\bf{A}[\begin{array}{cccc}{\bf{r_1}}&{\bf{r_2}}&{\bf{r_3}}&{\bf{t}}\end{array}]\left[\begin{array}{c}{X}\{Y}\{0}\{1}\end{array}\right]=A[\begin{array}{ccc}{\bf{r_1}}&{\bf{r_2}}&{\bf{t}}\end{array}]\left[\begin{array}{c}{X}\{Y}\{1}\end{array}\right] $$

即：

$$ s\widetilde{\bf{m}}=\bf{H\widetilde{M}} $$

其中，$\bf{H}$为单应矩阵，$\bf{H=A[\begin{array}{ccc}{\bf{r_1}}&{\bf{r_2}}&{\bf{t}}\end{array}]}$。

内参约束

假设$\bf{H}=\left[\begin{array}{ccc}{\bf{h_1}}&{\bf{h_2}}&{\bf{h_3}}\end{array}\right]$，根据单应关系有：

$$ \left[\begin{array}{ccc}{\bf{h_1}}&{\bf{h_2}}&{\bf{h_3}}\end{array}\right]=\lambda \bf{A}[\begin{array}{ccc}{\bf{r_1}}&{\bf{r_2}}&{\bf{t}}\end{array}] $$

可推出 $$ \mathbf{r}{1}=\lambda \mathbf{A}^{-1} \mathbf{h}{1} $$

$$ \mathbf{r}{2}=\lambda \mathbf{A}^{-1} \mathbf{h}{2} $$

$$ \mathbf{r}{3}=\mathbf{r}{1} \times \mathbf{r}_{2} $$

$$ \mathbf{t}=\lambda \mathbf{A}^{-1} \mathbf{h}_{3} $$

$$ \lambda=\frac{1}{\left|\mathbf{A}^{-1} \mathbf{h}{1}\right|}=\frac{1}{\left|\mathbf{A}^{-1} \mathbf{h}{2}\right|} $$

根据$\bf{r_1}$和$\bf{r_2}$正交，根据

  1. 正交
  2. 模相等

有： $$ h_1^TA^{-T}A^{-1}h_2=0 $$

$$ h_1^TA^{-T}A^{-1}h_1= h_2^TA^{-T}A^{-1}h_2 $$

几何解释

需要大于等于三张图像才能确定唯一解。

SfM

三角化

设 $x_1 ,x_2$ 为两个特征点的归一化坐标，那么它们满足： $$ s_{1} \boldsymbol{x}{1}=s{2} \boldsymbol{R} \boldsymbol{x}{2}+\boldsymbol{t} $$ 现在我们已经知道了$\boldsymbol{R}, \boldsymbol{t}$，想要求解的是两个特征点的深度 $s{1}, s_{2}$。

可以左侧使用$x_1$进行叉乘，使左侧为0，即可求得$s_2$： $$ s_{1} \boldsymbol{x}{1}^{\wedge} \boldsymbol{x}{1}=0=s_{2} \boldsymbol{x}{1}^{\wedge} \boldsymbol{R} \boldsymbol{x}{2}+\boldsymbol{x}_{1}^{\wedge} \boldsymbol{t} $$ 求$s_1$时同理。

PnP

求解 3D 到 2D 点对运动的问题。特征点的 3D 位置可以由三角化，或者由 RGB-D 相机的深度图确定。在单目视觉里程计中，必须先进行初始化，然后才能使用 PnP。

SLAM

ORB_SLAM流程

  1. System类生成一个对象slam，初始化slam对象，设置三个线程：Tracking ，LocalMapping ，LoopClosing

  2. 订阅 GrabImage() ——> TrackMonocular()——>GrabImageMonocular()——>Track()

三个线程作用

Tracking：

  1. Extract ORB，提取关键点和描述子
  2. Initialize pose：在Track() 中初始化位姿 pose ， 第一帧 ：mInitialFrame 当前帧：mCurrentFrame ，从这两帧得到 （R，t）。并且，这两帧转为KeyFrame，凡是关键帧KeyFrame都要插入地图 mpMap（mpMap->AddMapPoint(pMP);），根据关键帧与当前帧公共3D点的个数 更新关键帧间的连接关系。

MapPoint类表示 路标点，Map类用Hash表管理 路标点和关键帧。

  1. track local map：

    1. 更新局部地图，包括局部关键帧和关键点

    2. 对局部MapPoints进行投影匹配

    3. 根据匹配对估计当前帧的姿态

    4. 根据姿态剔除误匹配

  2. keyFrame decision

Local Mapping:

LocalMapping线程处理的关键帧都是Tracking线程发过来的

  1. 插入关键帧，
  2. 去除冗余的MapPoints；
  3. Local BA；
  4. 去除冗余的关键帧 : 检测并剔除当前帧相邻的关键帧中和关键帧，剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到。将当前帧加入到闭环检测队列中。

Loop Closing

  1. 回环检测Loop decision：计算闭环候选帧，在候选帧中检测具有连续性的候选帧；
  2. 计算相似变换阵sim3，求 [ sR | t ]；
  3. Loop Correction：

A. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的MapPoints的位置； B. 将闭环帧以及 与闭环帧相连的关键帧的MapPoints和与当前帧相连的关键帧的点进行匹配； C. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph D. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整 E. 创建线程进行全局Bundle Adjustment

main->构造SLAM对象，在构造函数中设置线程。

读取图像，循环调用trackMonocular，在其中先检查状态是否改变，再调用

1
GrabImageMonocular

构造了CurrentFrame，最后进行Track