V-SLAM重读(3):SVO代码阅读和调试修改

在本文内容开始之前，看一段调试之后的SVO测试gif（gif图刚开始学会做，大家将就着看一下～～）

个人经验(仅供参考)： 在阅读一个大型库源代码的时候，遵循以下步骤:

(1). 第一步看CMakeLists.txt， 在这个文件中会看到这个系统需要哪些依赖(比如 FIND_PACKAGE())， 以及哪些是可执行文件，哪些是库文件.

(2). 第二步看CMakeModules文件夹， 是否包含依赖库的***.cmake的查找文件，或者若是没有，确认该文件是否在默认cmake 路径可以查找得到.

(3). 第三步编译调试成功系统，然后从执行文件递归阅读源代码(也可借助代码阅读器如understand查看代码结构图)，调整参数。

1. SVO代码结构

src/ ├── bundle_adjustment.cpp // BA优化(图优化) ├── camera_model.cpp // 相机模型 ├── config.cpp // 配置参数 ├── debug.cpp // 调试参数 ├── depth_filter.cpp // 深度滤波器 ├── fast_10.cpp // 这5个文件是Fast角点检测和剔除 ├── fast_10_score.cpp // ├── fast_dect.cpp // ├── faster_corner_10_sse.cpp // ├── fast_nonmax_3x3.cpp // 这5个文件是Fast角点检测和剔除 ├── feature_alignment.cpp // 特征点校准 ├── feature_detection.cpp // 特征点检测 ├── five-point.cpp // 五点法 ├── frame.cpp // 帧 ├── frame_handler_base.cpp // 帧-跟踪处理的基础函数 ├── `frame_handler_mono.cpp // 帧-跟踪处理的核心函数` ├── homography.cpp // 单应矩阵、特征点光流跟踪. ├── initialization.cpp // 相机初始化， 添加第一、第二帧 ├── map.cpp // 地图点迭代 ├── matcher.cpp // 匹配 ├── math_utils.cpp ├── modelest.cpp ├── point.cpp // 点-空间坐标位姿等 ├── pose_optimizer.cpp // 位姿优化 ├── precomp.cpp ├── reprojector.cpp // 重投影 ├── robust_cost.cpp // 优化代价函数 ├── slamviewer.cpp // 系统可视化 ├── sparse_align.cpp // 稀疏校准 └── sparse_img_align.cpp // 稀疏图像校准(忽略， 未使用)

2. 分解核心函数

由于这个SVO解析讲解的人很多，一些基础部分(初始化，特征点检测， 匹配等)在这里就不讨论了， 直接讨论核心函数, 即为(frame_handler_mono.cpp的processFrame()函数)

test_live_vo.cpp: addImage() --> frame_handle_mono.cpp: processFrame().

processFrame()处理函数:

1. 稀疏校准(processFrame() --> sparse_align.cpp)

在稀疏校准中，传入当前帧和参考帧数据，获取并优化两帧之间的位姿变换T；

1.1 启动 SparseAlign::run()函数， 核心进入optimize(T) 优化变换矩阵； 1.2 核心进入computeResiduals()，计算残差( precomputeReferencePatches() )； 1.3 计算完成获取优化后的位姿， curr_frame->T = T * ref_frame->T; 1.4 反回校准之后还剩下追踪的点数量：img_align_n_tracked = img_align.run().

2. 地图点重投影，特征校准(processFrame() --> reprojector.cpp)

2.1 启动ReprojectMap(), 获取map数据的最临近关键帧getCloseKeyFrames(); 2.2getCloseKeyFrames(), 查找所有关键帧中的关键点是否在当前帧能够观测到(frame->isVisble(poins->pos))(世界坐标位置), 若是则将关键帧插入临近帧中； 2.3isVisible(): 将点的坐标转换到图像坐标，若z<0.0，则剔除； x,y坐标要在图像的宽高范围内; 2.4 遍历所有最邻近关键帧数据（也就是视野重叠的关键帧），然后重投影N(N=10)个关键帧上的点（地图点），reprojectPoint(); 2.5 reprojectPoint(): 将点坐标转换到像素坐标下，若坐标在图像范围之内， 则该重投影点作为候选点插入到网格细胞重排列之后的位置 grid_.cells.at(k)->push_back(Candidate(point, px));

k = const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols + static_cast<int>(px[0]/grid_.cell_size);

2.6 --> reprojectCell(), 对候选点再进行一次删减之后，对网格中的特征进行重投影，并更新匹配点的数量.

3. 位姿优化(processFrame() --> pose_optimizer::optimizeGaussNewton())

3.1 包含（pose_optimizer.cpp --> robust_cost.cpp）的一系列优化函数计算。

4. 场景恢复(processFrame() --> optimizeStructure())

记录一个STL的库函数（阅读一个大型库发现很多的默认函数，如for_each(), random_shuffle()等）， nth_element(first，k，end)：获取第k小的元素放在第k个位置(复杂度为O(1)). 对其他元素并没有排序. nth_element(begin, nth, end, compare); 第四个参数决定数据是升序还是降序给出；

4.1frame_handler_base.cpp: optimizeStructure() , --> point.cpp: optimize(), 优化迭代3D点位置。

5. 深度滤波器

std::numeric_limits: 对于给定的基础类型用来判断在当前系统上的最大值、最小值；头文件为"limits" std::numeric_limits::max(); 取当前系统double类型的最大值.

5.1 获取场景的深度信息：getSceneDepth(),给出帧的数据，然后将点转换到相机坐标平面，获取z坐标(即为深度)， 然后获取当前帧所有点的深度的平均值，认为是当前图像的深度(?) 5.2 然后根据深度值来进行判断是否需要创建新的关键帧， 或添加新的关键帧到地图中， 以及在地图中添加新的候选点(帧)， 更新深度滤波器； 5.3 选择一定数量的关键帧，并维持这个数量，可以保持VO一直快速进行； 因此需要对关键帧的数据进行删除，在这里的删除标准是去除最远的关键帧(map_.getFurthestKeyframe()), 这种方法并不可靠， 可行的方法是使用上一节博客DSO所涉及到的边缘化去除 。

关于优化部分比较复杂， 后续还得慢慢研究。这里只是大概介绍程序的运行流程