# Efficient Estimation of Accurate Maximum Likelihood Maps in 3D

←

→

**Page content transcription**

If your browser does not render page correctly, please read the page content below

Efficient Estimation of Accurate Maximum Likelihood Maps in 3D Giorgio Grisetti Slawomir Grzonka Cyrill Stachniss Patrick Pfaff Wolfram Burgard Abstract— Learning maps is one of the fundamental tasks of mobile robots. In the past, numerous efficient approaches to map learning have been proposed. Most of them, however, assume that the robot lives on a plane. In this paper, we consider the problem of learning maps with mobile robots that operate in non-flat environments and apply maximum likelihood techniques to solve the graph-based SLAM problem. Due to the non-commutativity of the rotational angles in 3D, major problems arise when applying approaches designed for the two-dimensional world. The non-commutativity introduces serious difficulties when distributing a rotational error over a sequence of poses. In this paper, we present an efficient solution Fig. 1. A simulated trajectory of a robot moving on the surface of a sphere. to the SLAM problem that is able to distribute a rotational error The left image shows an uncorrected trajectory and the right image depicts over a sequence of nodes. Our approach applies a variant of the corrected one (approx. 8,600 constraints, 100 iterations, 21s). gradient descent to solve the error minimization problem. We implemented our technique and tested it on large simulated and real world datasets. We furthermore compared our approach to in 3D. One way is to ignore the non-commutativity of the solving the problem by LU-decomposition. As the experiments rotational angles. In this case, however, the algorithm works illustrate, our technique converges significantly faster to an only in case of small noise and in small environments. A accurate map with low error and is able to correct maps with few maximum likelihood mapping techniques have been bigger noise than existing methods. proposed for the three-dimensional space [9], [12], [15]. I. I NTRODUCTION Some approaches ignore the error in pitch and roll [9] whereas others detect loops and divide the error by the Learning maps has been a major research focus in number of poses along the loop (weighted with path length, the robotics community over the last decades and is of- as in [12]). An alternative solution is to apply variants of ten referred to as the simultaneous localization and map- the approach of Lu and Milios [10] and to correct the whole ping (SLAM) problem. In the literature, a large variety network at once [15]. of solutions to this problem can be found. In this paper, The contribution of this paper is a technique to efficiently we consider the popular and so-called “graph-based” or distribute the error over a sequence of nodes in all six “network-based” formulation of the SLAM problem in which dimensions (x, y, z, and the three rotational angles φ, θ, the poses of the robot are modeled by nodes in a graph. ψ). This enables us to apply a variant of gradient descent Constraints between poses resulting from observations or in order to reduce the error in the network. As a result, from odometry are encoded in the edges between the nodes. our approach converges by orders of magnitudes faster than The goal of algorithms to solve this problem is to find a the approaches mentioned above to low error configurations. configuration of the nodes that maximizes the observation As a motivating example, consider Figure 1. It depicts a likelihood encoded in the constraints. trajectory of a simulated robot moving on the surface of a In the past, this concept has been successfully applied [3], sphere. The left image depicts the input data and the right [4], [7], [8], [9], [10], [12], [13], [15]. Such solutions apply one the result of the technique presented in this paper. an iterative error minimization techniques. They correct The remainder of this paper is organized as follows. After either all poses simultaneously [7], [9], [10], [15] or perform discussing related work, we explain in Section III the graph- local updates [3], [4], [8], [13]. Most approaches have been based formulation of the mapping problem as well as the designed for the two-dimensional space where the robot is key ideas of gradient descent in Section IV. Section V assumed to operate on a plane [3], [4], [7], [10], [13]. Among explains why the standard 2D approach cannot be used in all these approaches, multi-level relaxation [4] or Olson’s 3D and introduces our technique to correct the poses given a algorithm [13] belong to the most efficient ones. network of constraints. Section VI analyzes the complexity In the three-dimensional space, however, distributing an of our approach. We finally present our experimental results error between different nodes of a network is not straight- in Section VII. forward. One reason for that is the non-commutativity of the three rotational angles. As a result, most approaches II. R ELATED W ORK that provide good results in 2D are not directly applicable A popular approach to find maximum likelihood (ML) All authors are members of the University of Freiburg, Department of maps is to apply least square error minimization techniques Computer Science, 79110 Freiburg, Germany based on a network of relations. In this paper, we also

follow this way of describing the SLAM problem. Lu and gradient descent allows us to correct larger networks than Milios [10] first applied this approach in robotics to address most state-of-the-art approaches. the SLAM problem using a kind of brute force method. III. O N G RAPH - BASED SLAM Their approach seeks to optimize the whole network at once. Gutmann and Konolige [7] proposed an effective way for The goal of graph-based maximum-likelihood mapping constructing such a network and for detecting loop closures algorithms is to find the configuration of nodes that max- while running an incremental estimation algorithm. Howard imizes the likelihood of the observations. For a more precise et al. [8] apply relaxation to localize the robot and to build formulation consider the following definitions: T a map. Duckett et al. [3] propose the usage of Gauss-Seidel • x is a vector of parameters (x1 · · · xn ) which relaxation to minimize the error in the network of constraints. describes a configuration of the nodes. In order to make the problem linear, they assume knowledge • δji represents a constraint between the nodes i and j about the orientation of the robot. Frese et al. [4] propose based on measurements. These constraints are the edges a variant called multi-level relaxation (MLR). It applies in the graph structure. relaxation based on different resolutions. Recently, Olson • Ωji is the information matrix capturing the uncertainty et al. [13] presented a novel method for correction two- of δji . dimensional networks using (stochastic) gradient descent. • fji (x) is a function that computes a zero noise obser- Olson’s algorithm and MLR are currently the most efficient vation according to the current configuration of nodes. techniques available in 2D. All techniques discussed so far It returns an observation of node j from node i. have been presented as solutions to the SLAM problem in Given a constraint between node i and node j, we can the two-dimensional space. As we will illustrate in this paper, define the error eji introduced by the constraint and residual they typically fail to correct a network in 3D. rji as Dellaert proposed a smoothing method called square root eji (x) = fji (x) − δji = −rji (x). (1) smoothing and mapping [2]. It applies smoothing to correct At the equilibrium point, eji is equal to 0 since fji (x) = δji . the poses of the robot and feature locations. It is one of In this case, an observation perfectly matches the current the few techniques that can be applied in 2D as well as configuration of the nodes. Assuming a Gaussian observation in 3D. A technique that combines 2D pose estimates with error, the negative log likelihood of an observation fji is 3D data has been proposed by Howard et al. [9] to build maps of urban environments. They avoid the problem of 1 T Fji (x) = (fji (x) − δji ) Ωji (fji (x) − δji ) (2) distributing the error in all three dimensions by correcting 2 only the orientation in the x, y-plane of the vehicle. The roll ∝ rji (x)T Ωji rji (x). (3) and pitch is assumed to be measured accurately enough using Under the assumption that the observations are independent, an IMU. the overall negative log likelihood of a configuration x is In the context of three-dimensional maximum likelihood mapping, only a few approaches have been presented so 1 X F (x) = rji (x)T Ωji rji (x) (4) far [11], [12], [15]. The approach of Nüchter et al. [12] de- 2 ∈C scribes a mobile robot that builds accurate three-dimensional models. In their approach, loop closing is achieved by Here C = {< j1 , i1 >, . . . , < jM , iM >} is set of pairs of uniformly distributing the error resulting from odometry indices for which a constraint δjm im exists. over the poses in a loop. This technique provides good A maximum likelihood map learning approach seeks to estimates but typically requires a small error in the roll and find the configuration x∗ of the nodes that maximizes the pitch estimate. Newman et al. [11] presented a sophisticated likelihood of the observations which is equivalent to mini- approach for detecting loop closures using laser and vision. mizing the negative log likelihood written as Such an approach can be used to find the constraints which x∗ = argmin F (x). (5) are the input to our algorithm. x Recently, Triebel et al. [15] described an approach that IV. G RADIENT D ESCENT aims to globally correct the poses given the network of FOR M AXIMUM L IKELIHOOD M APPING constraints in all three dimensions. At each iteration the Gradient descent (GD) is an iterative technique to find the problem is linearized and solved using LU decomposition. minimum of a function. Olson et al. [13] were the first who This yields accurate results for small and medium size net- applied it in the context of the SLAM problem in the two- works especially when the error in the rotational component dimensional space. GD seeks for a solution of Eq. (5) by is small. We use this approach as a benchmark for our iteratively selecting a constraint < j, i > and by moving a technique presented in this paper. set of nodes of the network in order to decrease the error The contribution of this paper is a highly efficient tech- introduced by the selected constraint. The nodes are updated nique to compute maximum likelihood maps in 3D. We according to the following equation: present a way of distributing an error in all three rotational angles that accounts for the non-commutativity of these xt+1 = xt + λ · Jji T Ωji rji (6) | {z } angles. This technique in combination with a variant of ∆x

Here x is the set of variables describing the locations of the poses in the network. Jji is the Jacobian of fji , Ωji is the information matrix capturing the uncertainty of the observation, and rji is the residual. Reading the term ∆x of Eq. (6) from right to left gives an intuition about the iterative procedure used in GD: Fig. 2. A simple example that illustrates the problem of distributing the error in 3D. The left image shows the input data which was obtained by • rji is the residual which is the opposite of the error vec- moving a simulated robot over a hexagon twice with small Gaussian noise. tor. Changing the network configuration in the direction The middle image show the result obtained if the non-commutativity of the of the residual rji will decrease the error eji . rotation angles is ignored. The right images shows the result of our approach which is very close to the ground truth. • Ωji represents the information matrix of a constraint. Multiplying it with rji scales the residual components according to the information encoded in the constraint. the tree to the node i itself T • Jji : The role of the Jacobian is to map the residual term into a set of variations in the parameter space. xi = pi ⊖ pparent(i) , (7) • λ is the learning rate which decreases with the iteration of GD and which makes the system to converge to an with x0 = p0 . The operator ⊖ is the motion decomposition equilibrium point. operator in 3D which is analogous to the one defined in 2D (see Lu and Milios [10]). A detailed discussion on tree In practice, GD decomposes the overall problem into many parameterizations in combination with GD is out of the scope smaller problems by optimizing the constraints individually. of this document and we refer the reader to [6]. The difference between GD and stochastic GD is that the Before presenting our approach for correcting the poses in stochastic variant selects the constraints in a random order. a network, we want to illustrate the problem of distributing Obviously, updating the different constraints one after each an error over a sequence of nodes. Consider that we need other can have opposite effects on a subset of variables. To to distribute an error e over a sequence of n nodes. In the avoid infinitive oscillations, one uses the learning rate to two-dimensional space, this can be done in a straightforward reduce the fraction of the residual which is used for updating manner as follows. Given the residual r2D = (rx , ry , rθ ), we the variables. This makes the solutions of the different sub- can simply change the pose of the i-th node in the chain by problems to asymptotically converge towards an equilibrium i/n times r2D . This error propagation works well in 2D and point that is the solution found by the algorithm. This is performed in most maximum-likelihood methods in the equilibrium point is then reported as the maximum liklihood 2D space. In the three-dimensional space, however, such a solution to the mapping problem. technique is not applicable (with exception of very small V. 3D G RAPH O PTIMIZATION errors). The reason for that is the non-commutativity of the three rotations The graph-based formulation of the SLAM problem does n not specify how the poses are presented in the nodes of the Y φ θ ψ graph. In theory, one can choose an arbitrary parameteriza- R(φ, θ, ψ) 6= R( , , ), (8) 1 n n n tion. Our algorithm uses a tree based parameterization for describing the configuration of the nodes in the graph. To where R(φ, θ, ψ) is the three-dimensional rotation matrix. As obtain such a tree from an arbitrary graph, one can compute illustrated in Figure 2, applying such an error propagation a spanning tree. The root of the spanning tree is the node leads to divergence even for small and simple problems. at the origin p0 . Another possibility is to construct a graph Therefore, one has to find a different way of distributing based on the trajectory of the robot in case this is available. the error over a chain of poses which is described in the In this setting, we build our parameterization tree as follows: following. 1) We assign a unique id to each node based on the timestamps and process the nodes accordingly. A. The Error Introduced by a Constraint 2) The first node is the root of the tree. 3) As the parent of a node, we choose the node with the Let Pi be the homogenous transformation matrix corre- smallest id for which a constraint to the current node sponding to the pose pi of the node i and Xi the transfor- exists. mation matrix corresponding to the parameter xi . Let Pi,0 be the ordered list of nodes describing a path in the tree from This tree can be easily constructed on the fly. the root (here referred to as node 0) to the node i. We can In the following, we describe how to use this tree to define express the pose of a node as the parameterization of the nodes in the network. Each node Y i in the tree is related to a pose pi in the network and Pi = Xk . (9) maintains a parameter xi which is a 6D vector that describes k∈Pi,0 its configuration. Note that the parameter xi can be different from the pose pi . In our approach, the parameter xi is chosen The homogenous transformation matrix Xi consists of a as the relative movement from the parent of the node i in rotational matrix R and a translational component t. It has

the following form indices (compare Eq. (13)). The orientation of pj is described T by R k tk −1 Rk −RkT tk Xi = with Xi = 0 1 0 1 R1 R2 . . . Rn = R1:n , (14) (10) In order to compute the transformation between two where n is the length of the path Pji . nodes i and j, one needs to consider the path Pji from node i Distributing a given error over a sequence of 3D rotations, to node j. Since the nodes are arranged in a tree, this path can be described in the following way: we need to determine a consists of an ascending part and a descending part. Let Pji a set of increments in the intermediate rotations of the chain be the ascending part of the path starting from node i and so that the orientation of the last node (here node j) is R1:n B d Pji the descending part to node j. We can then compute the where B the matrix that rotates xj to the desired orientation error eji in the reference frame of pi as based on the error. Formulated in a mathematical way, we need to compute a set of rotations Ak so that eji = (pj ⊖ pi ) ⊖ δji . (11) n Y R1:n B = Rk Ak . (15) Using the matrix notation, the error is k=1 Eji = ∆−1 −1 ji Pi Pj (12) Once the matrices Ak are known, the new rotational matrices Y Y of the parameters xk are updated by = ∆−1 ji Xk−1 d · Xk a , (13) d kd ∈Pji a ka ∈Pji Rk ← Rk Ak . (16) where ∆ji is the matrix corresponding to δji . We can decompose the matrix B into a set of incremental So far, we described the prerequisites for applying GD to rotations B = B1:n . In our current implementation, we com- correct the poses of a network. The goal of the update rule pute the individual matrices Bk by using the spherical linear in GD is to iteratively update the configuration of a set of interpolation (slerp) [1]. We can decompose B using the slerp nodes in order to reduce the error introduced by a constraint. function with a parameter u ∈ [0, 1] with slerp(B, 0) = I T and slerp(B, 1) = B. According to this framework, we can In Eq. (6), the term Jji Ωji maps the variation of the error to a variation in the parameter space. This mapping is a compute the rotation Bk as linear function. As a result, the error might increase when T Bk = [slerp(B, uk−1 )] slerp(B, uk ). (17) applying GD in case of non-linear error surfaces. In the three- dimensional space, the rotational components often lead to To determine the values uk−1 and uk , we consider the highly non-linear error surfaces. Therefore, GD as well as eigenvalues of the covariances of the constraints connecting similar minimization techniques cannot be applied directly the nodes k −1 and k. This is an approximation which works to large mapping problems. well in case of roughly spherical covariances. Note that the In our approach, we therefore chose a slightly different eigenvalues need to be computed only once in the beginning update rule. To overcome the problem explained above, and are then stored in the tree. we allow the usage of non-linear functions to describe Using this decomposition of B leads to Eq. (15) in which the variation. The goal of this function is to compute a B is replaced by B1:n . This equation admits an infinitive transformation of the nodes along the path in the tree so number of solutions. However, we are only interested in that the error introduced by the corresponding constraint is solutions which can be combined incrementally. Informally reduced. In detail, we design this function in a way so that it speaking, this means when truncating the path from n to n−1 computes a new configuration of the variables xk ∈ Pji so nodes, the solution of the truncated path should be part of that it corrects only a fraction λ of the error, where λ is the the solution of the full path. Formally, we can express this learning rate. In our experiments, we observed that such an property by the following system of equations: update typically leads to a smooth deformation of the nodes ∀nk=1 : R1 A1 . . . Rk Ak = R1:k B1:k (18) along the path when reducing the error. In our approach, this deformation is done in two steps. First, we update the Given this set of equations, the solution for the matrices Ak rotational components Rk of the variables xk and second, can be computed as we update the translational components tk . Ak = RkT (B1:k−1 )T Rk B1:k . (19) B. Update of the Rotational Component This is an exact solution that is always defined since Ak , This section explains how to deform a path in order to Rk , and Bk are rotation matrices. The proof of Eq. (19) is reduce the error introduced by a constraint. Without loss given in the Section IX at the end of this document. Based of generality, we consider the origin of the path pi to be on Eq. (16) and Eq. (19), we have a closed form solution for in the origin of our reference system. The orientation of updating the rotational matrices of the parameters xk along pj (in the reference frame of pi ) can be computed by the path Pji from the node i to the node j. multiplying the rotational matrices along the path Pji . To Note that we also use the slerp function to compute the increase the readability of the document, we refer to the fraction of the rotational component of the residual that is rotational matrices along the path as Rk neglecting the introduced by λ (see Section V-A).

For simplicity of presentation, we showed how to dis- tribute the rotational error while keeping the node i fixed. In our implementation, however, we fix the position of the so-called “top node” in the path which is the node that is closest to the root of the tree (smallest level in the tree). As a result, the update of a constraint has less side-effects on other constraints in the network. Fixing the top node instead of node i can be obtained by simply saving the pose of the top node before updating the path. After the update, one transforms all nodes along path in way that the top node Fig. 3. A simulated trajectory of a robot moving on the surface of a cube. maintains its previous pose. Furthermore, we used the matrix The left image shows an uncorrected trajectory and the right image depicts notation in this paper to formulate the error distribution since the corrected one (approx. 4,700 constraints, 100 iterations, 11s). it provides a clearer formulation of the problem. In our im- plementation, however, we use quaternions for representing 100 Sphere 100 error/constraint Cube error/constraint rotations because they are numerically more stable. In theory, 80 80 60 60 however, both formulations are equivalent. An open source 40 40 implementation is available [5]. 20 20 0 0 0 [0s] 50 [10s] 100 [21s] 0 [0s] 100 [11s] 200 [21s] C. Update of the Translational Component iteration and execution time iteration and execution time Compared to the update of the rotational component Fig. 4. The evolution of the error for the sphere and cube experiment. described above, the update of the translational component can be done in a straightforward manner. In our mapping system, we distribute the translational error over the nodes in the tree, we therefore do not have to traverse the tree up along the path without changing the previously computed to the root anymore. It is sufficient to access the parent of rotational component. the top node in order to compute the poses for all nodes We distribute the translational error by linearly moving along a path P. As a result, updating a constraint requires the individual nodes along the path by a fraction of the a time proportional to |P| and the overall complexity per error. This fraction depends in the uncertainty of the indi- iteration turns into O(M · E(|P|)). Here M is the number vidual constraints encoded in the corresponding covariance of constraints, and E(|P|) is the average path length. In matrices. Equivalent to the case when updating the rotational all our experiments, we experienced that the average path component, these fractions is also scaled with the learning length grows more or less logarithmically with the number rate. of nodes in the graph. This explains the fast pose updates of our approach shown in the experimental section. VI. C OMPUTATIONAL C OMPLEXITY VII. E XPERIMENTS A single iteration of our algorithm requires to distribute the error introduced by the individual constraints over a set The experiments are designed to show the properties of of nodes. Therefore, the complexity is proportional to the our technique. We first present results obtained in simulated number of constraints times the number of operations needed experiments and then show results using real robot data. to distribute the error of a single constraint. In the remainder of this section, we analyze the number A. Experiments with Simulated Data of operations needed to distribute the error of a single In order to give the reader an intuition about the accuracy constraint. Once the poses of the nodes involved in an update of our approach, we generated two datasets in which the step are known, the operations described in Section V-B virtual robot moved on the surfaces of easy to visualize and V-C can be carried out in a time proportional to the geometric objects. In particular, we used a sphere and a number of nodes |P| along the path P. Computing the poses cube. The nodes of the network as well as the constraints of the nodes along a path requires to traverse the tree up between the nodes were distorted with Gaussian noise. The to the root according to Eq. (9). A naive implementation left images of Figure 1 and Figure 3 depict the distorted input requires repeated traversals of the tree up to the root. This, data whereas the images on the right illustrate the results however, can be avoided by choosing an intelligent order in obtained by our approach. As the figures indicate, the pose which to process the constraints. correction nicely recovers the original geometric structure. Let the “top node” of a path be the node with the smallest To provide more quantitative results, Figure 4 depicts the level in the tree. In our current implementation, we sort the evolution of the average error per link versus the iteration constraints according to level of the corresponding top node. number as well as execution time for the sphere and the This can be done as a preprocessing step. We can process cube experiment. As can be seen, our approach converges to the constraints according to this order. The advantage of this a configuration with small errors in less than 100 iterations. order is that a constraint never modifies a node that has a We also applied the approach of Triebel et al. to both smaller level in the tree. By storing the pose for each node datasets. As mentioned above, this approach linearizes the

Fig. 5. The real world dataset of the Intel Research Lab recorded in 2D is used to generate a large 3D dataset. Each of the four virtual buildings consist of four identical floors. The left image depicts the starting configuration. The image in the middle depicts an intermediate result and the right one the corrected map after 50 iterations of our approach. We plotted in the images constraints between buildings and floors. For a better visibility, we furthermore plotted the constraints between individual nodes which introduce a high error and not all constraints. Constraints are plotted in light gray (red) and the laser data in black. The small image on the right shows a (corrected) map of the two-dimensional laser range data. Fig. 6. The corrected trajectory plotted on top of an aerial image of the Fig. 7. The trajectory corrected by our approach is shown in black and the EPFL campus. trajectory of the (D)GPS and IMU-based localization system is shown in orange/gray. By considering Figure 6 one can see that the black one covers the streets accurately. problem and solves the resulting equation system using LU decomposition. Due to the comparably high noise in the C. Mapping with a Car-like Robot simulated experiments, the linearization errors prevented this approach to find an appropriate configuration of the nodes. Finally, we applied our method to a real world three- dimensional dataset. We used a Smart car equipped with 5 SICK laser range finders and various pose estimation sen- B. Experiments with Partially Real Robot Data sors to record the data. The robot constructs local three- dimensional maps, so-called multi-level surface maps [15], The next experiment is obtained by extending data ob- and builds a network of constrains where each node repre- tained from a 2D laser range finder into three dimensions. sents such a local map. Constraints between the maps are We used the 2D real world dataset of the Intel Research Lab obtained by matching the individual local maps. in Seattle and constructed virtual buildings with multiple We recorded a large-scale dataset at the EPFL campus in floors. The constraints between buildings and floors are which the robot moved on a 10 km long trajectory. Figure 6 manually added but all other data is real robot data. The depicts an overlay of the corrected trajectory on an aerial dataset consists of 15.000 nodes and 72.000 constraints. We image. As can be seen from the trajectory, several loops introduced a high error in the initial configuration of the have been closed. Furthermore, it includes multiple levels poses in all dimensions. This initial configuration is shown such as an underground parking garage and a bridge with an in the left image of Figure 5. As can be seen, no structure underpass. The localization system of the car which is based is recognizable. When we apply our mapping approach, we on (D)GPS and IMU data is used to compute the incremental get an accurate map of the environment. The image in the constraints. Additional constraints are obtained by matching middle depicts an intermediate result and the right image local maps. This is achieved by first classifying cells of show the resulting map after 50 iterations. To compute this the local maps into different classes and then applying a result, it took around 3 minutes on a dual core Pentium 4 variant of the ICP algorithm that considers these classes. processor with 2.4 GHz. More details on this matching can be found in our previous

Triebel et al. Triebel et al. 2 Our approach 2 Our approach k = n − 1 to substitute the term R1 A1 . . . Rn−1 An−1 error/constraint error/constraint 1.5 1.5 in the equation for k = n. This leads to 1 1 (R1:n−1 B1:n−1 )Rn An = R1:n−1 Rn B1:n . (22) 0.5 0.5 0 0 By multiplying (R1:n−1 B1:n−1 Rn )−1 from the left 0 1000 2000 3000 4000 0 0.1 0.2 0.3 0.4 hand side, this turns into time[s] time[s] An = Rn−1 (B1:n−1 )−1 (R1:n−1 )−1 R1:n−1 Rn B1:n Fig. 8. The evolution of the average error per constraint of the approach of Triebel et al. [15] and our approach for the dataset recorded with the Since Rk and Bk are rotation matrices, the inverse is autonomous car. The right image shows a magnified view to the first 400 ms. always defined and given by the transposed matrix: An = RnT (B1:n−1 )T Rn B1:n q.e.d.(23) work [14]. Figure 7 plots the trajectory corrected by our approach and the one of the (D)GPS/IMU-based localization ACKNOWLEDGMENT system. This work has been supported by the DFG within the We used the dataset from the EPFL campus to compare Research Training Group 1103 and under contract number our new algorithm to the approach of Triebel et al. In this SFB/TR-8 and by the EC under contract number FP6- experiment, both approaches converge to more or less the 2005-IST-5-muFly, FP6-2005-IST-6-RAWSEEDS, and FP6- same solution. The time needed to achieve this correction, 004250-CoSy. Thanks to Udo Frese for his insightful com- however, is by orders of magnitudes smaller when applying ments and to Rudolph Triebel for providing his mapping our new technique. This fact is illustrated in Figure 8 which system. Further thanks to Pierre Lamon for the joint effort plots the average error per constraints versus the execution in recording the EPFL dataset. time required by both techniques. We also applied our 3D optimizer to pure 2D problems R EFERENCES and compared its performance to our 2D method [6]. Both [1] T. Barrera, A. Hast, and E. Bengtsson. Incremental spherical linear techniques lead to similar results, the 2D version, however, is interpolation. In SIGRAD, volume 13, pages 7–13, 2004. [2] F. Dellaert. Square Root SAM. In Proc. of Robotics: Science and around 3 times faster that the 3D version. This results from Systems (RSS), pages 177–184, Cambridge, MA, USA, 2005. the additional DOF in the state space. [3] T. Duckett, S. Marsland, and J. Shapiro. Fast, on-line learning of globally consistent maps. Autonomous Robots, 12(3):287 – 300, 2002. VIII. C ONCLUSION [4] U. Frese, P. Larsson, and T. Duckett. A multilevel relaxation algorithm for simultaneous localisation and mapping. IEEE Transactions on In this paper, we presented a highly efficient solution to the Robotics, 21(2):1–12, 2005. [5] G. Grisetti, C. Stachniss, S. Grzonka, and W. Burgard. TORO project problem of learning three-dimensional maximum likelihood at OpenSLAM.org. http://www.openslam.org/toro.html, 2007. maps for mobile robots. Our technique is based on the graph- [6] G. Grisetti, C. Stachniss, S. Grzonka, and W. Burgard. A tree formulation of the simultaneous localization and mapping parameterization for efficiently computing maximum likelihood maps using gradient descent. In Proc. of Robotics: Science and Systems problem and applies a variant of gradient descent to minimize (RSS), Atlanta, GA, USA, 2007. the error in the network of relations. [7] J.-S. Gutmann and K. Konolige. Incremental mapping of large cyclic Our method has been implemented and exhaustively tested environments. In Proc. of the IEEE Int. Symposium on Computational Intelligence in Robotics and Automation (CIRA), pages 318–325, in simulation experiments as well as with real robot data. Monterey, CA, USA, 1999. We furthermore compared our method to a common existing [8] A. Howard, M.J. Matarić, and G. Sukhatme. Relaxation on a mesh: approach to learn such models in the three-dimensional a formalism for generalized localization. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2001. space. As shown in the experiments, our approach converges [9] A. Howard, D.F. Wolf, and G.S. Sukhatme. Towards 3d mapping in significantly faster and yields accurate maps with low errors. large urban environments. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), pages 419–424, 2004. IX. A PPENDIX : P ROOF OF E Q . (19) [10] F. Lu and E. Milios. Globally consistent range scan alignment for environment mapping. Autonomous Robots, 4:333–349, 1997. We can proof by induction that the equation system [11] P. Newman, D. Cole, and K. Ho. Outdoor slam using visual appearance and laser ranging. In Proc. of the IEEE Int. Conf. on Robotics & in Eq. (18) always has a solution which is given by Automation (ICRA), Orlando, FL, USA, 2006. [12] A. Nüchter, K. Lingemann, J. Hertzberg, and H. Surmann. 6d SLAM Ak = RkT (B1:k−1 )T Rk B1:k k = 1, . . . , n. (20) with approximate data association. In Proc. of the 12th Int. Conference on Advanced Robotics (ICAR), pages 242–249, 2005. • Basis (n = 1): [13] E. Olson, J. Leonard, and S. Teller. Fast iterative optimization of pose Based on Eq. (18) with n = 1 and by knowing that graphs with poor initial estimates. In Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), pages 2262–2269, 2006. R1 is a rotation matrix, a solution always exists and is [14] P. Pfaff, R. Triebel, C. Stachniss, P. Lamon, W. Burgard, and R. Sieg- given by wart. Towards mapping of cities. In Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), Rome, Italy, 2007. A1 = R1−1 R1 B1 = B1 . (21) [15] R. Triebel, P. Pfaff, and W. Burgard. Multi-level surface maps for outdoor terrain mapping and loop closing. In Proc. of the IEEE/RSJ • Inductive Step: Int. Conf. on Intelligent Robots and Systems (IROS), 2006. Assuming that Eq. (19) holds for k = 1, . . . , n − 1, we show that it holds also for k = n. We use Eq. (18) with

You can also read