(未完待续。。。)
1、依据当前的Vertex, 从Graph中找到与之相邻的所有vertex(一定距离范围内).
2、采取广度优先搜索的方式,将相邻(next)与相连(adjacentVertices)添加进nearLinkedScans.
3、从sensorManager中取从前到后,依据id序号挑选与当前在一定距离范围内,且不在nearLinkedScans中的candidateScans, 当数量达到一定size,返回。
4、loopScanMatcher进行scanTomap的匹配,当匹配response 和covariance达到一定要求认为闭环检测到。得到调整的correct pose.
5、Add link to loop : 调整边(全局闭环)
6、触发correctPose: ceres/spa/g2o优化
网友评论