nScan-Matching: Simultaneous Matching of ... - Semantic Scholar
nScan-Matching: Simultaneous Matching of Multiple Scans and Application to SLAM Peter Biber
Wolfgang Straßer
University of T¨ubingen WSI/GRIS Sand 14, 72070 T¨ubingen, Germany Email: [email protected]
University of T¨ubingen WSI/GRIS Sand 14, 72070 T¨ubingen, Germany Email: [email protected]
Abstract— Scan matching is a popular way of recovering a mobile robot’s motion and constitutes the basis of many localization and mapping approaches. Consequently, a variety of scan matching algorithms have been proposed in the past. All these algorithms share one common attribute: They match pairs of scans to obtain spatial relations between two robot poses. In this paper we present a method for matching multiple scans simultaneously. We discuss the need for such a method and describe how the result of such a multi-scan matching can be incorporated into relation-based SLAM in the Lu and Milios style.
I. I NTRODUCTION Scan matching is a popular way of recovering a mobile robot’s motion. Given two scans recorded at two different positions, the translation and the rotation that aligns these scans give an estimate of how the robot has moved. Such an estimate can be used as an enhanced odometry, can help to close loops or can be the perceptual basis of relation-based SLAM in the Lu and Milios style [1]. Many ways to calculate such estimates have been proposed but all algorithms share on common attribute: They match pairs of scans. This paper does not add a new algorithm to the collection of existing pairwise scan matchers. Instead, we present a method for matching multiple scans simultaneously based on a pairwise scan matcher (nScan-matching). We supply theoretical arguments and simple examples to motivate nScan-matching and to show that it outperforms pairwise matching in some respects and in certain situations. We also describe how the result of such a nScan-matching can be incorporated into relationbased SLAM and show some results of a complete mapping application using nScan-matching combined with relationbased SLAM. Most algorithms for matching scans or point clouds are iterative local algorithms that require an initial estimate. Algorithms that try to do a global matching, i.e., without initial estimate (e.g. [2]) are not considered here. Local scan matching algorithms can be divided in two different classes: ICPstyle algorithms and algorithms based on directly minimizing an energy function (including correlation-based approaches). The functions to be minimized may look quite similar in both kinds of algorithms but the difference is the way this is accomplished. ICP-style algorithms iteratively perform two
steps. First, the initial estimate is applied to the data and correspondences between features like points or lines are established based on several possible criteria like spatial closeness or similarity. Second, a spatial transformation is calculated that minimizes the distance between these features. Often, but not always, this transformation can be obtained in closed form. Having obtained a new transformation it is used to update the initial estimate for the next iteration until convergence. The term ICP has been coined in [3] and important variants are [4] and [5] (see [6] for an overview of more variants on 3D ICPalgorithms). 2D variants were first described in [7] and [8]. Gutmann and Schlegel compare different approaches in [9] and more recent variants include [10]–[12]. Bengtsson investigated means to extract a covariance matrix for ICP-based approaches [13], an important prerequisite for relation-based SLAM. The other type of algorithms define energy functions that do not require explicit correspondences. Such an energy function can be directly minimized using general numerical minimization methods. The same thing may be done in the second step of ICP-like algorithms but the function to be minimized there is different in each iteration if correspondences have changed. Thus, for different estimates of the transformation the function is different. But then this means that the function that is effectively minimized is often not continuous and, generally, not differentiable. In contrast, energy functions without explicit correspondences or based on correlation principles normally are smooth and differentiable. Mostly, such methods use grids or other discretizations of the spatial domain (an exception are the Gaussian Fields of Boughorble et al. [14]) but the assumption of smoothness is still reasonable. This is similar to intensitybased registration algorithms in computer vision that assume a continuous image and obtain subpixel estimates by interpolation ( [15] and many others thereafter). Examples for these methods include [16]–[18] and our own method [19]. The nScan-matching technique proposed in this paper is built upon a pairwise scan matching technique. Basically, an energy function defined on the domain of multiple poses is built by adding the corresponding pairwise energy functions. Any scan matching technique can be used here, but methods of the second type are better suited as our method assumes the existence of first and second derivatives of the pairwise
scan matching function. A similar technique could be applied for ICP-style algorithms but this case is not handled here. We conjecture that convergence would be slower in this case, as more correspondences would change between iterations and so the function to be minimized would be even less smooth than in the pairwise case. But this has not been verified here. Often results from scan matching are used in mapping and localization algorithms. For example, relation-based SLAM [1], [20], [21] uses results from pairwise scan matching to build a graph. In this graph, nodes are scans and edges denote that there is a (pairwise) relation between the nodes that are connected by that edge. Globally consistent poses for all scans can than be obtained by minimizing an energy function defined on this graph. Relation-based SLAM is also the application of nScanmatching considered in this paper. In this context, the contribution of this paper is a motivation and an investigation of a generalization of relation-based SLAM where joint relations are established between more than two unknown poses. Methods for both calculation of these multi-pose relations by nScan-matching and their integration into the formulation of a relation-based SLAM energy function are presented and discussed. The rest of the paper is organized as follows. The next section gives a motivation for our approach. It follows section III with the basic algorithm. In section IV we investigate how the results of nScan-matching can be integrated into relationbased SLAM. Section V concludes with experimental results and open problems.
is also possible that the pairwise results contradict each other as nothing enforces these constraints.
(a) (b)
II. M OTIVATION Two observations on the relation-based approach motivate the introduction of nScan-matching. The first observation addresses the number of parameters to be estimated in the course of obtaining n globally consistent poses given n scans. With parameters we mean in the following the three components of a pose, i.e., position (x, y) and angle φ. That is, for pairwise matching we have three parameters and for matching n scans we have 3(n − 1) parameters (one pose can be set to be the origin). Consider a case where it is possible to obtain relations between all n(n− 1) pairs of poses by pairwise scan matching (here and in the following it is assumed that pairwise scan matching is symmetric in the sense that matching i to j is the same as matching j to i). Then, the number of parameters to be estimated is 3n(n − 1) (3 for each relation). As already said, the number of parameters of the real problem (estimating consistent poses for n scans) is only 3(n − 1). These 3(n − 1) parameters are obtained in the second step of the relationbased approach from the 3n(n−1) parameters that result from pairwise matching. Thus, a method for estimating the 3(n− 1) parameters directly in a single, joint estimation and without the indirection over the n(n − 1) pairwise relations would be desirable because the fact, that the 3n(n − 1) parameters of all pairwise relations can be derived from only 3(n − 1) parameters is not used. Not only are possible improvements in convergence speed given away; because this fact is ignored it
(c) Fig. 1. Pairs of scans that are difficult to match. (a) Scans that have been taken from positions far away from each other. Different point densities make matching using standard methods difficult. (b) Scans taken from opposite viewing directions. Here, occlusions pose an additional difficulty in matching these scans as wrong correspondences could be established. (c) Scans with small overlaps. All these cases are difficult for algorithms matching pairs of scans. But clearly they can provide constraints and the nScan-matching scheme proposed in this paper is able to take advantage of them.
The second observation concerns the establishment of relations itself by scan matching. Matching only two scans may be difficult, ambiguous or weakly constrained in some cases. Examples for such cases are: scans taken from positions far away from each other or with a small amount of overlap (see Fig. 1). Scan matching can also be weakly constrained in environments like corridors. But the same situation may yield an unambiguous and sharp optimum if more than two scans are matched jointly. Fig. 2 shows one easy (yet rather artificial) example. A more realistic example is shown in Fig. 3. Such a situation is likely to occur if a robot enters a room, turns around there, and leaves the room. Both observations motivate the use of nScan-matching (joint matching of multiple scans) and the next section presents the basic method.
and measures how good scan j matches to it (the lower the energy the better the match) for a relative pose d such that fd maps points from j to i. We will now use Eij to build a scan matching energy function for simultaneous matching of n scans 1..n. Let p = (x1 , y1 , φ1 , ..., xn , yn , φn )t
Fig. 2. Matching only two scans may be difficult, ambiguous or weakly constrained in some cases. In this simple example matching any two of these three shapes is ambiguous, but not jointly matching all three shapes.
be the compound vector of all poses pi = (xi , yi , φi )t . Then for an index set J ⊂ [1..n] × [1..n] E(p) = Eij (pj pi )
(3)
(4)
(i,j)∈J
is the proposed energy function.
2
3
C. Optimization The proposed energy function is optimized using Newton’s algorithm. Newton’s algorithm iteratively finds the parameters p that minimize E. Each iteration solves the following equation: t , HE ∆p = −gE
1 Fig. 3. nScan-matching is an easier problem in some cases compared to pairwise scan-matching. In this example a robot enters a room (1), turns there (2), and leaves (3). For pairwise laser scan matching, scan 1 and scan 2 are unconstrained in one dimension, as are scan 1 and scan 3. However, the combination of all three scans yields a definite optimum for scan matching.
III. N S CAN MATCHING This section presents the basic nScan-matching algorithm after introducing the notation for poses used in the rest of the paper. A. Notation We follow the notation of [1]. Let pi = (xi , yi , φi )t denote the robot’s pose at time i meaning that cos φj − sin φj xi fpi (x) = x+ (1) sin φj cos φj yi maps 2D-points x from the local coordinate frame of the robot at time i to global coordinates. Let pi and pj be two different robot’s poses. We define Dij = pi pj to be the relative pose between frames i and j, that is fDij maps points from the coordinate frame i to the coordinate frame j. The entries of Dij are given by: (xi − xj ) cos φj + (yi − yj ) sin φj pi pj = −(xi − xj ) sin φj + (yi − yj ) cos φj (2) φi − φj B. Energy function Let
Eij (d) : R3 → R
be a twice differentiable scan matching energy function (as discussed in the introduction) that takes scan i as reference
where gE is the gradient of E ∂E , gE = ∂p and HE is the Hessian of f with entries
(5)
(6)
∂2E . (7) ∂p2 The solution of this linear system is an increment ∆p that is added to the current estimate: HE =
p ← p + ∆p.
(8)
Gradient and Hessian of E can be built by summing the gradients and Hessians of the basic scan matcher Eij modified as follows. Let u be the relative pose between j and i according to the compound poses vector p: u = pj pi .
(9)
Using the chain rule of derivation, the relation between gEij (that is, the gradient of Eij with respect to the relative pose u) and its contribution to gE (that is, the gradient of Eij with respect to p) is: ∂Eij ∂Eij ∂u = , (10) ∂pi ∂u ∂pi ∂Eij ∂Eij ∂u = , (11) ∂pj ∂u ∂pj and
∂Eij =0 ∂pk
(12)
for k = i and k = j. Similarly, an entry Hab of the 3n × 3n Hessian of E gets a contribution from the Hessian HEij of summand Eij as follows: t ∂ 2 Eij ∂u ∂u ∂Eij ∂ 2 u = HEij + , (13) ∂a∂b ∂a ∂b ∂u ∂a∂b
where of course again all partial derivatives equal zero if a or b is not one of the parameters of pi or pj . These equations allow to build gE and HE from the contributions of the single scan matchers and to perform one iteration of Newton’s algorithm as follows: 1) Set gE and HE equal zero. 2) For each pair (i, j) ∈ J a) Calculate u and its first and second partial derivatives with respect to pi and pj . b) Calculate gradient and Hessian of Eij at u. c) To these, apply Eqs. 10 -13 and add the results to gE and HE . 3) Solve Eq. 5 for ∆p. D. Number of parameters to estimate Recovering consistent poses for n scans clearly has only 3(n − 1) parameters as the problem is invariant against any global transformation of all poses. Two ways exist to deal with this during optimization. The first possibility is to fix the pose of one scan and estimate the remaining n − 1 poses. This approach may lead to different solutions for different fixed scans. Besides that, there will be no uncertainty estimate for the pose of the fixed scan (because it is fixed) and uncertainty will be increasing with distance to the first scan. So the solution may be biased. The second possibility is to, nevertheless, estimate all n poses simultaneously. The remaining degrees of freedom can be handled by choosing in every iteration the smallest update ∆p. This can be achieved using standard methods (solving for ∆p using a singular value decomposition of H and setting the inverses of the three smallest singular values to 0 when inverting H). We have observed a better convergence behavior for the second method, not dramatically but clearly observable, and therefore this method is recommended. However, in section V the uncertainty of the result will be needed with respect to a selected (fixed) scan. Such an estimate can then be obtained by recalculation of the Hessian at the obtained solution but keeping the selected scan fixed. E. Integrating prior knowledge Most mobile robots are equipped with an odometry providing them with estimates of self motion. Also other sensors and algorithms may be available for additional motion estimates or constraints (for example, from computer vision algorithms). Such data can not only be used to provide initial estimates for scan matching as it is often done. It can also be used directly as a prior for guiding scan matching. If there is a probabilistic model for the additional input and the scan matching function is a negative log likelihood, then integration is straightforward. Let E(x) be the scan matcher and p(x) the probabilistic model (for example for odometry) then E(x) − log p(x)
(14)
is the function that should be minimized. Again it is also possible to integrate such a modification in ICP-like algorithms
but it is much more straightforward in approaches based on an explicit energy function. F. Implementation and example We use our scan matching algorithm [19] as basic scan matching function. Optimization is performed by using our standard Newton algorithm implementation containing several step selection methods, e.g., a trust-region approach, as described in [22]. Figure 4 shows an example where ten scans are nScan-matched using odometry as initial estimate. Initial estimates are much better in typical applications like the relation-based SLAM application we describe later, and there the algorithm typically converges in five to ten iterations (but this may heavily depend on the underlying basic scan matcher). However, for closing loops such gross errors are realistic. Fig. 5 shows such an example. Six scans are nScanmatched in a “closing-the-loop” situation. In both examples the index set J contains all pairs. These results (and also all other results shown in this paper) are based on scans taken with a SICK LMS 200.
Fig. 4. Here ten scans are nScan-matched. Top: Initial estimate from odometry. Bottom: Result after convergence (30 iterations, 267 function evaluations). Circles show the robot positions where the scans have been taken.
IV. I NTEGRATION INTO RELATION - BASED SLAM Standard relation-based SLAM assumes a set I = {(i, j)| scan i and scan j have been matched successfully} with pij as estimate for the spatial relations between scan i and j and Cij as covariance matrix describing the uncertainty of this estimate. Let the poses in the global coordinate frame ˆ i = (ˆ xi , yˆi , φˆi )t and let be denoted by x ˆ = (ˆ x x1 , yˆ1 , φˆ1 , ..., xˆn , yˆn , φˆn )t
(15)
be the compound vector of global poses corresponding to the local poses p.
integration of several results is then straightforward. For ease of presentation it is assumed that index i denotes the same ˆ scan in the local system of p and in the global system of x ˆ i refer to the same pose in the local respectively that is pi and x global coordinate frame. First, the difference between the global and the local parameters are determined like in Eq. 16 and then the corresponding uncertainty estimates are used as in Eq. 17. The only difficulty lies in the first step. For calculation of the difference, in Eq.16 the global parameters have to be transformed into ˆ above). For the nScanthe local coordinate system (ˆ xi x matching there is no such obvious mapping. We have investigated two different solutions to this problem. First, we present the proposed solution. One pose pc = (xc , yc , φc ) is selected as fixed with respect to the mapping from global to local coordinates That means: If the global xc , yˆc , φˆc ) then coordinates of this pose are given by xˆc = (ˆ fpc ˆxc is this mapping from global to local coordinates. Thus, this mapping is not constant but a function of x ˆc (because xˆc is a free parameter of the function to be minimized in Eq. 17 and pc is constant here). Based on this definition we can establish a function f that ˆ to differences maps x ∆x = (∆x1 , ∆y1 , ∆φ1 , ..., ∆xn , ∆yn , ∆φn )t
(18)
based on the mapping from global to local coordinates: f
Fig. 5. Six scans are nScan-matched in a closing the loop situation. Top: Initialization from SLAM (see Section 6). Bottom: Result after convergence (30 iterations, 174 function evaluations).
:
∆xi
=
∆yi
=
∆φi
=
ˆ → ∆x x
cos(φc − φˆc )(ˆ xi − x ˆc ) − ˆ sin(φc − φc )(ˆ yi − yˆc ) + xc − x ˆi
sin(φc − φˆc )(ˆ xi − xˆc ) + ˆ yi − yˆc ) + yc − yˆi cos(φc − φc )(ˆ
φc − φˆc + φˆi − φi
ˆi x ˆ j − pij ∆ij = x
(16)
be the difference between the spatial relation between i and j obtained by scan matching and the spatial relation induced by the global poses pi and pj . ˆ is then the miniThe maximum likelihood solution for x mum of following function: E(ˆ x) = ∆tij C−1 (17) ij ∆ij . (i,j)∈I
Basically, the same technique can be used to integrate the results of nScan-matching (in the following called the local parameters) to obtain global parameters (that is in the global coordinate frame). In the following a method will be presented for incorporating one result of nScan-matching with result p = (x1 , y1 , φ1 , ..., xn , yn , φn )t and Hessian H. The
(20) (21) (22)
Now the Hessian H can be used to contribute a summand E to the global error function as in Eq. 17: f (x)t Hf (x).
Let then
(19)
(23)
Again, for optimization first and second derivatives can be used: ∂f ∂E = 2(Hx)t (24) ∂x ∂x and ∂2E ∂f t ∂f ∂2f = (2H ) + 2Hx (25) ∂xi ∂xj ∂x2 ∂x1 ∂x1 ∂x2 These derivatives are partly tedious but straightforward. One particularity is that due to the special role of pose pc its derivatives are different from the others and the derivatives of all poses with respect to this pose are not equal zero. The second possibility, that we investigated, is to fix the transformation from local to global coordinates for every iteration. For that, an “optimal” transformation can be calculated by using the local Hessian to build an energy function that assesses the deviation of the current global configuration mapped into the local coordinate frame and to minimize
V. E XPERIMENTAL RESULTS AND CONCLUSIONS The nScan-matching routines were included into a relationbased SLAM implementation. The implementation keeps a set of selected scans (keyframes) and relations between them obtained both by pairwise scan matching and nScan-matching and iteratively performs the following steps: 1) The current pose is tracked using scan matching with respect to the map. 2) If no keyframe is “near enough” (heuristic measure) the current scan is added to the map as new keyframe. This new keyframe is (pairwise) matched to the nearest keyframe using an odometry model as prior (if available) and the resulting relation is added to the map. 3) A nScan-matching is performed on all scans near the nearest keyframe up to a maximum number of contained scans, e.g., 10, and a maximum distance and the resulting relation is added to the map. Thereby all existing pairwise relations that apply are used as priors. The algorithm has been applied to many data sets from different origins including data sets with loops. In many cases, though not always, it was possible to close these loops without any special loop closing mechanisms like in [20]. Loop closing in general is possible if the accumulated error is small enough for nScan-matching to be successful as in Fig. 5. Generally, accumulated errors tend to get too large if some unmodeled error sources are present, especially if the 2D assumption is partially wrong, the surfaces are not totally plain and the robot is tilted in-between like in outdoor data sets. So for such cases the approach should be combined with a multi-hypotheses framework as also proposed in [21] and with special loop closing mechanisms like in [20]. Our algorithm for example fails closing the large loop in the fr campus data set but is successful for the standford-gates1 set from the Radish data set repository [23] (both are large outdoor runs). In indoor environments the accumulated error stays remarkably small even for relatively large distances. Fig. 6 shows a map of our department’s floor where no loop “helps” to keep the walls straight and the angles of the walls orthogonal. This data set has been acquired without odometry. Another resulting map is shown in Fig. 7. One important problem for future work is the selection of pairs for the index set J (Section III-B) that is the set of scan pairs being part of the nScan-matching energy function.
At the moment we have two options here: selection of all pairs or selection of all pairs where the estimated position of one scan is visible from the other position. Both methods cannot prevent cases where one scan has no overlap with all other scans resulting in a wrong estimate for this scan. Such situations could be detected afterwards by testing for each scan if the results are within the bounds given by the current estimate from the SLAM information matrix. Such situations occur rarely if the max. number of scan is large, e.g., 10, but more often if the maximum number of participating scans is set to a small number like 4. It might also not be necessary to use all possible pairs but only a subset for faster optimization. Also, there might be ways to only run a nScan-matching in special cases and to rely on pairwise matching if the results are sufficiently good. To summarize the paper: we have presented a nScanmatching technique that is built upon existing pairwise scan matching algorithms together with a theoretical motivation and a motivation by example. Furthermore, we have shown a basic way to include nScan-matching into relation-based SLAM.
30
20
10
0
-10
50 40 30
x [m]
-20
20 10 y [m]
0
Fig. 6. Floor of our department. This map was built using nScan-matching without using odometry. 10 8 6 4
y [m]
this function at the beginning of each iteration. Because this transformation is fixed and thus not dependent on the poses’ parameters the partial derivatives are simpler. In exchange, an additional minimization has to be performed for each local relation. In our experiments it turned out that this method results in a worse convergence behaviour when optimizing the global coordinates with significantly more iterations. This is even more true for simpler variations of this scheme: Using a fixed transformation by selecting a certain scan or calculating a mean transformation dependent on the current parameters. Based on the results of our experiments we strongly recommend to use the first method presented here.
2 0 -2 -4 -6
-30
-25
-20
-15 x [m]
-10
-5
0
Fig. 7. Another office environment. This map contains a room with a lot of chairs and black sofas (difficult for scan matching). The map was also built without odometry.
R EFERENCES [1] F. Lu and E. Milios, “Globally consistent range scan alignment for environment mapping,” Autonomous Robots, vol. 4, pp. 333–349, 1997. [2] A. Censi, L. Iocchi, and G. Grisetti, “Scan matching in the hough domain,” in Proc. of Intern. Conference on Robotics and Automation (ICRA’05), 2005. [3] P. Besl and N. McKay, “A method for registration of 3d shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239–256, 1992. [4] Y. Chen and G. Medioni, “Object modeling by registration of multiple range images.” Image and Vision Computing, vol. 10, no. 3, pp. 145– 155, 1992. [5] Z. Zhang, “Iterative point matching for registration of free-from curves and surfaces,” International Journal of Computer Vision, vol. 13, no. 2, pp. 119–152, 1994. [6] S. Rusinkiewicz and M. Levoy, “Efficient variants of the ICP algorithm,” in Proceedings of the Third Intl. Conf. on 3D Digital Imaging and Modeling, 2001, pp. 145–152. [Online]. Available: citeseer.nj.nec.com/rusinkiewicz01efficient.html [7] I. Cox, “Blanche: An experiment in guidance and navigation of an autonomous robot vehicle,” IEEE Transactions on Robotics and Automation, vol. 7, no. 2, pp. 193–204, 1991. [8] F. Lu and E. Milios, “Robot pose estimation in unknown environments by matching 2d range scans,” in CVPR94, 1994, pp. 935–938. [Online]. Available: citeseer.nj.nec.com/lu94robot.html [9] J. Gutmann and C. Schlegel, “Amos: Comparison of scan matching approaches for self-localization in indoor environments,” 1996. [Online]. Available: citeseer.nj.nec.com/94695.html [10] B. Jensen and R. Siegwart, “Scan alignment with probabilistic distance metric,” in IEEE/RSJ International Conference on : Intelligent Robots and Systems (IROS 2004), 2004. [11] J. Minguez, F. Lamiraux, and L. Montesano, “Metric-based scan matching algorithms for mobile robot displacement estimation,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Barcelona, Spain, 2005. [12] K. Lingemann, H. Surmann, A. N¨uchter, and J. Hertzberg, “High-speed laser localization for mobile robots,” Journal Robotics and Autonomous Systems, vol. 51, no. 4, pp. 275–296, 2005. [13] O. Bengtsson and A.-J. Baerveldt, “Robot localization based on scanmathcing - estimating the covariance matrix for the idc algorithm,” Robotics and Autonomous Systems, vol. 44, no. 1, pp. 29–40, 2003. [14] B. A. F. Boughorbel, A. Koschan and M. Abidi, “Gaussian fields: A new criterion for 3d rigid registration,” Pattern Recognition, vol. 37, no. 7, pp. 1567–1571, 2004. [15] B. Lucas and T. Kanade, “An iterative image registration technique with an application to stereo vision,” in IJCAI81, 1981, pp. 674–679. [16] D. H¨ahnel, D. Schulz, and W.Burgard, “Map building with mobile robots in populated environments,” in Proc. of International Conference on Intelligent Robots and Systems (IROS’02), 2002. [17] A. Fitzgibbon, “Robust registration of 2d and 3d point sets,” in Proceedings of the British Machine Vision Conference, 2001, pp. 662–670. [18] G. Champleboux, S. Lavallee, R. Szeliski, and L. Brunie, “From accurate range imaging sensor calibration to accurate model-based 3-d object localization,” in Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 1992, pp. 83–89. [19] P. Biber and W. Straßer, “The normal distributions transform: A new approach to laser scan matching,” in International Conference on Intelligent Robots and Systems (IROS), 2003. [20] J.-S. Gutmann and K. Konolige, “Incremental mapping of large cyclic environments,” in Computational Intelligence in Robotics and Automation, 1999. [21] U. Frese and T. Duckett, “A multigrid approach for accelerating relaxation-based SLAM,” in Proc. IJCAI Workshop on Reasoning with Uncertainty in Robotics (RUR 2003), 2003. [22] J. Dennis and R. B. Schnabel, “Numerical Methods for Unconstrained Optimization and Nonlinear Equations. SIAM Classics in Applied Mathematics,” 1996. [23] A. Howard and N. Roy, “The robotics data set repository (radish),” 2003. [Online]. Available: http://radish.sourceforge.net/
Wolfgang Straßer
University of T¨ubingen WSI/GRIS Sand 14, 72070 T¨ubingen, Germany Email: [email protected]
University of T¨ubingen WSI/GRIS Sand 14, 72070 T¨ubingen, Germany Email: [email protected]
Abstract— Scan matching is a popular way of recovering a mobile robot’s motion and constitutes the basis of many localization and mapping approaches. Consequently, a variety of scan matching algorithms have been proposed in the past. All these algorithms share one common attribute: They match pairs of scans to obtain spatial relations between two robot poses. In this paper we present a method for matching multiple scans simultaneously. We discuss the need for such a method and describe how the result of such a multi-scan matching can be incorporated into relation-based SLAM in the Lu and Milios style.
I. I NTRODUCTION Scan matching is a popular way of recovering a mobile robot’s motion. Given two scans recorded at two different positions, the translation and the rotation that aligns these scans give an estimate of how the robot has moved. Such an estimate can be used as an enhanced odometry, can help to close loops or can be the perceptual basis of relation-based SLAM in the Lu and Milios style [1]. Many ways to calculate such estimates have been proposed but all algorithms share on common attribute: They match pairs of scans. This paper does not add a new algorithm to the collection of existing pairwise scan matchers. Instead, we present a method for matching multiple scans simultaneously based on a pairwise scan matcher (nScan-matching). We supply theoretical arguments and simple examples to motivate nScan-matching and to show that it outperforms pairwise matching in some respects and in certain situations. We also describe how the result of such a nScan-matching can be incorporated into relationbased SLAM and show some results of a complete mapping application using nScan-matching combined with relationbased SLAM. Most algorithms for matching scans or point clouds are iterative local algorithms that require an initial estimate. Algorithms that try to do a global matching, i.e., without initial estimate (e.g. [2]) are not considered here. Local scan matching algorithms can be divided in two different classes: ICPstyle algorithms and algorithms based on directly minimizing an energy function (including correlation-based approaches). The functions to be minimized may look quite similar in both kinds of algorithms but the difference is the way this is accomplished. ICP-style algorithms iteratively perform two
steps. First, the initial estimate is applied to the data and correspondences between features like points or lines are established based on several possible criteria like spatial closeness or similarity. Second, a spatial transformation is calculated that minimizes the distance between these features. Often, but not always, this transformation can be obtained in closed form. Having obtained a new transformation it is used to update the initial estimate for the next iteration until convergence. The term ICP has been coined in [3] and important variants are [4] and [5] (see [6] for an overview of more variants on 3D ICPalgorithms). 2D variants were first described in [7] and [8]. Gutmann and Schlegel compare different approaches in [9] and more recent variants include [10]–[12]. Bengtsson investigated means to extract a covariance matrix for ICP-based approaches [13], an important prerequisite for relation-based SLAM. The other type of algorithms define energy functions that do not require explicit correspondences. Such an energy function can be directly minimized using general numerical minimization methods. The same thing may be done in the second step of ICP-like algorithms but the function to be minimized there is different in each iteration if correspondences have changed. Thus, for different estimates of the transformation the function is different. But then this means that the function that is effectively minimized is often not continuous and, generally, not differentiable. In contrast, energy functions without explicit correspondences or based on correlation principles normally are smooth and differentiable. Mostly, such methods use grids or other discretizations of the spatial domain (an exception are the Gaussian Fields of Boughorble et al. [14]) but the assumption of smoothness is still reasonable. This is similar to intensitybased registration algorithms in computer vision that assume a continuous image and obtain subpixel estimates by interpolation ( [15] and many others thereafter). Examples for these methods include [16]–[18] and our own method [19]. The nScan-matching technique proposed in this paper is built upon a pairwise scan matching technique. Basically, an energy function defined on the domain of multiple poses is built by adding the corresponding pairwise energy functions. Any scan matching technique can be used here, but methods of the second type are better suited as our method assumes the existence of first and second derivatives of the pairwise
scan matching function. A similar technique could be applied for ICP-style algorithms but this case is not handled here. We conjecture that convergence would be slower in this case, as more correspondences would change between iterations and so the function to be minimized would be even less smooth than in the pairwise case. But this has not been verified here. Often results from scan matching are used in mapping and localization algorithms. For example, relation-based SLAM [1], [20], [21] uses results from pairwise scan matching to build a graph. In this graph, nodes are scans and edges denote that there is a (pairwise) relation between the nodes that are connected by that edge. Globally consistent poses for all scans can than be obtained by minimizing an energy function defined on this graph. Relation-based SLAM is also the application of nScanmatching considered in this paper. In this context, the contribution of this paper is a motivation and an investigation of a generalization of relation-based SLAM where joint relations are established between more than two unknown poses. Methods for both calculation of these multi-pose relations by nScan-matching and their integration into the formulation of a relation-based SLAM energy function are presented and discussed. The rest of the paper is organized as follows. The next section gives a motivation for our approach. It follows section III with the basic algorithm. In section IV we investigate how the results of nScan-matching can be integrated into relationbased SLAM. Section V concludes with experimental results and open problems.
is also possible that the pairwise results contradict each other as nothing enforces these constraints.
(a) (b)
II. M OTIVATION Two observations on the relation-based approach motivate the introduction of nScan-matching. The first observation addresses the number of parameters to be estimated in the course of obtaining n globally consistent poses given n scans. With parameters we mean in the following the three components of a pose, i.e., position (x, y) and angle φ. That is, for pairwise matching we have three parameters and for matching n scans we have 3(n − 1) parameters (one pose can be set to be the origin). Consider a case where it is possible to obtain relations between all n(n− 1) pairs of poses by pairwise scan matching (here and in the following it is assumed that pairwise scan matching is symmetric in the sense that matching i to j is the same as matching j to i). Then, the number of parameters to be estimated is 3n(n − 1) (3 for each relation). As already said, the number of parameters of the real problem (estimating consistent poses for n scans) is only 3(n − 1). These 3(n − 1) parameters are obtained in the second step of the relationbased approach from the 3n(n−1) parameters that result from pairwise matching. Thus, a method for estimating the 3(n− 1) parameters directly in a single, joint estimation and without the indirection over the n(n − 1) pairwise relations would be desirable because the fact, that the 3n(n − 1) parameters of all pairwise relations can be derived from only 3(n − 1) parameters is not used. Not only are possible improvements in convergence speed given away; because this fact is ignored it
(c) Fig. 1. Pairs of scans that are difficult to match. (a) Scans that have been taken from positions far away from each other. Different point densities make matching using standard methods difficult. (b) Scans taken from opposite viewing directions. Here, occlusions pose an additional difficulty in matching these scans as wrong correspondences could be established. (c) Scans with small overlaps. All these cases are difficult for algorithms matching pairs of scans. But clearly they can provide constraints and the nScan-matching scheme proposed in this paper is able to take advantage of them.
The second observation concerns the establishment of relations itself by scan matching. Matching only two scans may be difficult, ambiguous or weakly constrained in some cases. Examples for such cases are: scans taken from positions far away from each other or with a small amount of overlap (see Fig. 1). Scan matching can also be weakly constrained in environments like corridors. But the same situation may yield an unambiguous and sharp optimum if more than two scans are matched jointly. Fig. 2 shows one easy (yet rather artificial) example. A more realistic example is shown in Fig. 3. Such a situation is likely to occur if a robot enters a room, turns around there, and leaves the room. Both observations motivate the use of nScan-matching (joint matching of multiple scans) and the next section presents the basic method.
and measures how good scan j matches to it (the lower the energy the better the match) for a relative pose d such that fd maps points from j to i. We will now use Eij to build a scan matching energy function for simultaneous matching of n scans 1..n. Let p = (x1 , y1 , φ1 , ..., xn , yn , φn )t
Fig. 2. Matching only two scans may be difficult, ambiguous or weakly constrained in some cases. In this simple example matching any two of these three shapes is ambiguous, but not jointly matching all three shapes.
be the compound vector of all poses pi = (xi , yi , φi )t . Then for an index set J ⊂ [1..n] × [1..n] E(p) = Eij (pj pi )
(3)
(4)
(i,j)∈J
is the proposed energy function.
2
3
C. Optimization The proposed energy function is optimized using Newton’s algorithm. Newton’s algorithm iteratively finds the parameters p that minimize E. Each iteration solves the following equation: t , HE ∆p = −gE
1 Fig. 3. nScan-matching is an easier problem in some cases compared to pairwise scan-matching. In this example a robot enters a room (1), turns there (2), and leaves (3). For pairwise laser scan matching, scan 1 and scan 2 are unconstrained in one dimension, as are scan 1 and scan 3. However, the combination of all three scans yields a definite optimum for scan matching.
III. N S CAN MATCHING This section presents the basic nScan-matching algorithm after introducing the notation for poses used in the rest of the paper. A. Notation We follow the notation of [1]. Let pi = (xi , yi , φi )t denote the robot’s pose at time i meaning that cos φj − sin φj xi fpi (x) = x+ (1) sin φj cos φj yi maps 2D-points x from the local coordinate frame of the robot at time i to global coordinates. Let pi and pj be two different robot’s poses. We define Dij = pi pj to be the relative pose between frames i and j, that is fDij maps points from the coordinate frame i to the coordinate frame j. The entries of Dij are given by: (xi − xj ) cos φj + (yi − yj ) sin φj pi pj = −(xi − xj ) sin φj + (yi − yj ) cos φj (2) φi − φj B. Energy function Let
Eij (d) : R3 → R
be a twice differentiable scan matching energy function (as discussed in the introduction) that takes scan i as reference
where gE is the gradient of E ∂E , gE = ∂p and HE is the Hessian of f with entries
(5)
(6)
∂2E . (7) ∂p2 The solution of this linear system is an increment ∆p that is added to the current estimate: HE =
p ← p + ∆p.
(8)
Gradient and Hessian of E can be built by summing the gradients and Hessians of the basic scan matcher Eij modified as follows. Let u be the relative pose between j and i according to the compound poses vector p: u = pj pi .
(9)
Using the chain rule of derivation, the relation between gEij (that is, the gradient of Eij with respect to the relative pose u) and its contribution to gE (that is, the gradient of Eij with respect to p) is: ∂Eij ∂Eij ∂u = , (10) ∂pi ∂u ∂pi ∂Eij ∂Eij ∂u = , (11) ∂pj ∂u ∂pj and
∂Eij =0 ∂pk
(12)
for k = i and k = j. Similarly, an entry Hab of the 3n × 3n Hessian of E gets a contribution from the Hessian HEij of summand Eij as follows: t ∂ 2 Eij ∂u ∂u ∂Eij ∂ 2 u = HEij + , (13) ∂a∂b ∂a ∂b ∂u ∂a∂b
where of course again all partial derivatives equal zero if a or b is not one of the parameters of pi or pj . These equations allow to build gE and HE from the contributions of the single scan matchers and to perform one iteration of Newton’s algorithm as follows: 1) Set gE and HE equal zero. 2) For each pair (i, j) ∈ J a) Calculate u and its first and second partial derivatives with respect to pi and pj . b) Calculate gradient and Hessian of Eij at u. c) To these, apply Eqs. 10 -13 and add the results to gE and HE . 3) Solve Eq. 5 for ∆p. D. Number of parameters to estimate Recovering consistent poses for n scans clearly has only 3(n − 1) parameters as the problem is invariant against any global transformation of all poses. Two ways exist to deal with this during optimization. The first possibility is to fix the pose of one scan and estimate the remaining n − 1 poses. This approach may lead to different solutions for different fixed scans. Besides that, there will be no uncertainty estimate for the pose of the fixed scan (because it is fixed) and uncertainty will be increasing with distance to the first scan. So the solution may be biased. The second possibility is to, nevertheless, estimate all n poses simultaneously. The remaining degrees of freedom can be handled by choosing in every iteration the smallest update ∆p. This can be achieved using standard methods (solving for ∆p using a singular value decomposition of H and setting the inverses of the three smallest singular values to 0 when inverting H). We have observed a better convergence behavior for the second method, not dramatically but clearly observable, and therefore this method is recommended. However, in section V the uncertainty of the result will be needed with respect to a selected (fixed) scan. Such an estimate can then be obtained by recalculation of the Hessian at the obtained solution but keeping the selected scan fixed. E. Integrating prior knowledge Most mobile robots are equipped with an odometry providing them with estimates of self motion. Also other sensors and algorithms may be available for additional motion estimates or constraints (for example, from computer vision algorithms). Such data can not only be used to provide initial estimates for scan matching as it is often done. It can also be used directly as a prior for guiding scan matching. If there is a probabilistic model for the additional input and the scan matching function is a negative log likelihood, then integration is straightforward. Let E(x) be the scan matcher and p(x) the probabilistic model (for example for odometry) then E(x) − log p(x)
(14)
is the function that should be minimized. Again it is also possible to integrate such a modification in ICP-like algorithms
but it is much more straightforward in approaches based on an explicit energy function. F. Implementation and example We use our scan matching algorithm [19] as basic scan matching function. Optimization is performed by using our standard Newton algorithm implementation containing several step selection methods, e.g., a trust-region approach, as described in [22]. Figure 4 shows an example where ten scans are nScan-matched using odometry as initial estimate. Initial estimates are much better in typical applications like the relation-based SLAM application we describe later, and there the algorithm typically converges in five to ten iterations (but this may heavily depend on the underlying basic scan matcher). However, for closing loops such gross errors are realistic. Fig. 5 shows such an example. Six scans are nScanmatched in a “closing-the-loop” situation. In both examples the index set J contains all pairs. These results (and also all other results shown in this paper) are based on scans taken with a SICK LMS 200.
Fig. 4. Here ten scans are nScan-matched. Top: Initial estimate from odometry. Bottom: Result after convergence (30 iterations, 267 function evaluations). Circles show the robot positions where the scans have been taken.
IV. I NTEGRATION INTO RELATION - BASED SLAM Standard relation-based SLAM assumes a set I = {(i, j)| scan i and scan j have been matched successfully} with pij as estimate for the spatial relations between scan i and j and Cij as covariance matrix describing the uncertainty of this estimate. Let the poses in the global coordinate frame ˆ i = (ˆ xi , yˆi , φˆi )t and let be denoted by x ˆ = (ˆ x x1 , yˆ1 , φˆ1 , ..., xˆn , yˆn , φˆn )t
(15)
be the compound vector of global poses corresponding to the local poses p.
integration of several results is then straightforward. For ease of presentation it is assumed that index i denotes the same ˆ scan in the local system of p and in the global system of x ˆ i refer to the same pose in the local respectively that is pi and x global coordinate frame. First, the difference between the global and the local parameters are determined like in Eq. 16 and then the corresponding uncertainty estimates are used as in Eq. 17. The only difficulty lies in the first step. For calculation of the difference, in Eq.16 the global parameters have to be transformed into ˆ above). For the nScanthe local coordinate system (ˆ xi x matching there is no such obvious mapping. We have investigated two different solutions to this problem. First, we present the proposed solution. One pose pc = (xc , yc , φc ) is selected as fixed with respect to the mapping from global to local coordinates That means: If the global xc , yˆc , φˆc ) then coordinates of this pose are given by xˆc = (ˆ fpc ˆxc is this mapping from global to local coordinates. Thus, this mapping is not constant but a function of x ˆc (because xˆc is a free parameter of the function to be minimized in Eq. 17 and pc is constant here). Based on this definition we can establish a function f that ˆ to differences maps x ∆x = (∆x1 , ∆y1 , ∆φ1 , ..., ∆xn , ∆yn , ∆φn )t
(18)
based on the mapping from global to local coordinates: f
Fig. 5. Six scans are nScan-matched in a closing the loop situation. Top: Initialization from SLAM (see Section 6). Bottom: Result after convergence (30 iterations, 174 function evaluations).
:
∆xi
=
∆yi
=
∆φi
=
ˆ → ∆x x
cos(φc − φˆc )(ˆ xi − x ˆc ) − ˆ sin(φc − φc )(ˆ yi − yˆc ) + xc − x ˆi
sin(φc − φˆc )(ˆ xi − xˆc ) + ˆ yi − yˆc ) + yc − yˆi cos(φc − φc )(ˆ
φc − φˆc + φˆi − φi
ˆi x ˆ j − pij ∆ij = x
(16)
be the difference between the spatial relation between i and j obtained by scan matching and the spatial relation induced by the global poses pi and pj . ˆ is then the miniThe maximum likelihood solution for x mum of following function: E(ˆ x) = ∆tij C−1 (17) ij ∆ij . (i,j)∈I
Basically, the same technique can be used to integrate the results of nScan-matching (in the following called the local parameters) to obtain global parameters (that is in the global coordinate frame). In the following a method will be presented for incorporating one result of nScan-matching with result p = (x1 , y1 , φ1 , ..., xn , yn , φn )t and Hessian H. The
(20) (21) (22)
Now the Hessian H can be used to contribute a summand E to the global error function as in Eq. 17: f (x)t Hf (x).
Let then
(19)
(23)
Again, for optimization first and second derivatives can be used: ∂f ∂E = 2(Hx)t (24) ∂x ∂x and ∂2E ∂f t ∂f ∂2f = (2H ) + 2Hx (25) ∂xi ∂xj ∂x2 ∂x1 ∂x1 ∂x2 These derivatives are partly tedious but straightforward. One particularity is that due to the special role of pose pc its derivatives are different from the others and the derivatives of all poses with respect to this pose are not equal zero. The second possibility, that we investigated, is to fix the transformation from local to global coordinates for every iteration. For that, an “optimal” transformation can be calculated by using the local Hessian to build an energy function that assesses the deviation of the current global configuration mapped into the local coordinate frame and to minimize
V. E XPERIMENTAL RESULTS AND CONCLUSIONS The nScan-matching routines were included into a relationbased SLAM implementation. The implementation keeps a set of selected scans (keyframes) and relations between them obtained both by pairwise scan matching and nScan-matching and iteratively performs the following steps: 1) The current pose is tracked using scan matching with respect to the map. 2) If no keyframe is “near enough” (heuristic measure) the current scan is added to the map as new keyframe. This new keyframe is (pairwise) matched to the nearest keyframe using an odometry model as prior (if available) and the resulting relation is added to the map. 3) A nScan-matching is performed on all scans near the nearest keyframe up to a maximum number of contained scans, e.g., 10, and a maximum distance and the resulting relation is added to the map. Thereby all existing pairwise relations that apply are used as priors. The algorithm has been applied to many data sets from different origins including data sets with loops. In many cases, though not always, it was possible to close these loops without any special loop closing mechanisms like in [20]. Loop closing in general is possible if the accumulated error is small enough for nScan-matching to be successful as in Fig. 5. Generally, accumulated errors tend to get too large if some unmodeled error sources are present, especially if the 2D assumption is partially wrong, the surfaces are not totally plain and the robot is tilted in-between like in outdoor data sets. So for such cases the approach should be combined with a multi-hypotheses framework as also proposed in [21] and with special loop closing mechanisms like in [20]. Our algorithm for example fails closing the large loop in the fr campus data set but is successful for the standford-gates1 set from the Radish data set repository [23] (both are large outdoor runs). In indoor environments the accumulated error stays remarkably small even for relatively large distances. Fig. 6 shows a map of our department’s floor where no loop “helps” to keep the walls straight and the angles of the walls orthogonal. This data set has been acquired without odometry. Another resulting map is shown in Fig. 7. One important problem for future work is the selection of pairs for the index set J (Section III-B) that is the set of scan pairs being part of the nScan-matching energy function.
At the moment we have two options here: selection of all pairs or selection of all pairs where the estimated position of one scan is visible from the other position. Both methods cannot prevent cases where one scan has no overlap with all other scans resulting in a wrong estimate for this scan. Such situations could be detected afterwards by testing for each scan if the results are within the bounds given by the current estimate from the SLAM information matrix. Such situations occur rarely if the max. number of scan is large, e.g., 10, but more often if the maximum number of participating scans is set to a small number like 4. It might also not be necessary to use all possible pairs but only a subset for faster optimization. Also, there might be ways to only run a nScan-matching in special cases and to rely on pairwise matching if the results are sufficiently good. To summarize the paper: we have presented a nScanmatching technique that is built upon existing pairwise scan matching algorithms together with a theoretical motivation and a motivation by example. Furthermore, we have shown a basic way to include nScan-matching into relation-based SLAM.
30
20
10
0
-10
50 40 30
x [m]
-20
20 10 y [m]
0
Fig. 6. Floor of our department. This map was built using nScan-matching without using odometry. 10 8 6 4
y [m]
this function at the beginning of each iteration. Because this transformation is fixed and thus not dependent on the poses’ parameters the partial derivatives are simpler. In exchange, an additional minimization has to be performed for each local relation. In our experiments it turned out that this method results in a worse convergence behaviour when optimizing the global coordinates with significantly more iterations. This is even more true for simpler variations of this scheme: Using a fixed transformation by selecting a certain scan or calculating a mean transformation dependent on the current parameters. Based on the results of our experiments we strongly recommend to use the first method presented here.
2 0 -2 -4 -6
-30
-25
-20
-15 x [m]
-10
-5
0
Fig. 7. Another office environment. This map contains a room with a lot of chairs and black sofas (difficult for scan matching). The map was also built without odometry.
R EFERENCES [1] F. Lu and E. Milios, “Globally consistent range scan alignment for environment mapping,” Autonomous Robots, vol. 4, pp. 333–349, 1997. [2] A. Censi, L. Iocchi, and G. Grisetti, “Scan matching in the hough domain,” in Proc. of Intern. Conference on Robotics and Automation (ICRA’05), 2005. [3] P. Besl and N. McKay, “A method for registration of 3d shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239–256, 1992. [4] Y. Chen and G. Medioni, “Object modeling by registration of multiple range images.” Image and Vision Computing, vol. 10, no. 3, pp. 145– 155, 1992. [5] Z. Zhang, “Iterative point matching for registration of free-from curves and surfaces,” International Journal of Computer Vision, vol. 13, no. 2, pp. 119–152, 1994. [6] S. Rusinkiewicz and M. Levoy, “Efficient variants of the ICP algorithm,” in Proceedings of the Third Intl. Conf. on 3D Digital Imaging and Modeling, 2001, pp. 145–152. [Online]. Available: citeseer.nj.nec.com/rusinkiewicz01efficient.html [7] I. Cox, “Blanche: An experiment in guidance and navigation of an autonomous robot vehicle,” IEEE Transactions on Robotics and Automation, vol. 7, no. 2, pp. 193–204, 1991. [8] F. Lu and E. Milios, “Robot pose estimation in unknown environments by matching 2d range scans,” in CVPR94, 1994, pp. 935–938. [Online]. Available: citeseer.nj.nec.com/lu94robot.html [9] J. Gutmann and C. Schlegel, “Amos: Comparison of scan matching approaches for self-localization in indoor environments,” 1996. [Online]. Available: citeseer.nj.nec.com/94695.html [10] B. Jensen and R. Siegwart, “Scan alignment with probabilistic distance metric,” in IEEE/RSJ International Conference on : Intelligent Robots and Systems (IROS 2004), 2004. [11] J. Minguez, F. Lamiraux, and L. Montesano, “Metric-based scan matching algorithms for mobile robot displacement estimation,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Barcelona, Spain, 2005. [12] K. Lingemann, H. Surmann, A. N¨uchter, and J. Hertzberg, “High-speed laser localization for mobile robots,” Journal Robotics and Autonomous Systems, vol. 51, no. 4, pp. 275–296, 2005. [13] O. Bengtsson and A.-J. Baerveldt, “Robot localization based on scanmathcing - estimating the covariance matrix for the idc algorithm,” Robotics and Autonomous Systems, vol. 44, no. 1, pp. 29–40, 2003. [14] B. A. F. Boughorbel, A. Koschan and M. Abidi, “Gaussian fields: A new criterion for 3d rigid registration,” Pattern Recognition, vol. 37, no. 7, pp. 1567–1571, 2004. [15] B. Lucas and T. Kanade, “An iterative image registration technique with an application to stereo vision,” in IJCAI81, 1981, pp. 674–679. [16] D. H¨ahnel, D. Schulz, and W.Burgard, “Map building with mobile robots in populated environments,” in Proc. of International Conference on Intelligent Robots and Systems (IROS’02), 2002. [17] A. Fitzgibbon, “Robust registration of 2d and 3d point sets,” in Proceedings of the British Machine Vision Conference, 2001, pp. 662–670. [18] G. Champleboux, S. Lavallee, R. Szeliski, and L. Brunie, “From accurate range imaging sensor calibration to accurate model-based 3-d object localization,” in Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 1992, pp. 83–89. [19] P. Biber and W. Straßer, “The normal distributions transform: A new approach to laser scan matching,” in International Conference on Intelligent Robots and Systems (IROS), 2003. [20] J.-S. Gutmann and K. Konolige, “Incremental mapping of large cyclic environments,” in Computational Intelligence in Robotics and Automation, 1999. [21] U. Frese and T. Duckett, “A multigrid approach for accelerating relaxation-based SLAM,” in Proc. IJCAI Workshop on Reasoning with Uncertainty in Robotics (RUR 2003), 2003. [22] J. Dennis and R. B. Schnabel, “Numerical Methods for Unconstrained Optimization and Nonlinear Equations. SIAM Classics in Applied Mathematics,” 1996. [23] A. Howard and N. Roy, “The robotics data set repository (radish),” 2003. [Online]. Available: http://radish.sourceforge.net/
