Fast Incremental Square Root Information Smoothing

1 Introduction

The problem of simultaneous localization and mapping (SLAM) has received considerable attention in mobile robotics, as it is one way to enable a robot to explore and navigate previously unknown environments. For a wide range of applications, such as commercial robotics, search-and-rescue and reconnaissance, the solution must be incremental and real-time in order to be useful. The problem consists of two components: a discrete correspondence and a continuous estimation problem. In this paper we focus on the continuous part, while keeping in mind that the two components are not completely independent, as information from the estimation part can simplify the correspondence problem.

∗This work was supported in part by the National Science Foundation award IIS - 0448111 "CAREER: Markov Chain Monte Carlo Methods for Large Scale Correspondence Problems in Computer Vision and Robotics".

Current real-time approaches to SLAM are typically based on filtering. While filters work well for linear problems, they are not suitable for most real-world problems, which are inherently non-linear [Julier and Uhlmann, 2001]. The reason for this is well known: Marginalization over previous poses bakes linearization errors into the system in a way that cannot be undone, leading to unbounded errors for large-scale applications. But repeated marginalization also complicates the problem by making the naturally sparse dependencies between poses and landmarks, which are summarized in the information matrix, dense. Sparse extended information filters [Thrun et al., 2005] and thin junction tree filters [Paskin, 2003] present approximations to deal with this complexity.

Smoothing approaches to SLAM recover the complete robot trajectory and the map, avoiding the problems inherent to filters. In particular, the information matrix is sparse and remains sparse over time, without the need for any approximations. Even though the number of variables increases continuously for smoothing, in many real-world scenarios, especially those involving exploration, the information matrix still contains far less entries than in filter based methods [Dellaert, 2005]. When repeatedly observing a small number of landmarks, filters seem to have an advantage, at least when ignoring the linearization issues. However, the correct way of dealing with this situation is to switch to localization after a map of sufficient quality is obtained.

In this paper, we present a fast incremental smoothing approach to the SLAM problem. We revisit the underlying probabilistic formulation of the smoothing problem and its equivalent non-linear optimization formulation in Section 2. The basis of our work is a factorization of the smoothing information matrix, that is directly suitable for performing optimization steps. Rather than recalculating this factorization in each step from the measurement equations, we incrementally update the factorized system whenever new measurements become available, as described in Section 3. This fully exploits the natural sparsity of the SLAM problem, resulting in constant time updates for exploration tasks. We apply variable reordering similar to [Dellaert, 2005] in order to keep the factored information matrix sparse even when closing multiple loops. All linearization choices can be corrected at any time, as no variables are marginalized out. From our factored representation, we can apply back-substitution at any time to obtain the complete map and trajectory in linear time. We evaluate the incremental algorithm based on the standard Victoria Park dataset in Section 4.

As our approach is incremental, it is suitable for real-time applications. In contrast, many other smoothing approaches like GraphSLAM [Thrun et al., 2005] perform batch processing, solving the complete problem in each step. Even though explicitly exploiting the structure of the SLAM problem can yield quite efficient batch algorithms [Dellaert, 2005], for large-scale applications an incremental solution is needed. Examples of such systems that incrementally add new measurements are Graphical SLAM [Folkesson and Christensen, 2004] and multi-level relaxation [Frese et al., 2005]. However, these and similar solutions have no efficient way of obtaining the marginal covariances needed for data association. In Section 5 we describe how our approach allows access to the exact marginal covariances without a full matrix inversion, and how to obtain a more efficient conservative estimate.

2 SLAM and Smoothing

In this section we review the formulation of the SLAM problem in a smoothing framework, following the notation of [Dellaert, 2005]. In contrast to filtering methods, no marginalization is performed, and all pose variables are retained. We describe the underlying probabilistic model of this full SLAM problem, and show how inference on this model leads to a least-squares problem.

2.1 A Probabilistic Model for SLAM

We formulate the SLAM problem in terms of the belief network model shown in Figure 2. We denote the robot state 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 is given by

where P(x0) is a prior on the initial state, P(xi|xi-1, ui) is the motion model, parametrized by the control input ui, and P (zk|xik , ljk ) is the landmark measurement model, assuming known correspondences (ik, jk) for each measurement zk.

As is standard in the SLAM literature, we assume Gaussian process and measurement models. The process model xi = fi(xi-1, ui) + wi describes the robot behavior in response to control input, where wi is normally distributed zero-mean process noise with covariance matrix Λi. The Gaussian measurement equation zk = hk(xik , ljk )+vk models the robot's sensors, where vk is normally distributed zeromean measurement noise with covariance Σk.

2.2 SLAMas a Least Squares Problem

In this section we discuss how to obtain an optimal estimate for the set of unknowns given all measurements available to us. As we perform smoothing rather than filtering, we are interested in the maximum a posterior (MAP) estimate for the entire trajectory X = {xi} and the map of landmarks L = {lj}, given the measurements Z = {zk} and the control inputs U = {ui}. Let us collect all unknowns in X and L in the vector Θ = (X, L). The MAP estimate Θ∗ is then obtained by minimizing the negative log of the joint probability P (X, L, Z) from equation (1):

Combined with the process and measurement models, this leads to the following non-linear least-squares problem

where we use the notation ||e||2Σ = eT Σ-1 e for the squared Mahalanobis distance given a covariance matrix Σ.

In practice one always considers a linearized version of this problem. If the process models fi and measurement equations hk are non-linear and a good linearization point is not available, non-linear optimization methods solve a succession of linear approximations to this equation in order to approach the minimum. We therefore linearize the least-squares problem by assuming that either a good linearization point is available or that we are working on one iteration of a non-linear optimization method, see [Dellaert, 2005] for the derivation:

where H ik k , J jk k are the Jacobians of hk with respect to a change in xik and ljk respectively, F i-1 i the Jacobian of fi at xi-1, and Gii = I for symmetry. ai and ck are the odometry and observation measurement prediction errors, respectively.

We combine all summands into one least-squares problem after dropping the covariance matrices Λi and Σk by pulling their matrix square roots inside the Mahalanobis norms:

For scalar matrices this simply means dividing each term by the measurement standard deviation. We then collect all Jacobian matrices into a single matrix A, the vectors ai and ck into a right hand side vector b, and all variables into the vector θ, to obtain the following standard least-squares problem:

3 Updating a Factored Representation

We present an incremental solution to the full SLAM problem based on updating a matrix factorization of the measurement Jacobian A from (6). For simplicity we initially only consider the case of linear process and measurement models. In the linear case, the measurement Jacobian A is independent of the current estimate θ. We therefore obtain the least squares solution θ∗ in (6) directly from the standard QR matrix factorization [Golub and Loan, 1996] of the Jacobian A ∈ Rm×n:

where Q ∈ Rm×m is orthogonal, and R ∈ Rn×n is upper triangular. Note that the information matrix I Δ= AT A can also be expressed in terms of this factor R as I = RT R, which we therefore call the square-root information matrix R. We rewrite the least-squares problem, noting that multiplying with the orthogonal matrix QT does not change the norm:

where we defined [d, e]T Δ= QT b. The first term ||Rθ - d||2 vanishes for the least-squares solution θ∗ , leaving the second term ||e||2 as the residual of the least-squares problem.

The square-root factor allows us to efficiently recover the complete robot trajectory as well as the map at any given time. This is simply achieved by back-substitution using the current factor R and right hand side (rhs) d to obtain an update for all model variables θ based on

Note that this is an efficient operation for sparse R, under the assumption of a (nearly) constant average number of entries per row in R. We have observed that this is a reasonable assumption even for loopy environments. Each variable is then obtained in constant time, yielding O(n) time complexity in the number n of variables that make up the map and trajectory. We can also restrict the calculation to a subset of variables by stopping the back-substitution process when all variables of interest are obtained. This constant time operation is typically sufficient unless loops are closed.

3.1 Givens Rotations

One standard way to obtain the QR factorization of the measurement Jacobian A is by using Givens rotations [Golub and Loan, 1996] to clean out all entries below the diagonal, one at a time. As we will see later, this approach readily extends to factorization updates, as will be needed to incorporate new measurements. The process starts from the bottom left-most non-zero entry, and proceeds either column- or row-wise, by applying the Givens rotation:

to rows i and k, with i > k. The parameter φ is chosen so that the (i, k) entry of A becomes 0, as shown in Figure 3. After all entries below the diagonal are zeroed out, the upper triangular entries will contain the R factor. Note that a sparse measurement Jacobian A will result in a sparse R factor. However, the orthogonal rotation matrix Q is typically dense, which is why this matrix is never explicitly stored or even formed in practice. Instead, it is sufficient to update the rhs b with the same rotations that are applied to A.

When a new measurement arrives, it is cheaper to update the current factorization than to factorize the complete measurement Jacobian A. Adding a new measurement row wT and rhs γ into the current factor R and rhs d yields a new system that is not in the correct factorized form:

Note that this is the same system that is obtained by applying Givens rotations to eliminate all entries below the diagonal, except for the last (new) row. Therefore Givens rotations can be determined that zero out this new row, yielding the updated factor R′ . In the same way as for the full factorization, we simultaneously update the right hand side with the same rotations to obtain d′ . In general, the maximum number of Givens rotations needed to add a new measurement is n. However, as R and the new measurement rows are sparse, only a constant number of Givens rotations are needed. Furthermore, new measurements typically refer to recently added variables, so that often only the right most part of the new measurement row is (sparsely) populated. An example of the locality of the update process is shown in Figure 1.

It is easy to add new landmark and pose variables to the QR factorization, as we can just expand the factor R by the appropriate number of zero columns and rows, before updating with the new measurement rows. Similarly, the rhs d is augmented by the same number of zero entries.

For an exploration task in the linear case, the number of rotations needed to incorporate a set of new landmark and odometry measurements is independent of the size of the trajectory and map, as the simulation results in Figure 4 show. Our algorithm therefore has O(1) time complexity for exploration tasks. Recovering all variables after each step requires O(n) time, but is still very efficient even after 10000 steps, at about 0.12 seconds per step. However, only the most recent variables change significantly enough to warrant recalculation, providing a good approximation in constant time.

3.2 Loops and Variable Reordering

Loops can be dealt with by a periodic reordering of variables. A loop is a cycle in the trajectory that brings the robot back to a previously visited location. This introduces correlations between current poses and previously observed landmarks, which themselves are connected to earlier parts of the trajectory. Results based on a simulated environment with multiple loops are shown in Figure 5. Even though the information matrix remains sparse in the process, the incremental updating of the factor R leads to fill-in. This fill-in is local and does not affect further exploration, as is evident from the example. However, this fill-in can be avoided by variable reordering, as has been described in [Dellaert, 2005]. The same factor R after reordering shows no signs of fill-in. However, reordering of the variables and subsequent factorization of the new measurement Jacobian itself is also expensive when performed in each step. We therefore propose fast incremental updates interleaved with occasional reordering, yielding a fast algorithm as supported by the dotted blue curve in Figure 5(d).

Figure 5: For a simulated environment consisting of an 8-loop that is traversed twice (a), the upper triangular factor R shows significant fill-in (b), yielding bad performance (d, continuous red). Some fill-in occurs when the first loop is closed (A). Note that this has no negative consequences on the subsequent exploration along the second loop until the next loop closure occurs (B). However, the fill-in then becomes significant when the complete 8-loop is traversed for the second time, with a peak when visiting the center point of the 8-loop for the third time (C). After variable reordering, the factor matrix again is completely sparse (c). Reordering after each step (d, dashed green) can be less expensive in the case of multiple loops. A considerable increase in efficiency is achieved by using fast incremental updates interleaved with periodic reordering (d, dotted blue), here every 100 steps.

When the robot continuously observes the same landmarks, for example by remaining in one small room, this approach will eventually fail, as the information matrix itself will become completely dense. However, in this case filters will also fail due to underestimation of uncertainties that will finally converge to 0. The correct solution to deal with this scenario is to eventually switch to localization.

3.3 Non-linear Systems

Our factored representation allows changing the linearization point of any variable at any time. Updating the linearization point based on a new variable estimate changes the measurement Jacobian. One way then to obtain a new factorization is to refactor this matrix. For the results presented here, we combine this with the periodic reordering of the variables for fill-in reduction as discussed earlier.

However, in many situations it is not necessary to perform relinearization [Steedly et al., 2003]. Measurements are typically fairly accurate on a local scale, so that relinearization is not needed in every step. Measurements are also local and only affect a small number of variables directly, with their effects rapidly declining while propagating through the constraint graph. An exception is a situation such as a loop closing, that can affect many variables at once. In any case it is sufficient to perform selective relinearization only over variables whose estimate has changed by more than some threshold. In this case, the affected measurement rows are first removed from the current factor by QR-downdating [Golub and Loan, 1996], followed by adding the relinearized measurement rows by QR-updating as described earlier.

4 Results

We have applied our approach to the Sydney Victoria Park dataset (available at http://www.acfr.usyd.edu.au/ homepages/academic/enebot/dataset.htm), a popular test dataset in the SLAM community. The trajectory consists of 7247 frames along a trajectory of 4 kilometer length, recorded over a time frame of 26 minutes. 6968 frames are left after removing all measurements where the robot is stationary. We have extracted 3631 measurements of 158 landmarks from the laser data by a simple tree detector.

The final optimized trajectory and map are shown in Figure 6. For known correspondences, the full incremental reconstruction, including solving for all variables after each new frame is added, took 337s (5.5 minutes) to calculate on a Pentium M 2 GHz laptop, which is significantly less than the 26 minutes it took to record the data. The average calculation time for the final 100 steps is 0.089s per step, which includes a full factorization including variable reordering, which took 1.9s. This shows that even after traversing a long trajectory with a significant number of loops, our algorithm still performs faster than the 0.22s per step needed for real-time. However, we can do even better by selective reordering based on a threshold on the running average over the number of Givens rotations, and back-substitution only every 10 steps, resulting in an execution time of only 116s. Note that this still provides good map and trajectory estimates after each step, due to the measurements being fairly accurate locally.

5 Covariances for Data Association

Our algorithm is helpful for performing data association, which generally is a difficult problem due to noisy data and the resulting uncertainties in the map and trajectory estimates. First it allows to undo data association decisions, but we have not exploited this yet. Second, it allows for recovering the underlying uncertainties, thereby reducing the search space.

In order to reduce the ambiguities in matching newly observed features to already existing landmarks, it is advantageous to know the projection Ξ of the combined pose and landmark uncertainty Σ′ into the measurement space:

where H is the Jacobian of the projection process, and Γ measurement noise. This requires knowledge of the marginal covariances

between the current pose mi and any visible landmark xj, where Σij are the corresponding blocks of the full covariance matrix Σ = I -1 . Calculating this covariance matrix in order to recover all entries of interest is not an option, because it is always completely populated with n2 entries.

Our representation allows us to retrieve the exact values of interest without having to calculate the complete dense covariance matrix, as well as to efficiently obtain a conservative estimate. The exact pose uncertainty Σii and the covariances Σij can be recovered in linear time. By design, the most recent pose is always the last variable in our factor R - if variable reordering is performed, it is done before the next pose is added. Therefore, the dim(mi) last columns X of the full covariance matrix (RT R)-1 contain Σii as well as all Σij, as observed in [Eustice et al., 2005]. But instead of having to keep an incremental estimate of these entries, we can retrieve the exact values efficiently from the factor R by backsubstitution. We define B as the last dim(mi) unit vectors and solve RT RX = B by two back-substitutions RT Y = B and RX = Y . The key to efficiency is that we never have to recover a full dense matrix, but due to R being upper triangular immediately obtain Y = [0, ..., 0, R-1 ii ]T . Hence only one back-substitution is needed to recover dim(mi) dense vectors, which only requires O(n) time due to the sparsity of R.

Conservative estimates for the structure uncertainties Σjj can be obtained as proposed by [Eustice et al., 2005]. As the covariance can only shrink over time during the smoothing process, we can use the initial uncertainties as conservative estimates. A more tight conservative estimate can be obtained after multiple measurements are available.

Recovering the exact structure uncertainties Σjj is more tricky, as they are spread out along the diagonal. As the information matrix is not band-diagonal in general, this would seem to require calculating all entries of the fully dense covariance matrix, which is infeasible. Here is where our factored representation, and especially the sparsity of the factor R are useful. Both, [Golub and Plemmons, 1980] and [Triggs et al., 2000] present an efficient method of recovering exactly all entries of the covariance matrix that coincide with nonzero entries in the factor R. As the upper triangular parts of the block diagonals of R are fully populated, and due to symmetry of the covariance matrix, this algorithm provides access to all blocks on the diagonal that correspond to the structure uncertainties Σjj . The inverse Z = (AT A)-1 is obtained based on the factor R in the standard way by noting that AT AZ = RT RZ = I, and performing two backsubstitutions RT Y = I and RZ = Y . As the covariance matrix is typically dense, this would still require calculation of O(n2 ) elements. However, we exploit the fact that many entries obtained during back-substitution of one column are not needed in the calculation of the next column. To be exact, only the ones are needed that correspond to non-zero entries in R, which leads to:

for l = n, . . . , 1 and i = l - 1, . . . , 1. Note that the summations only apply to non-zero entries of single columns or rows of the sparse R matrix. The algorithm therefore has O(n) time complexity for band-diagonal matrices and matrices with only a constant number of entries far from the diagonal, but can be more expensive for general sparse R.

6 Conclusion

We presented a new SLAM algorithm that overcomes the problems inherent to filters and is suitable for real-time applications. Efficiency arises from incrementally updating a factored representation of the smoothing information matrix. We have shown that at any time the exact trajectory and map can be retrieved in linear time. We have demonstrated that our algorithm is capable of solving large-scale SLAM problems in real-time. We have finally described how to obtain the uncertainties needed for data association, either exact or as a more efficient conservative estimate.

An open research question at this point is if a good incremental variable ordering can be found, so that full matrix factorizations can be avoided. For very large-scale environments it seems likely that the complexity can be bounded by approaches similar to submaps or multi-level representations, that have proven successful in combination with other SLAM algorithms. We plan to test our algorithm in a landmark-free setting, that uses constraints between poses instead, as provided by dense laser matching.