从RGB-D相机获取帧,对输入的图片进行预处理,寻找特征点,
进行特征匹配用BA来做最小化重投影误差,进行跟踪和定位每帧的相机(即算出R t)。运用局部BA算法设置局部地图并优化(当前帧与地图中的3D路标点进行匹配来估计相机位姿),
一、预处理:
从RGB相机提取出ORB特征(具有尺度旋转不变性),生成立体坐标,得到特征点。
ORB特征:
FAST关键点,例如FAST-9 FAST-12,只需要比较一个像素与邻域像素的灰度差别大不大。但是原始的FAST角点经常会出现“扎堆”现象,所以在第一遍检查后要进行非极大值抑制,在一定区域内仅仅保留响应极大值的角点。在ORB中对FAST进行改进,指定最终提取的角点数量N,对原始角点算Harris值后选取前N个角点最为最终角点集合。另外,构建图像金字塔并在每一层上面检测角点(分辨率越高角点数量越多)来保持尺度不变性,用灰度质心法来描述旋转。提取了Oriented FAST关键点后,对每个点用改进的BRIEF描述子计算,使得描述子具有平移、旋转和缩放的变换下仍然具有良好的表现。然后用“汉明距离“描述子距离描述两个特征点之间的相似程度,特征点数量很大,因此用FLANN快速近似最近邻算法匹配。
补充1:用改进的ORB做特征点匹配后,得到不同帧之间的特征点对,用RANSAC五点法求取本质矩阵F,进而得到相机的位姿R t。五点法的具体思想是:如果采用最小二乘法,难免因为一些误匹配而导致会有较大的偏差,RANSAC的思想就是先随机选取5个点,计算出本质矩阵F,然后检测其他的点是否满足本质矩阵,得到满足该本质矩阵的个数(内点),进而进行迭代,得到内点最多的那个本质矩阵即为所求。五点算法不受点集共面的影响。
二、跟踪:
主要思路是在当前帧和局部地图之间找出更多的对应关系,以此来优化当前帧的位姿。
跟踪里面的三种运动模型解释,这三种模型都是用来得到当前帧的位姿T的。
(1)MotionModel
恒速运动模型,假设运动是匀速的,根据上一帧的跟踪结果得到一个运动速度,v*t就可以估算当前帧的位姿
(2)RefrenceModel
根据匹配后两帧之间的约束关系来求解位姿估计
(3)RelocalizationModel
在跟丢了以后,把当前帧放到local map中去,找到在database中与当前帧最相似的那一帧(类似于loop closing)。先选出一些候选关键帧,然后将候选关键帧一个个与当前帧的BoW去匹配,不行就抛弃,最后用PnP去求解当前帧与关键帧的位姿,并优化,优化不行的也抛弃。
局部地图跟踪:前面已经得到了对位姿的估计,通过投影可以得到更多的对应关系,与current frame 有共同点的局部地图中关键帧序列为K1,在covisibility graph中与K1相邻的称为k2。loacl map 中有一个帧(属于K1),它和current frame共同观测到的点最多。针对k1 k2可见的每一个地图点云,通过如下步骤,在current frame中搜索:
a.将 地图点投影到当前帧上,如果超出图像范围,就将其舍弃;
b. 计算当前视线方向向量v与地图点云平均视线方向向量n的夹角,舍弃n·v < cos(60°)的点云;
c. 计算地图点到相机中心的距离d,认为[dmin, dmax]是尺度不变的区域,若d不在这个区域,就将其舍弃;
d. 计算图像的尺度因子,为d/dmin;
e . 将地图点的特征描述子D与还未匹配上的ORB特征进行比较,根据前面的尺度因子,找到最佳匹配。
关键帧的选择,可以根据时间间隔或者特征点数目的阈值来判断是否插入新的关键帧。