Pose graph optimization with hierarchical conditionally independent graph partitioning

In this paper, we propose a hierarchical pose graph optimization algorithm, which hierarchically divides a large pose graph into subgraphs and solves the optimization problem for each subgraph independently. Applying a modified graph partitioning algorithm, normalized cut, the original graph could be partitioned into subgraphs, which are conditionally independent on a set of key nodes. A modified normalized cut algorithm is applied to automatically partition a pose graph into several subgraphs, which are independent on each other when conditioned on a small number of keynodes. Preserving keynodes only, a simplified upper level graph is generated by a pose graph sparsification algorithm. Given the optimization results of all keynodes, which are obtained by solving the upper level graph, each subgraph in the lower level can be solved efficiently without concerning other subgraphs. The scale of each optimization is limited during graph partitioning. Therefore, the efficiency of the optimization is improved. Experiments with both public standard datasets and a dataset collected by an autonomous guided vehicle (AGV) system are conducted to test our algorithm.