Fast Iterative Alignment of Pose Graphs with Poor Initial Estimates

1. Fast Iterative Alignment of Pose Graphs with Poor Initial Estimates Edwin Olson eolson@mit.edu John Leonard, Seth Teller jleonard@mit.edu, teller@csail.mit.edu http://rvsn.csail.mit.edu/graphoptim

2. Where are we going? + =

3. Feature Pose Constraint Problem summary • Robot moves around environment. Poses are connected by constraints (odometry). • Constraint = rigid body transformation + covariance matrix

4. Feature Pose Constraint Problem summary • Re-observing features allows constraints between non-successive poses

5. Problem summary • Goal: find the arrangement of poses and features that best satisfies the constraints. • (e.g., maximizes the probability) Poorly satisfied constraints SLAM An improbable initial configuration Maximum likelihood configuration

6. Why is this hard? • Huge number of unknowns • |x| = 103is a small problem • Non-linear constraints • Function of robot orientation • Lousy initial estimates • Odometry! • (Today, we’ll ignore SLAM’s other difficult sub-problems: data association, exploration) Cost surface from a laser-scan matching problem. Many local maxima/minima

7. Our Work • We present an algorithm for optimizing pose graphs • Very fast • Estimates non-linear maximum likelihood • Unlike EKF, EIF and friends • Works well even with poor initial estimates • Under 100 lines of code

8. A peek at results Noisy (simulated) input: 3500 poses 3499 temporal constraints 2100 spatial constraints Ground Truth Gauss-Seidel, 60 sec. Multi-Level Relaxation, 8.6 sec. Our method, 2.8 sec.

9. Our Method: Overview • Marginalize out features • Use “incremental” state space • Enforce constraints serially • Linearize constraint to global incremental constraint • Taking smaller steps as t→∞

10. Marginalizing Features • Marginalize features, leaving only trajectory • Problem now involves only inter-pose constraints • Then compute features (trivial, as in FastSLAM) Marginalize

11. Marginalization: Cons • Con: More edges in graph • Feature with N observations leads to O(N2)edges • Slower/harder to solve • (Information Matrix less sparse) Marginalize

12. Marginalization: Not so bad? • Pro: Pose-pose edges better model data inter-dependence • “This cloud of points matches this cloud of points” • Individual point associations are not independent. • More correct to use a single “lumped” constraint • Bonus: Makes it easier to undo bad data associations later Observations at t=5 Observations at t=500 Data association Lumped Constraint

13. Iterative Optimization • Do forever: • Pick a constraint • Descend in direction of constraint’s gradient • Scale gradient magnitude by alpha/iteration • Clamp step size • iteration++ • alpha/iteration→0 as t→∞ • Robustness to local concavities • Hop around the state space, “stick” in the best one • Good solution very fast, “perfect” solution only as t→∞

14. 2 3 1 constraint 5 7 6 error 6 Importance of state space • Choice of state representation affects gradient • Pick a state space that: • Is computationally efficient • Interacts well with optimization algorithm

15. Previous work: global state space • Global state: x = [ x0 y0 θ0 x1 y1 θ1 x2 y2 θ2 … ]T • Constraint between pose a and b = f (xa,ya,θa,xb,yb,θb) • Gradient = 0 for all but poses a and b 2 2 3 3 1 1 4 4 constraint 5 5 7 7 6 error 6 6 • Slow convergence • Error for some links can go up dramatically Step direction

16. 2 3 1 4 5 7 6 State Space: Relative/Incremental • Robot motion is cumulative • Adjustment of one pose affects neighbors • Each constraint affects multiple nodes 2 3 1 4 5 7 6 6 • Faster convergence • Error decreases more predictably Step direction

17. 2 3 1 4 5 7 6 Relative versus Incremental • Relative (prior work): • State vector: rigid-body transformations P3 = P0T1T2T3 • More realistic • Projective • Small adjustments can have large effects • Instability • O(N) time to compute update • Incremental (contribution): • State vector: global-relative motions P3 = P0 + D1 + D2 + D3 • Less realistic (inexact) • Behaves linearly • Small adjustments have small effects • Good convergence properties • O(log N) to compute update

18. Algorithm Complexity • For N poses, M constraints: • Memory: O(N+M) • 1M poses, 2M constraints → 176MB • EKF/EIF: O(N2) • Time: • Impose constraint: O(log N) • (using incremental state space and tree-based data structure) • One full iteration: O(M log N) • Convergence: hard to make guarantees (nonlinear problem) • Δx bounded at each time step, but ∫ Δx dt might not be • In practice, SLAM problems fairly well behaved after a few iterations

19. Gauss Seidel Relaxation(exponential time scale)

20. Our algorithm(slowed down by ~4x)

21. Optimization result after 8.6 seconds Converges to (very) good result in 30 minutes Thanks to Udo Frese for running this data set Multi-Level Relaxation

22. Killian Court (Real Data) Laser-scan derived open-loop estimate

23. Continuing work… • Several 3D implementations under way • Both with and without full-rank constraints • Incremental (not batch) version • Adaptive learning rate • Reports of success with our algorithm from other universities…

24. Questions? • Updated paper, movies, source code: • http://rvsn.csail.mit.edu/graphoptim