[Paper Review] Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups

Jing Dong, Byron Boots+1 more|arXiv (Cornell University)|May 17, 2017

Robotics and Sensor-Based Localization27 references17 citations

TL;DR

This paper extends sparse Gaussian process regression to general matrix Lie groups, enabling efficient continuous-time trajectory estimation for robotics applications involving non-Euclidean state spaces such as SO(3) and Sim(3). By leveraging constant-velocity priors and locally linear GP approximations on Lie groups, the method achieves exact sparsity in inverse kernel matrices, allowing scalable inference while supporting asynchronous sensor fusion and generalization to SLAM and motion planning on manifolds.

ABSTRACT

Continuous-time trajectory representations are a powerful tool that can be used to address several issues in many practical simultaneous localization and mapping (SLAM) scenarios, like continuously collected measurements distorted by robot motion, or during with asynchronous sensor measurements. Sparse Gaussian processes (GP) allow for a probabilistic non-parametric trajectory representation that enables fast trajectory estimation by sparse GP regression. However, previous approaches are limited to dealing with vector space representations of state only. In this technical report we extend the work by Barfoot et al. [1] to general matrix Lie groups, by applying constant-velocity prior, and defining locally linear GP. This enables using sparse GP approach in a large space of practical SLAM settings. In this report we give the theory and leave the experimental evaluation in future publications.

Motivation & Objective

• To address the limitation of existing sparse GP methods that are restricted to vector space state representations in robotics.
• To enable continuous-time trajectory estimation on general matrix Lie groups such as SO(3) and Sim(3), which are common in 3D pose and motion estimation.
• To maintain computational efficiency through exact sparsity in inverse kernel matrices by using Lie-theoretic GP priors.
• To support asynchronous sensor measurements via continuous interpolation without adding explicit states.
• To lay the theoretical foundation for extending probabilistic, non-parametric trajectory estimation to broader classes of SLAM and motion planning problems on manifolds.

Proposed method

• Proposes a constant-velocity prior on matrix Lie groups to model trajectory dynamics, ensuring smooth and physically plausible motion.
• Defines locally linear Gaussian process approximations on Lie groups using the logarithmic map and Lie algebraic operations to enable sparse regression.
• Derives a sparse kernel structure by exploiting the time-varying SDE prior, resulting in block-tridiagonal inverse kernel matrices that allow efficient computation.
• Applies continuous-time interpolation via exponential map and Lie group logarithmic maps to estimate states at arbitrary times between observations.
• Fuses asynchronous measurements by expressing their cost in terms of interpolated trajectory states between keyframes, avoiding explicit state addition.
• Extends the framework to simultaneous trajectory estimation and mapping (STEAM) by combining trajectory states with landmark states in a joint factor graph with block-sparse structure.

Experimental results

Research questions

• RQ1 Can sparse Gaussian process regression be generalized to matrix Lie groups to model continuous-time trajectories in non-Euclidean state spaces?
• RQ2 How can a constant-velocity prior be formulated on matrix Lie groups to ensure smooth, physically consistent motion while preserving sparsity?
• RQ3 Can the inverse of the GP kernel matrix remain sparse when using Lie group dynamics, enabling scalable inference?
• RQ4 How can asynchronous sensor measurements be efficiently fused into a continuous-time trajectory estimation framework without introducing additional states?
• RQ5 Can the proposed method be extended to simultaneous trajectory estimation and mapping (STEAM) on manifolds such as SO(3) and Sim(3)?

Key findings

• The proposed method achieves exact sparsity in the inverse kernel matrix by using a time-varying SDE prior on matrix Lie groups, enabling linear-time GP regression.
• The use of locally linear GP approximations via logarithmic maps allows for accurate and efficient interpolation on Lie groups such as SO(3) and Sim(3).
• Asynchronous measurements can be incorporated into the optimization via interpolated trajectory states, reducing the need for explicit state variables.
• The framework supports continuous-time trajectory estimation on general matrix Lie groups, significantly broadening the scope of sparse GP-based SLAM.
• The block-sparse structure of the resulting linear system allows for efficient solution via block-wise sparse Cholesky decomposition, maintaining computational scalability.
• The theoretical foundation enables immediate extension to incremental estimation and motion planning on manifolds, with potential for future experimental validation.