Full Steam Ahead: Exactly Sparse Gaussian Process Regression For Batch Continuous-Time Trajectory Estimation On Se(3)

This paper shows how to carry out batch continuous-time trajectory estimation for bodies translating and rotating in three-dimensional (3D) space, using a very efficient form of Gaussian-process (GP) regression. The method is fast, singularity-free, uses a physically motivated prior (the mean is constant body-centric velocity), and permits trajectory queries at arbitrary times through GP interpolation. Landmark estimation can be folded in to allow for simultaneous trajectory estimation and mapping (STEAM), a variant of SLAM.The key to making the approach efficient is to select a GP prior that has a block-tridiagonal inverse kernel matrix, resulting in fast inference (at a set of measurement or subset of key times) and interpolation (at a set of additional query times). We define the prior using a first-order stochastic differential equation (SDE) model on SE(3) x R-6; by explicitly estimating both pose and body-centric velocity, the Markov property of the SDE ensures the exact sparsity of the inverse kernel matrix. While the exactly sparse GP approach has been investigated for linear, time-varying SDEs on vectorspaces, our contribution lies in the extension of these results to the Lie group, SE(3).We validate the method experimentally on a STEAM problem involving a stereo camera translating and rotating in 3D while observing point landmarks.