Multi-level submap based SLAM using nested dissection

We propose a novel batch algorithm for SLAM problems that distributes the workload in a hierarchical way. We show that the original SLAM graph can be recursively partitioned into multiple-level submaps using the nested dissection algorithm, which leads to the cluster tree, a powerful graph representation. By employing the nested dissection algorithm, our algorithm greatly minimizes the dependencies between two subtrees, and the optimization of the original SLAM graph can be done using a bottom-up inference along the corresponding cluster tree. To speed up the computation, we also introduce a base node for each submap and use it to represent the rigid transformation of the submap in the global coordinate frame. As a result, the optimization moves the base nodes rather than the actual submap variables. We demonstrate that our algorithm is not only exact but also much faster than alternative approaches in both simulations and real-world experiments.