Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra

Joshua G Mangelson, Maani Ghaffari, Ram Vasudevan, Ryan Eustice

Abstract:

Abstract—An accurate characterization of pose uncertainty is essential for safe autonomous navigation. Early pose uncertainty characterization methods proposed by Smith, Self, and Cheeseman (SCC), used coordinate-based first-order methods to propagate uncertainty through non-linear functions such as pose composition (head-to-tail), pose inversion, and relative pose extraction (tail-to-tail). Characterizing uncertainty in the Lie Algebra of the special Euclidean group results in better uncertainty estimates. However, existing Lie group based uncertainty propagation techniques assume that individual poses are independent. After solving a pose graph, however, the entire trajectory is jointly distributed as factors induce correlation. Hence, the independence assumption does not capture reality. In addition, prior work has focused primarily on the pose composition operation. This paper develops a framework for modeling the uncertainty of jointly distributed poses and describes how to perform the equivalent of the SSC pose operations while characterizing uncertainty in the Lie Algebra. Evaluation on simulated and open-source datasets shows that the proposed methods result in more accurate uncertainty estimates and thus more accurate f iltering of potential loop-closures. An accompanying C++ library implementation is also released.

Index Terms—SLAM, mobile robotics, uncertainty propagation, Lie group, Lie algebra, matrix groups, rigid body transformation, state estimation.