Batch Continuous-Time Trajectory Estimation as Exactly Sparse Gaussian Process Regression

In this paper, we revisit batch state estimation through the lens of Gaussian process (GP) regression. We consider continuous-discrete estimation problems wherein a trajectory is viewed as a one-dimensional GP, with time as the independent variable. Our continuous-time prior can be defined by any linear, time-varying stochastic differential equation driven by white noise; this allows the possibility of smoothing our trajectory estimates using a variety of vehicle dynamics models (e.g., ‘constant-velocity’). We show that this class of prior results in an inverse kernel matrix (i.e., covariance matrix between all pairs of measurement times) that is exactly sparse (block-tridiagonal) and that this can be exploited to carry out GP regression (and interpolation) very efficiently. Though the prior is continuous, we consider measurements to occur at discrete times. When the measurement model is also linear, this GP approach is equivalent to classical, discrete-time smoothing (at the measurement times). When the measurement model is nonlinear, we iterate over the whole trajectory (as is common in vision and robotics) to maximize accuracy. We test the approach experimentally on a simultaneous trajectory estimation and mapping problem using a mobile robot dataset.

[1]  Hauke Strasdat,et al.  Real-time monocular SLAM: Why filter? , 2010, 2010 IEEE International Conference on Robotics and Automation.

[2]  Neil D. Lawrence,et al.  Latent Force Models , 2009, AISTATS.

[3]  Arno Solin,et al.  Spatiotemporal Learning via Infinite-Dimensional Bayesian Filtering and Smoothing: A Look at Gaussian Process Regression Through Kalman Filtering , 2013, IEEE Signal Processing Magazine.

[4]  Paul Timothy Furgale,et al.  Continuous-time batch estimation using temporal basis functions , 2012, 2012 IEEE International Conference on Robotics and Automation.

[5]  Michael Bosse,et al.  Continuous 3D scan-matching with a spinning 2D laser , 2009, 2009 IEEE International Conference on Robotics and Automation.

[6]  Simo Särkkä,et al.  Gaussian filtering and smoothing for continuous-discrete dynamic systems , 2013, Signal Process..

[7]  Simo Särkkä,et al.  Bayesian Filtering and Smoothing , 2013, Institute of Mathematical Statistics textbooks.

[8]  Dieter Fox,et al.  Learning GP-BayesFilters via Gaussian process latent variable models , 2009, Auton. Robots.

[9]  Simo Särkkä,et al.  Recursive Bayesian inference on stochastic differential equations , 2006 .

[10]  Peter Cheeseman,et al.  On the Representation and Estimation of Spatial Uncertainty , 1986 .

[11]  Michael Felsberg,et al.  Rolling shutter bundle adjustment , 2012, 2012 IEEE Conference on Computer Vision and Pattern Recognition.

[12]  Randall Smith,et al.  Estimating Uncertain Spatial Relationships in Robotics , 1987, Autonomous Robot Vehicles.

[13]  A. Jazwinski.  Stochastic Processes and Filtering Theory , 1970 .

[14]  Evangelos E. Milios,et al.  Globally Consistent Range Scan Alignment for Environment Mapping , 1997, Auton. Robots.

[15]  Oliver Grau,et al.  Motion and velocity estimation of rolling shutter cameras , 2012, CVMP.

[16]  Paul Newman,et al.  Navigating, Recognizing and Describing Urban Spaces With Vision and Lasers , 2009, Int. J. Robotics Res..

[17]  Olivier Stasse,et al.  MonoSLAM: Real-Time Single Camera SLAM , 2007, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[18]  T. Başar,et al.  A New Approach to Linear Filtering and Prediction Problems , 2001 .

[19]  Carl E. Rasmussen,et al.  Robust Filtering and Smoothing with Gaussian Processes , 2012, IEEE Transactions on Automatic Control.

[20]  Hugh Durrant-Whyte,et al.  Simultaneous localization and mapping (SLAM): part II , 2006 .

[21]  Gabe Sibley,et al.  Sliding window filter with application to planetary landing , 2010 .

[22]  Frank Dellaert,et al.  iSAM2: Incremental smoothing and mapping using the Bayes tree , 2012, Int. J. Robotics Res..

[23]  Hang Dong,et al.  Lighting-Invariant Visual Odometry using Lidar Intensity Imagery and Pose Interpolation , 2012, FSR.

[24]  Andrew W. Fitzgibbon,et al.  Bundle Adjustment - A Modern Synthesis , 1999, Workshop on Vision Algorithms.

[25]  Carl E. Rasmussen,et al.  Gaussian processes for machine learning , 2005, Adaptive computation and machine learning.

[26]  Paul Timothy Furgale,et al.  Gaussian Process Gauss–Newton for non-parametric simultaneous localization and mapping , 2013, Int. J. Robotics Res..

[27]  R. E. Kalman,et al.  New Results in Linear Filtering and Prediction Theory , 1961 .

[28]  Gabe Sibley,et al.  Spline Fusion: A continuous-time representation for visual-inertial fusion with application to rolling shutter cameras , 2013, BMVC.

[29]  Matthew R. Walter,et al.  Exactly Sparse Extended Information Filters for Feature-based SLAM , 2007, Int. J. Robotics Res..

[30]  Dieter Fox,et al.  GP-BayesFilters: Bayesian filtering using Gaussian process prediction and observation models , 2008, 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[31]  Jouni Hartikainen,et al.  Kalman filtering and smoothing solutions to temporal Gaussian process regression models , 2010, 2010 IEEE International Workshop on Machine Learning for Signal Processing.

[32]  Frank Dellaert,et al.  iSAM: Incremental Smoothing and Mapping , 2008, IEEE Transactions on Robotics.

[33]  Simo Srkk,et al.  Bayesian Filtering and Smoothing , 2013 .

[34]  Frank Dellaert,et al.  Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing , 2006, Int. J. Robotics Res..

[35]  P. Newman,et al.  Navigating , Recognising and Describing Urban Spaces With Vision and Laser , 2009 .

[36]  Hanumant Singh,et al.  Exactly Sparse Delayed-State Filters for View-Based SLAM , 2006, IEEE Transactions on Robotics.

[37]  Arno Solin,et al.  Explicit Link Between Periodic Covariance Functions and State Space Models , 2014, AISTATS.

[38]  Frank Dellaert,et al.  Incremental smoothing and mapping , 2008 .

[39]  Robert F. Stengel,et al.  Optimal Control and Estimation , 1994 .

[40]  Dieter Fox,et al.  GP-BayesFilters: Bayesian filtering using Gaussian process prediction and observation models , 2008, IROS.

[41]  Peter S. Maybeck,et al.  Stochastic Models, Estimation And Control , 2012 .

[42]  H. Durrant-Whyte,et al.  Simultaneous Localisation and Mapping ( SLAM ) : Part II State of the Art , 2006 .

[43]  H. Rue,et al.  An explicit link between Gaussian fields and Gaussian Markov random fields: the stochastic partial differential equation approach , 2011 .

[44]  Dieter Fox,et al.  Gaussian Processes for Signal Strength-Based Location Estimation , 2006, Robotics: Science and Systems.

[45]  Hugh F. Durrant-Whyte.  Uncertain geometry in robotics , 1988, IEEE J. Robotics Autom..

[46]  N E Manos,et al.  Stochastic Models , 1960, Encyclopedia of Social Network Analysis and Mining. 2nd Ed..

[47]  Sebastian Thrun,et al.  The Graph SLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures , 2006, Int. J. Robotics Res..

[48]  Q. Shi,et al.  Gaussian Process Latent Variable Models for , 2011 .

[49]  Simo Särkkä,et al.  State-Space Inference for Non-Linear Latent Force Models with Application to Satellite Orbit Prediction , 2012, ICML.

[50]  Ian D. Reid,et al.  A hybrid SLAM representation for dynamic marine environments , 2010, 2010 IEEE International Conference on Robotics and Automation.

[51]  Neil D. Lawrence,et al.  WiFi-SLAM Using Gaussian Process Latent Variable Models , 2007, IJCAI.

[52]  Paul Timothy Furgale,et al.  Gaussian Process Gauss-Newton: Non-Parametric State Estimation , 2012, 2012 Ninth Conference on Computer and Robot Vision.