This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
Fast Iterative Alignment of Pose Fast Iterative Alignment of Pose Graphs with Poor Initial Graphs with Poor Initial
►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
► (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:
► (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
► 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…