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_curframe_ref中添加feature(都是位于0层,有些feature在其它层提取,也不加考虑了??),同时向point添加能观测到这个point的feature

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


2、运动估计

1、Sparse Model-based Image Alignment
图像的稀疏对齐操作的是两个普通的帧: last_frame_new_frame_,从max_level_=4max_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 ξ = δ       ⇒       ξ = ( J T J ) − 1 J T δ J\xi=\delta \ \ \ \ \ \Rightarrow \ \ \ \ \ \xi=(J^TJ)^{-1}J^T\delta Jξ=δ          ξ=(JTJ)1JTδ
由协方差的传递律可得, ξ \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} Pξ=((JTJ)1JT)Pδ((JTJ)1JT)T=(JTJ)1JTJ(JTJ)T=(JTJ)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_ftrpx_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


你可能感兴趣的:(slam)