DSO初始化流程
嗨，各位读者朋友们好！今天我们接着讲DSO全家桶中的第三讲——初始化。
这一讲的内容相对来说会简单一些，因为笔者会将初始化阶段的运动估计移到下一讲：DSO全家桶（四）——前端：前端跟踪中去讲，因为本质上这两个跟踪的方法比较接近，就没有必要赘述了。
在第二讲中，我们已经介绍了DSO提取梯度点的策略，那么这一讲就接着后续的操作，讲讲这些梯度点会怎么用。对于单目SLAM来说，我们通常至少需要两个帧才可以完成初始化，DSO也不例外。
那么，我们先看看DSO初始化阶段的主要流程：
图1. DSO初始化流程
1. 对第一帧的图像金字塔提取梯度点，并将每个点的逆深度值初始化为1，并设置每个点的外点阈值；
2. 对步骤1中的每一个梯度点构建最近邻（makeNN）索引，在同一层中找到10个与其距离最接近的点构建邻域关系。通过在下一层的NN索引中，找到与当前点距离最小的点，设置为parent点，目的是关联当前层与下一层，便于光流金字塔和逆深度值的传播；
3. 输入第二帧图像，将第一帧初始化的梯度点投影到当前帧中构建残差，并采用光流金字塔对初始相对运动进行估计，这部分内容我们会在下一讲详细介绍；
4. 将第一帧设置为关键帧，加入滑动窗口中；
5. 统计第一帧所有点的逆深度值，并计算归一化尺度因子，用于将相对位移归一化；
6. 遍历所有点，对所有点创建未成熟点，根据阈值条件判断可否进行激活；
7. 为所有可激活的点设置逆深度值，值得注意的是，DSO设置了两个逆深度值，一个是有尺度因子的，一个是没有尺度的；
8. 设置两帧各自的初始位姿；
9. 将第二帧加入滑动窗口中进行联合优化；
10. 联合优化后会根据滑动窗口内关键帧数量设置不同的阈值条件，通过判断均方根误差（RMSE）与阈值的大小，确定是否初始化成功。阈值设置规律为：滑窗内的关键帧越多，阈值越小。若连续4个关键帧的联合均方根误差低于阈值条件，则初始化成功！
KD-Tree + FLANN（Fast Library for Approximate Nearest Neighbors）
笔者仔细研究了一下上述10个步骤，由于我们计划将相对姿态初值的估计移到下一讲介绍，导致这一讲内容比较单薄。笔者一时之间不知道该详细介绍哪个。认真研究了一波，感觉这个近似最近邻好像有点可以讲，笔者就现学现卖，给大家整理一下这个玩意儿是怎么操作的。参考的资料都附引用，以表示对原作者的尊重。
OK，我们开始吧。
KD-Tree
KD-Tree又称 K 维树是计算机科学中使用的一种数据结构，用来组织表示 K 维空间中点集合，它是一种带有其他约束条件的二分查找树。
实际上，我们知道二叉搜索树有这样的特性：
对于根结点，其左子树上所有结点的值均小于根结点的值，其右子树上所有结点的值均大于根结点的值。
图2. 二叉搜索树（引用自百度百科）
那么，在KD-Tree中，也是保有这样的特性。对于一组无序的输入，假设我们常用到的激光雷达的点、或者是RGB-D图像得到的点、或者是我们DSO中估计得到的三维点，都是一系列无序的三维点。对于每一个输入点，为了能够快速地搜索到与其相邻的点（假设是特征匹配，或者叫最近邻匹配），我们需要对现有的数据进行重组，建立离散点间的拓扑关系，使其能够快速查找。这件事情与BOW构建词袋模型类似，笔者在介绍OBR-SLAM2中的回环检测时有提到过，不过当时讨论的是具备描述子的特征，感兴趣的读者可以看看。
首先确定一下，我们的输入是无序三维点，通常我们只在三个维度中进行处理，因此所有的KD-Tree都将是三维KD-Tree。
由二叉搜索树的思路我们很容易想到，每次指定某个坐标轴作为分割线，假设第一次选择X轴，理想情况下，处于中位数的结点应该被设置为根结点，随后X值小于根结点的点全部被挂在左子树，反之则挂在右子树上。由图3所示，可以非常直观的看到具体的操作。
图3. KD-Tree（引用自参考资料1）
于是，这里就有两个问题需要解决：
1. 怎么样确定理想的中位数？
2. 直接针对原始数据进行处理会导致频繁调用拷贝构造函数（swap），那么怎么避免计算中位数时出现的这个问题？
在这里，我们就借鉴快速排序的思路：
第一个问题，其实我们并不需要对整个数据进行排序，因此只需要找到横向或者纵向的中位数即可，采用 nth_element(...) 就可以非常容易得找到目标值；
第二个问题，参考FLANN中的方案，KD-Tree构建时，会同时引入一个与点云同等大小的index索引数组。在划分中位数的时候，改变的只是index，不需要拷贝复制任何点。
于是，通过上述的方案，我们就可以将无序三维点（点云）构建成一个KD-Tree了，构建KD-Tree的方法可以参考浅谈kdtree。
FLANN
图4. 最近邻搜索（引用自参考资料3）
这里我们需要依次介绍一下1-NN、K-NN和A-NN。
1-NN：在KD-Tree的基础上，给定一个查询点，需要在树中找到与其最近似的点。
在数据规模较小的情况下，我们可能会考虑采用暴力法一顿撸，这是有效的。但是数据规模非常大时，暴力法的低效是令人发指的。那么有了KD-Tree，要咋利用呢？
首先，我们可以知道，数据已经呈现某个规律排布了。那么我们就将从根结点开始计算查询点与根结点的距离作为初始的最小值，随后依次计算左子树和右子树结点与查询点的距离，若其中一个距离小于根结点，则更新最小距离。根据左右子树结点的大小，确定是先遍历左子树还是右子树。后续就是不断递归，找到最小的空间距离即可。
到这里，大家肯定会问，那这样和暴力法有啥区别？emmmm....区别在于这里有剪枝的功能，假设我们计算得到的左子树结点与搜索点的距离比根结点与搜索点的距离还大，那么左子树就整根被砍掉了，所以很显然比暴力法要高效多了。
值得注意的是，1-NN并不是在左子树找到了最近邻点就不遍历右子树了，而是左子树结束后再遍历右子树，直到确定当前筛选到的点确实是最近邻点为止。如图4所示，即在左子树中可以找到中间最底下的点，但是实际上右子树上有一个离搜索点更近的点，利用1-NN可以准确无误的找到它。
K-NN：思路与1-NN类似，不同的地方在于此处需要维护一个K近邻列表。
A-NN：在介绍A-NN之前，我们可以参考一下近似最近邻搜索ANN(Approximate Nearest Neighbor)中的最后举例的Kd-Tree的最近邻查找，在终止条件上，通常是利用搜索点和最近点的距离为半径构建一个圆，若上一层的结点与该圆相交或者是在该圆内才更新最近邻点信息，否则进行裁剪。而ANN的判断方式与该方法有一点点区别，那便是将上述的半径除去某个大于1的数，使得搜索半径变得更小，过滤掉了更多的点。emmm....据参考文献所述，这样的操作可以使得查询速度提高10-100倍。
DSO中的用法：
在这里，笔者也简单提一下，KD-Tree和FLANN在DSO中的用法，主要是在makeNN()这个函数中：
1. 对金字塔图像所有层提取到的的点，逐层构建树并保存对应的索引信息；
2. 遍历金字塔图像每一层：
2.1 对金字塔图像每一层的每个点查找10个邻域点；
2.2 统计每个邻域点与当前点的距离；
2.3 将当前点进行缩放，即横纵坐标*0.5，退到下一层金字塔图像中，搜索最近邻点，并构建父子链接，目的是用于后续在跟踪时的深度传播。
总结
到了总结的阶段了，这一讲我们主要讲的是DSO中的初始化模块，实际上最重要的是两帧的跟踪，完成初始地图和位姿的估计，但跟踪模块跟我们大纲的第四讲有重合，于是我们就留到下一次再讲。这一讲的内容主要是讲构建KD-Tree和FLANN，并关联前后金字塔图像的点。
由于笔者也是现学现卖，有讲的不对的地方，烦请各位读者指正。本讲给出的参考资料对于本讲的内容有非常大的丰富，感兴趣的可以阅读参考资料。
参考文献
[1] PCL 点云索引方法K维树（KD-tree）和八叉树（octree）介绍
[2] 浅谈kdtree
[5] 近似最近邻搜索ANN(Approximate Nearest Neighbor)
版权声明
如需转载，请联系本人邮箱peichu.ye at mail2.gdut.edu.cn。未经允许，不可转载。