1 of 17

Autonomous Mobile Manipulation

State Estimation: SLAM Graph Optimization

C. Papachristos

Robotic Workers (RoboWork) Lab

University of Nevada, Reno

CS-791

2 of 17

Simultaneous Localization And Mapping

 

CS791 C. Papachristos

3 of 17

Simultaneous Localization And Mapping

Remember: SLAM – Probabilistic Formulation

​

Graphical Representation of Dynamic Bayesian Network of the SLAM process:

​

CS791 C. Papachristos

4 of 17

Simultaneous Localization And Mapping

SLAM – with Full Graph Optimization

​

Take account of full system of nonlinear equations:

    • Minimize total least-squares cost function�(e.g. reprojection error)
    • Use a batch Max-Likelihood approach
    • Assume Gaussian noise densities

​

Core Principle

  • Find configuration of all nodes that minimizes the negative log-likelihood of all observations

​

Advantages

    • Future Information “moves backward” in time
    • Best (most likely) estimate given all data and models

​

Disadvantages

    • Computationally demanding
    • Real time implementation “out-of-the box” challenging

CS791 C. Papachristos

5 of 17

Simultaneous Localization And Mapping

SLAM – with Full Graph Optimization

​

Implementation practices for robotic applications:

​

  • Bisected into Front-end / Back-end
    • SLAM “Back-end” optimization focuses on computing the best map given the encoded constraints
    • SLAM “Front-end” seeks to interpret incoming sensor data to obtain the constraints

​

  • Front-end
    • Perform data association (e.g. landmark detection, matching across frames, across longer sequences, …)
    • Handle outlier rejection
    • Compute constraint terms (e.g. reprojection error)

​

  • Back-end
    • Detect loop-closures
    • Solve linearized problem by exploiting factorizations made possible via the resulting sparse matrix structure
    • Potentially leverage variable elimination techniques (e.g. leveraging marginalization on multivariate Gaussians)

CS791 C. Papachristos

6 of 17

Simultaneous Localization And Mapping

 

 

 

 

 

PDF

Log-Likelihood

Information Matrix

 

 

 

 

PDF

Log-Likelihood

Information Matrix

(Leverage Log-Likelihood form of Gaussians to recast posterior problem of PDF Products into Log-Likelihood sum)

 

CS791 C. Papachristos

7 of 17

Simultaneous Localization And Mapping

SLAM – with Full Graph Optimization

​

Core Approach:

​

  • Measurement Constraints:

​

    • “Spring-mass”-like terms of magnitude:

​

  • Transition Constraints:

​

    • “Spring-mass”-like terms of magnitude:

​

  • Full Graph-SLAM Problem

​

    • Batch Function of (negative)�Log-Likelihood:

​

 

 

 

 

 

Initial “Anchoring” Constraint,

Attention: Configuration of all graph nodes

 

 

CS791 C. Papachristos

8 of 17

Simultaneous Localization And Mapping

 

 

 

 

 

 

 

 

 

CS791 C. Papachristos

9 of 17

Simultaneous Localization And Mapping

 

 

I.e. solve:

 

Configuration of all graph nodes

All quadratic terms

All linear terms

Constant terms

By solving (linear) system:

 

 

 

 

 

CS791 C. Papachristos

10 of 17

Simultaneous Localization And Mapping

SLAM – with Full Graph Optimization

​

Core Approach:

​

  • Linear Problem to solve:

​

(of corresponding Information Form: )

​

  • Information Form construction is possible to do Incrementally:

​

  • Information�is additive:

 

 

 

 

 

 

 

 

 

Initial “Anchoring”

Add all�Motion / Control�Information

Then Add all�Observation�Information

For every ith constraint

(Linearized)�Information Matrix

(Linearized)�Information Vector

CS791 C. Papachristos

11 of 17

Simultaneous Localization And Mapping

SLAM – with Full Graph Optimization

​

An inspection of the Information Matrix structure:

​

  • Measurement Constraints:

​

​

​

  • How a landmark observation is�introduced as a constraint:

​

​

 

Dependence Graph

Dependence Graph

Information Matrix

CS791 C. Papachristos

12 of 17

Simultaneous Localization And Mapping

SLAM – with Full Graph Optimization

​

An inspection of the Information Matrix structure:

​

  • Transition Constraints:

​

​

​

  • How robot motion is�introduced as a constraint:

​

​

 

Dependence Graph

Information Matrix

CS791 C. Papachristos

13 of 17

Simultaneous Localization And Mapping

SLAM – with Full Graph Optimization

​

An inspection of the Information Matrix structure:

​

  • After incorporating a sequence of motion and observation constraints … :

​

​

​

  • Sparsity of information matrix:

​

​

Dependence Graph

Information Matrix

CS791 C. Papachristos

14 of 17

Simultaneous Localization And Mapping

 

 

 

 

 

Solution corresponds to:

Path-only posterior:

We have

 

CS791 C. Papachristos

15 of 17

Simultaneous Localization And Mapping

 

a)

b)

c)

 

 

 

 

 

 

 

CS791 C. Papachristos

16 of 17

Simultaneous Localization And Mapping

 

 

posterior over path only

 

 

 

Is block-diagonal,�we can decompose

For each jth landmark

We have:

 

 

 

 

 

 

 

CS791 C. Papachristos

17 of 17

Time for Questions !

CS-791

CS791 C. Papachristos