Efficient Sparse Pose Adjustment for 2D Mapping
Kurt Konolige
Willow Garage
Menlo Park, CA 94025
Email: konolige@willowgarage.com
Giorgio Grisetti, Rainer K
¨
ummerle,
Wolfram Burgard
University of Freiburg
Freiburg, Germany
Email: grisetti@informatik.uni-freiburg.de
Benson Limketkai, Regis Vincent
SRI International
Menlo Park, CA 94025
Email: regis.vincent@sri.com
Abstract— Pose graphs have become a popular representation
for solving the simultaneous localization and mapping (SLAM)
problem. A pose graph is a set of robot poses connected by
nonlinear constraints obtained from observations of features
common to nearby poses. Optimizing large pose graphs has
been a bottleneck for mobile robots, since the computation
time of direct nonlinear opt imization can grow cubically with
the size of the graph. In this paper, we propose an efficient
method for constructing and solving the linear subproblem,
which is the bottleneck of these direct methods. We compare our
method, called Sparse Pose Adjustment (SPA), with competing
indirect methods, and show that it outperforms them in terms of
convergence speed and accuracy. We demonstrate its effectiveness
on a large set of indoor real-world maps, and a very large
simulated dataset. Open-source implementations in C++, and the
datasets, are publicly available.
I. INTRODUCTION
The recent literature in robot mapping shows an increasing
interest in graph-based SLAM approaches. In the most general
form, the graph has nodes that represent both robot poses
and world features, with measurements connecting them as
constraints. The goal of all approaches is to jointly opti-
mize the poses of the nodes so as to minimize the error
introduced by the constraint. One classical variant of this
problem comes from computer vision and is denoted as Bundle
Adjustment [25], which is typically solved with a specialized
variant of the Levenberg-Marquardt (LM) nonlinear optimizer.
In the SLAM literature, Lu-Milios [18], GraphSLAM [24], and
√
SAM [4] are all variants of this technique.
Since features tend to outnumber robot poses, more compact
systems can be created by converting observations of fea-
tures into direct constraints among the robot poses, either by
marginalization [1, 24, 4], or by direct matching – for example,
matching laser scans between two robot poses yields a relative
pose estimate for the two. Pose constraint systems, in typical
robotic mapping applications, exhibit a sparse structure of
connections, since the range of the sensor is typically limited
to the vicinity of the robot.
Solving pose graphs efficiently (i.e., finding the optimal
positions of the nodes) is a key problem for these methods
especially in the context of online mapping problems. A
typical 2D laser map for a 100m x 100m office space may
have several thousand nodes and many more constraints (see
Figure 1). Furthermore, adding a loop closure constraint to
this map can affect almost all of the poses in the system.
Fig. 1. Large MIT corridor map, unoptimized (left) and optimized (right).
A full nonlinear optimization of this map (3603 nodes and 4986 constraints),
starting from the odometry positions of the left-side figure, takes just 150 ms
with our method.
At the heart of the LM method lies the solution of a large
sparse linear problem. In this paper, we develop a method
to efficiently compute the sparse matrix from the constraint
graph, and use direct sparse linear methods to solve it. In
analogy to Sparse Bundle Adjustment in the vision literature,
we call this method Sparse Pose Adjustment (SPA), since it
deals with the restricted case of pose-pose constraints. The
combination of an SBA/GraphSLAM optimizer with efficient
methods for solving the linear subproblem has the following
advantages.
• It takes the covariance information in the constraints into
account which leads to more accurate solutions.
• SPA is robust and tolerant to initialization, with very
low failure rates (getting stuck in local minima) for both
incremental and batch processing.
• Convergence is very fast as it requires only a few itera-
tions of the LM method.
• Unlike EKF and information filters, SPA is fully non-
linear: at every iteration, it linearizes all constraints
around their current pose.
• SPA is efficient in both batch and incremental mode.
We document these and other features of the method in
the experimental results section where we also compare our
method to other LM and non-LM state-of-the-art optimizers.
One of the benefits of the efficiency of SPA is that a
mapping system can continuously optimize its graph, pro-
viding the best global estimate of all nodes, with very little
评论0