DVO-slam原理介绍

参考论文1：Robust Odometry Estimation for RGB-D Cameras
参考论文2：Dense Visual SLAM for RGB-D Cameras

1、误差约束公式

基于直接法的稠密建图，误差采用两部分：灰度误差和深度误差。

灰度误差：
灰度误差表示匹配点在图像中被观测到的光度误差，误差方程定义如下：

深度误差：
深度误差表示标配点使用深度图观测到的深度误差，误差方程定义如下：

其中：

x与x‘是空间同一个点在两帧图像中显示的图像坐标，上述公式是将空间点对应的图像坐标变换为另一帧的图像坐标。两帧之间的相机位姿变换为T。

$\pi ()$将三维空间点转换为图像坐标。

2、权重最小二乘迭代求解(IRLS)：

2.1 论文1中的迭代公式：论文1中只说明了光度误差：

上式是说明第1帧和第二帧之间的光度误差，i表示第i个匹配点的id。

上式推导过程：
权重由来:
由贝叶斯最大后验：

要求得下述方程：

即要使得：

使用log似然，上式等式两侧均进行log操作：

求极点，右侧求导，极点处导数为0：
$$\sum _i-\frac{\partial logp(r_i|\xi )}{\partial \xi }-\frac{\partial p(\xi )}{\partial \xi }=0$$
$$\sum _i-\frac{\partial logp(r_i)}{\partial r_i}\frac{\partial r_i}{\partial \xi }-\frac{\partial logp(\xi )}{\partial \xi }=0$$
作者将上式两项分为两部分考虑：
第一部分，令：
$$\omega \left(r\right)=-\frac{\partial logp\left(r\right)}{\partial r}\cdot \frac{1}{r}$$
则：

满足上式的解，即为下式的解：

设$r_i$服从t分布，计算$\omega \left(r\right)$：
T分布的概率密度公式为：
$$p\left(r\right)=\frac{\Gamma \left(\frac{\nu +1}{2}\right)}{\Gamma \left(\frac{\nu }{2}\right)\sqrt{n\pi }}{\left(1+\frac{{\left(r-\mu \right)}^2}{\nu {\sigma }^2}\right)}^{-\frac{\nu +1}{2}}$$

设$r_i$服从t（0，$\Sigma$，$\nu$）分布，0为均值，$\Sigma ={\sigma }^2$为方差，$\nu$为t分布自由度，$\Gamma \left(\right)$为gamma函数，但是在log后，求导会作为常数约去，可不用多详细追意。
整理得：
$$p\left(r\right)=\frac{\Gamma \left(\frac{\nu +1}{2}\right)}{\Gamma \left(\frac{\nu }{2}\right)\sqrt{n\pi }}{\left(1+\frac{{\left(r\right)}^2}{\nu \Sigma }\right)}^{-\frac{\nu +1}{2}}$$
计算得：
$$\omega \left(r\right)=-\frac{\partial logp\left(r\right)}{\partial r}\cdot \frac{1}{r}=\frac{\nu +1}{\nu \Sigma +r^2}$$
注意：论文1的误差公式：

而论文2的误差公式：

如果对应论文1的误差公式：
$$\omega \left(r\right)=-\frac{\partial logp\left(r\right)}{\partial r}\cdot \frac{1}{r}=\frac{\nu +1}{\nu \Sigma +r^2}$$
如果对应论文2的误差公式：
$$\omega \left(r\right)=-\frac{\partial logp\left(r\right)}{\partial r}\cdot \frac{1}{r}\cdot \Sigma =\frac{\nu +1}{\nu +\frac{r^2}{\Sigma }}$$
只是按照论文中的最大后验分布假设推导的，但是也不能完全自圆其说。
两篇论文也均有模糊不清的地方。

第一部分求解迭代：（1）

其中，J为r的雅可比矩阵。
第二部分：计算$argmin\xi -logp(\xi )$
作者设：

正态分布概率密度函数：
$$p({\xi }_t)=\frac{1}{\sqrt{2\pi \Sigma }}exp\left(-\frac{{\left({\xi }_t-{\xi }_{t-1}\right)}^2}{2\Sigma }\right)$$
则求解$\frac{\partial logp\left(\xi \right)}{\partial \xi }$
$$J_2=-\frac{\partial logp\left({\xi }_t\right)}{\partial \xi }=\frac{{\xi }_t-{\xi }_{t-1}}{\Sigma }$$
求其hessian矩阵：
$$H_2=-\frac{\partial \partial logp\left({\xi }_t\right)}{\partial \partial {\xi }_t}=\frac{1}{\Sigma }$$

求解第二部分的迭代公式：（2）
$$H_2∆\xi =-J_2$$
（1）与（2）相加得：

个人觉得这样的迭代计算并没有完全自圆其说，包括对t分布的求导公式也不一致。
Dvo-slam代码中使用的是论文2的思路。

2.2 论文2中的迭代公式

其中$\omega \left(r\right)$：

r包含灰度误差和深度两部分：

并设其符合t分布：

t 分布一般只有两个参数，平均值及自由度，作者这里加了一个方差量，不知该作者的t分布概率模型是使用的哪个，标准t分布的导数计算结果如2.1论述，与上述不符。

从上图可知，权重w与残差r之间的关系，残差值越大，t分布和正态分布分布的权重就更多一些。
权重跟方差有关，方差的计算：

这个公式右侧也同时包含了$\sigma$，代码中计算的，找出代码对应部分：代码部分与论文有所出入。

文件：dvo_core/src/dense_tracking_impl.cpp

3、关键帧的选择

定义了一个微分熵：

输入变量x符合高斯分布：

作者设：


定义，阈值因子：

$H\left({\xi }_{k:k+j}\right)$表示关键帧k到第k+j帧对应的相机位姿变换的微分熵；
$H\left({\xi }_{k:k+1}\right)$表示关键帧k到其下一帧对应的相机位姿变换的微分熵。

可知：

临近帧之间的$\xi$的方差最小，距离关键帧越远，微分熵越大，则$\alpha$越大。
论文中说$\alpha$越小，不知其所以。
代码中实际的判断是accept中进行判断。

来自dvo_slam/src/local_tracker.cpp

此处accpt是个信号槽，将括号中的参数都送给信号槽connect的接口；
其connect的接口：

来自dvo_slam/src/keyframe_tracker.cpp

信号槽的输出All：

来自dvo_slam/include/dvo_slam/local_tracker.h

可知代码中最后关键帧是否创立是图中的所有回调函数均返回true才建立。而不是论文中所说的通过因子。

此外，回环检测文中，作者说也是通过微分熵因子判断当前帧是否有匹配的关键帧，不过不同的是，建立关键帧时，因子约束是保持当前图像帧与上一关键帧运动了一定的距离，而回环检测，是要找到与当前图像帧匹配的关键帧，保证匹配关键帧与当前图像帧的距离较小。

找到匹配关键帧后，则进行当前帧的优化。具体待看。