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

1. Sparse model-based image alignment

vsan存储嵌套esxi添加存储报错 vsan vmotion_参考帧


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

2. Feature alignment

vsan存储嵌套esxi添加存储报错 vsan vmotion_关键帧_02


在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

vsan存储嵌套esxi添加存储报错 vsan vmotion_参考帧_03


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