SearchInNeighbors
函数
数据来源:与当前关键帧共视程度最高的相邻关键帧,以及与邻帧共视程度最高的相邻关键帧;
更新的信息:更新融合后的3D点,并更新共视关系;
Step 1 获取数据
1、获取与当前关键帧共视程度最高的相邻关键帧;
2、获取与相邻关键帧共视程度最高的相邻关键帧,添加到待融合的关键帧向量中。
Step 2 将当前帧的所有地图点与所有的待融合关键帧融合
融合函数matcher.fuse(关键帧,地图点向量)
投影当前帧的MapPoints
到相邻关键帧pKFi
中,并判断是否有重复的MapPoints
。总结一句话:重复的地图点选择被观测次数多的地图点,没有对应地图点的,添加地图点。
- 如果
MapPoint
能匹配关键帧的特征点,并且该点有对应的MapPoint
,那么将两个MapPoint
合并(选择观测数多的); - 如果
MapPoint
能匹配关键帧的特征点,并且该点没有对应的MapPoint
,那么为该点添加MapPoint
;
注意这个时候对地图点融合的操作是立即生效的。
(1) 判断地图点是否满足条件
- 地图点反投影到像素坐标,判断是否在图像范围内;
- 地图点到相机中心的距离是否满足金字塔的尺度范围;
- 地图点与相机中心形成的向量与平均观测方向向量的夹角是否满足可视范围;
(2) 确定搜索范围内所有特征点的ID
根据上述约束得到所有满足条件的特征点的ID。
(3) 遍历得到的特征点vector
- 判断特征点的像素位置与地图点反投影的像素位置的距离是否满足基于卡方检验计算的阈值;
- 计算特征点的描述子与地图点对应的描述子之间的距离;
- 如果特征点对应的地图点存在,取被观测次数多的地图点,否则,将地图点和特征点对应的关键帧匹配。
Step 3 将当前帧与所有的待融合关键帧对应的地图点进行反向融合
- 获取所有一级邻接和二级邻接关键帧对应的地图点(确保无重复);
- 将当前关键帧与所有地图点进行再次融合。
Step 4 将当前帧与所有的待融合关键帧对应的地图点进行反向融合
对当前关键帧对应的地图点添加属性。