Fast Nonlinear Approximation of Pose Graph Node Marginalization

We present a fast nonlinear approximation method for marginalizing out nodes on pose graphs for longterm simultaneous localization, mapping, and navigation. Our approximation preserves the pose graph structure to leverage the rich literature of pose graphs and optimization schemes. By re-parameterizing from absolute-to relative-pose spaces, our method does not suffer from the choice of linearization points as in previous works. We then join our approximation process with a scaled version of the recently-demoted pose-composition approach. Our approach eschews the expenses of many state-of-the-art convex optimization schemes through our efficient and simple $O(N^{2})$ implementation for a given known topology of the approximate subgraph. We demonstrate its speed and near optimality in practice by comparing against state-of-the-art techniques on popular datasets.