By

文章目錄
  1. 1. 1.ORB-SLAM
    1. 1.1. 1.1. ORB-SLAM简介
    2. 1.2. 1.2. ORB-SLAM系统架构
      1. 1.2.1. 1.2.1. 追踪
      2. 1.2.2. 1.2.2. 地图构建
      3. 1.2.3. 1.2.3. 闭环检测
  2. 2. 2. Probabilistic Semi Dense SLAM
    1. 2.1. 2.1. Semi-Dense流程
    2. 2.2. 2.2. 详细流程
      1. 2.2.1. 2.2.1. 立体搜索限制
    3. 2.3. 2.2.2. 极线搜索
      1. 2.3.1. 2.2.3. 逆深度假设的融合
      2. 2.3.2. 2.2.4. 帧内深度检查、平滑和增长
      3. 2.3.3. 2.2.5. 帧间深度检验和平滑
    4. 2.4. 2.3. 总结


作者：Frank
时间：2016-09-22

ORBSLAM原作者的新论文《Probabilistic Semi Dense Mapping from Highly Accurate Feature-Based Method Monocular SLAM》结合了原来自身的ORBSLAM的流程和LSD-SLAM中利用概率统计的方式实现半稠密的深度重建，群里的贺所长根据这篇论文和ORBSLAM2的源码，实现了在ORB上的半稠密重建过程。这几天对这份代码进行了仔细的研究，受益颇多，这里就结合该论文讲述下半稠密的重建步骤。
贺所长的博客：http://blog.csdn.net/heyijia0327/article/details/52464278
Semi-Dense ORB：https://github.com/HeYijia/ORB_SLAM2

1.ORB-SLAM

1.1. ORB-SLAM简介

ORB-SLAM是一种基于ORB特征的SLAM算法。该算法由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年发表的。ORB-SLAM基于PTAM原有的架构思想，增加了Map的初始化部分和重复场景的Loop Closing部分，优化了关键帧的选取和地图构建的方法，在处理速度、追踪效果和地图精度上都取得了不错的效果。ORB-SLAM利用了几乎所有的当前的主流方法，而且开源的代码写的非常规范，对于做SLAM方向的有很大的借鉴意义。
原始的ORB-SLAM构建的Map是稀疏的，它一开始基于Monocular Camera，后来扩展到Stereo和RGB-D sensor上。以下所有的讲述都是基于Monocular(单目)的ORB进行讲述。
ORB-SLAM算法的一大特点是在所有步骤上统一使用ORB特征检测。ORB特征是一种非常快速的特征提取方法。具有旋转不变形，并可以利用金字塔构建出尺度不变性。使用统一的ORB特征有助于SLAM算法在特征提取和追踪、关键帧选取、三维重建和闭环检测等步骤中保持内在的一致性。

1.2. ORB-SLAM系统架构

以下是ORB-SLAM的系统架构：

ORB-SLAM利用三个线程分别进行追踪、地图构建和闭环检测。

1.2.1. 追踪

ORB-SLAM中的追踪过程如下：

  1. ORB特征提取；
  2. 初始姿态估计（匀速度运动估计模型）；
  3. 姿态优化（Track Local Map,利用邻近的地图点寻找更多的特征匹配，优化姿态）；
  4. 选取关键帧

1.2.2. 地图构建

ORB-SLAM中的地图构建过程如下：

  1. 加入关键帧（更新图）；
  2. 验证最近加入的地图点（Outlier Removal）；
  3. 生成新的地图点（三角法）；
  4. 局部BA（该关键帧和邻近关键帧，去除Outlier）；
  5. 验证关键帧（去除重复帧）；

1.2.3. 闭环检测

ORB-SLAM中的闭环检测过程如下：

  1. 选取相似帧（Bag of Words）；
  2. 检测闭环（计算相似变换，RANSAC计算内点数）；
  3. 融合三维点，更新图；
  4. 图优化，更新地图所有点；

2. Probabilistic Semi Dense SLAM

半稠密的ORB-SLAM是在原ORB-SLAM的基础上增加了一个半稠密的线程，对于原来的系统几乎没有影响，以下是Semi-Dense系统的架构：

图中虚线框中的流程和原ORB-SLAM的流程是一模一样的，只是在左下角部分增加了一个Semi-Dense Mapping的线程。

2.1. Semi-Dense流程

ORB-SLAM中的Semi-Dense Mapping流程如下：

  1. 对于每一帧关键帧$K_i$，我们都去其图像求梯度图像，只有在梯度值满足门限要求的像素点才会被拿来进行计算，对于每一个这样的像素点$p_j$，对其在临近的N帧关键帧中做极线搜索和匹配，得到N个对于逆深度的假设；

  2. 考虑到极线匹配中可能存在的图像的噪声、视差和二义性，我们将逆深度表示为服从高斯分布的点；

  3. 由于我们的关键帧$K_i$和帧间旋转位移R，T都是通过特征提取和匹配后计算出来的，因此得到的是大基线的搜索区域。因此首先需要对得到的所有的逆深度假设进行Outlier并对保留下来的结果进行融合得到一个服从$N(\rho _p,\sigma _{\rho _p}^2)$分布的逆深度假设；

  4. 在得到了梯度区间的所有点的深度估计后，我门对整个逆深度图进行平滑，对每个像素点和其周围的像素点做均值处理，并判定该像素点和其周围的像素点是否兼容（判定方式同3）；

  5. 在当前关键帧和它邻近关键帧的逆深度图都计算完成后，对当前帧和其邻近帧之间的一致性进行判定，对判定后的深度图做Gauss-Newton优化，得到最终的深度图结果。

接下来的步骤会大致按照上述流程进行详细的讲述。

2.2. 详细流程

2.2.1. 立体搜索限制

对于每一个关键帧，因为都是经过原ORB-SLAM流程计算的，所以其有对应特征点的及其深度。我们能从所有特征点的深度中得到深度倒数（逆深度参数化）的最大值$\rho _{max}$和最小值$\rho {min}$利用这些值我们可以构造一个服从高斯分布的特征方程：$N(\rho _0,\sigma _{\rho_0}^2)$，其中$\rho_{max}=\rho _0 +2\sigma _{\rho_0}$，$\rho_{min}=\rho _0 -2\sigma _{\rho_0}$。同时，我们维护一个N帧的最近邻关键帧（这些关键帧之间有最多的重叠地图点）。同时，所有的流程都会延迟10帧左右，这样可以使得当前帧不但能使用前帧进行重建，同时也能使用后帧来进行重建。

2.2.2. 极线搜索

极线搜索的示意图如下所示：

对于关键帧$K_i$中的某个像素点p，首先p的梯度必须大于门限值$\lambda _G$。对于满足该条件的p，我们会对其沿着极线$I_j$在$K_i$的每一个邻近帧$K_j$中进行搜索，并将其限制在范围$[\rho {min},\rho {max}]$之间。同时，极线$I_j$是通过$K_i$和$K_j$之间的基础矩阵$F_{ji}$计算得到的，为了简化步骤，将该方程表示为水平坐标$u_j$的方程：
$$ \matrix{
x_j F_{ji} x_p=x_j^T I_j=0 \cr
v_j =m \cdot u_j +n \cr
}\tag{1} $$
之前提过ORB是基于长基线的极线搜索，因此在进行极线搜索之前我们需要对像素点p先进行滤除，假设关键帧$K_i$的梯度图像为G，梯度的方向为$\Theta$，则像素点p对应的图像像素点为$I_p$，梯度为$G_p$，梯度的方向为$\Theta_p$，则其滤除规则为：

  1. $p_j$必须在高梯度值区域，即：$G(u_j)>\lambda_G$；

  2. 梯度的方向不能和极线的方向垂直，即$|\Theta (u_j)-(\Theta _L +\pi)|<\lambda
    _L$，其中$\Theta_L$表示极线的角度；

  3. $p-j$梯度的方向应该和$p_i$保持一致，即$|\Theta (u_j)-(\Theta_ (p_i)+\Delta \theta_{j,i})|<\lambda _\theta $，其中$\Delta \theta_{j,i}$表示两帧间的ORB特征点的平均旋转角度；

当邻近帧上的对应特征点满足上述三个条件后，其可以用来进行下一步计算。为了对剩余的像素点进行比较，我们定义一个相似性误差$e(u_j)$：
$$ \matrix{
e(u_j)={r_I^2 \over \sigma _I^2}+{r_G^2 \over \sigma _G^2} \cr
r_I=I_p-I_(u_j) \cr
r_G=G_p-G(u_j) \cr
} \tag{2} $$

其中$r_I$是图像误差,$r_G$是梯度误差；$\sigma _I$和$\sigma_G$是对应的标准差。因为梯度是由亮度得到的方程，所以有$\sigma_G^2=\theta \sigma_I^2$。如果使用Scharr计算梯度的话$\theta=0.23$则相似性误差可以表示为：
$$e(u_j)=(r_I^2+{1\over \theta}r_G^2){1 \over \sigma_I^2} \tag{3}$$
我们选择能最小化该误差的坐标$u_0$得到对应的残差$r_{I_0}$和$r_{G_0}$。然后计算误差的偏导数：
$$ {\partial e \over \partial u_j}={-2(r_Ig+{1 \over \theta}r_G q) \over \sigma _I^2} \tag{4}$$
其中g是亮度梯度，q是亮度梯度的方差，即：
$$g \approx {I(u_j+1)-I(u_j-1) \over 2},q \approx {G(u_j+1)-G(u_j-1) \over 2} \tag{5} $$
对式(2)进行一阶泰勒展开，并让式(4)等于0，可以得到亚像素精度的像素更新值：
$$u_0^{*}=u_0+{g(u_0)r_I(u_0)+{1 \over \theta}q(u_0)r_G(u_0) \over g^2(u_0)+{1 \over \theta}q^2(u_0)} \tag{6} $$
由此可以得到：
$$ \sigma _{u_0^{*}}^2={2\sigma_I^2 \over g^2(u_0)+{1 \over \theta}q^2(u_0)} \tag{7} $$
$K_i$中的像素点p对应的逆深度$\rho_p$是关于$u_j$的方程，因此我们可以得到（也可以通过将世界坐标系中点投影到相机坐标系中得到）：
$$\rho _p(u_j)={r_z^{ji}{\bar X} _p(u_j-c_x)-f_x r_x^{ji}{\bar X} \over -t_z^{ji}(u_j-c_x)+f_xt_x^{ji}} \tag{8} $$
其中$r_z^{ji}$和$r_x^{ji}$分别表示旋转矩阵$R_{ji}$的第三和第一行；$t_z^{ji}$和$t_x^{ji}$表示平移矩阵$t_{ji}$的第三个和第一个元素，${\bar X} _p=K^{-1}x_p$表示通过像素p的投影点，K是校正参数，$f_x$和$c_x$是焦距和光心坐标。从式(8)我们可以推导出逆深度假设$N(\rho _j,\sigma _{\rho_j}^2)$，其表达式如下所示：
$$ \matrix{
\rho_j=\rho_p(u_0^{*}) \cr
\sigma _{\rho _j}=max(|\rho_p(u_0^{*}+\sigma _{u_0^{*}})-\rho _j|,|\rho_p(u_0^{*}-\sigma _{u_0^{*}})-\rho _j|) \cr
} \tag{9} $$

2.2.3. 逆深度假设的融合

通过2.2.2节的极线搜索，我们得到了当前关键帧的中某个像素点从邻近帧或者的多个关于深度的逆深度假设值$\rho _j$.假设当前得到的逆深度假设有M个，我们需要从这M个中找到最少$\lambda N$个相互兼容的假设，并将其进行融合。假设之间是否兼容的判定标准是检查二者之间兼容性的$\chi$分布测试是否大于95%。判定准则如下：
$${(\rho _a -\rho _b)^2 \over \sigma _a ^2}+{(\rho _a -\rho _b)^2 \over \sigma _b ^2}<5.99 \tag{10}$$

我们从M个假设中任意选取一个，并与其他所有的假设做比较，如果兼容的个数$ n > \theta N$，则将这n个假设进行融合，得到关于像素点P的逆深度高斯分布$N(\rho_p,\sigma _{\rho _p}^2)$:
$$ \rho _p ={ {\sum _{n} {1 \over \sigma _{\rho _j}^2} \rho _j} \over {\sum _{n} {1 \over \sigma _{\rho_j}^2}}},\sigma _{\rho_p}^2={1 \over {\sum _{n} {1 \over \sigma _{\rho_j}^2}}} \tag{11} $$

2.2.4. 帧内深度检查、平滑和增长

在计算完半稠密的逆深度图之后，我们需要对该逆深度图进行过滤、平滑和增长。首先我们计算包围某个逆深度点的8个邻近点和该逆深度点的兼容性，其计算过程同式(10)。如果兼容的邻近点大于2，则保留该点，否则，丢弃该点。所有被保留的逆深度点都会利用式（11）和它兼容的邻近点求平均以平滑该逆深度点。在该步骤之后进行一个增长步骤，对于那些满足梯度要求但是不存在深度的像素段，如果其邻近的8个点中至少存在2个点存在逆深度，则该点设置为邻近点逆深度的平均值。该步骤可以让重建的深度图更稠密。

2.2.5. 帧间深度检验和平滑

当关键帧$K_i$的邻近关键帧都计算完毕之后，开始对逆深度图$K_i$中所有的逆深度分布的一致性进行检验。对于$K_i$中背个存在逆深度的像素点p，将其对应的3D点投影到每一个邻近关键帧$K_j$中，并按照下式计算对应的逆深度：
$$ \matrix{
x_j=KR_{ji}{1 \over \rho _p} {\bar X} _p ++Kt_{ji} \cr
\rho _j={\rho _p \over r_z^{ji}{\bar X} _p+\rho _p t_z^{ji}} \cr
} \tag{12}$$
由于得到的$x_j$可能不为整数，我们寻找包围$x_j$的四个像素点$p_{j,n}$中和该点兼容的逆深度，其兼容性检验方程为：
$$ {(\rho _j -\rho _{j,n}) \over \sigma _{\rho_{j,n}}^2}<3.84 \tag{13} $$
当在4个点中至少有一个点满足检验条件时，该邻近帧中点的深度就将被保留。当在所有邻近帧中至少有$\lambda N$帧满足前述检验条件，则该关键帧中的逆深度值得以保留。
最后一步，我们对所有兼容的像素点进行Gauss-Newton优化来最小化如下定义的深度误差：
$$ d_p^{*}=min \sum_{j,n}(d_{j,n}-d_p r_z^{ji}{\bar X}-t_z^{ji})^2{1 \over d_{j,n}^4\sigma _{\rho _{j,n}}^2} \tag{14} $$
这里我们的优化变量是深度而非逆深度，因为传播方程(12)是关于深度的线性方程，并且优化增量$d_p^{*}$能在一次迭代就得到结果。

2.3. 总结

经过上述步骤后，帧间的深度会从前帧传播到后帧，然后我们就可以通过三角化得到半稠密的重建地图，以下是半稠密重建的结果：



转载请注明出处
文章目錄
  1. 1. 1.ORB-SLAM
    1. 1.1. 1.1. ORB-SLAM简介
    2. 1.2. 1.2. ORB-SLAM系统架构
      1. 1.2.1. 1.2.1. 追踪
      2. 1.2.2. 1.2.2. 地图构建
      3. 1.2.3. 1.2.3. 闭环检测
  2. 2. 2. Probabilistic Semi Dense SLAM
    1. 2.1. 2.1. Semi-Dense流程
    2. 2.2. 2.2. 详细流程
      1. 2.2.1. 2.2.1. 立体搜索限制
    3. 2.3. 2.2.2. 极线搜索
      1. 2.3.1. 2.2.3. 逆深度假设的融合
      2. 2.3.2. 2.2.4. 帧内深度检查、平滑和增长
      3. 2.3.3. 2.2.5. 帧间深度检验和平滑
    4. 2.4. 2.3. 总结