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 adjacentrobot 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 experimentalvalidation 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. In0
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
