# A Lie Algebraic Approach for Consistent Pose Registration for General Euclidean Motion

←

**Page content transcription**

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

A Lie Algebraic Approach for Consistent Pose Registration for General Euclidean Motion∗ Motilal Agrawal SRI International 333 Ravenswood Ave. Menlo Park, CA 94025, USA agrawal@ai.sri.com Abstract— We study the problem of registering local rela- outdoor terrains. Therefore, the theoretical framework of tive pose estimates to produce a global consistent trajectory of 2D SLAM is not applicable for navigation over rough, a moving robot. Traditionally, this problem has been studied undulating outdoor terrains. Succesful navigation over such with a flat world assumption wherein the robot motion has only three degrees of freedom. In this paper, we generalize this terrains requires a euclidean motion model that utilizes the for the full six-degrees-of-freedom Euclidean motion. Given full six degrees of freedom. relative pose estimates and their covariances, our formulation In this paper, we present a framework for localization of uses the underlying Lie Algebra of the euclidean motion to a robot moving in an unconstrained environment with the compute the absolute poses. Ours is an iterative algorithm that full six degrees of freedom. We assume that we are given minimizes the sum of Mahalanobis distances by linearizing around the current estimate at each iteration. Our algorithm the relative pose constraints of the robot in motion. The is fast, does not depend on a good initialization, and can uncertainties in these relative poses are also known to us. be applied to large sequences in complex outdoor terrains. Typically, these local pose constraints are computed either It can also be applied to fuse uncertain pose information from odometry readings between the poses or by matching from different available sources including GPS, LADAR, features in the environment. Wheel encoders augmented wheel encoders and vision sensing to obtain more accurate odometry. Experimental results using both simulated and real with inertial navigation system is an example of one data support our claim. odometry-based technique to compute relative poses over short distances. In computing relative poses from features, Index Terms— consistent pose registration, Lie algebra, the 3D features that are visible from both locations are used odometry, special euclidean group, Mahalanobis distance to get an estimate of the relative pose and the uncertainty. The most commonly used sensors in this method are stereo- I. I NTRODUCTION vision systems [8], [7] and 3D laser range scanners [5]. We The ability of a mobile robot to localize itself is critical are indifferent to the process of obtaining these relative to its autonomous operation and navigation. Consequently, pose constraints and their uncertainties as long as we have there has been considerable effort on the problem of mobile a reliable and a robust way of computing these. robot localization and mapping. This problem is known as Given these local relative pose constraints, our goal is simultaneous localization and mapping (SLAM) and there to recover the global motion of the robot by linking these is a vast amount of literature on this topic (see e.g., [1] constraints. A straight-forward but error-prone method of for a comprehensive survey). SLAM has been especially estimating this trajectory is to localize based on adjacent succesful in indoor structured environments [2], [3]. For poses. In such a formulation, the previous position and the indoor mapping, the world is modeled as planar and the relative motion are used to estimate the current pose. This pose of the robot has only three degrees of freedom (2D recursive estimate is bootstrapped by assuming that the translation and the yaw). This is known as 2D SLAM. initial pose is known in the world frame. This technique, On the other hand, mapping for outdoor environments however, has two fundamental limitations is still an open research problem. Outdoor mapping is 1) Error-prone: Error tends to accumulate over the harder, primarily because of two factors. First, outdoor distance traveled. Small errors early in the motion environments are unstructured, and simpler features such as can have large effects on later position estimates. planes and lines can rarely be used. Approaches for outdoor This results in an overall drift of the odometry and navigation mapping systems therefore generally use range poor localization. Accommodating such systematic sensors that build a 3D model of the environment [4], [5], correlated errors is key to building maps successfully, [6], [7]. The second and perhaps the more fundamental and it is also a key complicating factor in robotic problem of applying SLAM for outdoor navigation is the mapping. fact that the planar-world assumption is rarely valid in most 2) Suboptimal: This method is suboptimal as it does not ∗ This work was partially supported by U.S. Army project #DAAD19- utilize all the available pose constraints optimally. 01-2-0012 under subcontract #9613P with General Dynamics Robotic This is due to the fact that the local pose constraints Systems can also be estimated reliably between all adjacent

robot positions, not just the consecutive positions. As II. G EOMETRY OF THE S PECIAL E UCLIDEAN G ROUP long as there are common visible features between For readers not familiar with Lie algebra, we will provide the two locations, we can get an estimate of the a brief introduction. For further details, please refer to the relative motion between those locations. Therefore, texts [13], [14] for a thorough exposition on this topic. the technique must take into account all the avail- able local constraints simultaneously and produce the A. Lie Algebra trajectory that best fits these constraints. A group G is a set with a binary operation, usually writ- In this paper, we will use the Consistent Pose Regis- ten as juxtaposition that satisfies the following properties. tration (CPR) framework of Lu and Milios [9] to effi- 1) The group must be closed under its binary operation. ciently use all the redundant local relative pose constraints 2) The group must be associative. and estimate a global trajectory. CPR is a batch SLAM 3) The group must have a unique identity element. method that estimates the global poses such that it is 4) Every element of the group must have a unique consistent with all the available local pose constraints and inverse. their uncertainties. Their approach solved a 2D SLAM Lie groups satisfy additional axioms. problem, and the 2D pose relations were obtained either 1) The set of group elements forms a differentiable by odometry or by matching range-finder data in adjacent manifold. frames. The global poses were estimated by minimizing 2) The group operation must be a differentiable mani- the Mahalanobis distance between the actual and derived fold. pose over this whole network of pose relations. Since this 3) The map from a group element to its inverse must involves inversion of a large matrix, this approach is likely also be differentiable. to be computationally expensive and was later extended by It can be easiliy seen that the set of nonsingular n × n Gutmann and Konolige [10] to perform online, incremental square matrices forms a group where the group product is pose estimation. The computational properties of CPR for modeled by matrix multiplication. Furthermore, this group very large environments were studied and presented by satisfies the additional axioms given above and hence is a Konolige [11]. Lie group, usually denoted by GL(n) for the general linear In applying CPR for pose estimation in the general group of order n. Intuitively, Lie groups are differentiable case of euclidean motion, one of the major challenges is manifolds on which we can do calculus. Locally, it is topo- the appropriate representation of the motion. In particular, logically equivalent to the vector space Rn , and the local rotations are noncommutative and highly nonlinear. The neighborhood of any group element G can be described space of all euclidean motions is a Lie group and can be by its tangent-space. The tangent-space at the identity represented succinctly using the underlying Lie algebra. We element forms its Lie algebra g. The exponential function propose to solve the CPR problem for the full euclidean maps an element of the Lie algebra to its corresponding case using the underlying Lie algebra. Ours is an iterative group element and the logarithm function is the inverse algorithm that minimizes the sum of Mahalanobis distances mapping from the Lie group to the algebra. For elements by linearizing around the current estimate of the pose at x, y belonging to the Lie algebra, the exponential function each iteration represented in the Lie algebra. satisfies the property exp(x) exp(y) = exp(BCH(x, y)), The work of Govindu [12] is very similar to the work de- where BCH(x, y) is the Baker-Campbell-Hausdorff for- scribed here. However, that approach performs averaging in mula [14]. The BCH formula is given by the series the Lie algebra. Therefore, Govindu treats all the local pose 1 constraints equally and cannot incorporate the uncertainties BCH(x, y) = x + y + [x, y] + O(|(x, y)|3 ) (1) in these pose constraints. The approach described in this 2 paper minimizes the Mahalanobis distance and therefore The bracket in this case is the commutator operation is much more general and has wider applicability. In [x, y] = xy − yx. particular, our approach can be used to fuse uncertain pose B. Euclidean Motion information from various sources in which some constraints are more reliable than others. In particular, the rotation group on R3 is a subgroup of GL(3), defined as The rest of the paper is organized as follows. Section II © ª provides a brief introduction to Lie groups. In particular, SO(3) = R|R ∈ GL(3), RRT = I, det(R) = 1 (2) the geometry of the special euclidean group is described along with its underlying algebra. Our problem formulation The Special Euclidean Group in three dimensions, SE(3), is also discussed in this section. The CPR framework of Lu represents the euclidean transformation of rotation followed and Milios is described in Section III for the linear case. by translation. It is a semidirect product of the Special The nonlinear iterative formulation of CPR is discussed Orthogonal group (SO(3)) and R3 . Using homogenous in IV. The algorithm for CPR for euclidean motion is coordinates, we can represent SE(3) as follows, ½µ ¶ ¯ ¾ also described. Results on both simulated data and real R t ¯ SE(3) = ¯ ∈ GL(4)¯ R ∈ SO(3), t ∈ R 3 sequences are discussed in Section V. Section VI concludes 0 1 this presentation and discusses ongoing and future work. (3)

The action of an element g ∈ SE(3) on a point p ∈ R3 is C. Problem Formulation given by For a motion of the robot, let n be the number of poses µ ¶ R t to be estimated. Let Xi be the pose at the position i, i = g = (4) 1, . . . , n. In the Lie algebra of SE(3), this pose can be 0 1 g.p = Rp + t (5) expressed as Xi ≡ (ωi , ui ). Each local pose constraint, Dij gives us relative motion of the j th pose with reference The Lie algebra of SE(3) is given by to the ith pose. In terms of the Lie algebra, ½µ ¶ ¯ ¾ ω̂ u ¯ exp(Dij ) = exp(Xj ) exp(−Xi ) (16) se(3) = ¯ ∈ GL(4)¯ ω̂ ∈ so(3), u ∈ R 3 0 0 Dij = BCH(Xj , −Xi ) (17) (6) Here ω̂ is the skew symmetric matrix form of the rotation We are also given the covariance matrix Cij for each T vector ω = (ωx , ωy , ωz ) and is an element of the Lie relative pose measurement, Dij . These relative poses may algebra for SO(3). be obtained through various modalities, each having its own characteristic covariance matrix. For example, DijGP S may 0 −ωz ωy be obtained through GPS measurement with the associated ω̂ = ωz 0 −ωx (7) GP S uncertainty Cij and this may be quite different from −ωy ωx 0 the relative pose obtained by, say, visual odometry. This The exponential map, exp : se(3) → SE(3), is given by covariance matrix may be obtained from the measurement µ ¶ µ ¶ equation that relates the measured variables to the pose ω̂ u exp (ω̂) Au coordinates. For example, for a laser range scanner, the exp = (8) 0 0 0 1 measured variables are the range estimates of the points where used to get an estimate of the relative pose. The goal then is to estimate the poses Xi , given these 1 − cos(kωk) kωk − sin kωk 2 A=I+ ω̂ + ω̂ (9) redundant relative pose constraint pairs (Dij , Cij ) which kωk2 kωk3 may be obtained from different modalities (e.g; GPS, range and exp(ω̂) is given by Rodrigue’s formula, scans, visual odometry, a priori information). We will use the Consistent Pose Registration (CPR) framework to solve sin kωk 1 − cos kωk 2 for these poses optimally, a brief introduction to which is exp(ω̂) = I + ω̂ + ω̂ (10) kωk kωk2 provided next. The logarithm map, log : SE(3) → se(3), is given by III. L INEAR C ONSISTENT P OSE R EGISTRATION (CPR) µ ¶ µ ¶ R t log(R) A−1 t The CPR framework of Lu and Milios [9] produces log = (11) 0 1 0 0 a global map by fusing uncertain local pose constraints. where These relative poses enforce geometric consistency, thereby φ producing a globally consistent map. log(R) = (R − RT ) ≡ ω̂ (12) In the linear case, the relative pose Dij is given by 2 sin(φ) and φ satisfies Dij = Xj − Xi (18) Tr (R) = 1 + 2 cos(φ), |φ| < π (13) These measurement equations can be expressed in a matrix form as and where D = HX (19) 1 2 sin kωk − kωk(1 + cos kωk) 2 A−1 = I − ω̂ + ω̂ (14) where X is the concatenation of all the poses X1 , . . . , Xn 2 2kωk2 sin kωk and D is the concatenation of all the relative poses Dij . H Using equations 11 and 8, an element of the Lie group is the incidence matrix with only zeros, and for each row a (R, t) can be mapped directly to an element of its Lie alge- one and a minus one at the appropriate column to form the bra (ω̂, u) and vice versa. We will assume that we are given difference (equation 18) for each relative pose constraint. the relative pose constraints in the Lie algebra. In addition, Assuming a Gaussian, independent generative model for by computing the Jacobian matrix of the transformation the relative pose errors with mean D̄ij and covariance Cij , (either analytically or numerically), it is possible to convert the maximum likelihood pose estimates can be obtained by the covariance matrix in one representation to the other. If minimizing the following Mahalanobis distance (where the J is the jacobian of the exponential mapping and Σse(3) the sum is over all the given relative pose measurements): covariance matrix in the Lie algebra representation, then the X¡ ¢T −1 ¡ ¢ covariance matrix in the Lie group representation is given W = Dij − D̄ij Cij Dij − D̄ij (20) by (i,j) ΣSO(3) = JΣse(3) J T (15) ¡ ¢T ¡ ¢ = D̄ − HX C −1 D̄ − HX (21)

where D̄ is the concatenation of all the observations D̄ij Notice that the form of this equation is identical to the and C is a block diagonal matrix whose entries are the linear case (equation 21), and therefore the Mahalanobis covariance matrices Cij . When C is taken as the identity distance is minimized for ∆X = ∆Xmin given by the matrix, this problem reduces to the standard least squares linear system minimization. The solution Xmin that minimizes this W is G∆Xmin = B (32) obtained by solving the linear system G = H T C −1 H (33) GXmin = B (22) B = H T C −1 Υn (34) G = H T C −1 H (23) Once we calculate ∆Xmin , each of the pose differences B = H T C −1 D̄ (24) δXin becomes known to us and therefore we can use IV. N ONLINEAR CPR equation 28 to update the pose estimates at the next iteration. This process is repeated until convergence. Unlike the linear case (equation 22), there does not exist Intuitively, each iteration of the algorithm seeks to find a closed-form expression for Xmin in the general case changes in the pose that will lead to reduction in the where the relative poses are determined by a nonlinear Mahalanobis distance. This is accomplished by solving a equation. Therefore, the minimum solution is obtained linear CPR problem at each step. Therefore, this algorithm iteratively by linearizing around the current solution at each is very much like a gradient descent algorithm and is guar- iteration. Let f be the pose compounding operation in the anteed to find only a local minima. In practice, however, nonlinear case. Therefore, the pose measurement equation the algorithm performs very well and finds solutions that is given by are globally optimal for most situations. Dij = f (Xj , −Xi ) (25) Lie-algebraic CPR To a first degree this can be approximated by Dij ≈ Xj − The iterative algorithm for nonlinear CPR described in Xi . Corresponding to a relative pose measurement D̄ij , the the previous section can be directly applied to the problem error in this measurement is given by of registering euclidean motions in the Lie algebra. The Υij = D̄ij − f (Xj , −Xi ) (26) pose compounding function in se(3) is given by the BCH formula, that is, f ≡ BCH. The BCH formula for SE(3) Therefore, the Mahalanobis distance becomes has a closed-form expression [14]. X The algorithm for performing CPR in the Lie algebra of −1 W = ΥTij Cij Υij (27) SE(3) is outlined below (i,j) 1) At the start of the algorithm compute G = H T C −1 H At the nth iteration, let Xin and Xjn be the pose estimates (equation 33) and K = H T C −1 (equation 34). at locations i and j, respectively, and Υnij be the error 2) Start with random initial poses Xi1 . corresponding to the relative pose constraint between these 3) Repeat steps 4-7 until convergence. two poses. For the next iteration, we will have to refine 4) Determine Υ, where for each constraint between these pose estimates. Let δXin be the change in the ith poses i and j, Υij = D̄ij − BCH(Xj , −Xi ) (equa- pose between the successive iterations n and n + 1. We tion 26). have 5) Determine B = KΥ (equation 34). Xin+1 = Xin + δXin (28) 6) Solve the linear system G∆Xmin = B for ∆Xmin (equation 32). Also, the error (Υij ) between the two iterations can be 7) Update the poses as Xin+1 = Xin + δXi (equa- approximated to a first degree as tion 28), where δXi is obtained from ∆Xmin . ¡ ¢ Υn+1 ij = Υnij − δXjn − δXin (29) The convergence of the algorithm is determined by looking at the norm of the update vector ∆Xmin . In other words, the This is easily derived through a Taylor series expansion of iterations are carried out as long as k∆Xmin k ≥ ², where the right side of equation 26. In matrix form, equation 29 ² is the desired accuracy. For computational efficiency, can be written as the inverse of the matrix G and the K matrix can be precomputed at the start of the algorithm. Therefore, each Υn+1 = Υn − H∆X (30) iteration of the algorithm involves only two matrix vector where Υn+1 and Υn are the concatenation of all Υn+1 ij products (O(n2 )). In the next section, we present exper- and Υnij , respectively, ∆X is the concatenation of all the imental results of applying our algorithm to a simulated pose differences δXin , and H is the incidence matrix (as sequence and also a real sequence where pose constraints in equation 19). Substituting for Υn+1 from this equation are obtained by visual odometry. into equation 27, the Mahalanobis distance at the (n + 1)th V. R ESULTS iteration can be written in matrix form as Figure 1 shows the 3D trajectory consisting of 100 poses T W = (Υn − H∆X) C −1 (Υn − H∆X) (31) X1 , . . . , X100 used in our simulation for experimental

validation of our algorithm. Although the orientation of the that the error is much more well behaved and more or robot at these poses is not shown, it also varies substantially less spread uniformly throughout. The accumulated error between two consecutive locations. Thus, the poses Xi in this case is 354 mm, which is only 1.58% of the have the full six degrees of freedom. The robot starts total distance travelled. Thus, incorporating covariances and ends at the same location and orientation, thereby and minimization of Mahalanobis distances reduces the resulting in a closed loop. The total distance traveled is error dramatically. 22346 mm. Relative pose measurements were obtained 7000 Plot 1 Trajectory of a moving robot with 101 poses Plot 2 6000 7000 5000 6000 localization error 4000 5000 4000 3000 Z 3000 2000 2000 1000 0 2000 1000 0 500 −2000 0 −4000 −500 0 −1000 0 10 20 30 40 50 60 70 80 90 100 −6000 pose index −1500 −8000 −2000 Y X Fig. 2. Error plot obtained from adjacent pose relations Fig. 1. 3D trajectory of a moving robot with 100 poses As the number of relative pose links is increased, more between 15 consecutive locations, that is, we have local constraints are added to the system. Therefore, the error pose constraints, Dij for all i, j such that j > i and |j−i| ≤ in localization should decrease. Figure 3 shows the effect 15. Each such pose measurement, D̃ij can be computed of adding more constraints on the localization error. Plot as D̃ij = BCH(Xj , −Xi ). For each such constraint, we 1 is the error plot obtained by linking consecutive poses need to simulate noise during the measurement process. together (this is same as the error plot 2 of the previous This is accomplished by first generating a 6 × 6 random figure). Plot 2 is the error plot obtained where constraints symmetric positive definite matrix, Cij , for each D̃ij . Next, from two consecutive pose constraints are incorporated, we generate a random correlated vector hij with covariance and plot 3 is the error plot obtained by adding constraints matrix Cij . hij is added to D̃ij to obtain the observed from five adjacent relative pose links. Thus, it is clear that measurement D̄ij . In other words the error dramatically reduces as we add more local pose links. The accumulated error for the trajectory obtained by D̄ij = BCH(Xj , −Xi ) + hij (35) considering five adjacent pose links is only 0.02%. Thus, the inputs to our algorithm are the measured relative 400 pose pairs (D̄ij , Cij ). The outputs are the poses X̄i com- Plot 1 (1 adjacent links) Plot 2 (2 adjacent links) puted at all the 100 locations on the trajectory. We initialize 350 Plot 3 (5 adjacent links) the algorithm with all the poses set to zero. The error of 300 the results obtained is measured by finding the distance between the computed locationp (t̄i ) and the ground truth 250 location (ti ); that is, ei = |ti − t̄i |. localization error Given these pose links, it is straightforward to link the 200 consecutive poses together and thus reconstruct the trajec- tory from the 99 consecutive local pose links (D̄i(i+1) ). 150 Obviously, this does not take into account the covariance 100 estimates of these adjacent links. Figure 2, plot 1 shows the error in the locations obtained by linking these adjacent 50 poses together. The X axis is the pose index, and the Y axis is the error. Notice that the error in localization 0 0 10 20 30 40 50 60 70 80 90 100 pose index keeps on accumulating as we move further. At the end, the accumulated error is 6052 which is about 27% of the Fig. 3. Effect of increasing the number of local pose constraints total distance traveled. Plot 2 in the same figure is an error plot obtained by applying our Lie algebraic CPR algorithm If we do not have information about the covariance to just the consecutive pose relations (D̄i(i+1) ). Notice matrix, then we may take it to be the identity matrix. In

0 such a case, the Mahalanobis distance becomes the sum Plot1 − Stitching consecutive frames Plot2 − CPR using consecutive poses Plot3 − CPR using 2 adjacent poses of the squared errors and the problem simply becomes −1000 Plot4 − CPR using 4 adjacent poses the standard least squares minimization. Figure 4 compares the error plot obtained by least squares minimization with −2000 that of Mahalanobis distance minimization for the case of five adjacent links. From this error plot, it is clear that Z mm −3000 incorporating the covariance matrix helps reduce the overall error. −4000 −5000 60 Plot 1 (Least squares minimization) Plot 2 (Mahalanobis distance minimization) −6000 200 100 50 0 −100 −3000 −2000 −1000 0 −6000 −5000 −4000 Y mm X mm 40 Fig. 5. CPR applied to a real-world sequence localization error 30 VI. C ONCLUSION 20 We have presented a Lie algebraic framework for the 10 global localization of a moving robot from uncertain rela- tive pose estimates. Given these pose constraints and their 0 0 10 20 30 40 50 60 70 80 90 100 uncertainties, we formulate the problem as minimizing the pose index Mahalanobis distances in the Lie algebra. This nonlinear minimization problem is solved by iterative application Fig. 4. Comparison of least squares minimization with our approach of the consistent pose registration framework of Lu and Milios. Experimental results demonstrate that our algorithm converges rapidly and the minimization is not very much Our algorithm typically converges in about 40 to 50 affected by the initial starting point. Our approach can be iterations for a precision corresponding to ² = 1e−5 . In applied to robots moving in outdoor environments where addition, we have observed that the initial starting point for the motion is not necessarily planar. This extends the the nonlinear minimization does not affect the computed applicability of robot navigation to rugged, natural terrains. poses significantly. Although we have demonstrated the proof of concept of our approach by applying it to a simulated sequence and a We have also applied our approach to a real-world real experiment with pose relations obtained through visual sequence consisting of 50 poses. A pair of stereo cameras odometry, we are in the midst of extensive testing and was mounted on a cart and moved in an indoor hallway experimental evaluations. In particular, we plan to combine at the intersection of two corridors. The camera was relative pose measurements from different modalities such moved so as to ‘cut the corner’ at the intersections of as visual sensing, vehicle odometry, and laser range finders the two corridors. Relative poses between every 4 adjacent to obtain the trajectory of the vehicle and compare it frames were obtained through visual odometry [8]. The with precise ground truth measurements using differential covariance matrix for the relative pose was computed from GPS. We are also investigating the effect of the number the Jacobian matrix of the nonlinear function which is of available relative pose measurements on the recovered minimized to estimate the pose from the visual features. trajectory. As formulated in this paper, our approach is Figure 5, plots 2,3,4 shows the estimated trajectory of essentially a batch method and requires the inversion of a the vehicle using the approach described in this paper. large matrix. This may become computationally prohibitive The different trajectories correspond to different number for large sequences. We are currently working on develop- of adjacent pose constraints used to solve the CPR. For ing an incremental version of our algorithm in the lines reference, the trajectory obtained by linking consecutive of [10] and also looking at computational aspects of our poses (as in Figure 2) is shown in plot 1. Since ground algorithm for large-scale map making [11]. truth is not available for this data, we can not compare the errors, but the trajectories from the plots 1 and 4 are quite different, especially towards the end of the sequence. ACKNOWLEDGMENT Linking consecutive poses leads to error accumulation as we proceed and therefore the endpoints are different. As The author thanks Kurt Konolige at SRI International more and more relative pose constraints are added, the CPR and Venu Govindu at the University of Maryland for initial algorithm, probably converges to the true pose estimates. discussions and ideas leading to this paper.

R EFERENCES [1] S. Thrun, “Robotic mapping: A survey,” in Exploring Artificial Intelligence in the New Millenium, G. Lakemeyer and B. Nebel, Eds. Morgan Kaufmann, 2002. [2] H. H. Gonzalez-Banos and J. C. Latombe, “Navigation strategies for exploring indoor environments,” International Journal of Robotics Research, 2002. [3] J. D. Tards, J. Neira, P. M. Newman, and J. J. Leonard, “Robust mapping and localization in indoor environments using sonar data,” The International Journal of Robotics Research, 2002. [4] C. Wang, C. Thorpe, and S. Thrun, “Online simultaneous localiza- tion and mapping with detection and tracking of moving objects: Theory and results from a ground vehicle in crowded urban areas,” in Proceedings IEEE International Conference on Robotics and Automation, May 2003. [5] C. Brenneke, O. Wulf, and B. Wagner, “Using 3d laser range data for slam in outdoor environments,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), October 2003, pp. 27–31. [6] J. Guivant, E. Nebot, and S. Baiker, “Autonomous navigation and map building using laser range sensors in outdoor applications,” Journal of Robotics Systems, vol. 17, no. 10, pp. 565–583, October 2000. [7] M.A.Garcia and A.Solanas, “3d simultaneous localization and mod- eling from stereo vision,” in IEEE International Conference on Robotics and Automation, 2004, pp. 847–853. [8] D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry,” in Proc. IEEE Conference on Computer Vision and Pattern Recognition, June 2004. [9] F. Lu and E. Milios, “Globally consistent range scan alignment for environment mapping,” Autonomous Robots, vol. 4, pp. 333–349, 1997. [10] J. Gutmann and K. Konolige, “Incremental mapping of large cyclic environments,” in Proc. IEEE International Symposium on Compu- tational Intelligence in Robotics and Automation (CIRA), Monterey, California, November 1999, pp. 318–325. [11] K. Konolige, “Large-scale map-making,” in Proc. National Confer- ence on AI (AAAI), 2004. [12] V. M. Govindu, “Lie-algebraic averaging for globally consistent motion estimation,” in Proc. IEEE Conference on Computer Vision and Pattern Recognition, June 2004. [13] J. Selig, Geometrical Methods in Robotics. Springer-Verlag, 1996. [14] V. Varadarajan, Lie Groups, Lie Algebras and Their Representations, ser. Graduate Texts in Mathematics. Springer-Verlag, 1984, vol. 102.

You can also read