SVO代码（一）从头到尾

阅读SVO代码过程中主要参考的下面两篇博客:
svo： semi-direct visual odometry 论文解析
SVO详细解读

基于帧间4x4的图像块的灰度不变形来优化相机位姿，这与直接法很像，直接法使用的是一个像素点，接着使用8x8的光流进一步优化关键点的位置，经过这一步特征点的位置就很精确了，最后通过构造重投影误差优化相机的位姿和路标点的深度，这与特征点法很像，SVO结合了直接法和特征点法，因此，称它为半直接法

---

生成新feature的地方只有三个地方：

• processSecondFrame()->addSecondFrame()
• processFrame()->reprojectMap()->reprojectCell()
• initializeSeeds()->detect()

---

1、初始化

1、processFirstFrame()不向frame中添加feature，只负责将检测到的feature存到px_ref_（feature的像素坐标）、f_ref_（feature对应的单位向量）中

feature的属性：

FeatureType type; //feature的类型：corner or edgelet
Frame* frame;     //feature所在的frame
Vector2d px;      //在0层的像素坐标系上坐标
Vector3d f;       //在单位圆上的向量，并非归一化平面坐标
int level;        //feature在哪一个层检测到的
Point* point;     //这个feature对应的MapPoint坐标
Vector2d grad;    //edgelet的梯度，归一化后的

addFirstFrame()中将px_ref_转存到px_cur_的目的是：为addSecondFrame()中的trackKlt()初始化一个好的值

setKeyframe()是将普通帧设置成关键帧，这里跟orb-slam中不一样，并没有新生成一个keyframe对象，而是通过设置关键帧标志位is_keyframe_来实现，此外，也为该关键帧选取5个有代表性keypoints，在后面用于processFrame()->reprojectMap()->getCloseKeyframes()中得到有共视关系的关键帧

2、在processSecondFrame中使用multi-scale Lucas-Kanade算法计算稀疏光流，进行跟踪

• 如果跟踪成功的角点数超过50个并且disparity的中值大于50个像素长度，则计算单应矩阵，三角化生成新的MapPoints
• 调整scale，orb-slam中也有这步，在这修改以使用里程计恢复初始尺度
• 然后就可以向frame_cur和frame_ref中添加feature(都是位于0层，有些feature在其它层提取，也不加考虑了？？），同时向point添加能观测到这个point的feature

使用twoViewBA()优化第二个关键帧的位姿(第一个关键帧固定)和MapPoints的位置，这里使用的是重投影误差
将new_frame_（第二个关键帧）加入到深度滤波器

---

2、运动估计

1、Sparse Model-based Image Alignment
图像的稀疏对齐操作的是两个普通的帧: last_frame_和 new_frame_，从max_level_=4到max_level_=2中的每一层进行遍历优化T_cur_from_ref，下面是优化的过程：

• 预计算雅可比矩阵（使用 inverse compositional，故只需计算一次），precomputeReferencePatches()遍历参考帧（last_frame_）中的每一个feature，即使这个feature不是在这一层提取的也这么遍历（这样没问题吗！？）
• 计算残差，computeResiduals()根据位姿变换将last_frame_上的feature（普通帧的feature哪里来的，看下面），投影到当前帧像素平面上，计算亚像素值用于计算残差，这里使用的是最小化“像素块”灰度值差值

2、Feature Alignment
上一步已经调整过T_cur_from_ref，那么就可以进行feature对齐了，为了准确（因为关键帧中的point都是比较准确的），使用的<当前帧>和<与当前有共视关系的关键帧>，调整的是当前帧feature（初始位置：由共视关键帧对应的point投影得到）的位置
1）使用创建关键帧时生成的5个keyPoints投影进行寻找，当前帧与Map中存储的关键帧有共视的关键帧，取前10个共视最好的关键帧
2）遍历每一个栅格(grid_size=30）中每一个地图点，grid_.cells中的每一个cell包括两个成员变量：point（由关键帧观测到）、point在当前帧上的投影，注意这里不含feature，因为这一步只是寻找point而已，feature要在下一步通过getCloseViewObs()确定
3）针对每一个地图点，通过优化寻找直接法匹配，即使用findMatchDirect()

• 使用getCloseViewObs()，选出夹角最小的那个关键帧作为参考关键帧，以及对应的feature: ref_ftr_
• 计算从参考关键帧到当前帧的仿射变换，为什么？这是因为如果帧间图像发生了旋转，我们还用同样的窗口，将出错，这等价于给fast（其实是图像块）加上旋转不变性！！
• 确定search_level_，为什么？这是因为当前帧很大可能与参考关键帧的距离比较远，所以feature的大小很有可能发生变化，这就等价于给fast角点（其实是图像块）加上尺度不变性？！

4）仿射变换矩阵是A_ref_cur，表示从当前帧的<0层>转换到参考关键帧的<0层>的仿射变换，warpAffine()用于将在当前帧的search_level_层上取到的10x10的图像块，投影到feature<提取层>对应的参考关键帧ref_ftr_->frame->img_pyr_[ref_ftr_->level]上，然后就可以使用align2D()或者align1D（edgelet特征）进行优化，对齐feature，使用的同样是 inverse compositional，可能是因为太好用了吧.
5）根据上一步的结果筛选point
6）如果findMatchDirect()匹配成功了，则向当前帧添加feature，通过下面代码生成新的feature，使用的层是search_level_，search_level_是针对每一个point在findMatchDirect()确定

Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);

3、Pose and Structure Refinement
1）optimizeGaussNewton()使用重投影误差优化当前帧的位姿frame->T_f_w_，观测是上一步生成featrue在当前帧归一化平面上的坐标，顶点当前帧的位姿（没有structrue）.
使用的redescending M-estimators鲁棒核函数，好处如下：

The redescending M-estimators are slightly more efficient than the Huber estimator for several symmetric, wider tailed distributions

优化结束后，计算位姿的协方差. 假设，在对应的层数上，测量值的协方差都为1个像素（orb-slam中不是这么做的，而是根据特征点在哪个层，就乘以相应的尺度，尺度越大，不确定性越大），即测量值满足方差为1的高斯分布，由高斯牛顿可得：
$$J\xi =\delta \Rightarrow \xi =(J^{T}J{)}^{-1}{J}^T\delta$$
由协方差的传递律可得，$\xi$ 的协方差为:
$$P_{\xi }=((J^{T}J{)}^{-1}{J}^T)P_{\delta }((J^{T}J{)}^{-1}{J}^T{)}^T=(J^{T}J{)}^{-1}{J}^{T}J(J^{T}J{)}^{-T}=(J^{T}J{)}^{-1}$$
这也是g2o里面计算状态变量的协方差，使用computeMarginals()函数，注意，它计算返回的是协方差而不是信息矩阵.
2）使用optimizeStructure()优化point的位置

---

3、深度滤波

1、DepthFilter新进来一个关键帧，则需要生成新的seed（种子）

• 新的关键帧到来时，则中断updateSeeds()，因为这时有很大可能正在处理普通帧，而不是关键帧，所以就丢掉了，即，关键帧优先
• setExistingFeatures()设置包含feature的栅格为占据状态，这时的feature只来自与相邻关键帧匹配得到，所以不需要通过深度滤波器再优化了
• 使用feature_detector_->detect()检测新的feature，然后加入到seeds_中

seed的属性如下，每一个seed包括一个新生成的feature

Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
    batch_id(batch_counter),
    id(seed_counter++),
    ftr(ftr),                  //对应的feature
    a(10),
    b(10),
    mu(1.0/depth_mean),        //均值，深度的倒数
    z_range(1.0/depth_min),    //深度范围为当前帧的最近的深度的倒数
	    sigma2(z_range*z_range/36) //方差
{}

2、使用updateSeeds()遍历每一个seed

1）首先检测该seed在当前帧中是否可见，
2）接着使用findEpipolarMatchDirect()处理这个seed，

• 对于边缘特征（Feature::EDGELET），如果把梯度仿射过来后，梯度的方向与极线方向的夹角大于45度，就认为沿着极线找，图块像素也不会变化很大，就不搜索了，直接返回false
• 沿着极线方向搜索，在当前帧上找到与ref_ftr对应的图像块最匹配的图像块，接着使用align2D()进一步优化，得到更为精准的px_cur_，而后就可以使用seed的对应的ref_ftr与px_cur_进行三角化，得到3D point，也就得到了待估计的深度（depth）

3）上一步得到深度，接下来使用updateSeed()得到深度的协方差，这里要参考论文:《 Video-based, Real-Time Multi View Stereo》
4）如果协方差小于阈值，就认为收敛了，它就不再是种子点，而是candidate点，使用回调函数，加入到候选点队列中

seed_converged_cb_(point, it->sigma2);

候选point类型如下

typedef pair PointCandidate;
typedef list PointCandidateList;

5）如果第2步匹配成功，则得到px_cur_，如果当前帧是关键帧的话，就将matcher_.px_cur_所在的栅格设置为占据状态

---

<完>
@leatherwang

---