(Georgia Institute of Technology, 2005)
Kipp, Alexander; Krauthausen, Peter; Dellaert, Frank
We present an extension of a smoothing approach to Simultaneous Localization and Mapping (SLAM). We have previously introduced Square-Root SAM, a Smoothing and Mapping approach to SLAM based on Levenberg-Marquardt (LM) optimization. It iteratively finds the optimal nonlinear least squares solution (ML), where one iteration comprises of a linearization step, a matrix factorization, and a back-substitution step. We introduce a submap parametrization which enables a rigid transformation of parts relative to each other during the optimization process. This parameterization is used in a multifrontal QR factorization approach, in which we partially fix the linearization point for a subset of the unknowns corresponding to sub-maps. This greatly accelerates the optimization of an entire SAM graph yet yields an exact solution.
(Georgia Institute of Technology, 2005)
Krauthausen, Peter; Dellaert, Frank; Kipp, Alexander
The problem of creating a map given only the erroneous odometry and feature measurements and locating the own position in this environment is known in the literature as the Simultaneous Localization and Mapping (SLAM) problem. In this paper we investigate how a Nested Dissection Ordering scheme can improve the the performance of a recently proposed Square Root Information Smoothing (SRIS) approach. As the SRIS does perform smoothing rather than filtering the SLAM problem becomes the Smoothing and Mapping problem (SAM). The computational complexity of the SRIS solution is dominated by the cost of transforming a matrix of all measurements into a square root form through factorization. The factorization of a fully dense measurement matrix has a cubic complexity in the worst case. We show that the computational complexity for the factorization of typical measurement matrices occurring in the SAM problem can be bound tighter under reasonable assumptions. Our work is motivated both from a numerical/linear algebra standpoint as well as by submaps used in EKF solutions to SLAM.