3. 视觉里程计前端

前端算法具有以下功能：

• 确定组成总光度误差的集合和obs（p）。即决定优化时所使用点和帧，和确定每帧中可观测到的点——这包括离群值剔除和遮挡检测。

• 提供用于优化高度非凸化的所需的参数初始值。根据经验，图像I的线性化仅在1-2像素半径内有效;因此计算中涉及的所有参数的初始值应该被足够精确，以使得的误差不超过1-2个像素。

• 决定点/帧何时被边缘化

  
  

因此，前端需要完成类似于间接法中关键帧检测和RANSAC算法等功能。注意，这里描述的许多过程是针对单目。比如，使用立体相机可直接获得初始深度值，而集成IMU可显著提高追踪的鲁棒性，甚至可直接提供新帧的位姿初值。

3.1 帧管理

我们的方法是保持一个最多包含个关键帧的窗口（我们使用
）。首先相对于这些参考帧跟踪每个新帧（步骤1）。之后创建关键帧或将新帧丢弃（步骤2）。一旦创建了新的关键帧和相应的新点，则优化总光度误差（8）。之后，我们边缘化一个或多个帧（步骤3）。

步骤1：新帧追踪        当新的关键帧被创建时，所有激活的地图点投影到该帧上，从而创建半稠密地图。当新帧产生时，仅相对最近关键帧进行图像直接配准，从而完成跟追踪，其中会使用到多尺度图像金字塔和恒速运动模型。图7给出一些示例——我们发现进一步增加地图点数量对于追踪精度和鲁棒性并无益处，同时显著增加计算时间。应该注意的是，如果下采样时至少有一个像素具有如在[1]中的深度值，将显著增加地图点密度。

如果一帧的RMSE（均方根误差亦称标准误差）是前一帧的两倍以上，则图像直接配准失败，将通过初始化恢复位姿。该恢复流程仅在分辨率最低的图像金字塔级别完成，每次将大约需要0.5ms。注意，除非相机非常快速地震动，否则这种类似于RANSAC的流程很少被调用。而且如果整合IMU则可以避免该流程。

图7.用于新帧初始化的深度图示例  顶行显示原始图像，底行显示具颜色编码的深度图。由于我们的目标是在优化框架中保持固定数量的激活点，因此在纹理密集场景（左）中深度图变得更稀疏，而在弱纹理特征场景（右）中生成类似于LSD-SLAM的半稠密地图。

步骤2：创建关键帧        与ORB-SLAM类似，我们的策略是首先获取很多关键帧（每秒大约5-10个关键帧），然后边缘化冗余关键帧。以下三种情况会创建新的关键帧：

1.在视角变化时会创建新的关键帧。这是通过在新帧追踪期间计算新帧和最近关键帧的光流的均方根值来判断视角变化。

2.在相机平移导致遮挡或遮挡去除时会创建新的关键帧。这是通过相机不发生旋转时的光流的均方根值来判断遮挡。

3.在曝光时间显着变化时会创建新的关键帧。这是通过两个帧之间的相对亮度因子来判断曝光时间的变化。

这三个量在新帧追踪阶段时可以容易求得。最后，如果，则新帧为新的关键帧，其中
是这三个指标的相对加权，并且默认。

步骤3：关键帧边缘化    边缘化流程如下（是激活的关键帧，其中
是最新关键帧，是最老的关键帧）：

1.我们总是保有最新的两个关键帧（和）。

2.在中可见的点少于5％的帧被边缘化。

3.如果激活的关键帧多于，则边缘化 “距离分数”最大的关键帧（除了和)，公式为

（20）

其中d(i,j)是关键帧和之间的欧几里得距离。采用距离分数是为了保证激活的关键帧在3D空间中的良好分布，使更多的关键帧靠近最近的关键帧。

一个关键帧被边缘化，首先边缘化该帧中的地图点，然后边缘化该关键帧，按照第2.3节中描述的边缘化过程。为了保证Hessian矩阵的稀疏结构，该关键帧不在中的点将被丢弃。虽然这不是最佳的方法（实际上大约有一半的残差项被丢弃），但它使得实时有效地优化总光度误差变成可能。图8给出了一个场景示例。

图8.关键帧管理    底部两行：滑动窗口中的6个旧关键帧，覆盖有属于该关键帧的地图点（已经被边缘化的点显示为黑色）。顶部图像显示完整的点云，以及所有关键帧（黑色摄像机锥截体）的位置，激活点和激活帧分别显示为红色和蓝色。右下角显示新添加的关键帧，覆盖有所有激活点，其将用于后续帧的图像直接配准。

3.2点管理

大多数现有的直接法使用尽可能多的图像数据。为了保证实时性，它们使用线性化或者深度三角测量等次优估计方法，或者忽略不同参数之间的相关性。但是在本文中，我们采用下采样的方式，以允许在联合优化框架中实时处理数据。事实上，我们的实验表明，图像数据是高度冗余的，简单地使用更多的数据点并无益处。注意，与间接法不同的是，我们的直接法框架允许从所有可用数据中采样，包括弱纹理区域、重复区域和边缘，而这确实是极有好处的（见第4部分）。

我们的目标是在优化时始终保持固定数量的激活点（我们使用
=2000），在3D空间和激活帧中均匀分布。首先，我们识别每个新关键帧中的个候选点（步骤1）。候选点不立即添加到优化框架中，而是在后续帧中单独追踪，生成粗略的深度值估计以用于初始化（步骤2）。当需要将新点添加到优化框架中时，我们激活一些候选点（来自优化窗口中的所有帧），添加到优化框架中（步骤3）。注意，我们在每个帧中选择个候选点，但只保留所有激活帧中的个激活点。

步骤1：候选点选择        我们旨在选择（1）在图像中良好分布和（2）足够高的强度梯度的点。我们将图像分割成32×32块来获得区域自适应梯度阈值。对于每个块，我们计算阈值，其中是该块中所有像素的强度梯度的中值，是全局常数（我们使用 = 7）。

为了使得在整个图像中的点的均匀分布，我们将图像分成d×d块，在每个块中选择具有最大梯度的像素点，如果它超过区域自适应阈值，则将其选为候选点，否则，我们不从该块中选择候选点。我们发现，从没有高梯度点的区域选择较弱梯度的像素点通常是有益的，例如从光照强度平滑变化白色墙壁的进行采样。为了实现这一点，我们多进行两次候选点选择，第三次梯度阈值比第二次降低，块的尺寸分别是2d和4d（如果这一帧选择过多候选点，则下一帧增加候选点，否则反之）。图9展示了一些示例场景的所选择的候选点。注意，对于候选点选择，我们使用未光度标定的原始图像。

图9.候选点选择    顶行显示原始图像，底行显示被选择出来的候选点（每帧中2000个点）。被第一次选出的点以绿色显示，第二次和第三次被选择的点分别以蓝色和红色显示。绿色的候选点均匀分布在梯度丰富的区域，而第二和第三次被选择的点覆盖具有非常弱的强度变化的区域，而且更加稀疏。

步骤2：候选点追踪        在后续帧中沿着候选点的极线进行离散化搜索，追踪候选点，从而最小化光度误差（4）。从最佳匹配结果中，我们计算深度值及其方差，其用于约束后续帧的搜索空间。这种跟踪策略的灵感来自LSD-SLAM。注意，一旦点被激活，计算的深度值仅用于初始化。

步骤3：候选点激活        在一组旧点被边缘化之后，需要激活候选点以替换它们。同样，我们的目标是保持地图点在整个图像中均匀分布。为此，我们首先将所有激活点投影到最近的关键帧上。然后激活与现有的地图点距离最大的候选点（在第二和第三次候选点选择时创建的候选点需要更大的距离阈值）。图7展示了在多个场景中地图点的分布。

离群值和遮挡检测    由于可用的图像数据通常比实时运行时用到的信息多很多，因此我们尝试尽可能早地识别和去除潜在的离群值。首先，在候选点追踪期间沿着极线搜索时，最小值没有超过阈值的点被永久丢弃，这大大减少了集中区域的错误匹配。第二，去除光度误差（4）超过阈值的点。注意，阈值并不是一成不变的，根据帧的不同区域的光度误差的中值，对阈值进行调整。对于“坏”帧（例如包含大量运动模糊的帧），阈值将更高，使得并非所有点都被移除。对于“好“帧则反之。

参考文献：

【1】T.Schops, J. Engel, and D. Cremers.¨       Semi-densevisual odometry for AR on a smartphone. In InternationalSymposium on Mixed and Augmented Reality (ISMAR), 2014. 8

【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

【注】商业转载请联系刘富强（liufuqiang_robot@hotmail.com）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。