基于嵌套剖分的位姿图分层优化算法

Hierarchical pose graph optimization algorithm based on nested dissection

位姿图优化(pose graph optimization,PGO)是一种在同时定位与地图构建(simultaneous localization and mapping,SLAM)后端优化中常用的高维非凸优化算法,通常建模成极大似然估计。由于目前的PGO算法优化大规模大噪声数据集时很难在保证精度的同时提升速度,所以提出了一种基于嵌套剖分的位姿图分层优化算法。该算法首先建立不同距离度量的χ2检验模型,进而剔除异常值点。然后利用嵌套剖分算法将位姿图分割成一组子图,再从这些子图中提取出一个表示原SLAM问题的抽象拓扑的骨架图,从而优化该骨架图,完成初始化。最后在模拟和真实的位姿图数据集上进行实验评估,结果表明该算法在不影响精度的情况下,可以提高算法的计算速度,具有可伸缩性。

PGO is a high-dimensional non-convex optimization algorithm commonly used in the back-end optimization of SLAM,which is usually modeled as maximum likelihood estimation.Since the current PGO algorithm faces difficulties in improving speed while ensuring accuracy when optimizing large-scale noise datasets,this paper proposed a hierarchical pose graph optimization algorithm based on nested dissection algorithm for large noise datasets.The algorithm firstly established aχ2 test model with different distance measures,and then removed outlier points.Secondly,it used nested dissection method to split the original pose graph into a set of subgraphs and extracted a skeleton graph from these subgraphs.The skeleton represented the abstract topology of the original SLAM problem.Then,the algorithm optimized the skeleton graph and completed the initialization.Finally,experimental evaluation on simulated and real pose graph datasets show that the proposed algorithm can improve the calculation speed and scalability of the algorithm without affecting the accuracy.