DSO论文学习有感

一、概述

程序看得太累了，就算及时记下来函数的用途，不停地跳转还是会头晕，我果然不适合玩3D游戏吗。
所以这里先把DSO论文梳理一遍，把算法弄明白大概会更好理解一些。

碎碎念：DSO的程序对新手真实不友好，要是能按模块分开该有多好。

二、简介

DSO论文全名《Direct Sparse Odometry》，作者是慕尼黑技术大学Jakob Engel，发表时间是2016年。

相关工作：
稀疏间接：monoSLAM、PTAM、ORBSLAM；
稠密间接：《Dense monocular
depth estimation in complex dynamic scenes》
稠密直接：DTAM、REMODE、LSDSLAM
稀疏直接：《A semi-direct approach to structure from motion》
混合：SVO
具体参考文献略。

动机：
1、直接法对像素操作，能生成更精细的模型，在稀疏纹理环境更加鲁棒；
2、稠密法需要考虑空间点之间的几何相关性，实时性降低，而且稠密建图时，几何相关性会引入偏置，降低精度；

贡献：
1、唯一的完整的估计所有模型参数的直接法。包括相机位姿、相机内参、逆深度；
2、进行完整几何光度校正，包括距离衰减、gamma校正和曝光时间，精度和鲁棒性都有提高；
3、在CPU上实时。（在笔记本上实时运行其实挺困难）

三、模型

参数说明：
x表示向量，H表示矩阵，t表示标量，$I$表示函数，$T_i\in \text{SE}(3)$表示点从世界坐标系到相机坐标系，$x_i\in se(3)$表示李代数
$\oplus$$:se(3)\times \text{SE}(3)\to \text{SE}(3)$表示左乘。

校正模型：
（1）几何相机校正模型：
采用针孔相机模型
定义${\Pi }_c:{\mathbb{R}}^3\to \Omega$为投影；
定义${\Pi }_c^{-1}:\Omega \times \mathbb{R}\to {\mathbb{R}}^3$为逆投影；
c定义为相机内参，$\Omega$表示像素坐标。

（2）光度相机校正模型
定义非线性响应函数$G:\mathbb{R}\to [0,255]$，距离衰减函数$V:\Omega \to [0,1]$。
模型可以写成
$$I_i(\mathbf{x})=G(t_{i}V(\mathbf{x})B_i(\mathbf{x}))$$其中$B_i$和$I_i$为第i帧的辐照度和观测到的像素灰度，$t_i$为曝光时间。
这样可以计算光度校正灰度
$$I_i^{\prime }(\mathbf{x})=t_i{B}_i(\mathbf{x})=\frac{G^{-1}(I_i(\mathbf{x}))}{V(\mathbf{x})}$$

这里想分享一下我对$B_i$的理解，我查了一下辐射度量，找到7个不同的量，描述辐射能量，和论文比较接近的有两个，一个是辐射出射度M，一个是辐照度E。
辐射出射度是指离开光源表面单位面元的辐射通量
$$M=\frac{d\Phi }{dA}=\frac{dQ}{dAdt}$$
辐照度是指单位面元被照射的辐射通量
$$E=\frac{d\Phi }{dA}=\frac{dQ}{dAdt}$$
公式上差不多，看定义的话，这里的$B_i$应该是辐照度E。说起来，要是物体吸收了一部分辐射，那不是说明$V(\mathbf{x})$其实不只是距离衰减，意外发现。

稀疏直接法模型：
参考帧：$I_i$
目标帧：$I_j$
像素点：$\mathbf{p}\in {\Omega }_i$
然后论文中写到像素小邻域内的SSD类似于为中心像素添加一阶和二阶辐照度导数常数项。我查到这里和图像模板匹配很类似，参考【图像配准】基于灰度的模板匹配算法（一）：MAD、SAD、SSD、MSD、NCC、SSDA、SATD算法，SSD公式如下：
$$D(i,j)=\sum _{s=1}^M\sum _{t=1}^N[S(i+s-1,j+t-1)-T(s,t){]}^2$$通过遍历图像，匹配$M\times N$大小的子图可以使用SSD。

注：误差平方和算法（Sum of Squared Differences，简称SSD算法）

这样能量函数可以写成：
$$E_{{\mathbf{p}}_j}=\sum _{\mathbf{p}\in N_{\mathbf{p}}}{\omega }_{\mathbf{p}}||I_j^{\prime }[{\mathbf{p}}^{\prime }]-I_i^{\prime }[\mathbf{p}]|{|}_{\gamma }$$其中$N_{\mathbf{p}}$表示小邻域，$\gamma$表示鲁棒核函数，$I_j^{\prime }$表示目标帧光度校正灰度。
即便添加了鲁棒核函数也还是和论文公式不同，主要是观测像素$I_j$和光度不变辐射$I_j^{\prime }$的关系还不明朗。

作者在另一篇论文（Large-Scale Direct SLAM with Stereo Cameras）中对能量函数进行过研究：
从论文图5的实验可以看到，两帧之间有较大的光度变化，图像匹配点的灰度曲线可以用直线拟合。
所以这里简化光度残差为（论文这里应该有笔误，不过问题不大）：
$$r_{\mathbf{u}}(\xi )=a{I}_1(\mathbf{u})+b-I_2({\mathbf{u}}^{\prime })$$看到这里我才大致明白这个最重要的能量函数公式：
$$E_{{\mathbf{p}}_j}=\sum _{\mathbf{p}\in N_{\mathbf{p}}}{\omega }_{\mathbf{p}}||(I_j[{\mathbf{p}}^{\prime }]-b_j)-\frac{t_j{e}^{a_j}}{t_i{e}^{a_i}}(I_i[\mathbf{p}]-b_i)|{|}_{\gamma }$$在《Direct Sparse Odometry》里将原来的代价函数修改为$e^{-a_i}(I_i-b_i)$，目的是避免出现负值和一些数值问题。
其中$${\mathbf{p}}^{\prime }={\Pi }_c(\mathbf{R}{\Pi }_c^{-1}(\mathbf{p},d_{\mathbf{p}})+\mathbf{t})$$$$\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]={\mathbf{T}}_j{\mathbf{T}}_i^{-1}$$
影响能量函数的变量：
（1）像素点p的逆深度$d_{\mathbf{p}}$
（2）相机内参$\mathbf{c}$
（3）对应帧位姿${\mathbf{T}}_{\mathbf{i}},{\mathbf{T}}_{\mathbf{j}}$
（4）光度变换参数$a_i,b_i,a_j,b_j$。

所有点、所有帧的完整光度误差：
$$E_{photo}=\sum _{i\in \mathcal{F}}\sum _{\mathbf{p}\in {\mathcal{P}}_i}\sum _{j\in obs(\mathbf{p})}{E}_{{\mathbf{p}}_j}$$i表示所有的帧，p表示第i帧所有点，j表示看见点p的所有帧。

窗口优化：
高斯牛顿优化模型
具体推导参考《视觉SLAM十四讲》，
$$\mathbf{H}={\mathbf{J}}^T\mathbf{W}\mathbf{J}$$$$\mathbf{b}=-{\mathbf{J}}^T\mathbf{W}\mathbf{r}$$其中J表示雅克比矩阵。
雅克比矩阵计算参考直接法光度误差导数推导，其实其他大部分推导问题也都可以在大神的博客里找到答案Jinge。

四、视觉里程计前端

目的：
（1）选择关键帧，特征点，包括去除外点算法和遮挡检测算法；
（2）初始化参数，并且要保证${\mathbf{p}}^{\prime }$的计算精度在2个像素以内；
（3）决定要边缘化的点和帧。

帧管理：
流程：
（1）初始帧跟踪：判断创建新的关键帧，所有激活点都投影到该关键帧，创建半稠密深度地图（应该就是dso运行时候那些彩色的点了），新的帧仅针对这一关键帧进行跟踪。
如果一帧的最终RMSE大于前一帧的两倍，认为图像匹配失败，这时尝试通过在不同方向上进行多达27个不同的小旋转来恢复，每次花费0.5ms。（这里应该就是对应程序里trackNewCoarse那一大段push_back了）

注：均方根误差（Root Mean Squared Error，简称RMSE）

这里分享一下我的疑问：图像RMSE的计算是不是所有像素的光度误差求均方根？

（2）关键帧创建
三个标准决定是否创建关键帧：
a、均方光流（视场变化）
$$f=(\frac{1}{n}{\Sigma }_{i=1}^n||\mathbf{p}-{\mathbf{p}}^{\prime }|{|}^2{)}^{\frac{1}{2}}$$b、无旋转均值光流（相机平移）
$$f=(\frac{1}{n}{\Sigma }_{i=1}^n||\mathbf{p}-{\mathbf{p}}_t^{\prime }|{|}^2{)}^{\frac{1}{2}}$$c、两帧间相对光度因子（相机曝光）
$$a=|log(e^{a_j-a_i}{t}_j{t}_i^{-1})|$$

（3）关键帧边缘化
边缘化策略：
a、最新的两个关键帧不参与；
b、帧的可见点数小于5%；
c、计算的距离分数s最大:
$$s(I_i)=\sqrt{d(i,1)}\sum _{j\in [3,n]/i}(d(i,j)+ϵ{)}^{-1}$$其中$d(i,j)$是$I_i$和$I_j$之间的欧氏距离，（图像之间的距离计算要怎么做？是稀疏特征之间吗？）

点管理：
实验说明图像数据高度冗余，dso维护一个固定的数量激活点（2000个），这些点服从均匀分布。
（1）候选点选取：
选择策略
a、在图像中分布均匀的点
b、相对于周围环境有足够高的图像梯度值的点

将图像分割成$32\times 32$ 个块，得到一个区域自适应梯度阈值，每个块计算一个阈值；
将图像分割为$d\times d$个块，为每个块选择梯度最大的像素，实验发现弱梯度也是有用的，因此需要多次提取，阈值不断减低，块大小d不断调整；

（2）候选点跟踪：
候选点沿极线离散搜索来跟踪以最小化光度误差。

极线搜索具体参考《视觉SLAM十四讲》对极几何。

（3）候选点激活
当一组旧的点被边缘化后，新的候选点会激活来取代它们。

a、所有激活点投影到最新的关键帧
b、候选点投影到关键帧，到所有点的距离最大，则激活候选点（应该是块内，维持均匀分布）

（4）外点和遮挡检测：
a、候选点极线搜索时，丢弃最小值不够明显的点；
b、去除光度误差超过阈值的点；

五、总结

很多理解还挺稚嫩的，看程序有新的理解再修改。