The GraphSLAM Algorithm

1. The GraphSLAM Algorithm Daniel Holman CS 5391: AI Robotics March 12, 2014

2. Introduction • The SLAM Problem • Given a robot’s control signals and observations of nearby features, calculate a map of the features and robot path • Full SLAM Problem • Calculate the entire path of the robot, or all poses from time 1 to t rather than simply the pose at time t • GraphSLAM provides a solution to the offline full SLAM problem

3. GraphSLAM • GraphSLAM- Describes the SLAM problem as a sparse graph • Each node in the graph represents a robot pose or a feature ofthe map • Each edge in the graph corresponds to a nonlinear constraint of either the motion or measurement models

4. EKFSlam vs. GraphSLAM • EKFSlam • Online, Proactive • As a filter, only maintain posterior pose at t • No time dependence for memory allocation • Represents information though mean vector and covariance matrix • Best estimate of robot pose and map • Updating covariance is computationally expensive, particularly for large maps • GraphSLAM • Offline, Batch, “lazy” • Estimates posterior for all poses 1:t • Graph increases linearly over time • Represents information as soft constraints, less expensive to compute • Adds an additional inference phase in which information is transformed to estimate state • Can revise past data association, multiple linearization: produce more accurate maps

5. Sum of all constraints is nonlinear least squares problem Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

6. Inference • Map and path are obtained from linearized information matrix and information vector • If each feature is only seen locally once, the graph represented by constraints is linear and thus can be reordered as a band-ordered diagonal matrix • Realistically, features are seen at multiple time steps, computing full inverse of can be expensive

7. Inference • Factorization Trick – variable elimination algorithm • Want to remove from and • Let be the set of all poses at which was observed • Set links between and to zero and introduce new link between poses • Update for all poses • Can now safely remove from and • Results in smaller and , reducing inference problem into a smaller one • Robot path can be recovered

8. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

9. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

10. Feature Recovery • Build a new information matrix and information vector for each • Both defined over and • Contains original links between and , but poses are set to values in , without uncertainty • From this information form, can calculate location of using common matrix inversion trick

11. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

12. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

13. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

14. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

15. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

16. Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic robotics. MIT press, 2005.

17. Conclusion • Addresses full SLAM problem: calculates posteriors for full robot path along with map • Constructs a sparse graph of nonlinear constraints between poses and sensed features, and motion commands into soft constraints between consecutive poses • Performs inference by mapping the graph into an isomorphic information matrix and vector, defined over all pose variables and the entire map • Construct linear information form, reduction of the form to remove the map, and solves resulting optimization problem over robot poses