Dense Visual SLAM for RGB-D Cameras论文阅读

论文链接
代码
出处：IROS,2013

本文亮点

本文中作者提出了一种基于RGB-D相机的稠密视觉slam方法，可以同时最小化全体像素点的光测和深度误差。和稀疏，基于特征点的slam方法相比，可以取得更加准确的位姿估计。进一步，作者在文章中还针对关键帧选取和闭环检测提出了基于熵的相似度测量方法，通过g2o框架优化后，取得了明显小于同类方法的轨迹误差，并且在弱纹理和弱结构的情况下，依然可以获得良好的效果。

---

一、研究目标和贡献

论文首先指出通过选取关键帧进行特征匹配的方法会丢失部分重要的图像信息，因此，论文通过构建稠密的slam方法，希望完成下述三个目标：
1、更加充分的利用从传感器中获得的可用数据信息
2、可以实时运行
3、可以有效消除漂移并且通过全局图优化纠正累计误差
该论文的主要贡献在于：
1、一个快速的帧到帧的配准方法，从而优化强度和深度误差
2、一种基于熵的特征点挑选方法，可以显著降低漂移
3、一种同样基于熵尺度的的闭环检测验证方法
4、整合上述全部方法，提出一种通用的slam图解决器，进一步降低漂移

二、具体细节

1.稠密的视觉里程计

首先，记$t$时的强度图为$I_t$，深度图为$Z_t$，相机的刚体运动为$g$。
图像连续性定理：记场景中的3D点坐标为$p$，相机真实移动为$g^{\ast }$，$p$在第一张图片以及第二张图片中对应的像素点分别为$x_1$和$x_2$，在场景稳定并且忽略噪声和光照影响的情况下，有$I_1(x)=I_2(x)$，即$x_1$和$x_2$像素点的强度相同。
假设强度图和深度图均满足图像连续性定理，然后有下述推导：
3D点坐标为$p$可具体表示为$(X,Y,Z,1{)}^T$，像素点$x$的坐标可表示为$(x,y{)}^T$,记深度测量$Z=Z(x)$，定义反投影函数${\pi }^{-1}$为：
$$p={\pi }^{-1}(x,Z)=(\frac{x-o_x}{f_x}Z,\frac{y-o_y}{f_y}Z,Z,1{)}^T$$
其中，$f_x$和$f_y$是焦距，$o_x$和$o_y$是标准针孔相机模型下相机中心点的坐标。
空间3D点的可以用投影函数$\pi$定义为：
$$x=\pi (p)=(\frac{X{f}_x}{Z}+o_x,\frac{Y{f}_y}{Z}+o_y{)}^T$$
相机的刚体运动$g$可以用变换矩阵$T$来表示：
$$T_{4\times 4}=\left[\begin{array}{cc}{R}_{3\times 3}& t_{3\times 1}\\ 0& 1\end{array}\right]$$
$T_{4\times 4}$中包含了一个旋转矩阵和一个平移向量，$p$点关于$g$的运动变换可以表示为：
$$g(p)=p^{\prime }=Tp$$
注意到$T$中包含了12个参数，而$g$中只有六个自由度,根据李代数$se(3)$和李群$SE(3)$，定义坐标$\xi$，$\xi$是六维向量，$T$和$\xi$间存在如下转换关系：
$$T=exp(\hat{\xi })$$
继续定义函数$\tau$，用来计算第一张图中的像素点$x$对应到第二张图中的像素点坐标$x^{\prime }$：
$$x^{\prime }=\tau (x,X)=\pi (T{\pi }^{-1}(x,Z_1(x))).$$
有了第一张图和第二张图对应的像素坐标，进而可以构造光度的误差函数$r_I$：
$$r_I=I_2(\tau (x,T))-I_1(x)$$
类似的，深度的误差函数可以定义为：
$$r_Z=Z_2(\tau (x,T))-[T{\pi }^{-1}(x,Z_1(x)){]}_Z$$
这里，$[\cdot {]}_Z$返回三维点的$Z$坐标。
基于$\xi$和$r_I$在考虑像素级误差的情况下，通过最大似然估计来得出相机的真实运动${\xi }^{\ast }$:
$${\xi }^{\ast }=arg\underset{\xi }{\max }p(\xi |r_I)$$
在采用贝叶斯法则之后，假设所有的误差都是独立的并且一致分布的，运用负对数可能性，可以得到：
$${\xi }^{\ast }=arg\underset{\xi }{\min }-\sum _i^{n}log(p(r_{I,i}|\xi ))-log(p(\xi ))$$
作者发现可以用$t$分布很好的拟合$p(r_{I,i}|\xi )$，进一步的，${\xi }^{\ast }$可以写成：
$${\xi }^{\ast }=arg\underset{\xi }{\min }\sum _i^n{w}_i{r}_{I,i}^2$$
为了同时优化光度和深度误差，定义$r\text{r}r$为一个含有二个变量元素的变量，$r\text{r}r=(r_I,r_Z{)}^T$，并且$r\text{r}r$满足$t$分布$p_t(0\text{0}0,\Sigma \text{Σ}\Sigma ,v)$,然后${\xi }^{\ast }$可以写成：
$${\xi }^{\ast }=arg\underset{\xi }{\min }\sum _i^n{w}_i{r\text{r}r}_i^T{\Sigma \text{Σ}\Sigma }^{-1}{r\text{r}r}_i$$
其中，$w_i$表示为$\frac{v+1}{v+{r\text{r}r}_i^T{\Sigma \text{Σ}\Sigma }^{-1}{r\text{r}r}_i}$
通过此误差函数，可以同时优化光度和深度误差，同时$\Sigma \text{Σ}\Sigma$使得误差项可以实现自适应。
然后运行非线性优化方法进行优化：
$$\sum _i^n{w}_i{J\text{J}J}_i^T{\Sigma \text{Σ}\Sigma }^{-1}{J\text{J}J}_i\Delta \xi \text{ξ}\xi =-\sum _i^n{w}_i{J\text{J}J}_i^T{\Sigma \text{Σ}\Sigma }^{-1}{r\text{r}r}_i$$
其中，${J\text{J}J}_i$是2×6的雅可比矩阵（${r\text{r}r}_i$对$\xi \text{ξ}\xi$求偏导，${r\text{r}r}_i$包含光度和深度两个维度，$\xi \text{ξ}\xi$为六维向量，三个维度旋转，三个维度平移），$n$为像素点数量。

2.基于关键帧的视觉slam

关键真的选择基于微分熵，满足正态分布$N(\mu ,\Sigma )$的m维点$x\text{x}x$的微分熵$H$定义为：
$$H(x\text{x}x)=0.5m(1+\ln (2\pi ))+0.5\ln (|\Sigma \text{Σ}\Sigma |)$$
${\xi }_{k:k+j}$表示第$k$帧到第$k+j$的运动，${\xi }_{k:k+1}$表示第$k$帧到第$k+1$的运动，定义熵比例$\alpha$:
$$\alpha =\frac{H({\xi \text{ξ}\xi }_{k:k+j})}{H({\xi \text{ξ}\xi }_{k:k+1})}$$
当$\alpha$小于所设置阈值时，设置新的关键帧。
回环检测采取最近邻域搜索的方法，最后采用g2o进行全局优化。

实验结果