Fast Iterative Alignment of Pose Graphs with Poor Initial Estimates Edwin Olson eolson@mit.edu John...

Fast Iterative Alignment of Pose Fast Iterative Alignment of Pose Graphs with Poor Initial Graphs with Poor Initial

EstimatesEstimates

Edwin OlsonEdwin Olsoneolson@mit.edueolson@mit.edu

John Leonard, Seth TellerJohn Leonard, Seth Tellerjleonard@mit.edu, teller@csail.mit.edujleonard@mit.edu, teller@csail.mit.edu

http://rvsn.csail.mit.edu/graphoptimhttp://rvsn.csail.mit.edu/graphoptim

Where are we going?Where are we going?

Problem summaryProblem summary

►Robot moves around environment. Robot moves around environment. Poses are connected by constraints Poses are connected by constraints (odometry). (odometry).

Feature

Pose

Constraint

► Constraint = rigid body transformation Constraint = rigid body transformation

+ covariance matrix+ covariance matrix

Problem summaryProblem summary

►Re-observing features allows Re-observing features allows constraints between non-successive constraints between non-successive posesposes

Feature

Pose

Constraint

Problem summaryProblem summary

► Goal: find the arrangement of poses and Goal: find the arrangement of poses and features that best satisfies the constraints.features that best satisfies the constraints. (e.g., maximizes the probability)(e.g., maximizes the probability)

An improbable initial configuration Maximum likelihood configuration

Poorly satisfied constraints

SLAM

Why is this hard?Why is this hard?

► Huge number of unknownsHuge number of unknowns |x| = 10|x| = 1033

is a is a smallsmall problem problem

► Non-linear constraintsNon-linear constraints Function of robot orientationFunction of robot orientation

► Lousy initial estimatesLousy initial estimates Odometry!Odometry!

► (Today, we’ll ignore SLAM’s other (Today, we’ll ignore SLAM’s other difficult sub-problems: data difficult sub-problems: data association, exploration)association, exploration)

Cost surface from a laser-scan matching problem. Many local

maxima/minima

Our WorkOur Work

►We present an algorithm for optimizing We present an algorithm for optimizing pose graphspose graphs Very fastVery fast Estimates non-linear maximum likelihoodEstimates non-linear maximum likelihood

►Unlike EKF, EIF and friendsUnlike EKF, EIF and friends

Works well even with poor initial Works well even with poor initial estimatesestimates

Under 100 lines of codeUnder 100 lines of code

A peek at resultsA peek at results

Gauss-Seidel, 60 sec.

Multi-Level Relaxation, 8.6 sec.

Our method, 2.8 sec.

Noisy (simulated) input:

3500 poses

3499 temporal constraints

2100 spatial constraints

Ground Truth

Our Method: OverviewOur Method: Overview

►Marginalize out featuresMarginalize out features

►Use “incremental” state spaceUse “incremental” state space

►Enforce constraints seriallyEnforce constraints serially Linearize constraint to global incremental Linearize constraint to global incremental

constraintconstraint Taking smaller steps as t→∞Taking smaller steps as t→∞

Marginalizing FeaturesMarginalizing Features

Marginalize features, leaving only trajectoryMarginalize features, leaving only trajectory

Problem now involves only inter-pose constraintsProblem now involves only inter-pose constraints ThenThen compute features (trivial, as in FastSLAM) compute features (trivial, as in FastSLAM)

Marginalize

Marginalization: ConsMarginalization: Cons

►Con:Con: More edges in graph More edges in graph Feature with N observations leads to O(NFeature with N observations leads to O(N22))

edgesedges Slower/harder to solveSlower/harder to solve

► (Information Matrix less sparse)(Information Matrix less sparse)

Marginalize

Marginalization: Not so bad?Marginalization: Not so bad?

► Pro:Pro: Pose-pose edges better Pose-pose edges better model data inter-model data inter-dependencedependence ““This cloud of points matches This cloud of points matches

this cloud of points”this cloud of points”► Individual point associations are Individual point associations are

not not independent. independent. More correct to use a single More correct to use a single

“lumped” constraint“lumped” constraint► Bonus: Makes it easier to undo Bonus: Makes it easier to undo

bad data associations laterbad data associations later

Observations at t=5

Observations at t=500

Data association

Lumped Constraint

Iterative OptimizationIterative Optimization

► Do forever:Do forever: Pick a constraintPick a constraint Descend in direction of constraint’s gradientDescend in direction of constraint’s gradient

► Scale gradient magnitude by Scale gradient magnitude by alpha/iterationalpha/iteration► Clamp step sizeClamp step size

iteration++iteration++

► alpha/iterationalpha/iteration→0 as t→∞→0 as t→∞

► Robustness to local concavitiesRobustness to local concavities Hop around the state space, “stick” in the best oneHop around the state space, “stick” in the best one

► Good solution very fast, “perfect” solution only as Good solution very fast, “perfect” solution only as t→∞ t→∞

Importance of state spaceImportance of state space

► Choice of state Choice of state representation affects representation affects gradientgradient

► Pick a state space Pick a state space that:that: Is computationally Is computationally

efficientefficient Interacts well with Interacts well with

optimization algorithmoptimization algorithm

1

2

3

5

67

6

constraint

error

Previous work: global state Previous work: global state spacespace

► Global state: Global state: xx = [ x = [ x0 0 yy0 0 θ θ00 x x11 y y1 1 θ θ1 1 x x22 y y2 2 θ θ2 2 … ]… ]TT

► Constraint between pose a and b =Constraint between pose a and b = f f ( (xxaa,y,yaa,θ,θaa,x,xbb,y,ybb,θ,θbb)) Gradient = 0 for all but poses a and bGradient = 0 for all but poses a and b

6

► Slow convergenceSlow convergence► Error for some links can go up dramaticallyError for some links can go up dramatically

1

2

3

4

5

67

1

2

3

4

57

constraint

error6

Step direction

State Space: State Space: Relative/IncrementalRelative/Incremental

► Robot motion is cumulativeRobot motion is cumulative Adjustment of one pose affects neighborsAdjustment of one pose affects neighbors

► Each constraint affects multiple nodesEach constraint affects multiple nodes

1

23

4

5

67

6

1

2

3

4

5

76

► Faster convergenceFaster convergence► Error decreases more predictablyError decreases more predictably

Step direction

Relative versus IncrementalRelative versus Incremental

► Relative (prior work):Relative (prior work): State vector: rigid-body State vector: rigid-body

transformationstransformations

PP3 3 = P= P00TT11TT22TT33

More realisticMore realistic ProjectiveProjective

► Small adjustments can have Small adjustments can have large effectslarge effects

► InstabilityInstability O(N) time to compute updateO(N) time to compute update

12

3

4

5

76

► Incremental (contribution):Incremental (contribution): State vector: global-relative State vector: global-relative

motionsmotionsPP3 3 = P= P00 + D + D11 + D + D22 + D + D33

Less realistic (inexact)Less realistic (inexact) Behaves linearlyBehaves linearly

► Small adjustments have Small adjustments have small effectssmall effects

► Good convergence propertiesGood convergence properties O(log N) to compute updateO(log N) to compute update

Algorithm ComplexityAlgorithm Complexity

► For N poses, M constraints:For N poses, M constraints:

► Memory: O(N+M)Memory: O(N+M) 1M poses, 2M constraints → 176MB1M poses, 2M constraints → 176MB EKF/EIF: O(NEKF/EIF: O(N22))

► Time:Time: Impose constraint: O(log N)Impose constraint: O(log N)

► (using incremental state space and tree-based data structure)(using incremental state space and tree-based data structure) One full iteration: O(M log N)One full iteration: O(M log N) Convergence: hard to make guarantees (nonlinear Convergence: hard to make guarantees (nonlinear

problem)problem)► Δx bounded at each time step, but ∫ Δx dt might not beΔx bounded at each time step, but ∫ Δx dt might not be► In practice, SLAM problems fairly well behaved after a few In practice, SLAM problems fairly well behaved after a few

iterationsiterations

Gauss Seidel RelaxationGauss Seidel Relaxation(exponential time scale)(exponential time scale)

Our algorithmOur algorithm(slowed down by ~4x)(slowed down by ~4x)

Multi-Level RelaxationMulti-Level Relaxation

► Optimization Optimization result after 8.6 result after 8.6 secondsseconds

► Converges to Converges to (very) good result (very) good result in 30 minutesin 30 minutes

► Thanks to Udo Frese for running Thanks to Udo Frese for running this data setthis data set

Killian Court (Real Data)Killian Court (Real Data)

Laser-scan derived open-loop estimate

Continuing work…Continuing work…

► Several 3D implementations under waySeveral 3D implementations under way Both with and without full-rank constraintsBoth with and without full-rank constraints

► Incremental (not batch) versionIncremental (not batch) version

► Adaptive learning rateAdaptive learning rate

► Reports of success with our algorithm from Reports of success with our algorithm from other universities…other universities…

Questions?Questions?

►Updated paper, movies, source code:Updated paper, movies, source code: http://rvsn.csail.mit.edu/graphoptimhttp://rvsn.csail.mit.edu/graphoptim