Fast and Accurate PoseSLAM by Combining Relative and Global State Spaces

Fast and Accurate PoseSLAM by Combining Relative and Global State SpacesBrian Peasley and Stan BirchfieldMicrosoft RoboticsClemson University

PoseSLAMProblem: Given a sequence of robot poses and loop closure(s), update the poses

dab = (dxab,dyab,dqab)

pa=(xa,ya,qa)error caused by sensor drift{loop closure edgestartendpb=(xb,yb,qb)Initial (noisy) pose estimatesFinal pose estimatesUpdaterelative measurementbetween a and b:pose at a:pose at b:Video

Key: Formulate as graphNodes = posesEdges = relative pose measurements

Solution: Minimize an objective function

where

PoseSLAM as graph optimizationtotalerrorresidual for single edgeinfo matrixobservedmeasurementpredictedby modelstate

Question: How does state xrelate to poses p?PoseSLAM as nonlinear minimizationWith a little math,

becomes

JacobiancurrentstateestimateincrementtostateestimateAx=bRepeatedly solve this linear system till convergenceUpdate based on single edge

Drawback:Requires many iterations*

*With global state spacewhere is learning rateSolve the equation directly

Drawback:Solving equation requires external sparse linear algebra package (e.g., CHOLMOD)where is preconditioning matrixUse sparse linear algebraPerform gradient descent

Example: TORO(Olson et al. ICRA 2006; Grisetti et al. RSS 2007)Example: g2o(Kummerle et al. ICRA 2011)How to solve linear system?Choosing the state spaceGlobal state space (GSS)allows fine adjustmentsIncremental state space (ISS)simple Jacobian, decoupled parameters,slow convergence Relative state space (RSS)simple Jacobian, coupled parameters,fast convergence

(Olson et al. ICRA 2006)(our approach)

statepose

Global state space (GSS)Global State Space (x, y, ) Incremental state space (ISS)Incremental State Space (x, y, ) Relative state space (RSS)Relative State Space (x, y, ) Proposed approachTwo Phases:Non-Stochastic Gradient Descent with Relative State Space (POReSS)RSS allows many poses to be affected in each iterationnSGD prevents being trapped in local minimaResult: Quickly gets to a good solutionDrawback: Long time to convergence

Gauss-Seidel with Global State Space (Graph-Seidel)Initialize using output from POReSSGSS only changes one pose at a time in each iterationGauss-Seidel allows for quick calculationsResult: Fast fine tuningPose Optimization using a Relative State Space (POReSS)nSGD only considers one edge at a time:ISS a+1 b

RSS (between consecutive nodes) a+1 b(between non-consecutive nodes)How edges affect states0123x0x1x2x3e01e12e23e30e01e12e23e03e01 affects x1e12 affects x2e23 affects x3e03 affects x0, x1, x2, x3

Gauss-SeidelAxbRepeat: Linearize about current estimate, then solve Ax=b. A is updated each time

Graph-SeidelAxb

Do not linearize! Instead assume qs are constant A remains constantRefining the estimate using a Global State Space (Graph-Seidel)

Form linear systemCompute new stateUsing Graph-SeidelRefining the estimate using a Global State Space (Graph-Seidel)Results: Manhattan world

On Manhattan World, our approach isfaster and more powerful than TOROfaster and comparable with g2o

Performance comparison

(Manhattan world dataset)(corrupted square dataset)Residual vs. timeTime vs. size of graphOur approach combines the Minimization capability of g2o (with faster convergence)Simplicity of TORO

On parking lot data (Blanco et al. AR 2009), our approach is faster and more powerful than TOROmore powerful than g2oResults: Parking lot

Graph-Seidel

Results: Simple square

Rotational version (rPOReSS)Majority of drift is caused by rotational errorsRemove rotational driftTreat (x,y) components of poses as constantsGreatly reduces complexity of Jacobian derivation

Results of rPOReSS: Intel dataset

Incremental version (irPOReSS)Incremental:Rotational POReSS can be run incrementallyGoal is to keep graph close to optimizedRun Graph-Seidel whenever a fully optimized graph is desired(Note: Further iterations of POReSS will undo fine adjustments made by Graph-Seidel)Video

ConclusionIntroduced a new method for graph optimization for loop closureTwo phases:POReSS uses relative state spacenon-stochastic gradient descent fast but rough convergenceGraph-Seidel does not linearize about current estimateinstead assumes orientation constant very fast iterations, refines estimateNo linear algebra needed! ~100 lines of C++ codeNatural extensions:rotationalincrementalFuture work:Landmarks3D Thanks!Video