Globally Consistent Range Scan Alignment for Environment Mapping.
A robot exploring an unknown environment may need to build a world model from sensor measurements. In order to integrate all the frames of sensor data, it is essential to align the data properly. An incremental approach has been typically used in the past, in which each local frame of data is aligned to a cumulative global model, and then merged to the model. Because different parts of the model are updated independently while there are errors in the registration, such an approach may result in an inconsistent model.In this paper, we study the problem of consistent registration of multiple frames of measurements (range scans), together with the related issues of representation and manipulation of spatial uncertainties. Our approach is to maintain all the local frames of data as well as the relative spatial relationships between local frames. These spatial relationships are modeled as random variables and are derived from matching pairwise scans or from odometry. Then we formulate a procedure based on the maximum likelihood criterion to optimally combine all the spatial relations. Consistency is achieved by using all the spatial relations as constraints to solve for the data frame poses simultaneously. Experiments with both simulated and real data will be presented.
Introduction
1.1 Problem Definition
The general problem we want to solve is to let a mobile robot explore an unknown environment
using range sensing and build a map of the environment from sensor data. In this paper, we
address the issue of consistent alignment of data frames so that they can be integrated to form
a world model. However, the issue of building a high-level model from registered sensor data is
beyond the scope of this paper.
A horizontal range scan is a collection of range measurements taken from a single robot position.
In previous robot navigation systems, range scans have often been used for robot self-localization
in known environments [3]. Using range measurements (sonar or laser) for modeling an unknown
environment has also been studied in the past [11, 4, 8]. A range scan represents a partial view of
the world. By merging many such scans taken at different locations, a more complete description
of the world can be obtained. Figure 1 gives an example of a single range scan and a world model
consisting of many scans.
a b

Figure

1: Building world model from range scans. (a) One range scan in a simulated world; (b)
model consisting of many scans. The small circles show the poses at which the scans are taken.
The essential issue here is to align the scans properly so that they can be merged. But the
difficulty is that odometry information alone is usually inadequate for determining the relative
scan poses (because of odometry errors that accumulate). On the other hand, we are unable to
use pre-mapped external landmarks to correct pose errors because the environment is unknown.
A generally employed approach of building a world model is to incrementally integrate new data
to the model. When each frame of sensor data is obtained, it is aligned to a previous frame or to
a cumulative global model. Then the new frame of data is integrated into the global model by
averaging the data or using a Kalman filter [1, 10, 11, 4, 8]. A major problem with this approach
is that the resulting world model may eventually become inconsistent as different parts of the
model are updated independently. Moreover, it may be difficult to resolve such inconsistency if
the data frames have already been permanently integrated.
To be able to resolve inconsistency once it is detected at a later stage, we need to maintain
the local frames of data together with their estimated poses. In addition, we need a systematic
method to propagate pose corrections to all related frames.
Consider an example as shown in Fig. 2(a). The robot starts at P 1 and returns to a nearby
location P n after visiting along the way. By registering the scan taken at P n against
scan P n\Gamma1 , the pose of P n can be estimated. However since P n is close to P 1 , it is also possible to
derive pose P n based on P 1 by matching these two scans. Because of errors, the two estimates of
could be conflicting. If a weighted average of the two is used as the estimate of P n , the pose
of P should also be updated as otherwise the relation P will be inconsistent with its
previous estimate. This inconsistency could be significant if the looped path is long. Similarly,
other poses along the path should also be updated. In general, the result of matching pairwise
scans is a complex, and possibly conflicting, network of pose relations. We need a uniform
framework to integrate all these relations and resolve the conflicts.
In this paper, we present such a framework for consistently registering multiple range scans. The
idea of our approach is to maintain all the local frames of data as well as a network of spatial
relations among the local frames. Here each local frame is defined as the collection of sensor data
measured from a single robot pose. The robot pose, in some global reference frame, is also used
to define the local coordinate system of the data frame. Spatial relations between local frames are
derived from matching pairs of scans or from odometry measurements. We treat the history of
robot poses in a global coordinate system (which define all the local frame positions) as variables.
Our goal is to estimate all these pose variables using the network of constraints, and register the
scans based on the solved poses. Consistency among the local frames is ensured as all the spatial
relations are taken into account simultaneously.

Figure

2 shows an example of consistently aligning a set of simulated scans. Part (a) shows the
original scans badly misaligned due to accumulated pose errors. Part (b) shows the result of
aligning these scans based on a network of relative pose constraints (with edges indicated by line
segments).
a
Pn

Figure

2: An example of consistently aligning a set of simulated scans. (a) Original scans badly
misaligned due to accumulated pose errors; (b) the result of aligning these scans based on a
network of relative pose constraints. The constraints are indicated by line segments connecting
pairs of poses. Two types of constraints are used: those derived from aligning a pair of scans
(marked by both solid and dotted lines), and those from odometry measurements (marked by
solid lines).
1.2 Related Work
The first project that systematically studied the consistency issue in dynamic world modeling
is the HILARE project [2]. In this system, range signals are segmented into objects which are
associated with local object frames. Each local frame is referenced in an absolute global frame
along with the uncertainty on the robot's pose at which the object frame is constructed. New
sensor data are matched to the current model of individual object frames. If some object which
has been discovered earlier is observed again, its object frame pose is updated (by averaging).
In circumstances that the uncertainty of some object frame is less than the uncertainty of the
current robot pose, as it happens when the object frame is created earlier, and later the robot
sees the object again, the robot's pose may be corrected with respect to that object frame. After
correcting the current robot pose, the correction is propagated backwards with a "fading" effect
to correct the previous poses. Although the HILARE system addressed the issue of resolving
model inconsistency, its solution has the following potential problems. First of all, the system
associates local frames with "objects". But if the results of segmenting sensor data or matching the
data with model are imperfect, the "objects" and therefore the local frames may not be defined
or maintained consistently. When a previously recorded object is detected again, the system
only attempts to update the poses (and the associated frames) along the path between the two
instances of detecting this object, while the global consistency among all frames in the model
may not be maintained. HILARE uses a scalar random variable to represent the uncertainty of
a three-degree-of-freedom pose, therefore it can not model the confidences in the individual pose
components.
Moutarlier and Chatila presented a theoretical framework for fusing uncertain measurements for
environment modeling [14]. They first discussed two types of representations: relation-based
and location-based. In relation-based representation, an object is related to another by the
uncertain transform between their reference frames. A network of relationships is maintained as
the world model. When new observations are made, all the relationships need to be updated
to preserve consistency. In location-based representation, the global references of individual
object frames are maintained together with their uncertainties. When objects are re-observed,
these object frames and other related frames are updated with respect to the global reference
frame. After comparing these two approaches, Moutarlier and Chatila choose to use the location-based
approach. They treat the object and robot locations as state variables and maintain all
the object variance/covariance matrices as state information. A stochastic-based formulation
for fusing new measurements and updating the state variables is introduced. In addition to a
global updating approach, they also introduced a relocation-fusion approach which first updates
the robot position based on the new observations and then updates the object frames. The
relocation-fusion approach reduces the influence of sensor bias in the estimation, although the
algorithm is suboptimal.
In a series of work by Durrant-Whyte [5, 6, 7], the problem of maintaining consistency in a
network of spatial relations was studied thoroughly. In their formulation, the environment model
is represented by a set of spatial relations between objects. A probabilistic fusion algorithm similar
to the Kalman filter is employed to integrate new measurements to the a priori model. When
some relations are updated as a result of new observations, the consistency among all relations
are enforced by using explicit constraints on the loops of the network. The updating procedure is
formulated as constrained optimization and it allows new observations to be propagated through
the network while consistency between prior constraints and observed information is maintained.
In another similar approach, Tang and Lee [17] formulated a geometric feature relation graph for
consistent sensor data fusion. They proposed a two-step procedure for resolving inconsistency in
a network of measurements of relations. In the first step, a compromise between the conflicting
measurements of relations is achieved by the fusion of these measurements. Then in the second
step, corrections are propagated to other relations in the network.
The difficulty in maintaining model consistency in a relation-based representation is that the
relations are not independent variables. Therefore additional constraints are needed in formulating
an updating procedure. The constrained optimization approach seems very complicated and
difficult to apply in practice.
In view of the previous methods, we present a new approach which has the following distinctive
characteristics:
1. We use an unambiguous definition of an object frame as the collection of sensor measurements
observed from a single robot position. Thus we avoid the difficult task of segmenting and recognizing
objects (which the previous methods rely on in order to create and update object frames).
It is also important to note that we use a robot pose to define the reference for an object frame.
In a local frame, the relative object positions with respect to the robot pose are fixed (whose
uncertainty is no more than bounded sensing errors). During the estimation process, when the
robot position in the global reference frame is updated, effectively the global coordinates of all
objects in the current frame are updated accordingly. Therefore by maintaining the history of
robot poses, we also maintain the spatial relationships among the object frames.
2. Our approach uses a combination of relation-based and location-based representations. We
treat relations as primitives, but treat locations as free variables. This is different from the pure
relation-based approach in that we do not directly update the existing relations in the network
when new observations are made. We simply add new relations to the network. All the relations
are used as constraints to solve for the location variables which, in turn, define a set of updated and
consistent relations. On the other hand, our approach is different from the location-base approach
by Moutarlier and Chatila [14] in that we do not assume the covariance matrices between the
object frames as known. Our state information is the entire set of raw relations. We derive the
covariance matrices at the same time as we solve for the position variables.
3. We obtain direct spatial relations between object frames. Because our object frames are tied to
robot poses, odometry measurements directly provide spatial relations between the frames. More
importantly, we may align two overlapping frames of data (in our case range scans) to derive more
accurate relations between frames. In previous approaches, the robot typically relies on odometry
to first determine its new pose. Then the detection of objects allows the robot pose as well as
the object locations to be updated. Since the relations between object frames are updated rather
indirectly through the robot pose, biases in odometry measurements may lead to divergence in the
estimation of object positions, as reported in [14]. Moutarlier and Chatila propose an algorithm
that is supposed to address the divergence problem at the expense of a sub-optimal solution.
Our formulation does not have this problem, as we obtain direct spatial relations between object
frames by aligning the data, and therefore we are less sensitive to odometry biases.
2 Overview of Approach
We formulate our approach to multiple scan registration as one of estimating the global poses
of the scans, by using all the pose relations as constraints. Here the scan poses are considered
as variables. A pose relation is an estimated spatial relation between the two poses which can
be derived from matching two range scans. We also obtain pose relations from odometry mea-
surements. Finally, we estimate all the poses by solving an optimization problem. The issues
involved in this approach are discussed in the following subsections.
2.1 Deriving Pose Relations
Since we use a robot pose to define the local coordinate system of a scan, pose relations between
scans can be directly obtained from odometry which measures the relative movement of the robot.
In section 4.2, we will discuss the representation of odometry pose constraint and its uncertainty.
More accurate relations between scan poses are derived from aligning pairwise scans of points.
Here any pairwise scan matching algorithm can be used. One possible choice is the extension to
Cox's algorithm [3] where line segments are first fit to one scan and then points in another scan
are matched to the derived line segments. In our previous studies, we proposed another scan
matching algorithm which is based on direct point to point matching [12, 13]. In either case, the
scan matching algorithm takes two scans and a rough initial estimate of their relative pose (for
example from odometry information) as input. The output is a much improved estimate of the
relative pose.
After aligning two scans, we can record a set of corresponding points on the two scans. This
correspondence set will form a constraint between the two poses. In section 4.3, we will formulate
this type of constraint and its uncertainty as used in the estimation algorithm.
When we match two scans, we first project one scan to the local coordinate of the other scan,
and discard the points which are likely not visible from the second pose. The amount of overlap
between two scans is estimated empirically from the spatial extent of the matching parts between
the two scans. A pose relation is only derived when the overlap is significant enough (larger than
a given threshold).
2.2 Constructing a Network of Pose Relations
Given the pairwise pose relations, we can form a network. Formally, the network of constraints
is defined as a set of nodes and a set of links between pairs of nodes. A node of the network is
a pose of the robot on its trajectory at which a range scan is taken. Here a pose is defined as
a three dimensional vector (x; consisting of a 2D robot position and the home orientation
of the rotating range sensor. We then define two types of links between a pair of nodes. First,
if two poses are adjacent along the robot path, we say that there is a weak link between the two
nodes which is the odometry measurement of the relative pose. Second, if the range scans taken
at two poses have a sufficient overlap, we say that there is a strong link between the two nodes.
To decide whether there is sufficient overlap between scans, we use an empirical measure. The
spatial extent in the overlapping part of two scans should be larger than a fixed percentage of
the spatial extent covered by both scans.
For each strong link, a constraint on the relative pose is determined by the set of corresponding
points on the two scans given by the matching algorithm. It is possible to have multiple links
between two nodes. Figure 3 shows an environment and the constructed network of pose relations.
2.3 Combining Pose Relations in a Network
The pose relations in a network can be potentially inconsistent because they are not independent
variables (the number of relations may be more than the degrees of freedom in the network),
while the individually estimated relations are prone to errors. Our task is to combine all the
pose relations and resolve any inconsistency. This problem is formulated as one of optimally

Figure

3: Example of constructing a network of pose relations from matching pairwise scans. (a)
A simulated environment where the scan poses are labeled by circles; (b) the network of pose
relations constructed from matching overlapping scans.
estimating the global poses of nodes in the network. We do not deal with the relations directly.
Rather, we first solve for the nodes which constitute a set of free variables. Then a consistent set
of relations which represents a compromise of all a priori relations is defined by the poses on the
nodes.
An optimization problem is defined as follows. We construct an objective function from the
network with all the pose coordinates as variables (except one pose which defines our reference
coordinate system). Every link in the network is translated into a term in the objective function
which can be conceived as a spring connecting two nodes. The spring achieves minimum energy
when the relative pose between the two nodes equals the measured value (either from matching
two scans or from odometry). Then the objective function represents the total energy in the
network. We finally solve for all the pose variables at once by minimizing this total energy
function.
2.4 The Three-Node Example
Using the 3-node example, we illustrate the difference of our formulation from previous approaches

Assume that the network consists of three nodes: relations
. When there is new measurement -
the algorithm by
Durrant-Whyte [6] updates the three relations to T 0
3 based on an optimization criterion
which is subject to the constraint T 0
In our approach, we pool together all the relations T
T 1 to form an optimization
problem and solve for a new estimate for the nodes: P 0
3 . These node positions define a
consistent set of relations: T 0
1 . Note that the node positions are
so we do not need to solve a complex constrained system.
Moutarlier and Chatila [14] also treat the node positions as variables when updating the network
with new measurements. But they assume the knowledge of covariance matrices among the a
priori estimates of However, we only require the variances of individual measurement
errors on the relations T
are directly available from sensor models.
The rest of the paper is organized as follows. In section 3, we present the optimization criterion
by considering a generic optimal estimation problem. We derive a closed-form solution in a linear
special case. In section 4, we formulate the pose relations as well as the objective function in
the context of range scan registration. The closed-form solution derived in section 3 is applied to
solve for the scan poses. In section 5, we present experimental results.
Optimal Estimation from a Network of Relations
In this section, we formulate a generic optimal estimation algorithm which combines a set of
relations in a network. This algorithm will later be applied in section 4 in the context of robot
pose estimation and scan data registration.
3.1 Definition of the Estimation Problem
We consider the following generic optimal estimation problem. Assume that we are given a net-work
of uncertain measurements about n+1 nodes X Here each node X i represents
a d-dimensional position vector. A link D ij between two nodes X i and X j represents a measurable
difference of the two positions. Generally, D ij is a (possibly nonlinear) function of X i and
and we refer to this function as the measurement equation. Especially interesting to us is the
simple linear case where the measurement equation is
We model an observation of D ij as -
a random Gaussian error
with zero mean and known covariance matrix C ij . Given a set of measurements -
pairs of nodes and the covariance C ij , our goal is to derive the optimal estimate of the position
by combining all the measurements. Moreover, we want to derive the covariance matrices of
the estimated X i 's based on the covariance matrices of the measurements.
Our criterion of optimal estimation is based on the maximum likelihood or minimum variance
concept. The node position X i 's (and hence the position difference D ij 's) are determined in
such a way that the conditional joint probability of the derived D ij 's, given their observations
ij 's, is maximized. If we assume that all the observation errors are Gaussian and mutually
independent, the criterion is equivalent to minimizing the following Mahalanobis distance (where
the summation is over all the given measurements):
(D
Even if the observation errors are not independent, a similar distance function can still be formed.
However, it will involve the correlation matrices of the measurements. The assumption of independence
is actually not necessary in our formulation. The assumption makes practical sense as
the covariances of errors are difficult to estimate.
A typical application of the optimal estimation problem is in mobile robot navigation, where we
want to estimate the robot pose and its uncertainty in three degrees of freedom (x; '). The
observations are the relative robot poses from odometry, and also possible from matching sensor
measurements. We want to utilize all the available measurements to derive the optimal estimate
of the robot poses. Note that in this application, the measurement equation is non-linear because
of the ' component in the robot pose.
Our approach above differs from the one typically used within a Kalman filter formulation, in
which only the current pose is estimated, while the history of previous poses and associated
measurements is collapsed into the current state of the Kalman filter. Our objective, however, is
not simply getting from A to B safely and accurately, but also building a map of the environment.
It is, therefore, meaningful to use all the measurements obtained so far in the mapping process.
The old poses themselves are not particularly useful. But we are using the poses to define local
object frames. Thus maintaining the history of robot poses is equivalent to maintaining the
structure of the environment model. The advantage of using a pose to define a data frame is
that it is unambiguous and it avoids the difficult segmentation and object identification problem
present in other work.
Next, we study the case when the measurement equation is linear and we derive closed-form
solutions for the optimal estimates of the nodes and their covariances. Later, we will solve the non-linear
robot pose estimation problem by approximately forming linear measurement equations.
3.2 Solution of Optimal Linear Estimation
We consider the special case where the measurement equation has the simple linear
are the nodes in the network which are d-dimensional vectors
and the D ij 's are the links of the network. Without loss of generality, we assume that there is a
link D ij between every pair of nodes . For each D ij , we have an observation -
which is
assumed to have Gaussian distribution with mean value D ij and known covariance C ij . In case
the actual link D ij is missing, we can simply let the corresponding C
ij be 0. Then the criterion
for the optimal estimation is to minimize the following Mahalanobis distance:
0-i!j-n
Note that W is a function of all the position X i 's. Since we can only solve for relative positions
given the relative measurements, we choose one node X 0 as a reference and consider its coordinate
as constant. Without loss of generality, we let X
relative positions from X 0 .
We can express the measurement equations in a matrix form as
where X is the nd-dimensional vector which is the concatenation of is the
concatenation of all the position differences of the form D H is the incidence
matrix with all entries being 1, \Gamma1, or 0. Then the function W can be represented in matrix form
as:
D is the concatenation of all the observations -
ij for the corresponding D ij and C is the
covariance of -
D which is a square matrix consists of C ij 's as sub-matrices.
Then the solution for X which minimizes W is given by
The covariance of X is
If the measurement errors are independent, C will be block-diagonal and the solution can be
simplified. Denote the nd \Theta nd matrix H t C \Gamma1 H by G and expand the matrix multiplications.
We can obtain the d \Theta d sub-matrices of G as
Denote the nd-dimensional vector H t C
D by B. Its d-dimensional sub-vectors are the following
(let -
Then the position estimates and covariance can be written as
The above algorithm requires to be invertible. If the network is fully connected
and the individual error covariances are normally behaved, we believe it is possible to prove that
G is invertible. Note the dimension of G (number of free nodes) is less than or equal to the
dimension of C (number of edges) in a fully connected graph.
3.3 Special Networks
(b)
(a)

Figure

4: (a) Serial connection; (b) parallel connection.
We will apply the formula in Eq. 9 to two interesting special cases as in Figure 4. First, if the
network consists of two serially connected links, D 01 and D 12 , the derived estimate of X 2 and its
covariance matrix are
Another case to consider is the network which consists of two parallel links D 0 and D 00 between
two nodes X 0 and X 1 . If the covariance of the two links are C 0 and C 00 , the estimate of X 1 and
its covariance are given by
The solution is equivalent to the Kalman filter formulation. The above two cases correspond to
the compounding and merging operations given by Smith and Cheeseman [16], which are used to
reduce a complex network to a single relation. Smith and Cheeseman's algorithm has a limitation
that it only applies to networks formed by serial and parallel connections.

Figure

5: A Wheatstone bridge network.
Consider the network in the form of a Wheatstone bridge (Fig. 5). The estimate of X 3 can not
be obtained through compounding and merging operations. Therefore, the method by Smith
and Cheeseman can not be directly applied to simplify this network 1 , while in our method, the
variables can be solved from the linear system
G =B @
\GammaC \Gamma1-
The covariance matrix for the estimated position X 3 has a nice symmetric form (derived by
expanding G \Gamma1
1 It is possible to first convert a triangle in the network to an equivalent Y-shaped connection and then the
network becomes one with serial and parallel links. However, this Delta-to-Y conversion still can not turn every
network into a combination of serial and parallel connections.
4 Derivation of Pose Relations
In this section, we will apply the optimal estimation algorithm, as derived in section 3, to the
robot pose estimation and scan data registration problem. To do this, we need to derive linearized
measurement equations for the pose relations. In the following subsections, we study a constraint
on pose difference given by matched scans or odometry measurements. For each constraint, we
formulate a term in the form of Mahalanobis distance. For convenience in discussions of pose
measurements, we will first define a pose compounding operation.
4.1 Pose Compounding Operation
Assume that the robot starts at a pose its pose by
ending up at a new pose V a = (x a ; y a ; ' a ) t . Then we say that pose V a is
the compounding of V b and D. We denote it as:
The coordinates of the poses are related by:
y
This is the same compounding operation as defined by Smith and Cheeseman [16]. If we consider
that an absolute pose defines a coordinate system (the xy coordinates of the origin and the
direction of one axis), and a relative pose defines a change of coordinate system (a translation
followed by a rotation), then the compounding operation gives the pose which defines the new
coordinate system after the transformation. The compounding operation is not commutative,
but it is associative. We can thus define the compounding of a series of poses.
It is also useful to define the inverse of compounding which takes two poses and gives the relative
pose:
The coordinates are related by the following equations:
If D ab is the relative pose V a \Psi V b , the reversed relative pose D a can be obtained from
D ab by a unary operation:
We can verify that (\PsiD) \Phi
We also want to define a compounding operation between a full 3D pose
2D position vector . The result is another 2D vector u We still denote the
operation as
The coordinates for u 0 are given by the first two equations of the full 3D pose compounding
(Eq. 18,19). This 2D compounding operation is useful for transforming an non-oriented point
(typically from a range sensor) from its local sensor coordinate system to the global coordinate
system.
4.2 Pose Relations from Matched Scans
Let V a and V b be two nodes in the network and assume there is a strong link connecting the
two poses. From the pairwise scan matching algorithm, we get a set of pairs of corresponding
points: u a
where the 2D non-oriented points u a
k are from scan S a and S b ,
respectively. Each pair (u a
corresponds to the same physical point in the robot's environment
while they are represented in different local coordinate systems. If we ignore any sensing or
matching errors, two corresponding points are related by:
If we take the random observation errors into account, we can regard \DeltaZ k as a random variable
with zero mean and some unknown covariance C Z
k . From the correspondence pairs, we can form
a constraint on the pose difference by minimizing the following distance function:
F ab (V a
k(V a \Phi u a
If we notice that a pose change is a rigid transformation under which the squared Euclidean
distance k \Delta k 2 is invariant, we can rewrite the function in an equivalent form:
F ab (V a
k((V a \Psi V b ) \Phi u a
Thus F ab is a function of D . The solution of D 0 which minimizes F ab can be derived
in closed-form (see [12]). The relation D is the measurement equation.
In order to reduce F ab into the Mahalanobis distance form, we linearize each term \DeltaZ k . Let
close estimates of V a and V b . Denote \DeltaV
and \DeltaV
(the global coordinates of a pair of
matching points). Then for small \DeltaV a and \DeltaV b , we can derive from Taylor expansion:
V a \Phi u a
\DeltaV a \Gamma
\DeltaV b
V a \Phi u a
H a \DeltaV a \Gamma -
where
H a =B @
y a
We can rewrite Eq.
where
V a \Phi u a
H a \DeltaV a \Gamma -
Thus we can now regard D in Eq. 35 as the pose difference measurement equation to replace
. For the m correspondence pairs, we can form m equations as in Eq. 32. If we
concatenate the -
Z k 's to form a 2m \Theta 1 vector Z, and stack the M k 's to form a 2m \Theta 3 matrix M,
then F ab can be rewritten as a quadratic function of D:
F ab
We can then solve for the
D which minimizes F ab as
The criterion of minimizing F ab (D) constitutes a least-squares linear regression. In Eq. 32, M k is
known and -
Z k is observed with an error \DeltaZ k having zero mean and unknown covariance C Z
k . If
we assume that all the errors are independent variables having the same Gaussian distribution,
and further assume that the error covariance matrices have the form:
then the least squares solution -
D has the Gaussian distribution whose mean value is the true
underlying value and whose estimated covariance matrix is given by
is the unbiased estimate of oe
D)
D)
Moreover, we notice that Eq. 37 can be rewritten as
F ab (D) -
We can define the energy term W ab corresponding to the pose relation which is equivalent to a
Mahalanobis distance:
where
is the estimated covariance of -
D. Note that D (as given in Eq. 35) is the linearized pose difference
measurement equation.
In deriving the covariance matrix CD , we made assumptions that the matrix is diagonal and
the individual components of errors are zero mean Gaussian. It is probably difficult to justify
these assumption. However, we believe that they are reasonable ones in practice. If any other
estimates of the covariance matrices are available, they can certainly also be incorporated in our
global estimation formulation.
4.3 Pose Relations from Odometry
We also form an energy term in the objective function for each weak link. Suppose odometry
gives a measurement -
D 0 of the relative pose D 0 as the robot travels from pose V b to pose V a . The
measurement equation is:
We define the energy term in the objective function as follows:
where C 0 is the covariance of the odometry error in the measurement -
The covariance of measurement error is estimated as follows. Consider that a cycle of pose change
consists of: (1) the robot platform rotation by an angle ff to face towards the new target position;
(2) the robot translation by a distance L to arrive at the new position; (3) the sensor rotation
by a total cumulative angle fi (usually 360 ffi ) to take a scan of measurements while the platform
is stationary. We model the deviations oe ff , oe L , oe fi , of the errors in the variables ff, L, and fi as
proportional to their corresponding values, while the constant ratios are determined empirically.
The 3D pose change D derived as:
Then the covariance C 0 of the pose change D 0 can be approximated as:
fiC A J t (48)
where J is the Jacobian matrix consisting of the partial derivatives of (x; with respect to
\GammaL sin ff cos ff 0
We would also like to linearize and transform the measurement equation of D 0 to make the pose
difference representation for odometry measurements consistent with that for matched sensing
data. Consider the observation error \DeltaD
of odometry. Let -
close estimates of V a and V b . Denote \DeltaV
Then through Taylor expansion, the observation error \DeltaD 0 becomes:
V a \Psi -
b (\DeltaV a \Gamma -
H ab \DeltaV b ) (52)
where
sin -
H ab =B @
Notice that -
a
H a and -
H b are defined in Eq. 31. If we define a new observation
error
H a
then we can rewrite Eq. 52 as
H a \DeltaV a \Gamma -
where we denote
H a
V a \Psi -
H a \DeltaV a \Gamma -
Notice that now we are dealing with the measurement equation for D which is consistent with
that for matched sensing data. -
D can be considered as an observation of D. The covariance C
of -
D can be computed from the covariance C 0 of -
as:
H a
The energy term in the objective function now becomes:
ab -
4.4 Optimal Pose Estimation
Once we have uniformly formulated the two types of measurements, we can apply the estimate
algorithm in section 3 to solve for the pose variables. Denote the robot poses as
The total energy function from all the measurements is :
is the linearized pose difference between V j and
and -
ij is an observation of D ij ( -
is derived from the true observations, either range data or
odometry measurements). The covariance C ij is also known.
By regarding X
as the state vector corresponding to a node of the network as in
Section 3.2, we can directly apply the closed-form linear solution to solve for the X i 's as well as
their covariance C X
. The formulas are in Eq. 5 to Eq. 9. Then the pose V i and its covariance C i
can be updated as:
Note that the pose estimate V i and the covariance C i is given based on the assumption that the
reference pose is non-zero, the solution should be transformed
to
where
sin
4.5 Sequential Estimation
The estimation algorithm we previously discussed is a one-step procedure which solves for all
the pose variables at the same time. The algorithm is to be applied only after collecting all the
measurements. Yet it will be more practically useful if we have a sequential algorithm which
continuously provides estimates about the current or past pose variables after getting each new
measurement. Here we will describe such a sequential procedure.
Our sequential algorithm maintains the current best estimate about the poses of previously visited
places. At each step, a new location is visited and measurements about the new location as well
as the previous locations are gathered. By using these new measurements, the current pose can
be estimated while the previous poses can be updated.
be the pose vectors which we have previously estimated and let X n be the current
new pose which we are about to measure. Let X represent the concatenation of X
. Assume that we currently have an estimate X 0 of X whose inverse covariance matrix is C
Because we have no knowledge about X n yet, the X n component in X 0 contains an arbitrary
value and the matrix C
has all zeros in the last d rows and d columns, where
consider the addition of a set of new measurements relating X n to some of the past pose vari-
ables. Let the measurement equation, in matrix form, be is a constant matrix).
Assume that the set of measurements is -
D which is an unbiased observation of D whose error
has Gaussian distribution with covariance matrix CD . The updated estimate of X after using
the new measurements is the one which minimizes the following function, using the maximum
likelihood criterion, and assuming independent errors:
The solution can be derived as
D) (65)
and the new covariance of X is
A convenient way of updating X and CX is to maintain a matrix
(the summation is over different sets of measurements). Then at each step,
the updating algorithm is the following: First increase the dimensions of G and B to include the
new pose X n . Update G and B as
Then the new X and CX are given by
One potential problem with the above sequential updating procedure is that the state variable
expanding as it is augmented by a new state at each step. In case the robot path is
very long, the variable size may become too large, causing storage or performance problems. A
possible solution is to delete some of the old variables while adding the new ones.
We propose a strategy of reducing the number of state variables as follows. In order to choose
a pose to be deleted, we check all pairs of poses and find a pair (X the correlation
between the two poses is the strongest. We then force the relative pose between X i and X j to be
fixed as a constant. Then X i can be deleted from the state variables as it can be obtained from
When deleting the node X i from the network, we transform any link (X link
from X j to X k . Note that the covariance matrix CX contains all the pairwise covariance between
any two poses. A correlation ratio between two poses can be computed from the covariance and
variance.
By only fixing some relative poses, the basic structure in the network is still maintained. Thus
we are still able to consistently update all the pose variables once given new measurements. This
strategy is more flexible than the simple strategy of fixing selected absolute poses as constants.
Another approach to reducing the size of the system is to decompose the large network into smaller
components. The estimation algorithm is to be applied to each sub-network. The relative pose
between two nodes in different sub-networks can be obtained through pose compounding. If
there is a single link connecting two parts of a network, the poses in two parts can be estimated
separately and then combined with compounding, without loss of information. If, however, the
network is strongly connected that there are two or more links between any two nodes, then a
decomposition could give a sub-optimal estimation.
5 Implementation and Experiments
5.1 Implementation of Estimation Procedure
The implementation of the estimation algorithm is as follows. After building the network, we
obtain the initial pose estimates -
by compounding the odometry measurements. Then
for each link, we compute a measurement vector -
ij and a covariance matrix C ij according
to Eq. 38, 44 or Eq. 55, 57. Finally, we form a large linear system explained in
Section 3.2 and solve for the pose variables X.
The components needed to build G and B are C \Gamma1
ij . In the case of a strong link
(from matching a pair of scans), these components can be readily computed as C \Gamma1
which can be expanded into simple summations by noting the regularity in
the matrix M. In the case of a weak link (from odometry), these components can be computed by
multiplications of small matrices (3 \Theta 3). The most expensive operation in the estimation process
is to compute the inverse of a 3n \Theta 3n matrix G which gives the covariance of X.
The network is stored as a list of links and a list of nodes. Each link contains the following
information: type of link, labels of the two nodes, the computed measurement (relative pose),
and the covariance matrix of the measurement. Each node contains a range scan.
Note that we made linear approximations in the measurement equations in formulating the optimization
criterion. The first order approximation error is proportional to the error in the initial
pose estimate. Clearly, if we employ the newly derived pose estimate to formulate the linear
algorithm again, a even more accurate pose estimate can be obtained.
The iterative strategy based on this observation converges very fast. Typically, the first iteration
corrects over 90% of the total pose error correctable by iterating the process. It usually takes
four or five iterations to converge to the limit of machine accuracy.
5.2 Experiments with Simulated and Real Scan Data
We now present experiments of applying our algorithm to register simulated and real range scan
data. We first show an example using a simulated environment and measurements. This is useful
because ground truth is known. Then an example using real data is presented.
In the first example, we simulate a rectangular environment with a width of 10 units. The robot
travels around a central object and forms a loop in the path. There are 13 poses along the path at
a

Figure

Global registration of multiple scans using simulated scan data. (a) scans recorded in
a global coordinate system where the pose of each scan is obtained from compounding odometry
measurements. The scans align poorly because of accumulation of odometry error. (b) the result
of correcting pose errors. Both the dashed lines and solid lines show the constraints from matching
scan pairs. The solid lines also give the robot path and odometry constraints.
which simulated range scans are generated (with random measurement errors). We also simulate
a random odometry error (which is the difference between a pose change the robot thinks it made
and the actual pose change) at each leg of the trajectory. The magnitude of the accumulated
odometry error is typically in the range of 0.5 units.
We apply our iterative global pose estimation algorithm to correct the pose errors. In Fig. 6(a),
we show all the scans recorded in the initial coordinate system where the pose of each scan is
obtained by compounding odometry measurements. Due to the accumulation of odometry error
the scan data are aligned poorly. In Fig. 6(b), we show the result of correcting the pose errors
and realigning the scan data. Each line segment (either dashed or solid) in the figure represents
a strong link obtained from matching two scans. In addition, the solid lines show the robot
path which corresponds to the weak links. A plot of orientational and positional errors of the
poses along the path, both before and after the correction, is given in Fig. 7. Pose errors are
accumulated along the path while the corrected pose errors are bounded. For comparison, we
also apply a local registration procedure which matches one scan only to the previous scan. The
pose errors along the path after this local correction are also shown in Fig. 7. Although pose
errors are also significantly reduced after local corrections, they can still potentially grow without
bound. In this example, global registration produces more accurate results than local correction.0.010.030.050.070.090 2 4
Radian
Pose Number
Magnitude of Orientational Errors along the Path
Before correction
After local correction
After global correction0.10.30.50
Unit
Pose Number
Magnitude of Positional Errors along the Path
Before correction
After local correction
After global correction
a b

Figure

7: Pose errors along the path, before correction, after local correction, and after global
correction. (a) Orientational errors; (b) positional errors.
Then we present the experiment using real range scans and odometry data. The testing environment
is the cafeteria and nearby corridor in FAW, Ulm, Germany. The robot travels through
the environment following a given path. A sequence of 30 scans which were taken by the robot
with an interval of about 2 meters between scan poses were obtained. The laser sensor is a Ladar
2D IBEO Lasertechnik which is mounted on the AMOS robot. This laser sensor has a maximum

Figure

8: Consistent global registration of real range scans which are collected by a robot at
FAW, Ulm, Germany. (a) unregistered scans whose poses are subject to large odometry errors.
(b) registered scans after correcting the pose errors. The robot path estimated from odometry is
shown in dashed lines. The corrected path is shown in solid lines.

Figure

9: Mapping of a Hallway using the RWI Pioneer platform and a SICK laser range scanner
(a) Raw laser range scans (b) Aligned laser range scans.
viewing angle of 220 degrees. Thus having only the 2D positions of two poses close together
does not necessarily ensure a sufficient overlap between the scans taken at the two poses; we also
need the sensor heading directions to be similar. Among the links from matching
overlapping scan pairs are constructed. Some of these pairwise scan matching results have been
shown in [12]. In Fig. 8, we show (a) the unregistered scans and (b) the globally registered scans
in part (b).
Further experimental results with a variant of our algorithm are reported in [9]. Figure 9 contains
experimental results which are obtained using our global registration procedure together with a
modified version of Cox's pairwise scan matching algorithm 2 . The laser data are collected on
the RWI Pioneer platform using the SICK laser ranging device [15]. The Pioneer is a low-cost
platform with odometry error significantly higher than the much more expensive platforms used
in our other experiments. The hallway environment shown in Figure 9 is poor in features that
allow localization of the robot along the hallway. The data was collected by a robot that went
up and down the hallway several times. A large rotation error was introduced by the large turns
at the ends of the hallway.
6 Discussion
In this paper, we formulated the problem of consistent range data registration as one of optimal
pose estimation from a network of relations. The main ideas are as follows. We associate a robot
pose to a range scan to define an unambiguous object frame. By consistently maintaining the
history of robot poses, we effectively allow all object frames to be consistently registered in the
global reference frame. We use a combination of relation-based and location-based approach to
represent the world model. It can be viewed as a two-step procedure. First, spatial relations
between object frames are directly derived from odometry measurements and matching pairwise
frames. These relations, along with their uncertainties, constitute all the information in the
model. In the second step, the relations are converted to object frame locations based on an
optimization criterion. This formulation avoids the use of complex constrained optimization.
Furthermore, it does not require the assumption of known a priori covariance between object
frames.
We also derived measurement equations compatible with the formulation. It allows practical
implementation of the algorithm. We have experimentally demonstrated the effectiveness of
our estimation procedure in maintaining consistency among multiple range scans. The most
2 We are grateful to Steffen Gutmann of the AI Laboratory at the Albert-Ludwigs-Universit?t Freiburg for
providing us with these experimental results.
expensive operation, besides pairwise scan matching, is to compute the inverse of an 3n \Theta 3n
matrix. Although the number of poses n may be large for a long robot path, there are ways
to limit this size to speed up the computation. The sequential procedure enables the robot to
continuously maintain the optimal registration result.
Our approach assumes that the robot stops to collect a complete range scan at its current position.
An alternative would be to perform continuous scanning as the robot moves. Continuous scanning
would generate large amounts of data that would have to be sampled. In addition, the problem
of associating measurements with the correct robot position arises, as different parts of a scan
will have been obtained from different robot positions. Solving this problem would require an
accurate model of the robots motion. A possible solution to the problem of excessive amounts of
data is to partition the continuous scan data and transform each part to one pose on the path,
based on the odometry model. These are both worthwhile problems, which we consider outside
the scope of this paper.
Although we develop our method for mapping a 2D environment using 2D range scans, our formulation
is general and it can be applied to the 3D case as well, by generalizing pose composition
and linearization [12].

Acknowledgement

Funding for this work was provided by NSERC Canada and by the ARK project which receives its
funding from PRECARN Associates Inc., the Department of Industry, Science and Technology,
NRC Canada, the Ontario Technology Fund, Ontario Hydro, and AECL.
The authors would like to thank Steffen Gutmann, Joerg Illmann, Thomas Kaempke, Manfred
Knick, Erwin Prassler, and Christian Schlegel from FAW, Ulm for collecting range scans and
making the data available for our experiments. We thank Dr. Ingemar Cox, and the anonymous
reviewers for many constructive comments.



--R

Maintaining representations of the environment of a mobile robot.
Position referencing and consistent world modeling for mobile robots.
Blanche: An experiment in guidance and navigation of an autonomous robot vehicle.
World modeling and position estimation for a mobile robot using ultrasonic ranging.
Consistent integration and propagation of disparate sensor observa- tions
Integration, coordination and control of multisensor robot systems.
Uncertain geometry in robotics.
Map building for a mobile robot equipped with a 2D laser rangefinder.
AMOS: Comparison of scan matching approaches for self-localization in indoor environments
Stereo vision and navigation in buildings for mobile robots.
Dynamic map building for an autonomous mobile robot.
Shape Registration Using Optimization for Mobile Robot Navigation.
Robot pose estimation in unknown environments by matching 2D range scans.
Stochastic multisensory data fusion for mobile robot location and environment modelling.
SICK Laser range scanner.
On the representation and estimation of spatial uncertainty.
A geometric feature relation graph formulation for consistent sensor fusion.
--TR

--CTR
Reid Simmons , Dani Goldberg , Adam Goode , Michael Montemerlo , Nicholas Roy , Brennan Sellner , Chris Urmson , Alan Schultz , Myriam Abramson , William Adams , Amin Atrash , Magda Bugajska , Michael Coblenz , Matt MacMahon , Dennis Perzanowski , Ian Horswill , Robert Zubek , David Kortenkamp , Bryn Wolfe , Tod Milam , Bruce Maxwell, GRACE: an autonomous robot for the AAAI Robot challenge, AI Magazine, v.24 n.2, p.51-72, Summer
Michael Montemerlo , Sebastian Thrun , Daphne Koller , Ben Wegbreit, FastSLAM: a factored solution to the simultaneous localization and mapping problem, Eighteenth national conference on Artificial intelligence, p.593-598, July 28-August 01, 2002, Edmonton, Alberta, Canada
Tom Duckett , Stephen Marsland , Jonathan Shapiro, Fast, On-Line Learning of Globally Consistent Maps, Autonomous Robots, v.12 n.3, p.287-300, May 2002
Andrew Howard , Lynne E. Parker , Gaurav S. Sukhatme, Experiments with a Large Heterogeneous Mobile Robot Team: Exploration, Mapping, Deployment and Detection, International Journal of Robotics Research, v.25 n.5-6, p.431-447, May-June 2006
Andrew Howard, Multi-robot Simultaneous Localization and Mapping using Particle Filters, International Journal of Robotics Research, v.25 n.12, p.1243-1256, December  2006
Giorgio Grisetti , Gian Diego Tipaldi , Cyrill Stachniss , Wolfram Burgard , Daniele Nardi, Fast and accurate SLAM with Rao-Blackwellized particle filters, Robotics and Autonomous Systems, v.55 n.1, p.30-38, January, 2007
Sebastian Thrun , Wolfram Burgard , Dieter Fox, A Probabilistic Approach to Concurrent Mapping and Localization for Mobile Robots, Autonomous Robots, v.5 n.3-4, p.253-271, July-August 1998
Denis F. Wolf , Gaurav S. Sukhatme, Mobile Robot Simultaneous Localization and Mapping in Dynamic Environments, Autonomous Robots, v.19 n.1, p.53-65, July      2005
Sebastian Thrun , Wolfram Burgard , Dieter Fox, A Probabilistic Approach to Concurrent Mapping and Localization for Mobile Robots, Machine Learning, v.31 n.1-3, p.29-53, April/May/June 1998
Wolfram Burgard , Panos Trahanias , Dirk Hhnel , Mark Moors , Dirk Schulz , Haris Baltzakis , Antonis Argyros, Tele-Presence in Populated Exhibitions Through Web-Operated Mobile Robots, Autonomous Robots, v.15 n.3, p.299-316, November
Wesley H. Huang , Kristopher R. Beevers, Topological Map Merging, International Journal of Robotics Research, v.24 n.8, p.601-613, August    2005
Cheng Chen , Han Wang, Appearance-Based Topological Bayesian Inference for Loop-Closing Detection in a Cross-Country Environment, International Journal of Robotics Research, v.25 n.10, p.953-983, October   2006
Y. L. Ip , A. B. Rad, Incorporation of Feature Tracking into Simultaneous Localization and Map Building via Sonar Data, Journal of Intelligent and Robotic Systems, v.39 n.2, p.149-172, February 2004
Wolfgang Hbner , Hanspeter A. Mallot, Metric embedding of view-graphs, Autonomous Robots, v.23 n.3, p.183-196, October   2007
Christian Frh , Avideh Zakhor, An Automated Method for Large-Scale, Ground-Based City Model Acquisition, International Journal of Computer Vision, v.60 n.1, p.5-24, October 2004
Maarja Kruusmaa, Global Level Path Planning for Mobile Robots in Dynamic Environments, Journal of Intelligent and Robotic Systems, v.38 n.1, p.55-83, September
Udo Frese, A Discussion of Simultaneous Localization and Mapping, Autonomous Robots, v.20 n.1, p.25-42, January   2006
Sebastian Thrun , Michael Montemerlo, The Graph SLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures, International Journal of Robotics Research, v.25 n.5-6, p.403-429, May-June 2006
Ioannis Rekleitis , Gregory Dudek , Evangelos Milios, Multi-robot collaboration for robust exploration, Annals of Mathematics and Artificial Intelligence, v.31 n.1-4, p.7-40, 2001
Dieter Fox , Wolfram Burgard , Hannes Kruppa , Sebastian Thrun, A Probabilistic Approach to Collaborative Multi-Robot Localization, Autonomous Robots, v.8 n.3, p.325-344, June 2000
Frank Dellaert , Michael Kaess, Square Root SAM: Simultaneous Localization and Mapping via Square Root                 Information Smoothing, International Journal of Robotics Research, v.25 n.12, p.1181-1203, December  2006
Sebastian Thrun, Robotic mapping: a survey, Exploring artificial intelligence in the new millennium, Morgan Kaufmann Publishers Inc., San Francisco, CA,
range scan alignment;mapping;range scan registration;sensor-based mobile robotics;laser range scanning