ORB-SLAM2源码分析(单目)——恒速模型跟踪
一、 恒速模型跟踪简介
大部分时间都用这个跟踪,只利用到了上一帧的信息。
- 用恒速模型先估计一个初始位姿
- 用该位姿进行投影匹配 SearchByProjection,候选点来自GetFeaturesInArea,未使用BoW
- BA优化(仅优化位姿),提供比较粗糙的位姿
假设短时间内(相邻帧)物体处于匀速运动状态,可以用上一帧的位姿和速度来估计当前帧的位姿。
移动模式跟踪跟踪前后两帧得到变换矩阵。
上一帧的地图3d点反投影到当前帧图像像素坐标上,在不同尺度下不同的搜索半径内,做描述子匹配搜索可以加快匹配。
在投影点附近根据描述子距离进行匹配(需要>20对匹配,否则匀速模型跟踪失败,运动变化太大时会出现这种情况),然后以运动模型预测的位姿为初值,优化当前位姿,
优化完成后再剔除外点,若剩余的匹配依然>=10对,则跟踪成功,否则跟踪失败,需要Relocalization
二、 更新上一帧位姿,在上一帧中生成临时地图点
单目情况:只计算了上一帧的世界坐标系位姿
双目和rgbd情况:选取有有深度值的并且没有被选为地图点的点生成新的临时地图点,提高跟踪鲁棒性
通过上一帧的参考关键帧位姿与上一帧的参考关键帧到上一帧的变换矩阵,计算出上一帧的位姿
双目或者RGBD的相机下:
如果上一帧图像中没有当前帧的地图点,则将这个地图点加入到上一帧图像上,作为局部地图点
三、 用上一帧地图点进行投影匹配,如果匹配点不够,则扩大搜索半径再来一次(SearchByProjection)
因为当前帧的位姿是不准确的,所以使用上一帧的地图点投影到当前帧上得图像坐标,以图像坐标为圆心,半径为r进行搜索,通过搜索到的特征点与上一帧地图点中对应的特征点坐标进行误差比较,得到对应的匹配点,记录对应的匹配点信息。如果获得到的匹配点少于阈值,则认为跟踪失败
1、 建立旋转直方图,用于检测旋转一致性
2、 计算当前帧和前一帧的平移向量
判断前后两帧的运动是前进还是后退
3、 对于前一帧的每一个地图点,通过相机投影模型,得到投影到当前帧的像素坐标
4、 根据相机的前后前进方向来判断搜索尺度范围。
如果是前进了,那么目标所在的图
像上,应该比上一帧面积增大了,反之则缩小了
5、 遍历候选匹配点,寻找距离最小的最佳匹配点
6、 计算匹配点旋转角度差所在的直方图
7、 进行旋转一致检测,剔除不一致的匹配
四、 利用3D-2D投影关系,优化当前帧位姿
通过当前帧与参考关键帧的共视地图点,通过3D-2D的重投影误差(将地图点转化到当前的像素坐标系与当前帧的特征点对比得到误差值),对当前帧位姿进行优化。