lsd-slam深度滤波器

lsd-slam深度滤波器

论文：
《Semi-Dense Visual Odometry for a Monocular Camera》
深度滤波完成的内容是估计图像中梯度较大的像素点的深度，并计算该深度假设的不确定性，通过新的观测进行优化，当深度值收敛，则将其融入全局地图。

一、背景

基于特征点的单目SLAM：
分为两步：
  1. 提取离散特征点并进行特征点匹配
  2. 通过匹配的特征点计算相机及特征点的位姿解算
两个缺点：
  1. 使用的图像特征通常仅为角点，斑点及边角线。
  2. 匹配特征点的计算消耗较大

基于直接法的稠密单目SLAM：直接使用图像解算相机位姿。并建立稠密世界地图

二、算法大纲

LSD深度滤波算法核心点：
  1. 概率深度地图构建
  2. 使用全图像直接法计算相机位姿及点云分布
  3. 图像有效信息裁剪及保留
  4. 三维立体测量不确定性因素的计算

  提出一种半稠密单目视觉里程计。能够评估半稠密像素点逆深度，地图是由具有大梯度的像素点对应的空间点组成。计算得到的图像深度，会根据估算得到的相机位姿进行帧间传播，利用自适应基线、极线搜索、几何计算得到每个像素点新的深度估计值，根据新的观测值，调整像素深度假设及可信度。每个像素只维持一个符合高斯分布的深度假设。

  LSD的一个核心是半稠密逆深度地图的计算。LSD并非对所有像素都进行深度估计，仅对图像具有大深度的角点，斑点及边线等进行深度估计，因为使用这些像素具备的灰度值更容易匹配，能够更准确地找到匹配点。LSD的深度计算分为3个部分：
  1. 三维立体深度计算
  2. 深度及深度可信度的帧间传递
  3. 深度正则化及离群点处理

2.1 立体深度计算

  使用立体几何的方法(对极几何)计算深度，需使用两个图像帧之间的像素点匹配关系，基线(baseline)越大，意味着这两帧图像距离越长(两帧图像对应的相机的位姿变化更大)。

  使用像素灰度值作为匹配图像点的约束(指使用灰度一致在另一帧图像中搜索匹配点，并计算像素点深度)。深度值为求解值，灰度值作为约束，则Baseline的大小与约束方程的关系如下图所示：

  从图中可以看出，越小的baseline，约束方程的局部最小解就越小，上图例子中的黑线具有唯一最小解，但是其凹点的斜率较小，因此会导致最后的求解结果的精确度有所下降。
  而较大的baseline，具有很多的凹点(即局部最优解)，求解不唯一，但是在真实解处的值却足够精确。

  直观的理解，当baseline比较大时，使用灰度作为约束，整个baseline之间有多个接近参考像素点灰度值的图像点，因此会出现多个解。

  该文提出先用小的baseline求解，再用大的baseline优化。

深度估计的整个步骤如下：
  1. 选取当前帧图像中合适的像素点
  2. 为选取的像素点选取合适的参考帧
  3. 一维极线搜索匹配点，并利用先验的深度信息(先前帧传递得到的深度估计值)，减少搜索的范围，最后计算得到像素点深度

2.1.1 参考帧的选取

  参考帧的选取要满足深度计算的准确度，同时也要保证两帧之间相机的姿态变化足够小。像素点的参考帧使用相对当前帧的相机角度变化小于指定阈值的最早的观察到该像素的帧(并不是最早观察到像素的帧，而是在阈值范围内包含该像素的最早的帧)，且视差(disparity)搜索范围较小。视差的搜索范围是由先验的深度预估计值的可信度确定的。可信度高，方差小，搜索范围越小。

  如下所示：左上角为当前帧图像，当前帧图像不同像素对应的参考帧有所不同。

2.1.2 极点匹配

  得到参考帧后，利用帧间的运动位姿估计计算得到参考帧中的极线。若像素有先前帧传递得到的深度估计，则深度搜索缩小为$d\pm 2{\sigma }_d$，对应的缩小极线的搜索范围。同时计算极线上等距5个点的SSD误差(灰度误差平方)来提高搜索效率。

  其中$x$与$u$均为图像二维坐标。

2.1.3 不确定性评估

  简化为及线上只有单一灰度匹配，使用逐点搜索
  使用方差${\sigma }_d^2$来表述逆深度$d$的不确定性。逆深度$d$与两帧图像$I_0$，$I_1$，两帧图像之间的相机运动$\xi$及相机的映射矩阵$\pi$相关，可描述为以下方程：

  则$d$的方差${\sigma }_d^2$为：

  其中$J_d$为逆深度$d$方程的雅可比矩阵，$\Sigma$为输入变量的方差

极线匹配分为以下3个步骤：
  1. 参考帧对应的极线计算
  2. 极线搜索，寻找匹配点
  3. 根据匹配点计算像素深度
  前两步的误差构成分为：几何误差(geometric error)度误差(photometric error).
  几何误差：是由两帧图像之间的相机运动$\xi$及相机的映射矩阵$\pi$的误差引起。使第一步极线产生。
  光度误差：由于图像$I_0$，$I_1$的畸变等因素而引起第二步搜索匹配点产生误差。
  几何误差和光度误差综合影响第三步的计算误差。根据基线的大小放缩几何误差和光度误差。

  根据以上可知：
  同样的基线，逆深度与视差$u_L-u_R$成正比关系。

几何误差
设极线的方程为：

  其中：${\left(l_x,l_y\right)}^T$是单位化后的极线方向向量。$l_0$是参考帧极线上对应当前帧像素点无穷深度的点。$l_0$的位置误差${ϵ}_l$符合高斯分布。$\lambda$为极线匹配点的搜索范围。
$l_0$的位置误差${ϵ}_l$会引起搜索视差${ϵ}_{\lambda }$。当极线与图像梯度垂直时，${ϵ}_l$会引起搜索视差${ϵ}_{\lambda }$较小。如下图所示：

  图中的右下角的波浪线表明梯度及极线的方向。虚线表示等光度线。图像梯度方向垂直于虚线方向。${ϵ}_l$表示极线的偏移，寻找匹配点的约束使用光度一致，因此匹配点真实值和实际估计值处于同一等光度线上。得到以下方程：

其中，$g:=(g_x,g_y{)}^T$表示图像梯度方向，$g_0$为等光度线上的一个点。$g_0$的误差不是由几何误差引起，而是由光度误差引起，因此$g_0$所引起的误差在下一节(光度误差中)讨论，此节只考虑几何误差，因此，设$g_0$无误差。\lambda的误差只由几何误差引起，即此处只与$l_0$有关。
由上式求解得到$\lambda$，若$l_0$固定，则$\lambda$值唯一。则得到最优${\lambda }^{\ast }$与$l_0$的关系：

求解过程：

由几何误差引入的匹配误差方差：${\sigma }_{\lambda \left(\xi ,\pi \right)}^2$为：

其中$g$为单位化的匹配点所在的图像梯度向量，l是单位化的极线方向向量，${\sigma }_l^2$为$l_0$的位置误差${ϵ}_l$的方差。此误差只与相机运动\xi及相机的映射矩阵$\pi$的误差有关。与图像灰度无关。

光度误差
当匹配点对应的像素灰度梯度较大时，光度误差对搜索匹配的结果影响较小，反之，当像素梯度较小，则光度误差会对匹配结果产生很大的影响，如下图所示：

设灰度一致性：当前帧与参考帧匹配点的像素灰度值尽可能相等：

其中${\lambda }_0$为匹配点上一步的搜索结果值，作为当前的进一步优化初始值。$I_p\left(\lambda \right)$为匹配点灰度值，这个公式表达的意思明确，但是极线搜索是放在参考帧的，所以公式写成以下更契合：

将$\lambda$在${\lambda }_0$处进行一阶泰勒展开，得$\lambda$与${\lambda }_0$的关系如下：

其中，$g_p$是$I_p$处的灰度，且$I_p$位于极线上。光度误差表现在$I_p$和$i_{ref}$上。
计算得到光度误差引入的匹配误差方差：${\sigma }_{\lambda \left(I\right)}^2$为：

此时的误差只与光度误差有关。
像素到逆深度的转换
上文中提到的，逆深度与正比于视差，同样，非理想情况下，逆深度与$\lambda$也比例关系。
因此由几何误差和光度产生的$\lambda$的误差也与待求解的逆深度误差成比例关系，因此，使用以下公式求解逆深度的观测误差方差：

其中：

${\delta }_d$表示逆深度的搜索半径，也由先前帧传递的深度信息决定搜索半径
${\delta }_{\lambda }$ 表示极线上匹配点的搜索半径
在极线上搜索匹配点的时候，是使用了多个点，因此实际上这里是给出了逆深度误差的上限：

2.1.4 深度观测融合

将深度观测值集成道深度地图中：如果像素点没有深度假设，则基于深度观测评估结果为像素点初始化一个深度假设。如果像素点已有深度估计，则为利用EKF(卡尔曼滤波)为该像素更新深度估计。设原先的深度估计分布为：$\mathcal{N}\left(d_p,{\sigma }_p^2\right)$，新的观测值误差分布为$\mathcal{N}\left(d_o,{\sigma }_o^2\right)$，求得后验新的深度误差模型为：

2.1.5 深度传播总结

通过为每个像素点选定对应的参考帧，并进行一维极线搜索，从而得到每个像素新的几何深度观测值。影响深度估算的因素包括：(论文中将几何误差和光度误差写反了)

1. 几何误差${\sigma }_{\lambda \left(\xi ,\pi \right)}^2$，由两帧图像之间的相机运动\xi及相机的映射矩阵\pi的误差引起。几何误差的大小同时受像素点所在的图像梯度影响。
2. 光度误差${\sigma }_{\lambda \left(I\right)}^2$：由于图像的畸变等因素而引起，同样受像素带你所在的图像梯度影响。
3. 像素反深度比$\alpha$：由几何误差和光度误差的综合产生。
   以上三者将决定像素深度的可靠性，从而选取可靠的像素点进行深度几何计算。同时也将此观测深度向后传播，与之前的深度估计克尔曼结合，向后传播深度的可靠性。

2.2 深度传播

深度的传播同样是以反深度$d$进行传播。当得到下一帧的相机位姿之后，将当前帧得到的反深度观测值$d_0$，根据得到的两帧之间的相机运动，传递到下一帧，设下一帧该像素点对应的反深度值为$d_1$。然后将深度假设分配到最近的整数像素位置–为了消除离散化错误，将保持投影点的亚像素精确图像位置，并重新用于下一个帧传播。
假设相机在两帧之间的旋转较小，可以忽略，则$d_1$与$d_0$之间的关系可以表示为:

方差为：

其中${\sigma }_p^2$表示深度预估不确定性。即$d_0$的不确定性。如果有两个点经过深度传播时传递到同一个像素点，则分两种情况处理：两者相近（在 [公式] 内）进行不确定度的加权融合；否则舍弃那个较远的。

2.3 深度地图正则化

在得到所有的深度观测之后，执行一次正则化，将像素点周围的深度的加权平均值作为该像素点的深度估计。加权的权重由估计的深度方差决定。如果相邻的像素深度相差超过$2\sigma$，则他们不互相参与对方的深度估计。在正则化过程中，各方差不会发生变化，用以表明相邻假设之间的高度相关性。在定义立体搜索范围时，使用所有相邻像素的最小方差，并将其作为跟踪的加权因子。
离群点移除
评估深度估计的有效性，如果一个成功的立体几何观测，经过EKF之后，深度估计的方差会减小。在正则化过程中，如果周围的概率乘积大于阈值，则删除该点的深度估计；若一个“空点”周围概率乘积小鱼阈值，则创建出该点。

三、稠密跟踪

根据得到的上一帧的反深度地图，估计得到当前帧与先前帧之间的位姿变化。光度误差公式如下：

其中，$\omega ()$ 将$I_1$下的图像坐标点x_i映射到$I_1$下，$\omega ()$的参数除了$x_i$，还需要两帧相机之间的位姿变化$\xi$，及像素点$x_i$在$I_1$帧下是深度估计，注意，这里没有用到$x_i$在I_2下的深度估计。
增加鲁棒性，给约束方程加入权重，约束方程如下

其中，$\alpha \left(\right)$为与残差$r_i\left(\xi \right)$相关的权重方程。
然后，将上述方程转化为最小二乘问题，并使用GN(高斯牛顿)算法进行迭代求解，得到相机在两帧之间的位姿变化$\xi$。

四、参考链接

https://www.zybuluo.com/lancelot-vim/note/412293
https://blog.csdn.net/MAX_Hope/article/details/89418802
https://blog.csdn.net/lancelot_vim/article/details/51730676
https://www.cnblogs.com/hitcm/p/4907536.html
https://zhuanlan.zhihu.com/p/47742232