Square Root SAM: Frank Dellaert College of Computing Georgia Institute of Technology
Square Root SAM: Frank Dellaert College of Computing Georgia Institute of Technology
Frank Dellaert
College of Computing
Georgia Institute of Technology
I. I NTRODUCTION
The problem of simultaneous localization and mapping
(SLAM) [1][3] has received considerable attention in mobile
robotics as it is one way to enable a robot to explore, map, and
navigate in previously unknown environments. The traditional
approach in the literature is to phrase the problem as an
extended Kalman filter (EKF), with the robot pose and static
landmarks as the evolving filter state [2], [4], [5]. It is well
known that the computational requirements of the EKF become impractical fairly quickly, once the number of landmarks
in the environment grows beyond a few hundred. As a result,
many authors have looked at ways to reduce the computation
associated with the EKF, using both approximating [6][8] and
non-approximating [9] algorithms.
In this paper we propose that square root information
smoothing (SRIS) is a fundamentally better approach to the
problem of SLAM than the EKF, based on the realization that,
in contrast to the extended Kalman filter covariance or
information matrix, which both become fully dense over
time [7], [8], the information matrix I associated with
smoothing is and stays sparse;
in typical mapping scenarios this matrix I or, alternatively, the measurement matrix A, are much more
compact representations of the map covariance structure
I or A, both sparse, can be factorized efficiently using
sparse Cholesky or QR factorization, respectively, yielding a square root information matrix R that immediately
yields the optimal robot trajectory and map;
Factoring the information matrix is known in the sequential estimation literature as square root information filtering (SRIF).
the state of the robot at the ith time step by xi , with i 0..M ,
a landmark by lj , with j 1..N , and a measurement by zk ,
with k 1..K. The joint probability model corresponding to
this network is
P (X, M, Z) =
P (x0 )
M
Y
P (xi |xi1 , ui )
i=1
x0
z1
x1
z2
l1
x2
z3
...
...
z4
l2
...
xM
zK
lN
Fig. 2.
Bayesian belief network representation of the SLAM problem.
The state x of the robot is governed by a Markov chain, on top, and the
environment of the robot is represented at the bottom by a set of landmarks
l. The measurements z, in the middle layer, are governed both by the state
of the robot and the parameters of the landmark measured.
K
Y
k=1
(3)
(4)
(5)
Jkjk
where
and
are respectively the Jacobians of hk (.)
with respect to a change in xik and ljk , and are evaluated at
the linearization point (x0ik , lj0k ):
jk hk (xik , ljk )
ik hk (xik , ljk )
Hk =
Jk =
0 0
xik
ljk
(x0 ,l0 )
(x ,l )
ik
jk
ik
jk
i=1
K
X
k=1
M
X
(7)
k=1
kek2 = eT 1 e = (T /2 e)T (T /2 e) =
T /2 e
2
1/2
(8)
i=1
K
X
with
the matrix square root of , we can always
eliminate i from (7) by pre-multiplying Fii1 , Gii , and ai in
T /2
each term with i
. A similar story holds for the matrices
k , where for scalar measurements this simply means dividing
each term by the measurement standard deviation. Below we
assume that this has been done and drop the Mahalanobis norm
in favor of the regular 2-norm.
G11
F21 G22
2
3
F
G
3
3
1
1
H
J
A=
1
1
H21
J
2
H32
J31
H43
J42
Above the top half describes the robot motion, and the
bottom half the measurements. A mixture of landmarks and/or
measurements of different types (and dimensions) is easily
accommodated. Note that the non-zero blocks pattern of
the measurement part is also the adjacency matrix for the
measurement part of a belief net like the one in Figure 2.
(9)
This is normally done by Cholesky factorization of the information matrix I, defined and factorized as follows:
I = AT A = RT R
(10)
(11)
1 Some treatments, including [19], define the Cholesky triangle as the lowertriangular matrix G = RT , but the other convention is more convenient here.
(a)
(b)
(c)
Fig. 4. a) The graph of the Markov random field of the associated SLAM problem from Figure 1. b) The triangulated graph: each edge corresponds to a nonzero in the Cholesky triangle R. c) The corresponding elimination tree showing how the state and landmarks estimates will be computed via back-substitution:
the root is computed first - in this case a landmark near the top left - after which the computation progresses further down the tree.
SAM
M
200
500
1000
N
180
500
1280
2000
180
500
1280
2000
180
500
1280
2000
none
0.031
0.034
0.036
0.037
0.055
0.062
0.068
0.07
0.104
0.109
0.124
0.126
ldl
0.062
0.062
0.068
0.07
0.176
0.177
0.175
0.181
0.401
0.738
0.522
0.437
chol
0.092
0.094
0.102
0.104
0.247
0.271
0.272
0.279
0.523
0.945
0.746
0.657
mfqr
0.868
1.19
1.502
1.543
2.785
3.559
5.143
5.548
10.297
12.112
14.151
15.914
qr
1.685
1.256
1.21
1.329
11.92
8.43
6.348
6.908
42.986
77.849
35.719
25.611
A. Batch SAM
We have experimented at length with different implementations of Cholesky, LDL, and QR factorization to establish
which performed best. All simulations were done in MATLAB
on a 2GHz. Pentium 4 workstation running Linux. Experiments were run in synthetic environments like the one shown
in Figure 5, with 180 to 2000 landmarks, for trajectories of
length 200, 500, and 1000. Each experiment was run 10 times
for 5 different methods:
none: no factorization performed
ldl: Davis sparse LDL factorization [24]
chol: MATLAB built-in Cholesky factorization
mfqr: multifrontal QR factorization [23]
qr: MATLAB built in QR factorization
The results are summarized in Figure 6. We have found that,
under those circumstances,
1) The freely available sparse LDL implementation by T.
Davis [24] beats MATLABs built-in Cholesky factorization by about 30%.
B. Incremental SAM
We also
compared the performance of an incremental version of SAM, described in Section VI, with a standard EKF
implementation by simulating 500 time steps in a synthetic
environment with 2000 landmarks. The results are shown in
Figure 10. The factorization of I was done using sparse LDL
[24], while for the column ordering we used symamd [22], a
version of colamd for symmetric positive definite matrices.
Smoothing every time step becomes cheaper than the EKF
when the number of landmarks N reaches 600. At the end,
with N = 1, 100, each factorization took about 0.6 s., and the
slope is nearly linear over time. In contrast, the computational
requirements of the EKF increase quadratically with N , and
by the end each update of the EKF took over a second.
As implementation independent measures, we have also
plotted N 2 , as well as the number of non-zeros nnz in the
Cholesky triangle R. The behavior of the latter is exactly
opposite to that of the EKF: when new, unexplored terrain
is encountered, there is almost no correlation between new
features and the past trajectory and/or map, and nnz stays
almost constant. In contrast, the EKFs computation is not
affected when re-entering previously visited areas -closing the
loop- whereas that is exactly when R fill-in occurs.
We have reason to belief that these results can be further improved: profiling of our implementation showed that roughly
2/3 of the time is spent updating I, which is an artifact of
our sparse matrix representation. A compressed row scheme
would probably increase the speed by another factor of 2. In
addition, incremental QR updating methods should be much
faster than doing full factorizations every time.
VIII. C ONCLUSION
In conclusion, we believe square root information smoothing to be of great practical interest to the SLAM community. It
recovers the entire trajectory and is exact, and even the decidedly sub-optimal incremental scheme we evaluated behaves
Fig. 7.
Original information matrix I and its Cholesky triangle. Note the dense fill-in on the right, linking the entire trajectory to all landmarks.
Fig. 8. Information matrix I after reordering and its Cholesky triangle. Reordering of columns (unknowns) does not affect the sparseness of I, but the
number of non-zeroes in R has dropped from approximately 2.8 million to about 250 thousand.
Fig. 9. By doing the reordering while taking into account the special block-structure of the SLAM problem, the non-zero count can be eliminated even
further, to about 130K, a reduction by a factor 20 with respect to the original R, and substantially less than the 500K entries in the filtering covariance matrix.
1200
12
x 10
1
0.9
1000
10
800
600
400
0.8
0.7
0.6
0.5
0.4
0.3
200
0.2
0.1
50
100
150
200
250
300
350
400
450
500
50
100
150
200
250
300
350
400
450
500
50
100
150
200
250
300
350
400
450
500
Fig. 10. Timing results for incremental SAM in a simulated environment with 2000 landmarks, similar to the one in Figure 5, but 10 blocks on the side. As
the number of landmarks seen increases, the EKF becomes quadratically slower. Note that the number of non-zeros nnz increases faster when large loops
are encountered around i = 200 and i = 350.
[7] M. Paskin, Thin junction tree filters for simultaneous localization and
mapping, in Intl. Joint Conf. on Artificial Intelligence (IJCAI), 2003.
[8] S. Thrun, Y. Liu, D. Koller, A. Ng, Z. Ghahramani, and H. DurrantWhyte, Simultaneous localization and mapping with sparse extended
information filters, Intl. J. of Robotics Research, vol. 23, no. 7-8, pp.
693716, 2004.
[9] J. Guivant and E. Nebot, Optimization of the simultaneous localization
and map building algorithm for real time implementation, IEEE Trans.
Robot. Automat., vol. 17, no. 3, pp. 242257, June 2001.
[10] G. Bierman, Factorization methods for discrete sequential estimation,
ser. Mathematics in Science and Engineering. New York: Academic
Press, 1977, vol. 128.
[11] P. Maybeck, Stochastic Models, Estimation and Control. New York:
Academic Press, 1979, vol. 1.
[12] K. Murphy, Bayesian map learning in dynamic environments, in
Advances in Neural Information Processing Systems (NIPS), 1999.
[13] U. Frese, P. Larsson, and T. Duckett, A multilevel relaxation algorithm
for simultaneous localisation and mapping, IEEE Trans. Robototics,
vol. 21, no. 2, pp. 196207, April 2005.
[14] J. Folkesson, P.Jensfelt, and H. I. Christensen, Graphical slam using
vision and the measurement subspace, in IEEE/RSJ Intl. Conf. on
Intelligent Robots and Systems (IROS), 2005.
[15] R. G. Cowell, A. P. Dawid, S. L. Lauritzen, and D. J. Spiegelhalter,
Probabilistic Networks and Expert Systems, ser. Statistics for Engineering and Information Science. Springer-Verlag, 1999.
[16] J. Dennis and R. Schnabel, Numerical methods for unconstrained
optimization and nonlinear equations. Prentice-Hall, 1983.
[17] R. Smith, M. Self, and P. Cheeseman, A stochastic map for uncertain
spatial relationships, in Int. Symp on Robotics Research, 1987.
[18] J. Leonard and H. Durrant-Whyte, Simultaneous map building and
localization for an autonomous mobile robot, in IEEE Int. Workshop
on Intelligent Robots and Systems, 1991, pp. 14421447.
[19] G. Golub and C. V. Loan, Matrix Computations, 3rd ed. Baltimore:
Johns Hopkins University Press, 1996.
[20] G. Winkler, Image analysis, random fields and dynamic Monte Carlo
methods. Springer Verlag, 1995.
[21] J. Yedidia, W. Freeman, and Y.Weiss, Generalized belief propagation,
in Advances in Neural Information Processing Systems (NIPS), 2000,
pp. 689695.
[22] P. R. Amestoy, T. Davis, and I. S. Duff, An approximate minimum
degree ordering algorithm, SIAM Journal on Matrix Analysis and
Applications, vol. 17, no. 4, pp. 886905, 1996.
[23] P. Matstoms, Sparse QR factorization in MATLAB, ACM Trans. Math.
Softw., vol. 20, no. 1, pp. 136159, 1994.
[24] T. A. Davis, Algorithm 8xx: a concise sparse Cholesky factorization
package, Univ. of Florida, Tech. Rep. TR-04-001, January 2004,
submitted to ACM Trans. Math. Software.