Boosting the Performance of Iterative Closest Point ...

Boosting the Performance of Iterative Closest Point Scan Matching Algorithms using Quad Trees Bayu A. Slamet∗ , Arnoud Visser∗ , Max Pfingsthorn† ∗ Universiteit

van Amsterdam, Kruislaan 403, 1098 SJ Amsterdam, The Netherlands University Bremen, Campus Ring 1, 28759 Bremen, Germany

† Jacobs

Abstract— This paper presents a boosting approach to improve the robustness and accuracy of iterative closest point (ICP) scan matching algorithms. The focus is on reducing the correspondence error by providing a more complete and denser frame of reference to match new scans. Using our approach a new scan is matched simultaneously against all relevant scans observed in the past. By making use of the efficient quad tree data structure this can be realized with the preservation of real-time performance. In our experiments we illustrate the significant gains in robustness and accuracy that can be the result when the presented boosting framework is applied.

I. I NTRODUCTION Robot motion estimation is a central topic in probabilistic robotics research. The general idea is that ’raw’ motion estimates provided by odometry sensors or inertial navigation systems (INS) accumulate significant error and therefore preclude an accurate spatial interpretation of sensor information, e.g. for the purpose of mapping, planning or navigation. By establishing correspondences between current and past observations these motion estimates can be corrected. As laser range scanners usually provide rather accurate measurements of a robot’s physical surroundings they are a key enabler for more accurate displacement estimation based on such a correspondence analysis. Algorithms that estimate displacement by comparing the current laser scan with one or more previously acquired laser scans are typically referred to as scan matchers. While scan matching algorithms usually indeed yield more accurate motion estimates, they do not resolve the fact that all robot observations remain correlated through the robot’s trajectory. This implies that every estimate builds on the previous one. Hence, the estimates may still diverge incrementally from the robot’s true motions in the long run. This fundamental issue lies at the core of simultaneous localization and mapping (SLAM) research. As such, many advanced techniques that aim to detect and correct error accumulation have been put forward by SLAM researchers. Particle filters [1], explicit loop closure strategies [2], Kalman filters [3] and information filters [4], elaborate exploration strategies that explicitly take the localization and mapping accuracy into account [5] and highly advanced data structures [6] to facilitate these algorithms are just some examples, see also [7] for a survey on several of these techniques and [8] for a more in-depth discussion. Although these SLAM techniques have proved to be very effective in achieving their objective, they usually only come

into action after the error already accumulated. In this paper we will present a novel idea that strikes at the heart of the problem: we aim to boost the robustness of the scan matcher itself, so that there are significantly more accurate estimates to start with. An analysis of the physical phenomena involved reveals three sources for uncertainty that complicate the challenge for scan matchers [9]. The measurement noise and sensor bias are the well known sources of uncertainty. In [10] it is demonstrated that the accuracy of the scan matching improves considerably when the remaining correspondence error is explicitly taken into account as third source of uncertainty. Our approach focuses on further reducing the correspondence error by involving all relevant scans observed in the past in the matching process. Quad trees [11] are used to make this feasible in real time. After a survey of related work and the description of our algorithm, we will show in several experiments that our innovation enables ICP to yield significantly more accurate estimates, up to the point that they are comparable to the corrections of advanced SLAM methods. II. R ELATED W ORK In [10] we used the local submaps introduced in [6] that also aim to provide a richer frame of reference for matching new scans. In its original form the reference scan (scan A in Fig. 1) was extended with all measurements from a few scans in the neighborhood. The extent of the local submap is limited to a few scans because the computational effort of the scan matching algorithm grows quadratically with the number of points considered. The technique presented in this paper could be seen as an efficient variant of the local submap that considers as matching candidates only a few measurements in the neighborhood derived from a data structure that stores all previous scans. In [12] the authors present a technique for specific use with really sparse sensor readings that is based on multiscans. A single multiscan then accumulates sparse sensor readings until the integrated scan is dense enough. This scan is then matched against features in the environment and a particle filter is used to maintain multiple hypotheses. In our approach the quad tree also integrates multiple scans into a denser frame of reference, but the matching process is point based (not dependent on the presence of particular features

(a) Odometry Estimate

(b) Scan Matcher Estimate

(c) Correspondence Error

Fig. 1. Two scans A (red) and B (green). a) The second scan B 0 at an (erroneously) estimated location, B shows the true location of the second scan. b) The scans describe the same geometry after matching. c) The residual correspondence error after matching.

in the environment). Also, we present results that did not (yet) take advantage of particle filtering. Quad trees seem a natural extension to occupancy grid maps as they efficiently index the occupancy information and thereby enable their fast retrieval. However, only few robotics applications benefit from their potential so far. Efficient path planning was pursued in [13]. The performance boost possible with scan matching is already recognized for higher dimensions. Oct trees and advanced kd-tree search algorithms are for instance used in [14] for 3D point clouds. Oct trees enable [15] to maintain a particle filter with 5000 particles in real-time for an underwater vehicle. III. I TERATIVE C LOSEST P OINT S CAN M ATCHING An iterative closest point (ICP) scan matcher generally iterates a least squares minimization procedure that is based on the well known Expectation-Maximization (EM) framework, see [16] for an overview and taxonomy on the steps involved. Figure 1 illustrates the fundamental idea behind ICP. Consider two poses θA and θB and the scans A and B acquired at these poses, then ∆θAB = (τ, ρ) denotes the displacement with translation τ and rotation ρ. Let us further define pA and pB as the projections of the individual scan beams on the local coordinate frames defined by the poses θA and θB respectively, then cA ⊂ pA and cB ⊂ pB are the subsets holding the points that are deemed correlated. ICP then aims to find the optimal match between cA and cB by finding the estimate ∆θˆAB = (ˆ τ , ρˆ) that minimizes the error  defined as:

 where R =

 = cA − R · cB − τˆ (1)  cos ρˆ − sin ρˆ is the rotation matrix for sin ρˆ cos ρˆ

ρˆ. Like any algorithm that is based on EM, also ICP sometimes converges to a non-global optimum. This challenge is typically circumvented by giving the algorithm accurate initial estimates from a motion model by which the chance of getting into local optima is reduced.

One classic application of ICP to robot motion estimation is the Iterative Dual Correspondence (IDC) algorithm published in [17]. This method has been the de-facto standard for estimating the displacement of a robot based on range data for years. However, as robots enter increasingly more challenging environments, improved variations of ICP became a requirement. One approach that worked very well [10] is the Weighted Scan Matcher (WSM) published in [9]. This approach explicitly models the measurement noise and potential correspondence error for every single scan beam using zeromean Gaussians. These uncertainty measures are then used to weight the influence of point correspondences in the matching process. In the respective paper a direct comparison was made with IDC that shows that WSM is able to return more accurate displacement estimates in less iterations. In [18] we re-confirm this in a broader set of experiments that also involved the Metric-based scan matcher (MbICP) from [19]. Unfortunately, the metric proposed in [19] is non-linear and therefore does not lend itself well for boosting using quad trees. In this paper we will limit ourselves to experiments based on IDC and WSM. However, the presented improvement is not specific to these variants of ICP and could very well be used on other ICP-based scan matchers as well. The only constraint for direct usability is that the scan matcher should use a linear distance metric for finding correspondences. Note that our approach may also be used to boost the matching of other data structures like lines and polygons. IV. B OOSTING ICP USING Q UAD T REES As a robot explores a planar environment there are several aspects that affect the range sensing process and thereby complicate the scan matching [9]. The bias error refers to the measurement bias that may be inherent to the used scanning device and the measurement error is the uncertainty attributed to the noise involved in the measurement process. For every point correspondence pair, the measurements cA and cB in Eq. (1) can thus be decomposed into the terms:

i ciA = rA + biA + δciA

(2)

i where rA is the ’true’ measurement and biA and δciA respectively denote the bias error and noise error. Note that in this paper the bias error will further be ignored as for many datasets the bias of the used sensor is unknown, and as demonstrated in [9] it has minor impact when compared to the other sources for uncertainty. The correspondence error is the consequence from a range sensor that is mounted on a moving robot. It models the fact that two corresponding points ciA ∈ cA and ciB ∈ cB do not necessarily measure the exact same physical point in reality. This phenomenon is illustrated in Fig. 1(c) where the ellipses indicate the associated correspondence error. The correspondence error between two corresponding points is given by

ξ i = ciA − R · ciB − τˆ

(3)

for a given displacement estimate (ˆ τ , ρˆ). Substituting Eq. (2) into Eq. (3) and ignoring the bias error then gives the following error ξ i per pair of corresponding points: ξi =



i i rA − R · rB − τˆ + δciA − R · δciB {z } | {z } |

Correspondence Error

(4)

M easurement Error

Combining this error for all correspondences results in the general ICP error function from Eq. (1) with the error being decomposed into a measurement error and correspondence error for every point correspondence pair involved in the matching process. The sub-problem of the measurement error was handled in [9] by modeling the uncertainty of every single scan beam with a Gaussian distribution. Also the correspondence error was addressed in [9], but leaves room for significant improvement as will be demonstrated here. The central idea is to boost ICP by using a quad tree [11] to keep an index of all relevant scans observed in the past as outlined in Alg. 1. Assume that a particular scan st acquired at time t was found as being relevant. Then all global projections of the points p ∈ st will be inserted into the quad tree Q that is maintained. The relevance of a scan is determined by simple thresholds on the estimated displacement returned by the scan matcher. Given the displacement estimate ∆θˆ for a new scan then the scan is considered relevant if either the translation threshold τmax or the rotation threshold ρmax is exceeded. Hence, these thresholds are no probabilistic measures and only serve as a simple constraint to reduce redundancy in the quad tree. Key to our approach is that the ICP scan matcher matches the new scan st against the quad tree, which means that st is matched simultaneously against all past relevant scans. This is feasible in real-time due to the excellent performance of quad trees on nearest neighbor finding. The benefit of matching a scan simultaneously against multiple previous scans is illustrated in Fig. 2 where a robot zigzags through a

Data: the current quad tree Qt and pose estimate θt , the new range scan st and the relevance thresholds τmax and ρmax Result: the possibly updated quad tree Qt+1 and the new pose estimate θt+1 ∆θˆ = (ˆ τ , ρˆ) = ICP (Qt , st ); ˆ θt+1 = θt + ∆θ; if τˆx > τmax or τˆy > τmax or ρˆ > ρmax then sglobal = the global projection of the scan st ; Qt+1 = insert all scan points of sglobal into Qt ; end Algorithm 1: Iterative ICP using a quad tree

corridor. In the example the robot is configured with a laser range scanner that has a field of view of 180 degrees, which is indicated using different shades of gray. Assume that the quad tree stored the scans acquired at time steps t1 and t2 , then two alternatives for t3 illustrate the difference. The robot could proceed with traditional incremental scan matching and only match the new scan against the one from t2 , or it can benefit from the quad tree and match simultaneously against both previous scans. The latter has two immediate benefits: the quad tree provides a denser and more complete frame of reference for matching the new scan. The density gain involves parts of the wall marked in blue that are covered by both scans of t1 and t2 . The gain in completeness involves the part of the wall marked in red that is not covered by the scan of t2 but is covered by the one of t1 .

(a) t1

(b) t2

(c) t3 incremental

(d) t3 using quad tree

Fig. 2. The benefit of using a quad tree that indexes all scans observed in the past. After the scans acquired at time-steps t1 and t2 the quad tree has a denser and more complete reference frame to offer for matching the scan of t3 . The walls shaded with blue were covered by both scans of t1 and t2 and illustrate the density gain. The part of the wall shaded with red marks reference data not present in the scan of t2 but which is covered by the quad tree that also holds the scan of t1 .

When considering the density and completeness gains illustrated in Fig. 2 one may indeed expect the correspondence error of Eq. (3) to be reduced.

Data: the quad tree Qt constructed so far and the new range scan st n Result: The correlated point pairs C = {(cA , cB ) } Qscan = temporary quad tree for all scan points p ∈ st ; C = initialize C as an empty set ∅; for each point p in st do cA = find nearest neighbor for p in Qt ; cB = find nearest neighbor for cA in Qscan ; if cB = p then C = append new correlation (cA , cB ) to C; end end

(a) Incremental IDC

(b) Q-IDC

(c) Incremental WSM

(d) Q-WSM

Algorithm 2: Quad tree-based scan correlation

V. E XPERIMENTS AND R ESULTS In our experiments we will use two implementations of the ICP algorithm; IDC [17] and WSM [9]. The pointcorrelation procedures of the original implementations were replaced with the algorithm outlined in Alg. 2 to enable matching new scans against a quad tree. We did not make any additional modifications to the internal workings of these scan matchers, so we refer the interested reader to prior research [10], [18] and the original papers for further details. The experiments will investigate the improvements that can be gained from using quad trees for both algorihtms. The visualizations were created with the standard occupancy rendering techniques from [10]. All presented results are strictly based on scan matching. In the remainder of this section we will refer to the original implementations of IDC and WSM as the incremental scan matchers and we use Q-IDC and Q-WSM to refer to the versions that benefit from quad trees using Alg. 2. A. Performance Benchmarks For these experiments we used the high-fidelity simulator [20] that is also used during the Virtual Robots competition that is part of the RoboCup Rescue World Championships 1 . Figure 3 shows the results acquired on a dataset that was recorded in ’The Park’2 . The challenge posed by this dataset is that often the range scanner only observes one or two trees and that the robot moves on slippery grass. The dataset covers an area of approximately 80 by 40 meters where the robot started in the bottom-right corner and traverses the park in clockwise direction. The robot’s path is shaded with gray for clarity and should describe a single closed loop from tip to tail. Both incremental scan matchers accumulate significant error, IDC ’overshoots’ the end of the loop and WSM leaves a gap of several meters. Using the presented boosting approach both IDC and WSM close the loop implicitly. In Fig. 4.a-b the average correlation distances that remain after matching are plotted. Both Q-IDC and Q-WSM 1 USARSim source code and manuals are available online at the project website: http://www.sourceforge.net/projects/usarsim 2 Outdoor area of the DM compWorldDay1 map

Fig. 3. Comparison of scan matching algorithms for a drive through a park, with poor odometry and sparse range scans.

(a) residual correlation distance, (b) residual correlation distance, Q-IDC in front of incremental Q-WSM in front of incremental IDC WSM

(c) matching certainty, Q-IDC (d) matching certainty, Q-WSM scaled with respect to incremen- scaled with respect to incremental IDC tal WSM Fig. 4. Detailed comparison of the performance of the quad tree-based and incremental scan matchers for a drive through a park. The top row gives the residual correlation distance. The x-axis represents the time line defined by the 800 scans that comprised this dataset and the y-axis plots for each scan the average correlation distance that remains after applying the estimated displacement. The bottom row indicates the matching certainty returned by the scan matchers. The thicker black horizontal line in each chart marks the average.

find matches with less residual correspondence error almost throughout the dataset. Over the whole dataset the average correlation distance reduces from 9.83 mm to 4.83 mm for IDC and from 10.20 mm to 5.62 mm for WSM. To see if the reduced correlation distance also improves the accuracy of the scan matcher the uncertainty measures returned by the scan matchers were analyzed. This uncertainty measure is the full 3-by-3 covariance matrix of the Gaussian distribution over the displacement estimate and does not lend itself well for charting in its original form. Therefore we took the on-diagonal elements that describe the independent uncertainty in x and y direction and combined these into a Euclidean distance measure. The measures

C. Additional Results

(a) Incremental IDC

(b) Q-IDC

(c) Incremental WSM

(d) Q-WSM

Fig. 5. Comparison of scan matching algorithms for a drive through an office environment (Intel Lab Seattle), with many overlapping feature-rich, dense laser scans.

acquired with Q-IDC and Q-WSM were scaled with respect to their incremental versions to facilitate their comparison in Fig. 4.c-d. The average uncertainty reduced to 38% and 36% of the original values for IDC and WSM respectively. The used laser range scanner was configured at 1 Hz with a resolution of 181 beams over a field of view of 180 degrees. The time necessary to match a scan ranged from 20 to 330 milliseconds for Q-IDC and from 40 to 411 milliseconds for Q-WSM. The averages were 94 milliseconds and 128 milliseconds respectively. These results were acquired on a three-year old notebook (1.9GHz CPU and 1GB RAM) and clearly illustrate that the boosting approach does not preclude real-time performance for this dataset. B. Validation on Real-Robot Data To validate the results acquired in the simulator on data that suffers from real-world odometric errors and sensor noise we use the ’Intel Lab Seattle’ dataset3 from [21]. At some point during the exploration the robot traverses a big loop through the main corridor along which it shortly visits every room bordering on this corridor. As can be verified in Fig. 5, the incremental IDC algorithm accumulates error and is unable to maintain a good estimate of the robot’s location. Incremental WSM on the other hand performs rather well already. The boost in robustness and accuracy is clearly visible in the result of Q-IDC. Also note the sharpness of the vertical corridor in the middle for QWSM. 3 Available

on http://radish.sourceforge.net

The results in Fig. 6 serve to illustrate the general applicability of the presented boosting approach. For this purpose we employed a boosted scan matcher on several publicly available robotics datasets, some of which are also often referred in SLAM research. The ’AP Hill’ dataset [22], [23] is particularly challenging as the robot navigates all over the environment and visits some of the rooms numerous times. Yet, the boosted scan matcher manages to maintain a reasonably accurate estimate throughout the run. The ’CMU Newell Simon Hall’ is a smaller dataset also used in [12] for which Q-WSM yields a rather accurate result. The ’Edmonton Convention Center’ concerns quite a large area with lots of open spaces and ’Stanford Gates’ is a huge dataset where the robot traverses several hundreds of meters. Especially note the amount of detail that is preserved by the boosted scan matcher for these datasets, like table legs and other small obstacles. VI. C ONCLUSIONS This paper presented a boosting framework for ICP scan matchers that targets the correspondence error by providing a denser and more complete frame of reference for matching new scans. This is realized by storing all relevant scans observed in the past in a quad tree, which enables the boosted scan matcher to preserve real-time performance. In our experiments we showed that our approach can reduce the residual correspondence error and lead to increased accuracy. In several tests on real-robot data the applicability of our boosting framework was illustrated. VII. F UTURE W ORK In future work we will investigate the possibilities to acquire a complete and robust localization and mapping solution by embedding the boosted ICP scan matcher in a SLAM framework, e.g. the particle filter of [24]. Another direction for future research could be to store scans in a graph data structure as in [10] which would pave the way for explicit loop-closure algorithms, e.g. the algorithms presented in [25] or [26]. VIII. ACKNOWLEDGEMENTS Part of the research reported here is performed in the context of the Interactive Collaborative Information Systems (ICIS) project, supported by the Dutch Ministry of Economic Affairs, grant nr: BSIK03024. The authors thank Olaf Booij and Zoran Zivkovic for their useful comments. R EFERENCES [1] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “Fastslam: A factored solution to the simultaneous localization and mapping problem,” in Proceedings of the AAAI National Conference on Artificial Intelligence. Edmonton, Canada: AAAI, 2002. [2] E. Olson, J. Leonard, and S. Teller, “Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates,” in Proceedings of the IEEE International Conference on Robotics and Automation, 2006, pp. 2262–2269. [3] R. C. Smith and P. Cheeseman, “On the representation and estimation of spatial uncertainty,” International Journal of Robotics Research, vol. 5, pp. 56–68, 1986.

(a) AP Hill - from [22]

(b) CMU Newell Simon Hall - (c) Edmonton Convention (d) Stanford Gates - courtesy courtesy from Roy Center - courtesy from Roy from Gerkey

(e) AP Hill - Q-WSM

(f) CMU Newell Simon Hall - Q- (g) Edmonton Convention (h) Stanford Gates - Q-WSM WSM Center - Q-WSM

Fig. 6. Several popular datasets from Radish. Here we present results that are acquired just by running a boosted scan matcher, Q-WSM, as an iterative process, without any global optimization at the end of the process.

[4] S. Thrun, Y. Liu, D. Koller, A. Ng, Z. Ghahramani, and H. DurrantWhyte, “Simultaneous localization and mapping with sparse extended information filters.” International Journal of Robotics Research, vol. 23(7-8), 2004. [5] C. Stachniss, D. H¨ahnel, W. Burgard, and G. Grisetti, “On actively closing loops in grid-based fastslam,” Advanced Robotics, vol. 19, no. 10, pp. 1059–1080, 2005. [6] A. Howard, G. S. Sukhatme, and M. J. Matari´c, “Multi-robot mapping using manifold representations,” Proceedings of the IEEE - Special Issue on Multi-robot Systems, 2006. [7] S. Thrun, “Robotic Mapping: A Survey,” in Exploring Artificial Intelligence in the New Millenium, G. Lakemeyer and B. Nebel, Eds. Morgan Kaufmann, 2002. [8] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The MIT Press, September 2005. [9] S. T. Pfister, K. L. Kriechbaum, S. I. Roumeliotis, and J. W. Burdick, “A weighted range sensor matching algorithm for mobile robot displacement estimation,” IEEE Transactions on Robotics, 2007, (to appear). [10] M. Pfingsthorn, B. Slamet, and A. Visser, “A scalable hybrid multirobot slam method for highly detailed maps,” in Proceedings of the 11th RoboCup International Symposium, July 2007. [Online]. Available: http://www.science.uva.nl/∼arnoud/publications/ slam method.pdf [11] H. Samet, “The quadtree and related hierarchical data structures,” ACM Comput. Surv., vol. 16, no. 2, pp. 187–260, 1984. [12] K. R. Beevers and W. H. Huang, “SLAM with sparse sensing,” in ”IEEE International Conference on Robotics and Automation”, 2006, pp. 2285–2290. [13] A. Yahja, S. Singh, and A. Stentz, “An efficient on-line path planner for outdoor mobile robots operating in vast environments,” 2000. [Online]. Available: citeseer.ist.psu.edu/yahja00efficient.html [14] A. N¨uchter, K. Lingemann, and J. Hertzberg, “6d slam with cached kd-tree search,” in Robot Navigation, ser. Dagstuhl Seminar Proceedings, S. Fekete, R. Fleischer, R. Klein, and A. Lopez-Ortiz, Eds., no. 06421. Internationales Begegnungs- und Forschungszentrum fuer Informatik (IBFI), Schloss Dagstuhl, Germany, 2007, [date of citation: 2007-01-01]. [15] N. Fairfield, G. Kantor, and D. Wettergreen, “Real-time slam with octree evidence grids for exploration in underwater tunnels,” Journal

[16] [17] [18]

[19]

[20] [21]

[22]

[23] [24] [25]

[26]

of Field Robotics, vol. 24, pp. 3–21, February 2007, online publishing date. S. Rusinkiewicz and M. Levoy, “Efficient variants of the ICP algorithm,” in Third International Conference on 3D Digital Imaging and Modeling (3DIM), June 2001. F. Lu and E. Milios, “Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans,” Journal of Intelligent and Robotic Systems, vol. 18, pp. 249–275, 1997. B. Slamet and M. Pfingsthorn, “ManifoldSLAM: a Multi-Agent Simultaneous Localization and Mapping System for the RoboCup Rescue Virtual Robots Competition,” Master’s thesis, Universiteit van Amsterdam, 12 2006. J. Minguez, L. Montesano, and F. Lamiraux, “Metric-based iterative closest point scan matching for sensor displacement estimation,” IEEE Transactions on Robotics, vol. 22, no. 5, pp. 1047–1054, October 2006. [Online]. Available: http://webdiis.unizar.es/∼jminguez/ MbICP TRO.pdf S. Balakirsky, C. Scrapper, S. Carpin, and M. Lewis, “Usarsim: providing a framework for multi-robot performance evaluation,” in Proceedings of PerMIS 2006, 2006. D. H¨ahnel, D. Fox, W. Burgard, and S. Thrun, “A highly efficient fastslam algorithm for generating cyclic maps of large-scale environments from raw laser range measurements,” in Proceedings of the Conference on Intelligent Robots and Systems (IROS), 2003. A. Howard, L. E. Parker, and G. S. Sukhatme, “Experiments with large heterogeneous mobile robot team: Exploration, mapping, deployment and detection,” International Journal of Robotics Research, vol. 25, no. 5, pp. 431–447, May 2006. D. Fox, J. Ko, K. Konolige, B. Limketkai, D. Schulz, and B. Stewart, “Distributed multirobot exploration and mapping,” in Proceedings of the IEEE, vol. 94, July 2006, pp. 1325 – 1339. G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid mapping with rao-blackwellized particle filters,” IEEE Transactions on Robotics, vol. 32, pp. 16–25, 2006. G. Grisetti, C. Stachniss, S. Grzonka, and W. Burgard, “A tree parameterization for efficiently computing maximum likelihood maps using gradient descent,” in Robotics: Science and Systems, Atlanta, GA, USA, 2007, to appear. E. Olson, J. Leonard, and S. Teller, “Spatially-adaptive learning rates for online incremental slam,” in Proceedings of Robotics: Science and Systems, Atlanta, GA, USA, June 2007.