初始化 | SVO 论文与代码分析分讲

论文： SVO: Fast Semi-Direct Monocular Visual Odometry
代码（注释版）：https://github.com/cggos/svo_cg

SVO 结合了直接法和特征点法，称为 半直接单目视觉里程计。

初始化
获取第一关键帧和第二关键帧的相对位姿，并建立初始地图，代码主要在 initialization.cpp 中。
1）处理第一帧FrameHandlerMono::processFirstFrame

第一帧位姿为单位阵 new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero()) addFirstFrame
detectFeatures: 获取 Shi-Tomas得分较高 且 均匀分布 的 FAST 角点，并创建 Features
初始化 px_vec(Coordinates in pixels on pyramid level 0) 和 f_vec(Unit-bearing vector of the feature)
将当前帧设置为关键帧，并检测 5 个对应的关键点
添加关键帧到地图 map_.addKeyframe(new_frame_);

2）处理第二帧 FrameHandlerMono::processSecondFrame

addSecondFrame
光流跟踪trackKlt
计算 单应性矩阵（假设局部平面场景），估计相对位姿T_cur_from_ref_
创建初始地图
BA 优化 ba::twoViewBA
添加关键帧到地图 map_.addKeyframe(new_frame_);
添加关键帧到深度滤波器 depth_filter_->addKeyframe