Date post: | 02-Jan-2016 |
Category: |
Documents |
Upload: | harvey-hilary-anthony |
View: | 213 times |
Download: | 0 times |
SA-1
Stochastic Gradient Descent and Tree Parameterizations
in SLAM
G. Grisetti
Autonomous Intelligent Systems Lab
Department of Computer Science,University of Freiburg, Germany
Special thanks to E. Olson, G.D. Tipaldi, S. Grzonka, C. Stachniss, D. Rizzini, B. Steder, W. Burgard, …
What is this Talk about?
mapping
path planning
localizationSLAM
active localization
exploration
integrated approaches (SPLAM)
[courtesy of Cyrill and Wolfram]
What is “SLAM” ?
Estimate the pose and the map at the same time
SLAM is hard, because a map is needed for localization and a good pose estimate is needed for mapping
courtesy of Dirk Haehnel
SLAM: Simultaneous Localization and Mapping
Full SLAM:
Online SLAM:
Integrations typically done one at a time
),|,( :1:1:1 ttt uzmxp
121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp
Estimates entire path and map!
Estimates most recent pose and map!
Map Representations
Grid maps or scans
[Lu & Milios, `97; Gutmann, `98: Thrun `98; Burgard, `99; Konolige & Gutmann, `00; Thrun, `00; Arras, `99; Haehnel, `01;…]
Landmark-based
[Leonard et al., `98; Castelanos et al., `99: Dissanayake et al., `01; Montemerlo et al., `02;…
Path Representations
How to represent the belief about the location of the robot at a given time? Gaussian
Compact Analytical Updates Non multi-modal
Sample-based Flexible Multi-modal Poor representation of large uncertainties Past estimates cannot be refined in a straightforward
fashion
Is the Gaussian a Good Approximation?
[Stachniss et al., `07]
Techniques for Generating Consistent Maps
Incremental SLAM Gaussian Filter SLAM
Smith & Cheesman, `92
Castelanos et al., `99
Dissanayake et al., `01
…
Fast-SLAM
Haehnel, `01
Montemerlo et al., `03
Grisetti et al., ‘04
…
Full SLAM EM
Burgard et al., `99
Thrun et al., `98
Graph-SLAM
Folkesson et al.,`98
Frese et al.,`03
Howard et al.,`05
Thrun et al.,`05
Our Approach
What This Presentation is About
Estimate the Gaussian posterior of the poses in the path, given an instance of full SLAM problem.
Two Steps: Estimate the means via nonlinear
optimization (maximum likelihood) Estimate the covariance matrices via
belief propagation and covariance intersection
Graph Based Maximum Likelihood Mapping
• Goal:– Find the configuration of poses which better
explains the set of pairwise observations.
Related Work
2D approaches: Lu and Milios, ‘97 Montemerlo et al., ‘03 Howard et al., ‘03 Dellaert et al., ‘03 Frese and Duckett, ‘05 Olson et al., ‘06 Grisetti et al., ‘07
First to introduce SGD in ML mapping
First to introduce the Tree Parameterization
3D approaches: Nuechter et al., ‘05 Dellaert et al., ‘05 Triebel et al., ‘06
Problem Formulation
The problem can be described by a graph
Goal: Find the assignment of poses to the nodes of
the graph which minimizes the negative log likelihood of the observations:
nodes
Observation of from
error
Preconditioned Gradient Descent
Decomposes the overall problem into a set of simple sub-problems. Each constraint is optimized individually.
The magnitude of the correction decreases with each iteration.
A solution is found when an equilibrium is reached. Update rule for a single constraint:
Information matrixPrevious solution
[Olson et al., ’06]
residualJacobian
Hessian
Learning rateCurrent solution
Parameterizations
Transform the problem into a different space so that: the structure of the problem is exploited. the calculations become easier and faster.
Mapping function
posesparameters
transformed problem
Construct a spanning tree from the graph. The mapping function between the poses and the parameters is:
Error of a constraint in the new parameterization.
Only variables in the path of a constraint are involved in the update.
Tree Parameterization
Gradient Descent on a Tree Parameterization
Using a tree parameterization we decompose the problem in many small sub-problems which are either:• constraints on the tree (open loop)• constraints not in the tree (single loop)
Each GD equation independently solves one sub-problem at a time.
The solutions are integrated via the learning rate.
Fast Computation of the Update 3D rotations lead to a highly nonlinear
system. Update the poses directly according to the GD
equation may lead to poor convergence. This effect increases with the connectivity of
the graph. Key idea in the GD update:
Distribute a fraction of the residual along the parameters so that the error of that constraint is reduced.
Fast Computation of the Update
The “spirit” of the GD update: smoothly deform the path along the
constraints so that the error is reduced.
Distribute the rotations
Distribute the translations
Distribution of the Rotational Error
In 3D the rotational error cannot be simply added to the parameters because the rotations are not commutative.
Our goal is to find a set of rotations so that the following equality holds:
rotations along the path fraction of the rotational residual in the local frame
corrected terms for the rotations
Rotational Error in the Global Reference Frame We transfer the rotational residual to the global
reference frame
We decompose the rotational residual into a chain of incremental rotations obtained by spherical linear interpolation:
And we recursively solve the system
Simulated Experiment
Highly connected graph
Poor initial guessLU & friends fail2200 nodes8600 constraints
Real World Experiment
10km long trajectory and 3D lasers recorded with a carProblem not tractable by standard optimizers
The video is accelerated by a factor of 1/50!
Comparison with Standard Approaches (LU Decomposition)
Tractable subset of the EPFL dataset Optimization carried out in less than one second. The approach is so fast that in typical applications
one can run it while incrementally constructing the graph.
Cost of a Constraint Update
Time Comparison (2D)
Incremental Optimization
An incremental version requires to optimize the graph while it is built
The complexity increases with the size of the graph and with
the quality of the initial guess
We can limit it by using the previous solution to compute the new one.
refining only portions of the graph which may be altered by the insertion of new constraints.
performing the optimization only when needed.
dropping the information which are not valuable enough.
The problem grows only with the size of the mapped area and not with the time.
Real Example (EPFL)
Runtime
Data Association
So far we explained how to compute the mean of the distribution given the data associations.
However, to determine the data associations we need to know the covariance matrices of the nodes.
Standard approaches include: Matrix inversion Loopy belief propagation Belief propagation on spanning tree Loopy intersection propagation [Tipaldi et al. IROS 07]
Graphical SLAM as a GMRF
Factor the distribution local potentials pairwise potentials
Belief Propagation
Inference by local message passing
Iterative process Collect messages
Send messages
CB
D
A
Belief Propagation - Trees
Exact inference Message passing Two iterations
From leaves to root: variable elimination
From root to leaves: back substitution
A
C
D
B
Belief Propagation - loops
Approximation Multiple paths Overconfidence
Correlations between path A and path B
How to integrate information at D?
A
C
D
B
A
B
Covariance Intersection
Fusion rule for unknown correlations
Combine A and B to obtain C
C
A B
Loopy Intersection Propagation
Key ideas Exact inference on a
spanning tree of the graph
Augment the tree with information coming from loops
How Approximation by
means of cutting matrices
Loop information within local potentials (priors)
Approximation via Cutting Matrix Removal as matrix subtraction
Regular cutting matrixA
C
D
B
Fusing Loops with Spanning Trees
Estimate A and B
Fuse the estimates
Compute the priors
A
C
D
B
A
B
LIP – Algorithm
1. Compute a spanning tree
2. Run belief propagation on the tree
3. For every off-tree edge
1. compute the off-tree estimates,
2. compute the new priors, and
3. delete the edge
4. Re-run belief propagation
Experiments – Setup & Metrics
Simulated data Randomly
generated networks of different sizes
Real data Graph extracted
from Intel and ACES dataset from radish
Approximation error Frobenius norm
Conservativeness Smallest eigenvalue
of matrix difference
Experiments – Simulated Data
Approximation error
Conservativeness
Experiments – Real Data (Intel)
Loopy belief propagation
Spanning tree belief propagation
Overconfident Too conservative
Experiments – Real Data (Intel)
Loopy intersection propagation
Approximation Error
Conservativeness
Conclusions Novel algorithm for optimizing 2D and 3D graphs
of poses Error distribution in 2D and 3D and efficient tree
parameterization of the nodes Orders of magnitude faster than standard nonlinear
optimization approaches Easy to implement (~100 lines of c++ code) Open source implementation available at
www.openslam.org
Novel algorithm for computing the covariance matrices of the nodes Linear time complexity Tighter estimates Generally conservative
Applications to both range based and vision based SLAM.
Questions?