1. 通过投影点进行特征匹配
(1)投影匹配原理
投影匹配的大致思路是将地图点通过相机位姿投影到当前帧的二位像素平面上,然后在对应像素坐标周围的圆形区域内寻找候选匹配点,通过一定的筛选条件剔除误匹配,最终在当前帧中得到与该地图点匹配的特征点。
地图点的主要来源如下:
- 在恒速模型跟踪中,投影的地图点来自于前一个普通帧
- 在局部地图跟踪中,投影的地图点来自于所有的局部地图点
- 在重定位跟踪中,投影的地图点来自于候选关键帧
- 在闭环线程中,投影的地图点来自于闭环关键帧及其共视关键帧
筛选条件主要包括:
- 在候选匹配点中与待匹配特征点的描述子距离最小
- 最小描述子距离需要小于所设的距离阈值
- 候选匹配点没有与之对应的匹配点
- 两帧匹配特征点的角度差需要在直方图的前3个bin中
(2)基于投影匹配的恒速模型跟踪
大致流程
- 定义特征点角度差统计用的直方图
- 计算上一帧到当前帧的位姿变换,根据Z方向的平移量确定相机运动变化(前进还是后退)
- 遍历上一帧中的特征点,执行如下操作:
- 根据特征点索引获取上一帧中对应的地图点
- 判断该地图点的有效性:是否存在,是否为外点
- 将上一帧中的该地图点转换到当前帧中的像素坐标系上,并判断该像素点的有效性(是否在图像范围内)
- 根据上一帧中该特征点所在的金字塔层级,以及窗口扩大系数,确定在当前帧中的窗口搜索大小radius
- 根据相机运动变化、窗口搜索大小、金字塔层级搜索范围,在当前帧中获取候选匹配点vIndices2
- 遍历当前帧中获取到的候选匹配点vIndices2,通过如下操作获取与上一帧特征点的最佳匹配点:
- 判断该候选匹配点是否已存在对应的地图点,若有则跳过,无需与上一帧中的地图点匹配
- 在双目情况下,还要保证右图的点也在搜索范围内
- 计算遍历到的当前候选匹配点的描述子
- 计算两个特征点描述子之间的汉明距离,并更新最佳距离和对应的候选匹配点
- 若计算出的最佳距离小于设定的距离阈值,则将对应的候选匹配点作为最佳匹配点,同时建立最佳匹配的旋转角度差直方图
- 剔除直方图中旋转角度差不在前3个bin中的匹配(前3个bin中的为符合主流方向的最佳匹配)
根据相机运动确定金字塔搜索层级范围
当相机发生运动(前进或后退)时,特征点的尺度会发生变化,需要确定图像金字塔的层级搜索范围,才能保证特征点能够正确匹配。相机运动的位姿变化计算如下:
记 T c w , T l w T_{cw},T_{lw} Tcw,Tlw分别是当前帧、上一帧的位姿, R , t R,t R,t分别是旋转矩阵和平移向量,写成矩阵形式如下:
T c w = [ R c w t c w 0 1 ] T c w − 1 = [ R c w T − R c w T t c w 0 1 ] T l w = [ R l w t l w 0 1 ] T_{cw} = \left[\begin{array}{cc} R_{cw} & t_{cw} \\ 0 & 1 \end{array} \right] \\ T_{cw}^{-1} = \left[\begin{array}{cc} R_{cw}^T & -R_{cw}^T t_{cw} \\ 0 & 1 \end{array} \right] \\ T_{lw} = \left[\begin{array}{cc} R_{lw} & t_{lw} \\ 0 & 1 \end{array} \right] Tcw=[Rcw0tcw1]Tcw−1=[RcwT0−RcwTtcw1]Tlw=[Rlw0tlw1]
于是,当前帧到上一帧的位姿变换为:
T l c = T l w T w c = T l w T c w − 1 = [ R l w t l w 0 1 ] [ R c w T − R c w T t c w 0 1 ] = [ R l w R c w T − R l w R c w T t c w + t l w 0 1 ] T_{lc} = T_{lw}T_{wc} = T_{lw}T_{cw}^{-1} = \left[\begin{array}{cc} R_{lw} & t_{lw} \\ 0 & 1 \end{array} \right]\left[\begin{array}{cc} R_{cw}^T & -R_{cw}^T t_{cw} \\ 0 & 1 \end{array} \right] = \left[\begin{array}{cc}R_{lw} R_{cw}^T & - R_{lw} R_{cw}^T t_{cw} + t_{lw} \\ 0 & 1 \end{array} \right] Tlc=TlwTwc=TlwTcw</