Exactly sparse delayed state filter on Lie groups for long-term pose graph SLAM

Abstract

In this paper we propose a simultaneous localization and mapping (SLAM) back-end solution called the exactly sparse delayed state filter on Lie groups (LG-ESDSF). We derive LG-ESDSF and demonstrate that it retains all the good characteristics of the classic Euclidean ESDSF, the main advantage being the exact sparsity of the information matrix. The key advantage of LG-ESDSF in comparison with the classic ESDSF lies in the ability to respect the state space geometry by negotiating uncertainties and employing filtering equations directly on Lie groups. We also exploit the special structure of the information matrix in order to allow long-term operation while the robot is moving repeatedly through the same environment. To prove the effectiveness of the proposed SLAM solution, we conducted extensive experiments on two different publicly available datasets, namely the KITTI and EuRoC datasets, using two front-ends: one based on the stereo camera and the other on the 3D LIDAR. We compare LG-ESDSF with the general graph optimization framework (g 2 o) when coupled with the same front-ends. Similarly to g 2 o the proposed LG-ESDSF is front-end agnostic and the comparison demonstrates that our solution can match the accuracy of g 2 o, while maintaining faster computation times. Furthermore, the proposed back-end coupled with the stereo camera front-end forms a complete visual SLAM solution dubbed LG-SLAM. Finally, we evaluated LG-SLAM using the online KITTI protocol and at the time of writing it achieved the second best result among the stereo odometry solutions and the best result among the tested SLAM algorithms.

LiePoseNet: Heterogeneous Loss Function Based on Lie Group for Significant Speed-up of PoseNet Training Process

“…This distribution is an extension of simple Gaussian distribution for translation and rotation. Concentrated Gaussian distribution is often used for Kalman filter in robotics and for solving the SLAM problem [31], [32].…”

Heading Estimation Using Ultra-Wideband Received Signal Strength and Gaussian Processes

“…where θ k ∼ N (0, R θ k ) and which is left-invariant according to the definition in Section II-B. This is similar to the error definition used on quaternions in [18] and on SE(3) matrix Lie group elements in [19]. As such, linearization can be done directly on the measurement model by rewriting both Y k and C ab k in terms of the left-invariant error as…”

Intrinsic Slepian-Bangs Type Formula for Parameters on LGs with Unknown Measurement Noise Variance

“…Especially, it is deduced an intrinsic Slepian-Bangs (ISB) formula for Gaussian model on LGs. It is relevant in some applications, for instance, when sensors provides orientation measurements such as odometer [16] or LIDAR system [17].…”