SVO代码笔记（一）

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的梯度，归一化后的

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

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

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

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

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

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

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

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

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 ξ = δ \Rightarrow ξ = (J^{T} J)^{- 1} J^{T} δ

$J\xi=\delta \ \ \ \ \ \Rightarrow \ \ \ \ \ \xi=(J^TJ)^{-1}J^T\delta$
由协方差的传递律可得，

ξ

$\xi$ 的协方差为:

P_{ξ} = ((J^{T} J)^{- 1} J^{T}) P_{δ} ((J^{T} J)^{- 1} J^{T})^{T} = (J^{T} J)^{- 1} J^{T} J (J^{T} J)^{- T} = (J^{T} J)^{- 1}

$P_{\xi}=((J^TJ)^{-1}J^T)P_{\delta}((J^TJ)^{-1}J^T)^T=(J^TJ)^{-1}J^TJ(J^TJ)^{-T}=(J^TJ)^{-1}$
这也是g2o里面计算状态变量的协方差，使用 computeMarginals()函数，注意，它计算返回的是协方差而不是信息矩阵.
2）使用 optimizeStructure()优化point的位置

深度滤波

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<Point*, Feature*> PointCandidate;
typedef list<PointCandidate> PointCandidateList;

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

<完>
@leatherwang

SVO代码笔记（一）

深度滤波

猜你喜欢