SVO源码阅读笔记——motion estimation概述

SVO属于半直接稀疏法，说它是半直接是因为其只在前端线程的第一步，利用前后帧图像做初始位姿估计时用到了直接法，后面的一些步骤还都是特征点法的常见操作。以下是SVO运动估计的整体概述。

1. Sparse model-based image alignment

利用直接法，恢复前后连续帧之间的运动，作为初始估计。如果我们正在做一个VI-SLAM system，应该可以用IMU预积分代替此初始值。

2. Feature alignment

在1的基础上我们找出当前帧看到的map中存储的特征，即特征匹配。通过Reprojector::reprojectMap()实现：
(1) resetGrid()
(2) 在map中找出和当前frame有共视关系的所有keyframe，计算他们和当前frame的相对位置，并把这些信息都存到close_kfs中。–>Map::getCloseKeyframes()
(3) 在close_kfs中找出options_.max_n_kfs个和当前frame共视程度最高的keyframes，这里的方法是直接比较上面计算出的相对位置(translation)，越近共视程度越高。
(4) 遍历这些临近关键帧，将每一帧的features对应的points都投到当前帧。–>reprojectPoint()
(5) 地图中的point_candidates_是已经收敛但尚未插入到keyframe的点，把这些点也投影到当前frame中。
(6) 步骤(4)和(5)所投影过来的点，目前都是存在Reprojector::grid_中的，通过Reprojector::reprojectCell()遍历所有cell，在当前frame里最终找到以上点对应的匹配：

1. 对每一个cell中所有point按质量由高到低进行排序，具体是比较point此时的type_，TYPE_GOOD> TYPE_UNKOWN> TYPE_CANDIDATE> TYPE_DELETED。
2. 从质量最高的点开始，依次与当前frame进行匹配，这里通过函数
   Matcher::findMatchDirect(const Point& pt, const Frame& cur_frame, Vector2d& px_cur)
   实现，是整个feature alignment中最重要的环节：
   i. Point存了keyframes对它的观测信息，从这些keyframe中找出和当前frame夹角最小并小于60°的那一帧作为参考关键帧。–>getCloseViewObs() 同时确保该点不能太靠近边缘，这样才能取到图像块做匹配。
   ii. 计算仿射变换矩阵A_cur_ref，该矩阵是feature在参考帧ref_ftr->level层(表示最开始这一帧在该层获取到的特征)到当前帧frame第0层的变换。–>getWarpMatrixAffine()
   iii. 获取当前frame做匹配的最佳层数search_level_ –>warp::getBestSearchLevel()
   iv. 仿射变换warp::warpAffine()：SVO做块匹配时在当前frame选取正方形块，利用仿射矩阵投影到参考帧，得到在参考帧中以feature为中心的一个平行四边形块，因为后续要计算梯度，所以我们计算出一个带边界的图像块。进一步插值得到像素值。
   v. 在上一步骤的基础上，得到不带边界的图像块。
   vi. 将初始值(px_cur)转到search_level_层，然后做feature align。(Align1D for edgelet feafture。)
   vii. 将align过的px_cur转回0层，返回。

到此我们便完成了当前帧与map的特征匹配。做完这一步，系统要决定该帧是否是一个关键帧，如果是，depth filter去初始化一些新的seeds；若不是，depth filter利用该帧的观测去更新还没有收敛的seeds。

3. Pose & structure refinement

BA优化，进一步优化精度。