Gaussian Processes as Continuous-time Trajectory Representations: Applications in SLAM and Motion Planning

Similar documents
Incremental Sparse GP Regression for Continuous-time Trajectory Estimation & Mapping

Incremental Sparse GP Regression for Continuous-time Trajectory Estimation & Mapping

Incremental Sparse GP Regression for Continuous-time Trajectory Estimation & Mapping

ADVANCED MACHINE LEARNING ADVANCED MACHINE LEARNING. Non-linear regression techniques Part - II

Visual SLAM Tutorial: Bundle Adjustment

arxiv: v1 [cs.ro] 1 Dec 2014

Model Selection for Gaussian Processes

Bayesian Deep Learning

Gaussian Processes. 1 What problems can be solved by Gaussian Processes?

Probabilistic Machine Learning. Industrial AI Lab.

Statistical Techniques in Robotics (16-831, F12) Lecture#20 (Monday November 12) Gaussian Processes

Gaussian Process Motion Planning

Lecture: Gaussian Process Regression. STAT 6474 Instructor: Hongxiao Zhu

Introduction. Chapter 1

Gaussian Processes (10/16/13)

Statistical Techniques in Robotics (16-831, F12) Lecture#21 (Monday November 12) Gaussian Processes

Bayesian Learning (II)

State Space Gaussian Processes with Non-Gaussian Likelihoods

arxiv: v4 [stat.ml] 28 Sep 2017

Expectation Propagation in Dynamical Systems

AUTOMOTIVE ENVIRONMENT SENSORS

SLAM for Ship Hull Inspection using Exactly Sparse Extended Information Filters

ECE521 week 3: 23/26 January 2017

Neutron inverse kinetics via Gaussian Processes

Manipulators. Robotics. Outline. Non-holonomic robots. Sensors. Mobile Robots

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

Pose tracking of magnetic objects

Towards airborne seismic. Thomas Rapstine & Paul Sava

Nonparametric Bayesian Methods (Gaussian Processes)

Partially Observable Markov Decision Processes (POMDPs)

Modeling and state estimation Examples State estimation Probabilities Bayes filter Particle filter. Modeling. CSC752 Autonomous Robotic Systems

STA414/2104 Statistical Methods for Machine Learning II

Distributed Robust Localization from Arbitrary Initial Poses via EM and Model Selection

Part 1: Expectation Propagation

Parameter Estimation. Industrial AI Lab.

PILCO: A Model-Based and Data-Efficient Approach to Policy Search

Pattern Recognition and Machine Learning

COMP 551 Applied Machine Learning Lecture 20: Gaussian processes

Autonomous Mobile Robot Design

2D Image Processing. Bayes filter implementation: Kalman filter

Reliability Monitoring Using Log Gaussian Process Regression

SLAM Techniques and Algorithms. Jack Collier. Canada. Recherche et développement pour la défense Canada. Defence Research and Development Canada

Part 2: Multivariate fmri analysis using a sparsifying spatio-temporal prior

Rao-Blackwellized Particle Filtering for 6-DOF Estimation of Attitude and Position via GPS and Inertial Sensors

Results: MCMC Dancers, q=10, n=500

Introduction to Gaussian Process

Robotics. Mobile Robotics. Marc Toussaint U Stuttgart

STAT 518 Intro Student Presentation

Gaussian with mean ( µ ) and standard deviation ( σ)

Learning Gaussian Process Models from Uncertain Data

Toward Online Probabilistic Path Replanning

CMU-Q Lecture 24:

Kalman filtering and friends: Inference in time series models. Herke van Hoof slides mostly by Michael Rubinstein

From Bayes to Extended Kalman Filter

1 Bayesian Linear Regression (BLR)

STA 4273H: Statistical Machine Learning

Nonparameteric Regression:

Lecture 6: Bayesian Inference in SDE Models

Bayesian Calibration of Simulators with Structured Discretization Uncertainty

STA414/2104. Lecture 11: Gaussian Processes. Department of Statistics

Midterm Review CS 7301: Advanced Machine Learning. Vibhav Gogate The University of Texas at Dallas

GWAS V: Gaussian processes

Terrain Navigation Using the Ambient Magnetic Field as a Map

2D Image Processing. Bayes filter implementation: Kalman filter

Probabilistic numerics for deep learning

Introduction to Gaussian Processes

Scalable Sparsification for Efficient Decision Making Under Uncertainty in High Dimensional State Spaces

CS-E3210 Machine Learning: Basic Principles

20: Gaussian Processes

ECE521 lecture 4: 19 January Optimization, MLE, regularization

Machine Learning. Bayesian Regression & Classification. Marc Toussaint U Stuttgart

MCMC Sampling for Bayesian Inference using L1-type Priors

Ch 4. Linear Models for Classification

Robotics. Lecture 4: Probabilistic Robotics. See course website for up to date information.

Linear Dynamical Systems

Statistical and Learning Techniques in Computer Vision Lecture 2: Maximum Likelihood and Bayesian Estimation Jens Rittscher and Chuck Stewart

What is (certain) Spatio-Temporal Data?

Introduction to machine learning and pattern recognition Lecture 2 Coryn Bailer-Jones

The geometry of Gaussian processes and Bayesian optimization. Contal CMLA, ENS Cachan

Probabilistic modeling. The slides are closely adapted from Subhransu Maji s slides

Introduction to Machine Learning

CS491/691: Introduction to Aerial Robotics

Fast Incremental Square Root Information Smoothing

Deep Learning & Artificial Intelligence WS 2018/2019

Naïve Bayes Introduction to Machine Learning. Matt Gormley Lecture 18 Oct. 31, 2018

2D Image Processing (Extended) Kalman and particle filter

Path Integral Stochastic Optimal Control for Reinforcement Learning

Linear Models for Regression CS534

CPSC 540: Machine Learning

SYDE 372 Introduction to Pattern Recognition. Probability Measures for Classification: Part I

Introduction to Gaussian Processes

Deep Poisson Factorization Machines: a factor analysis model for mapping behaviors in journalist ecosystem

A latent variable modelling approach to the acoustic-to-articulatory mapping problem

Abnormal Activity Detection and Tracking Namrata Vaswani Dept. of Electrical and Computer Engineering Iowa State University

COMP90051 Statistical Machine Learning

Gaussian Processes for Bayesian Filtering

Introduction to Machine Learning

Bayesian Machine Learning

L11. EKF SLAM: PART I. NA568 Mobile Robotics: Methods & Algorithms

CPSC 540: Machine Learning

Transcription:

Gaussian Processes as Continuous-time Trajectory Representations: Applications in SLAM and Motion Planning Jing Dong jdong@gatech.edu 2017-06-20 License CC BY-NC-SA 3.0

Discrete time SLAM Downsides: Measurements distorted by motions Asynchronous measurements Not a compact representation Rolling shutter effect GPS: 5Hz Camera: 3.75Hz Asynchronous data C. H. Tong, P. Furgale, and T. D. Barfoot, Gaussian process gauss newton for non-parametric simultaneous localization and mapping, Intl. J. of Robotics Research, vol. 32, no. 5, pp. 507 525, 2013.

Discrete time SLAM Downsides: Measurements distorted by motions Asynchronous measurements Not a compact representation Continuous-time representation Linear interpolation Splines Wavelets Gaussian process C. H. Tong, P. Furgale, and T. D. Barfoot, Gaussian process gauss newton for non-parametric simultaneous localization and mapping, Intl. J. of Robotics Research, vol. 32, no. 5, pp. 507 525, 2013.

Outlines GP as continuous-time trajectory representation Extend sparse GP to Lie groups Use sparse GP in SLAM Use sparse GP in motion planning

Outlines GP as continuous-time trajectory representation Extend sparse GP to Lie groups Use sparse GP in SLAM Use sparse GP in motion planning

GP as 1D continuous-time trajectory with uncertainty X x=f(t) X t Rasmussen, Carl Edward. "Gaussian processes for machine learning." (2006).

GP with noise-free measurements X x=f(t) t=1s t*=1.5s? t=2s X t*? t Measurements Query Points Rasmussen, Carl Edward. "Gaussian processes for machine learning." (2006).

GP with noise-free measurements Rasmussen, Carl Edward. "Gaussian processes for machine learning." (2006).

GP with noisy measurements X x=f(t) t=1s t*=1.5s? t=2s X t*? t Measurement function: Measurements Query Points Rasmussen, Carl Edward. "Gaussian processes for machine learning." (2006).

GP with noisy measurements X x=f(t) t=1s? t=1.5s? t=2s? X Measurement function: t Measurements and Query points Measurement and Query share same time stamps: Rasmussen, Carl Edward. "Gaussian processes for machine learning." (2006).

Another perspective... Measurement function: Mean value: Maximum A Posteriori Estimation Sherman Morrison Woodbury formula Rasmussen, Carl Edward. "Gaussian processes for machine learning." (2006).

Nonlinear measurement function and high DOF... Problem definition: Mean value: MAP Estimation Gauss-Newton method T. Barfoot, C. H. Tong, and S. Sarkka, Batch continuous-time trajectory estimation as exactly sparse gaussian process regression, Robotics: Science and Systems (RSS), 2014.

Sparsity and Efficiency Gauss-Newton step Inverse Kernel (Prior) Measurements (Likelihood) Particular kernel is needed to make sparsity T. Barfoot, C. H. Tong, and S. Sarkka, Batch continuous-time trajectory estimation as exactly sparse gaussian process regression, Robotics: Science and Systems (RSS), 2014.

Sparse information matrix Information matrix: Generally the inverse kernel matrix is not sparse Trajectory Landmarks C. H. Tong, P. Furgale, and T. D. Barfoot, Gaussian process gauss newton for non-parametric simultaneous localization and mapping, Intl. J. of Robotics Research, vol. 32, no. 5, pp. 507 525, 2013.

GPs generated by LTV-SDEs linear time-varying stochastic differential equations (LTV-SDEs) solution: Inverse kernel matrix is tridiagonal block-wise sparse if the GP is generated by a LTV-SDE T. Barfoot, C. H. Tong, and S. Sarkka, Batch continuous-time trajectory estimation as exactly sparse gaussian process regression, Robotics: Science and Systems (RSS), 2014.

Constant velocity LTV-SDEs V: change slowly X Inject white noise in acceleration Rewrite LTV-SDE T. Barfoot, C. H. Tong, and S. Sarkka, Batch continuous-time trajectory estimation as exactly sparse gaussian process regression, Robotics: Science and Systems (RSS), 2014.

Factor Graph view Trajectory Landmarks Tri-diagonal inverse kernel T. Barfoot, C. H. Tong, and S. Sarkka, Batch continuous-time trajectory estimation as exactly sparse gaussian process regression, Robotics: Science and Systems (RSS), 2014. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Const-time Interpolation Any time could be interpolated by nearby two states Time complexity O(1) Measurements at any time fused in graph by interpolated factor T. Barfoot, C. H. Tong, and S. Sarkka, Batch continuous-time trajectory estimation as exactly sparse gaussian process regression, Robotics: Science and Systems (RSS), 2014. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Results 2D SLAM cases,

Outlines GP as continuous-time trajectory representation Extend sparse GP to Lie groups Use sparse GP in SLAM Use sparse GP in motion planning

Example Lie Group: SO(2) θ S. Anderson and T. D. Barfoot, Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE (3), (IROS), 2015. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Gaussian Distribution on SO(2) Lie Group: SO(2) Tangent space: so(2) \in R^2 S. Anderson and T. D. Barfoot, Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE (3), (IROS), 2015. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Gaussian Distribution on SO(2) Expmap: so(2) -> SO(2) Logmap: SO(2) -> so(2) Lie Group: SO(2) Tangent space: so(2) \in R^2 S. Anderson and T. D. Barfoot, Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE (3), (IROS), 2015. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Gaussian Distribution on SO(2) Expmap: so(2) -> SO(2) Logmap: SO(2) -> so(2) Lie Group: SO(2) Tangent space: so(2) \in R^2 We define Gaussian on Lie group by defining Gaussian on tangent space, then map back by exponential map Note that tangent space is defined on a particular θ on Lie group S. Anderson and T. D. Barfoot, Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE (3), (IROS), 2015. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Local GP on SO(2) Ti Local GP range of Ti S. Anderson and T. D. Barfoot, Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE (3), (IROS), 2015. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Local GP on SO(2) Ti ti < t < ti+1 range of ξi(t) We define a local linear variable ξi(t) around Ti, which locally meets a zero-mean Gaussian process defined by kernel K(ti, t) S. Anderson and T. D. Barfoot, Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE (3), (IROS), 2015. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Constant Velocity LVT-SDE Const body frame velocity SDE: nonlinear Equivalence to LTV-SDE on loca variable Local constant velocity LTV-SDE S. Anderson and T. D. Barfoot, Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE (3), (IROS), 2015. Dong, Jing, Byron Boots, and Frank Dellaert. "Sparse Gaussian Processes for Continuous-Time Trajectory Estimation on Matrix Lie Groups." arxiv preprint arxiv:1705.06020 (2017).

Outlines GP as continuous-time trajectory representation Extend sparse GP to Lie groups Use sparse GP in SLAM Use sparse GP in motion planning

Background Application: autonomous crop monitoring using UAV / UGV Get crop height / size / color / etc. Requirement: use low-cost sensors: camera, GPS, etc. Existing techniques: Structure from Motion (SfM): 3D reconstruction Difficulties: dynamic scene with crop growing: 4D reconstruction

Dataset Dataset: color images + GPS + IMU, on ground vehicle and UAV Ground dataset: Twice per week for 3 months: total 23 sessions With ground truth height / leaf chlorophyll at multiple sampling sites 50m Dong, Jing, et al. "4D Crop Monitoring: Spatio-Temporal Reconstruction for Agriculture." ICRA (2017).

Approach 3D Reconstruction GPS IMU 4D Reconstruction Point cloud analysis Dong, Jing, et al. "4D Crop Monitoring: Spatio-Temporal Reconstruction for Agriculture." ICRA (2017).

1. Multi-Sensor Fusion SLAM Multi-sensor (camera, IMU, GPS) fusion Using factor graph and maximum a posterior (MAP) estimation Optional dense reconstruction by PMVS 3D factor graph Dong, Jing, et al. "4D Crop Monitoring: Spatio-Temporal Reconstruction for Agriculture." ICRA (2017).

2. 4D Data Association and Reconstruction View-invariant feature matching by bounded search and homography warping Spatio-temporal (4D) factor graph optimization View-invariant robust feature matching Day i Day i+1 Spatio-temporal (4D) factor graph Dong, Jing, et al. "4D Crop Monitoring: Spatio-Temporal Reconstruction for Agriculture." ICRA (2017).

Results https://youtu.be/bgllllskwzi Dong, Jing, et al. "4D Crop Monitoring: Spatio-Temporal Reconstruction for Agriculture." ICRA (2017).

Outlines GP as continuous-time trajectory representation Extend sparse GP to Lie groups Use sparse GP in SLAM Use sparse GP in motion planning

Introduction Planning on high DOF system is difficult Sampling based approaches like RRT and RPM are slow due to high DOF State-of-the-art approaches are mostly optimization based Energy reduction (Zucker et al, 2013 IJRR, Ratliff et al, 2009 ICRA) Constrained optimization (Schluman et al, 2013 RSS) Factor Graph and message passing (Toussaint et al, 2010 Book) Main Contributions: Formulate motion planning as continuous trajectory estimation by GP Enable efficient motion planning by sparse GP and isam2

Motion Planning as Probabilistic inference Two requirements of motion planning: Start Feasibility: trajectory is collision-free Optimality: smooth output by reducing the energy used or trajectory length End Calculate the trajectory by maximize the probability Prior distribution: enforce smoothness Collision-free likelihood: enforce feasibility Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

Collision-free Likelihood Collision-free likelihood is calculated at each discretized points of trajectory Collision-free likelihood is defined by exponential family Start End Obstacle cost function is defined by hinge loss function on signed distance Signed distance field is pre-calculated Robot body use sphere model to approximate Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

GP prior and Factor graph Trajectory is represented by a GP Trajectory prior use constant velocity GP prior Additional prior on start/goal configuration (Since they are known) Inference problem represented by a factor graph Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

Duality between inference and energy minimization Trajectory Prior Collision-free Likelihood Smoothness Cost Collision Cost Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

Efficient planning: Use GP interpolation GP enable us to query any time of interest So we can check collision cost at more points with less states to optimize A single query state is interpolated by near by two states Interpolated collision cost factor is binary Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

Evaluation: Batch planning Dataset: 7-DOF WAM arm and 7-DOF PR2, total 114 problems Criteria: Success rate, average run-time, maximum run-time Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

Efficient planning: Replanning Example replanning problem: given current state and a new goal Naive approach: plan from scratch given current (as start) and goal states Start Current state New Goal Fact: planning problem changes little Original Goal Constraint on current state Goal changes Current state Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016). Mukadam, Mustafa, et al. "Simultaneous Trajectory Estimation and Planning via Probabilistic Inference." Robotics: Science and Systems (RSS) (2017).

Efficient replanning: Bayes tree and isam2 Original graph Goal changes Constraint on current state

Evaluation: Replanning Dataset: 7-DOF WAM arm and 7-DOF PR2, total 60 problems Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

Evaluation: Replanning Dataset: 7-DOF WAM arm and 7-DOF PR2, total 60 problems https://github.com/gtrll/gpmp2/blob/master/ doc/pics/wam_replan_2.gif https://github.com/gtrll/gpmp2/blob/master/ doc/pics/wam_replan_1.gif Dong, Jing, et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." Robotics: Science and Systems (RSS) (2016).

Real-time Replanning on Robots https://youtu.be/lyaynkv1eaq Mukadam, Mustafa, et al. "Simultaneous Trajectory Estimation and Planning via Probabilistic Inference." Robotics: Science and Systems (RSS) (2017).

Thanks!

2024 © sciencedocbox.com Privacy Policy | Terms of Service | Feedback