分三步，主要都包含在processFrame()里。

1. sparse image align

先用上一帧的位姿初始化本帧位姿，这是没有先验的情况。

运用了SparseImgAlign类，继承自vk::NLLSSolver<6, SE3>模板类，专门用来求解SE2，SE3的优化问题。由于优化变量仅一个transform，所以比较简单，直接用LDLT分解求Hessian矩阵和增量。

误差项是上一帧的特征点（当中有3D对应点的）向下一帧投影，只涉及上一帧和当前帧

[INFO] Img Align: Tracked = 469

SVO运用了逆构法（inverse compositional approach），用来近似计算当前帧位姿优化的更新量。方法是固定3D点和当前帧不动（对，是当前帧），将增量扰动施加在参考帧上，求解增量方程，最后在更新的时候把得到的增量求逆，乘以当前帧。也就是说最终变化的还是当前帧，但是求解过程中用参考帧的变化代替。
这种方法的有点是效率高。普通的求法由于每次更新3D点在当前帧上的投影都会变化，因此Jacobian矩阵都会变化（变化来源有二：一是当前帧投影点附近的梯度；二是3D点在当前帧的坐标，二者均是最终求导后余留的量）。这使得每次迭代都要计算一次。
而逆构近似法由于增量施加在参考帧上，而参考帧在更新的时候实际是不动的，因此Jacobian矩阵也不会变。
这种近似的假设是：1）参考帧特征点的梯度和当前帧投影点整个局部区域的梯度相差都不大，或趋势相同。因为每次当前帧更新后投影点会改变的，但我们仍然用参考帧的梯度近似计算。

2. map reprojection & feature alignment

调用reprojector_.reprojectMap(new_frame_, overlap_kfs_)。

void Reprojector::reprojectMap(FramePtr frame, std::vector >& overlap_kfs)：

先搞清楚几个概念：Candidate，点，包括3D和像素坐标；
                                Cell，一系列Candidate，即一系列点。可以理解为一个Cell里有一系列点；
                                CandidateGrid，一系列Cell，网格是由一系列Cell组成的；
                                Grid，就是CandidateGrid，多了一些其它field。
Grid在reprojector_初始化的时候就划分好了，并且被编了号，只不过是乱序编的。
    回到函数：
a）投影共视帧的点
先清理Grid，接着调用map_.getCloseKeyframes(frame, close_kfs)，此函数根据之前算的每个keyframe的keypoint是否能被本帧看到来判断有没有共视的点，若有一个则表示这两帧有共同视野，记录在close_kfs里，由与当前帧共视的帧和translation组成的pair。
根据translation的由近到远给共视帧排序，但只保留前10个。
遍历共视帧，再遍历其上每一个特征点，将其投影到当前帧（合法的），投影到的Cell会增加一个Candidate。投影的共视帧被记录在overlap_kfs_里，是共视帧和其合法投影数组成的pair。
b）投影map_里candidate的点
注意，这里的candidate是map_的candidate_，指已收敛的深度点。
遍历map_的candidate点，投影的当前帧，过程和之前一样，投影到的Cell会增加一个Candidate。如果失败了，要累计该点的失败次数，过多则删除。
最后开始整理Cell，按照之前排的乱序，因为不会每个都整理，乱序确保平均、随机，最终旨在在每一个Cell中至多选择一个最好的重投影点：
    bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)：
    先按点的quality排序；
    删除被标记为删除的点；
    调用matcher_.findMatchDirect(*it->pt, *frame, it->px)，该函数运用subpixel refinement来寻找匹配，只能找到2-3pixel内的正确匹配；
    如果成功便增加成功计数，失败则增加失败计数，根据点pt的当前类型决定进一步命令。
    成功投影的点会在该帧添加一个对应的feature，然后就返回成功了。也就是说该Cell里剩下的都不看了，因为我们已经按quality排序了
    都不成功则返回失败。
最终一个成功的reprojectCell()使得记录有多少个Cell已经找到feature的变量n_matches_ +1，当其超过一个设定好的数，循环停止。也就是说不必所有的Cell都提取特征，这也是我们之前乱序的原因。

[INFO] Reprojection: nPoints = 122 nMatches = 121
重投影尝试了多少个点，最终匹配了多少个点。

如果匹配的点过少，这一真的process就失败了，返回RESULT_FAILURE

3. pose optimization

Motion-only bundle adjustment. Minimize the reprojection error of a single frame.

仅调用pose_optimizer::optimizeGaussNewton(...，new_frame_, ...)：
计算当前帧所有feature的位置与它们对应的3D点的投影之间的误差（考虑了level）；
找到误差的中位数，作为estimated_scale；
接下来开始iteration：
    遍历所有feature，累加其jacobian，Hessian。其中用到了Tukey Weight；
    求解增量，还是使用分解法，因为只有六维；
    判断是否收敛、更新、判断可否停止。
再次投影3D点，移除误差过大的。
【其他的chi2_vec_final之类的东西应该只是用来显示和debug的】

[INFO] PoseOptimizer: ErrInit = 0.182047px thresh = 0.269429
中位数投影误差平方，中位数投影误差（都乘了写系数）
[INFO] PoseOptimizer: ErrFin. = 0.192953px nObsFin. = 121
中位数投影误差平方，保留下来的点个数

4. structure optimization

Optimize some of the observed 3D points.

很简单，仅调用optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter())：
首先将该帧里所有有对应点的feature的3D点找出来，按上次structure optimization所在的帧的id，last_structure_optim_（等价于上次优化的时间）排序。因为该field在初始化的时候是0，所以没经历过优化的点都被排在后面，我们取前20个点；
然后调用点的优化函数，优化点的三维坐标，减小重投影误差（一点，一帧之间足矣）；
然后更新last_structure_optim_。

自此，SVO的三步优化就全部完成了。我们看到SVO之所以很快是因为它没有运行bundle adjustment。在前端，它是一个变量一个变量优化：先第一遍优化pose，变量只有pose；然后优化feature的投影位置，是一个特征一个特征依次优化；再来是第二遍pose；最后优化了该帧feature对应的3D点，但这居然也不是一起优化，而是一个点一个点地优化。其实和一般间接法类比，后两个过程是可以用一个BA的，考虑效率问题，SVO选择了分开优化。这可能也是效果稍差的原因。

5. select keyframe

之前pose optimization的时候保留下来的点如果较少或drop的点稍多都会使这次tracking被标记为TRACKING_INSUFFICIENT，从而放弃此次追踪！！！！

确认保留，则根据needNewKf(depth_mean)，判断和所有视野重叠的帧的translation在x,y,z各方向是否都超过一定值，都超过则要设立关键帧。

6. new keyframe selected

当被选为关键帧后，还需要把其上特征对应的点添加指向该帧的观察obs_。然后遍历map_的point_candidate_，其中刚完成刚才添加该帧为obs_的可以从candidate毕业了，成为三维世界的“自由点”，不受帧的约束。其type_为TYPE_UNKNOWN，n_failed_reproj_ =0。

回顾点的一生：首先在某个关键帧红被初始化为一个Seed，随着非关键帧被不断更新，直至收敛，升级为点，加入map_.candidate_；随后在tracking过程中被不断重投影到当前帧，记录成功和失败数，其间很有可能被删除candidate；终于当再次投影到某一关键帧成功后，即可脱离帧的束缚，升级为地图点，而地图点可以重置一次投影失败记录；而后仍然要不断被共视帧投影，也可能被删除。

（7. optional bundle adjustment）

找到重叠视野最多的3帧放入core_kfs_；

调用bundle_adjustment（g2o）求解。

8. 后续

深度滤波器添关键帧（见之前文章）；

map_里的关键帧数量是有限的，当超过的时候就删掉最远的一帧，深度滤波器里也要删除，当然它所对应的点如果obs_只有2也要删除；

添加当前帧进入map_的关键帧。

Attachment

bool Matcher::findMatchDirect(const Point& pt, const Frame& cur_frame, Vector2d& px_cur)：
对于一个点pt，找到其对应的feature中，与当前帧视角最小的，且必须小于60°，否则就算没找到；
再排除非法投影，主要是是否满足boundary；
    调用warp::getWarpMatrixAffine(...)。此函数计算patch从参考帧投影到当前帧的仿射变换，因为要计算patch的相似度。

warp::getWarpMatrixAffine  的原理

严格的仿射变换矩阵不好求，但我们注意到patch在原图中都是四方且与图像边平行的。所以只要知道两个边hp在warping后的向量就可以了（参考这里）。先计算px，du，dv在当前帧的投影（深度都是px的），然后计算向量hu，hv。hp的长度为halfpatch和scale的积，因为特征虽然是第level层提的，但坐标是第0层的，且往world的投影用到的都是第0层的像素点，第0层是往外的一个接口。这个halfpatch大小是随便取的，与具体匹配的patch无关，最终仿射矩阵要把它给除掉。由于du'，dv'都是第0层的，除以halfpatch后其大小只与scale有关。我们要根据这个scale在当前帧上找到合适的匹配level。那么为什么不level对level，在参考帧上是第几次，当前帧就是第几层呢？因为仿射变换后可能变化过大，尺度不一样了，此时要找到尺度相近的层search_level_，其patch也差不多，才好做匹配。因此：
    调用int getBestSearchLevel(const Matrix2d& A_cur_ref, const int max_level)。其壮如下：

getBestSearchLevel

这两个函数是配合使用的。D是仿射变化后四边形的面积，D要不大于3。考虑一层的尺度相差2*2=4，加上扭曲之后面积变小，故设3。
    接着是warp::warpAffine(...)，实施仿射变换:
注意我们之前说仿射变换矩阵大小只与scale有关，在此先求逆，SVO是把当前帧的点投影到参考帧的点，然后再对比；

求patch warping的过程

这里比较绕，思路是将search_level_上的patch的25个坐标点（带有一个像素的boarder）先放大到第0层，然后用逆仿射矩阵处理，得到仿射后的patch坐标。然后加上level_ref的中心像素坐标。我们可能会绝对放大到第0层后再仿射，得到的也是参考帧上的第0层仿射patch。实际上是错的，因为之前的正仿射矩阵大小与level_ref的scale有关，求逆的时候就与1/scale相关了，此时乘以当前帧第0层上的点，得到的就是缩小了scale的点，即level_ref上的patch坐标，因此可以和level_ref的中心像素坐标直接相加。
最后判断是否合法，合法便计算reference上patch的像素插值，存储在patch_with_border_
    调用createPatchFromPatchWithBorder()，把boarder内部的patch提出来，放入patch_【不知道为什么要多这一步】
    调用feature_alignment::align2D(...)实行特征点位置优化。这是一个自己实现的像素点优化函数，有多个版本，如SSE【可以学学】。之前计算patch_with_border_是为了在这一步能够计算patch_边缘的梯度。该函数还考虑了patch的平均误差，将其减去，避免明暗变换的影响。它采用3x3的Hessian矩阵的最后一维就是干这个用的。
    如果优化成功，最后便得到了优化后的特征点像素位置，返回成功；否则就不变，返回失败。