MonoSLAM：Real-Time Single Camera SLAM全文总结

本文为博主原创博客，转载请注明出处：https://blog.csdn.net/q_z_r_s

机器感知 一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号

MonoSLAM: Real-Time Single Camera SLAM 全文总结

Abstract

我们讲述了一个可以通过单目相机恢复快速通过未知场景时的3D轨迹的实时算法。我们把此系统称为MonoSLAM，这是第一个将SFM方法应用到SLAM中的成功应用。此方法的核心是，在概率框架下在线创建稀疏但persistent的地图。我们的主要贡献包括主动(active)建图和测量、使用针对相机平滑运动的通用运动模型以及单目特征初始化和特征方位估计的解决方法。总之，这些都是一种非常有效和健壮的算法，可以在标准PC和相机上以30Hz运行。这项工作扩展了机器人系统的范围，其中SLAM可以有效地应用，而且开启了新的领域。我们展示了MonoSLAM在仿人机器人实时3D定位和建图以及手持相机的在线增强现实应用。

1. INTRODUCTION

2. RELATED WORK

2.1 Vision-Based SLAM

我们工作的一个重点就是简化SLAM对硬件的要求。

3. METHOD

3.1 Probabilistic 3D Map

我们方法的核心概念是基于特征的概率地图，表示当前相机的状态估计及所有感兴趣的特征的快照，还包括这些估计中的不确定性。地图使用状态向量$\hat{x}$和协方差矩阵$P$表示。状态向量$\hat{x}$是有相机和特征的状态堆成的，$P$是一个方阵，分别表示如下：
$$\hat{x}=\left(\begin{array}{c}{\hat{x}}_v\\ {\hat{y}}_1\\ {\hat{y}}_2\\ ⋮\end{array}\right),P=\left[\begin{array}{ccc}{P}_{xx}& P_{x{y}_1}& P_{x{y}_2}\cdots \\ P_{y_{1}x}& P_{y_1{y}_1}& P_{y_1{y}_2}\cdots \\ P_{y_{2}x}& P_{y_2{y}_1}& P_{y_2{y}_2}\cdots \\ ⋮& ⋮& ⋮\end{array}\right]\text{\hspace{1em}(1)}$$
相机状态向量$x_v$由3D位置向量$r^W$，四元数$q^{RW}$，速度向量$v^W$以及角速度向量${\omega }^R$组成，共13个参数：
$$x_v=\left(\begin{array}{c}{r}^W\\ q^{RW}\\ v^W\\ {\omega }^R\end{array}\right)\text{\hspace{1em}(2)}$$
本文中，特征状态$y_i$是特征点的3D位置向量。此地图的主要目的是为了能够实时定位，而不作为完整的环境描述，因此我们致力于捕获高质量的稀疏landmarks。

3.2 Natural Visual Landmarks

使用image patches来作为landmark features，根据相机位置信息，改善匹配性能。使用Shi and Tomasi算子检测角点，这里是用的是灰度图像。对匹配而言，假设每个路标点的图像块像素点都共面——此假设在大多数情况下都可以都可以很好的工作。本文中不更新以作为特征保存的图像模板。

3.3 System Initialization

我们选择用一些已知目标放在相机前来对相机进行初始化。

3.4 Motion Modeling and Prediction

由于我们并没有任何关于相机运动的先验信息，任何模型都会停止在在某个细节层面，用概率假设表示所建模型和真实模型之间的差异，所以我们选择用常速度，常角速度模型，这并不是说我们认为相机整个过程中是常速运动的，这是针对的是每个time step而言的。

假设每个time step有未知的加速度$a^W$和角加速度${\alpha }^W$噪声，其服从0均值高斯分布，对速度和角速度产生冲击：
$$n=\left(\begin{array}{c}{V}^W\\ {\Omega }^R\end{array}\right)=\left(\begin{array}{c}{a}^W\Delta t\\ {\alpha }^R\Delta t\end{array}\right)\text{\hspace{1em}(3)}$$
虽然$V^W$和${\Omega }^R$可能会耦合，但是，现在我们假设$n$的协方差矩阵是对角阵，这表示它们是不相关的。状态更新如下：

$$f_v=\left(\begin{array}{c}{r}_{new}^W\\ q_{new}^{WR}\\ V_{new}^W\\ {\omega }_{new}^R\end{array}\right)=\left(\begin{array}{c}{r}^W+(v^W+V^W)\Delta t\\ q^{WR}\times q(({\omega }^R+{\Omega }^R)\Delta t)\\ v^W+V^W\\ {\omega }^R+{\Omega }^R\end{array}\right)\text{\hspace{1em}(4)}$$

其中，姿态使用四元数来表示，四元数又通过角轴$({\omega }^R+{\Omega }^R)\Delta t$来计算得到。

在EKF中，相机经过运动方程得到的新的状态估计$f_v(x_v,u)$必须伴随着状态不确定性(过程噪声协方差)$Q_v$的增加。$Q_v$通过雅可比计算得到：

$$Q_v=\frac{\partial f_v}{\partial n}{P}_n\frac{\partial f_v^T}{\partial n}\text{\hspace{1em}(5)}$$

其中，$P_n$是噪声向量的协方差矩阵。运动方程中的不确定性增长速率由$P_n$决定，它额大小表示我们期望的运动的平滑性。

3.5 Active Feature Measurement and Map Update

我们的方法的一个关键部分就是在观测之前，预测每个特征在图像中出现的位置。特征匹配使用NCC进行模板匹配，如果将模板与整幅图像进行匹配，这计算量会很大；预测是一个主动的方法，缩小搜索，提高效率。

首先，使用相机位姿的状态估计$x_v$、特征位置$y_i$，所以一个点特征相对于相机的位置预计出现在：

$$h_L^R=R^{RW}(y_i^W-r^W)\text{\hspace{1em}(6)}$$

根据立体几何，特征预计出现在图像中的位置$(u,v)$，使用标准针孔相机模型：

$$h_i=\left(\begin{array}{c}u\\ v\end{array}\right)=\left(\begin{array}{c}{u}_0-f{k}_u\frac{h_{L_x}^R}{h_{L_z}^R}\\ v_0-f{k}_u\frac{h_{L_y}^R}{h_{L_z}^R}\end{array}\right)\text{\hspace{1em}(7)}$$

其中，$f{k}_u,f{k}_v,u_0,v_0$为相机内参。

在当前的工作中，我们使用的是广角相机，这样就可以同时观测到很多不同视角的特征，它的缺点就是，图像不是透视几何的，例如直线不再是直线。尽管如此，我们在进行特征匹配时使用的还是原始图像，而不是畸变矫正后的图像。值得一提的是，当应用到增强现实的时候，我们使用的是畸变矫正后的图像，因为OpenGL仅支持透视几何相机模型。

因此，我们使用径向畸变来扭曲透视几何投影坐标$u=(u,v)$，来获得最终预测的图像位置$u_d=(u_d,v_d)$，所采用的可逆的径向畸变模型如下：
$$\begin{gathered} u_d-u_0=\frac{u-u_0}{\sqrt{1+2K_1{r}^2}} \\ {v}_d-v_0=\frac{v-v_0}{\sqrt{1+2K_1{r}^2}} \end{gathered}$$
其中，$r=\sqrt{(u-u_0{)}^2+(v-v_0{)}^2}$。

分别计算这两步的投影函数对相机和特征的雅可比矩阵，这使得我们可以计算预测特征在图像上的位置的不确定性，用$2\times 2$对称协方差矩阵$S_i$表示：
$$S_i=\frac{\partial u_{di}}{\partial x_v}{P}_{xx}{\frac{\partial u_{di}}{\partial x_v}}^T+\frac{\partial u_{di}}{\partial x_v}{P}_{x{y}_i}{\frac{\partial u_{di}}{\partial y_i}}^T+\frac{\partial u_{di}}{\partial y_i}{P}_{y_{i}x}{\frac{\partial u_{di}}{\partial x_v}}^T+\frac{\partial u_{di}}{\partial y_i}{P}_{y_i{y}_i}{\frac{\partial u_{di}}{\partial y_i}}^T+R\text{\hspace{1em}(8)}$$
其中，$R$为观测噪声，与图像分辨率有关。$S_i$允许我们进行主动图像搜索，它表示图像坐标的2D高斯概率密度分布，通过选择一个标准方差阈值，比如$3\sigma$来定义一个椭圆搜索窗，特征会在很高的概率下出现在此区域内。

3.6 Feature Initialization

单目相机并不能通过特征的观测直接给出特征的位置，因为特征的深度是未知的。估计特征的深度需要相机运动，并从不同的视角观测特征。然而，我们避免在图像中跟踪若干帧的新特征而不试图估计其3D位置的方法，然后用多视几何三角测量来执行minibatch估计初始化特征的深度。

我们的方法是在地图中初始化一个3D射线，起点为相机的位置，指向特征点。在SLAM地图中表示如下：
$$y_{pi}=\left(\begin{array}{c}{r}^W\\ {\hat{h}}_i^W\end{array}\right)$$
其中$r_i$是相机位置，${\hat{h}}_i^W$是描述方向的单位向量。深度假设可以使用粒子滤波来表示。

在作者后续的论文中可以看到，为了将新的特征点即刻加入到状态向量中，采用了现在应用很多的逆深度方法，这样就可以直接将新观测的到的特征添加到状态更新当中了，而不用等到深度初始完成了才加入。

3.7 Map Management

特征提取是使用Shi-Tomasi算子，匹配时使用image patch。在特征提取时，随机选取搜索框，然后在其中寻找特征点，搜索框选取的要求时不能与现有的特征(包括当前帧新提取到的)重叠，因为新产生的一帧图像还不知道上一帧观测到的特征会出现在当前帧的哪个位置，因此这里就是用恒速模型来预测可能出现的位置，这些预测的位置上都不允许再提取特征点。

3.8 Feature Orientation Estimation

在初始化特征的时候，用作特征匹配的不是特征描述子，而是image patch，一旦特征初始化完成，那么这个image patch将不再改变，但是这里有一个问题，当相机运动的时候，从不同的视角观测同一个image patch时，会和最初提取到的不一样，最简单的就是旋转了，如果直接计算patch之间的相似度，很可能就认为这两个是不匹配的，所以为了解决旋转不变性的问题，论文中采用了在匹配之前先预测图像可能会被看到的样子，这样就在一定的程度上修正了由于相机运动所造成的视角问题。特征方向估计的变换方程为：
$$H=CR[n^T{x}_{p}I-t{n}^T]C^{-1}\text{\hspace{1em}(9)}$$
其中，$C$是相机校正矩阵(透视几何投影)，$R$和$t$为相机的位姿，$n$是法向量，$x_p$是image patch中心在图像上的投影。这个公式没看懂，有懂得大神还请不吝赐教~，或者哪天(估计不会再看了:))我懂了再补上来。

4. 总结

• 恒速模型：很有创意
• 主动搜索：根据恒速模型预测下一时刻的位置，配合搜索区域使用
• 利用协方差矩阵预测搜索区域，大大加快匹配速度
• 根据预测的位姿修正模板图像外貌特征
• 剩下的就是传统的EKF的知识了，predict–>update

参考文献

[1] Davison A J , Reid I D , Molton N D , et al. MonoSLAM: Real-Time Single Camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6):1052-1067.