一种立体相机辅助的关键帧实时定位方法

本发明提供了一种立体相机辅助的关键帧实时定位方法，通过对立体相机辅助的关键帧数据进行两次优化定位，将局部优化扩展为多组关键帧的全局优化，一方面获得了全局次最优的定位解，提高了实时定位精度，另一方面能够适应不同类型的立体相机辅助场景，具有较好的鲁棒性。采用了本发明的技术方案以后，当关键帧序列中存在循环闭包时，能够提供更高定位精度的实时处理效果；另外，其在位姿图中采用了当前更新帧优化后的姿态，优化代价函数为J(xnew)，通过结合循环闭合约束，进一步改善了定位质量；而且，对连续的相邻关键帧满足重定位假设的数量没有严格的限制，具有较好的算法适应性，其整体设计十分合理巧妙。