SOFT-SLAM系列论文解读

SOFT2在KITTI数据集上霸榜了，作为视觉方案的定位方法，取得了超过激光-视觉融合的方法，凸显了该算法的优越性，所以赶紧紧跟潮流，研读相关论文。
StereoScan: Dense 3D reconstruction in real-time
该论文是SOFT-SLAM的基础，主要提出了所用到的特征点及匹配方法：Blob/corner detector，在此也主要介绍该方法。
本论文由4个模块构成，运行在双核CPU上：
(1) Sparse feature matching; (2) egomotion estimation; (3) dense stereo matching; (4) 3d reconstruction.

特征匹配 Feature Matching
首先利用5 X 5的blob and corner masks对输入灰度图像进行滤波，然后提取特征点并分为四类(blob max, blob min, corner max,corner min)，基于SAD（sum of absolute differences )进行特征点的匹配，并利用“circle match”进行检验以去除外点，并给出一些加速匹配的策略。

Stereo odometry based on careful feature selection and tracking
SOFT-SLAM工作的基础，主要通过精细化选择特征点来提高视觉里程计的精度。
特征处理
特征点匹配
利用blob and corner masks对输入图像进行滤波—>非极大值抑制提取特征点—>在图像差分域基于SAD( Sum of Absolute
Differences)进行特征点匹配—>circular matching去除外匹配—>NCC(Normalized Cross Correlation)检验
特征点选择
特征点图像平面均匀分布和空间远近均匀分布有利于提高位姿估计精度(Precise estimation of the egomotion requires that both far and near features are used in calculation of egomotion, and that features are uniformly distributed over the image)。为此采用bucketing技术进行特征点的选取，即将图像按照大小50像素进行网格划分，然后在各个网格里选取一定数量的特征点，具体流程如下：

1. The image is divided into 50 × 50 pixels sized rectangles. Buckets划分；
2. Features are separated into four distinct classes (corner max, corner min, blob max and blob min).特征点分类；
3. Features inside each class are sorted by strength 特征点排序；
4. The strongest feature from each class is pushed into the final list.
5. Step 3) is repeated until all features are pushed into the final list.
6. First n features from the final list are selected for motion estimation.
   (3)(4)(5)选取各类中strong特征点。

特征点跟踪
每个特征点被记录如下属性：
• unique identifier (ID),
• age,
• refined current position in the image,
• feature strength,
• belonging class,
• initial descriptor.
特征点跟踪过程中，特别强调初始描述子、像素位置增强和two pass matching的作用： “As long as the feature is alive, the same initial descriptor is used for feature position refinement. Using the same descriptor reduces drift and serves as a measure of dissimilarity between the current appearance of the feature and the initial one”；“Drift is reduced through the mechanism of propagating refined position”;“In order to retain as much tracks as possible and to further decrease the number of outliers, two pass matching is implemented”.
Two pass matching: Firstly, only one feature from each bucket is used and initial rotation and translation is estimated. Secondly, the pass is executed with greater number of features per bucket, and the matching is constrained with initially estimated rotation and translation. Constrained matching provides set of features with less outliers which leads to more accurate estimation.
位姿估计
将旋转和平移分开进行估计。
首先基于五点法求本质矩阵，并分解得到旋转矩阵，然后使用最小化重投影误差的方法求平移矩阵；
IMU融合

利用EKF融合IMU中的陀螺仪和视觉里程计，可以更好地估计位姿，同时陀螺仪可用于外匹配的去除和减少匹配搜索域，提升速度。

SOFT-SLAM: Computationally efficient stereo visual simultaneous localization and mapping for autonomous unmanned aerial vehicles
本方法与ORB-SLAM2相同之处在于： Use the same features for odometry, relocalization, and loop-closing 使用相同的特征点进行里程计、重定位和回环。
动机： The core idea of the proposed approach was to develop a highly accurate localization module that runs in real and constant time on a small scale UAV and still leaves enough processing power for control and navigation algorithms. 高效、准确的视觉定位方法。
不同之处亦即贡献为：

1. Instead of the computationally more intensive bundle adjustment for local localization, i.e., local SLAM, we use SOFT visual odometry, which yields ≈0.8% position error with respect to the traversed path and execution frequency of the proposed method of 20 Hz。不使用局部BA，直接使用计算效率和效果较好的SOFT里程计。
2. The proposed SLAM framework, which supports large loop closings, completely decouples the odometry and mapping thread,yielding a constant runtime odometry with global consistency, i.e.,
   an odometry that does not have to wait at any point for the mapping thread. Furthermore, unlike ORB-SLAM2, which due to the nondeterministic nature of the multithreaded implementation produces different results for the same data, SOFT-SLAM results are deterministic, i.e., it always yields equal output for the same data.多线程完全解耦，任何一个线程无需等待另一个线程完成，在同一数据集上的每次运行都会得到确定性相同的结果。
3. We use features from SOFT for loop closing that, as in the case of ORB-SLAM2, makes the system highly efficient, simple, and reliable on a subpixel accuracy. Even though SOFT features are not invariant to rotation and scale, we demonstrate on relevant public datasets that loop closing occurs frequently enough to achieve state-of-the-art results. 使用SOFT特征点，使得系统高效、简单和可靠，且有亚像素精度。

系统介绍

从流程来看，相较于ORB-SLAM，该方法抛弃了局部优化建图模块(Local BA),也就是说里程计部分相当于ORB-SLAM的跟踪+局部优化模块。
ODOMETRY THREAD 里程计线程
1，特征点处理

1. 特征点提取：Utilizing blob and corner masks on the gradient image.
2. 特征点匹配： Correspondences are determined through calculating the sum of absolute differences over a pattern of pixels around the detected maxima.And filtering the feature set by applying circular matching.
3. 描述子维护： Keeping the initial value of the descriptor throughout the whole lifetime, having a
   more reliable feature set. 特征点的描述子只用初始时刻的值.
4. 特征点位置refine： Propagate the refined position on a subpixel level while establishing matches between current and previous occurrences in the vein of aMarkov process.
5. 特征点选择：Propose a selection policy that provides both a spatial and temporal variety of strong features across the image. Number of far and near features is well balanced, and that the features are uniformly distributed over the entire image.Allow new strong features to enter the selection, preventing the veterans from dominating the odometry. The odometry is based on a heterogeneous set of features.
   里程计的优异性能来源于所提出的启发式特征点选择策略。

2，无IMU辅助下的位姿估计

先利用五点法估计两帧之间的旋转矩阵，并利用与前几帧估计的旋转矩阵平滑融合得到最终结果，然后利用一点法和全优化估计平移，最后通过积分得到最终的位姿。

3，有IMU辅助下的位姿估计

利用IMU的陀螺仪取代纯视觉流程中的五点法进行旋转矩阵的估计，并用EKF估计陀螺仪的偏差。
MAPPING THREAD 建图线程
建图线程类似ORB-SLAM中的全局BA线程，该线程进行关键帧选择、回环检测和全局优化。
Keyframes management and Loop closing

采用运动距离和角度阈值来判断关键帧，也用距离来获取回环关键帧(这种方法相对ORB-SLAM2非常简单，依赖于非常准确的里程计)。
回环检测：

1. Determine correspondences between the candidate keyframe and the new keyframe using circular matching and NCC. 获取新关键帧和候选回环关键帧之间的关联；
2. Perform three-point RANSAC for determining the best suitable hypothesis transformation and accordingly apply the outlier rejection. 使用3点法获取最合适的变换假设；
3. If the remaining number of inliers is larger than the threshold, we apply the full refinement optimization by which we obtain the final linking transformation. 计算最终的变换联系；
4. Additionally, we check if the final linking distance is too large, and eventually stop with the loop closing. 检查连接距离是否太大。