A Linear Approximation for Graph-based

Simultaneous Localization and Mapping

Luca Carlone

, Rosario Aragues

, Jos e A. Castellanos

and Basilio Bona

CSPP, Laboratorio di Meccatronica, Politecnico di Torino, Torino, Italy - Email: luca.carlone@polito.it

Departamento de Inform atica e Ingeniera de Sistemas, Instituto de Investigaci on en Ingeniera de Arag on,
Universidad de Zaragoza, Zaragoza, Spain - Email: {raragues, jacaste}@unizar.es

Dipartimento di Automatica e Informatica, Politecnico di Torino, Torino, Italy - Email: basilio.bona@polito.it

AbstractThis article investigates the problem of Simultaneous
Localization and Mapping (SLAM) from the perspective of linear
estimation theory. The problem is rst formulated in terms of
graph embedding: a graph describing robot poses at subsequent
instants of time needs be embedded in a three-dimensional space,
assuring that the estimated conguration maximizes measure-
ment likelihood. Combining tools belonging to linear estimation
and graph theory, a closed-form approximation to the full
SLAM problem is proposed, under the assumption that the
relative position and the relative orientation measurements are
independent. The approach needs no initial guess for optimization
and is formally proven to admit solution under the SLAM setup.
The resulting estimate can be used as an approximation of the
actual nonlinear solution or can be further rened by using it
as an initial guess for nonlinear optimization techniques. Finally,
the experimental analysis demonstrates that such renement is
often unnecessary, since the linear estimate is already accurate.
I. INTRODUCTION
In the landscape of Simultaneous Localization and Mapping
problems it is possible to distinguish between online and
full SLAM approaches [29]. The former methods estimate
the SLAM posterior by recursively including the most recent
measurements in the posterior density. This is typically the
case of a robot that acquires proprioceptive and exterocep-
tive measurements from the environment and, at each time
step, incrementally includes such information in the posterior
describing robot pose and a representation of the surrounding
environment. Full SLAM approaches, instead, tackle the prob-
lem of retrieving the whole posterior, taking into account at the
same time all the available measurements; this batch solution
may occur, for instance, when the data, acquired during
robot operation, have to be processed off-line to produce a
meaningful representation of the scenario.
From the algorithmic point of view, literature on SLAM can
be roughly divided in three mainstream approaches: Gaussian
lter-based, Particle lters-based and graphical approaches.
The rst category includes Extended Kalman Filter SLAM
(EKF-SLAM) [26], Sparse Extended Information Filter [28],
delayed-state EKF-SLAM [9] and related approaches. EKF-
like techniques require the linearization of both process and
measurement models; the rst-order approximation, however,
often comes at a price of inconsistency and divergence [3].
Moreover the complexity of the lter is quadratic in the
number of landmarks, hence a naive implementation prevents
large scale mapping [29]. Finally, we remark that the effects of
partial observability in standard EKF-SLAM implementation
are not fully understood, this being witnessed by recent
results [15]. The second class of approaches, namely Rao-
Blackwellized particle lters [8], is based on a factorization of
the posterior and was successfully employed for building both
landmark-based and occupancy grid map representations of the
environment [29]. Also in this case, fundamental limitations
are well known to the robotic community [21]: a nite number
of particles may not be able to correctly represent the trajectory
posterior, and this issue becomes critical when the uncertainty
in the posterior increases. The third category encompasses the
techniques in which measurements acquired during robot mo-
tion are modeled as constraints in a graphical model. The poses
assumed by the robot during its motion correspond to vertices
of a directed graph, whereas the constraints are modeled as
edges in such graph. Hence SLAM can be stated in terms
of maximum likelihood and solved through nonlinear opti-
mization techniques [20]. Although graph-based approaches
are recognized to outperform other SLAM techniques in terms
of accuracy, they present the classical drawbacks of nonlinear
optimization techniques: convergence to a global minimum of
the cost function cannot be guaranteed in general and, if the
initial guess for optimization is outside the basin of attraction
of the global optimum, the iterative process is likely to be
stuck in a local minimum.
In this article we address the full SLAM problem in planar
setups. Combining tools of linear estimation and graph theory,
a closed-form approximation to SLAM is retrieved, under
mild assumptions on the structure of the involved covariance
matrices. The linear approximation requires no initial guess;
moreover, experimental evidence demonstrates that it is accu-
rate in practice, hence it can be used in place of the exact
solution or for bootstrapping nonlinear techniques.
II. RELATED WORK
The seminal paper [20] paved the way for several results
towards the objective of a sustainable solution to full SLAM.
The setup considered by Lu and Milios was based on scan
matching for retrieving relative constraints between robot
poses, but the use of a Gauss-Newton scheme for optimization
prevented its applicability in large scale scenarios. This frame-
work was extended to the general case of a graph containing
both robot poses and landmark positions in [27]. Thrun and
Montemerlo showed that it is possible to marginalize vari-
ables corresponding to landmarks, hence reducing the problem
to the pose estimation setup discussed in [20]. Moreover
they enabled the estimation of large maps (10
8
landmarks),
applying a conjugate gradient-based optimization. Konolige
investigated a reduction scheme for the purpose of improving
the computational effort of nonlinear optimization [17]: the
optimization process was restricted to poses involved in at
least one loop constraint, providing a remarkable advantage
for graphs with low connectivity. Frese et al. proposed a
multilevel relaxation approach for full SLAM [10], allowing
to considerably reduce the computational time of optimization
by applying a multi-grid algorithm. A further breakthrough
in the literature of graph-based approaches came with the
use of incremental pose parametrization, proposed in [23].
Olson et al. showed how a clever selection of the optimization
variables can greatly simplify the problem structure, enabling
fast computation. Moreover they proposed not to optimize all
the constraints at the same time, but to sequentially select
each constraint and rene nodes conguration so to reduce
the residual error for such constraint. Grisetti et al. extended
such framework, taking advantage of the use of stochastic
gradient descent in planar and three-dimensional scenarios
[11]. Moreover they proposed a parametrization based on a
spanning tree of the graph, for speeding up the correction of
the residual errors over the network. This approach was shown
to be quite resilient to the orientation wraparound problem
(which causes the conguration estimate to be stuck in a local
minimum [11]) and currently constitutes the state-of-the-art.
All the aforementioned techniques are iterative, in the sense
that, at each iteration, they solve a local convex approximation
of the original problem, and use such local solution to update
the conguration. This process is then repeated until the
optimization variable converges to a local minimum of the
cost function. In particular, when a linear approximation of
the residual errors in the cost function is considered, the
problem becomes an unconstrained quadratic problem and
the local correction can be obtained as solution of a system
of linear equations, known as normal equation [7]. In such
a case the source of complexity stems from the need of
repeatedly solving large scale linear systems. A discussion on
the performances of direct and iterative methods for solving
large scale equations, with application to SLAM, can be found
in [7]. Some original attempts to exploit the mathematical
structure of the optimization problem and the properties of
the underlying graph have been recently proposed [5, 7, 11].
Although the state-of-the-art approaches have been demon-
strated to produce impressive results on real world problems,
their iterative nature requires the availability of an initial
guess for nonlinear optimization, which needs be sufciently
accurate for the technique to converge to a global solution of
the problem. In this work we provide a linear approximation
of the full SLAM solution that can be used as initial guess for
an iterative nonlinear approach. We remark that the idea of a
linear initialization is not new in the literature. For instance,
in computer vision, linear methods based on algebraic errors
(e.g., [13, 24]) are often employed for bootstrapping nonlinear
techniques (i.e., bundle adjustment [13]). Similar intuitions can
be also found within the SLAM community, see [6].
Notation and preliminaries. In denotes the nn identity matrix, 0n
denotes a (column) vector of all zeros of dimension n. Mnm denotes
a matrix with n rows and m columns and denotes the Kronecker
product. The cardinality of a generic set S is written as |S|. A directed
graph G is a pair (V, E), where V is a nite set of elements, called
vertices or nodes, and E is a set containing ordered pairs of nodes. A
generic element e E, referred to as edge, is in the form e = (i, j),
meaning that edge e, incident on nodes i and j, leaves i (tail) and is
directed towards node j (head) [16]. The number of nodes and edges
are denoted with n+1 (the reason for this choice will be clear later)
and m, respectively, i.e., |V| = n + 1 and |E| = m. The incidence
matrix A of a directed graph is a matrix in R
(n+1)m
in which each
column contains the information of an edge in E; in particular the
column corresponding to the edge e = (i, j), has the i-th element
equal to 1, the j-th element equal to +1 and all the others equal
to zero. A directed graph is connected (or weakly connected) if there
exists an undirected path (regardless edges orientation) connecting
any pair of nodes. A spanning tree of G is a subgraph with n edges
that contains all the nodes in G. The edges of G that do not belong to a
given spanning tree of the graph are usually referred to as chords [4].
III. PROBLEM FORMULATION
Let V = {v
0
, . . . , v
n
} be a set of n+1 nodes (representing
subsequent poses assumed by a mobile robot) and let P =
{p
0
, . . . , p
n
} denote a corresponding set of absolute poses in a
planar setup, i.e., p
i
= [

i

i
]

R
3
, where
i
= [x
i
y
i
]

R
2
is the Cartesian position of the i-th node, and
i
is its
orientation. We shall call P a conguration of nodes. Suppose
that it is possible to measure the relative pose between some
nodes, say nodes (i, j); in particular node i can measure the
pose of j in its local reference frame R
i
, i.e.,

ij
= p
j
p
i
,
where is a standard pose compounding operator that can be
rewritten in explicit form as:
pj pi
.
=
_
R

i
(j i)
j i
_
, (1)
being R
i
R
22
a planar rotation matrix of an angle
i
.
Since relative pose measurements are affected by noise, the
measured quantities are in the form
ij
=

ij
+
ij
, where

ij
R
3
is a zero mean Gaussian noise, i.e.,
ij
N(0, P
ij
),
being P
ij
a 3 by 3 covariance matrix. In practice, we can
distinguish two kinds of relative pose measurements:
odometric constraints: relative measurements between
poses assumed by the robot at subsequent instants of time.
These constraints are connected to measurements of the
ego-motion (odometry) of the robot and are provided by
proprioceptive sensors (wheel odometry, IMU, etc.) or by
exteroceptive sensors-based techniques (scan matching,
visual features registration, etc.) [29];
loop closing constraints: are connected to place revisiting
episodes. This phase clearly requires the use of exterocep-
tive sensors, among which it is worth mentioning vision
sensors and laser range nders.
In this context, we assume the constraints to be given, since
the reliable determination of both odometric constraints and
loop closing constraints is still an active research topic [25],
whose implications are out of the scope of the present article.
The problem can be naturally modeled using graph for-
malism: each node in the set V corresponds to a vertex of a
directed graph G(V, E) (often referred to as pose graph), where
E (graph edges) is the set containing the unordered node pairs
(i, j) such that a relative pose measurement exists between i
and j. By convention, if an edge is directed from node i to
node j, the corresponding relative measurement is expressed
in the reference frame of node i. We denote with the set
of all available measurements, i.e., = {
ij
, (i, j) E}. The
cardinality of the set is || = |E| = m. The relationship
between odometric (and loop closing) constraints and graph
edges is discussed in detail in Section IV-E.
From the knowledge of relative pose measurements and the
corresponding uncertainty, the robot is required to estimate the
conguration P in a given reference frame R
0
. By convention,
we set the initial pose of the robot to be the origin of such
reference frame, i.e., p
0
= [0 0 0]

. In topological graph
theory the problem is also referred to as graph embedding, re-
alization or drawing, depending on the context and on problem
constraints [12]. Therefore our objective is to determine an
estimated conguration P

= {p

1
, . . . , p

n
}, that maximizes
the likelihood of the observations. It is common in literature
to assume independence among observations, hence we can
write the log-likelihood function of a conguration as:
lnL(P|) =

(i,j)E
ln (ij|P), (2)
where (
ij
|P) is the conditional probability density of the
measurement
ij
given the node conguration. Since we
assumed that the involved densities are Gaussians, it is easy
to demonstrate that maximizing the likelihood function (2) is
equivalent to minimize the sum of the weighted residual errors:
f(P) =

(i,j)E
(pj pi ij)

P
1
ij
(pj pi ij). (3)
The full SLAM problem is hence formulated as a mini-
mization of the nonlinear non-convex cost function (3), i.e.,
the optimal conguration is P

= arg minf. The nonlinearity

of the function f(P) is due to the structure of the pose
compounding operator (1) and, in particular, to the nonlinear
terms in the orientations of the robot.
IV. A LINEAR ESTIMATION FRAMEWORK FOR SLAM
We rst observe that each relative pose measurement is
composed by a two-valued vector (relative position) corre-
sponding to the rst two components of
ij
, see (1), and a
scalar (relative orientation). Therefore we can rewrite each
measurements as
ij
= [
l
ij

ij
]

, where the superscript l

denotes that the relative position vector is expressed in a local
frame. According to the notation introduced so far the cost
function (3) can be rewritten as:

(i,j)E
_
R

i
(
j

i
)
l
ij
(
j

i
)
ij
_

P
1
ij
_
R

i
(
j

i
)
l
ij
(
j

i
)
ij
_
.
In this context we assume that the relative position infor-
mation and the relative orientation measurements are indepen-
dent, i.e. P
ij
= diag(P

l
ij
, P
ij
). This hypothesis is a technical
requirement for attacking the problem, see also Remark 2
in Section V. Under this hypothesis the cost function f(P)
becomes:

(i,j)E
_
R

i
(
j

i
)
l
ij
_

P
1

l
ij
_
R

i
(
j

i
)
l
ij
_
+
+

(i,j)E
[(
j

i
)
ij
]

P
1

ij
[(
j

i
)
ij
].
(4)
In order to put the previous formulation in a more com-
pact form, let us number the nodes pairs, for which a rel-
ative pose measurement is available, from 1 to m; let us
stack all the relative position measurements in the vector

l
= [(
l
1
)

, (
l
2
)

, . . . , (
l
m
)

, and all the relative

orientation measurements in the vector = [
1
,
2
, . . . ,
m
]

.
Repeating the same procedure for all the positions and ori-
entations assumed by the robot we get the nodes posi-
tion
+
= [

0
,

1
, . . . ,

n
]

and the nodes orientation

+
= [
0
,
1
, . . . ,
n
]

. Therefore it can be easily veried

that equation (4) can be rewritten as:
f(P) =
_
A

2

+
R
l
_

_
RP

l R

_
1
_
A

2

+
R
l
_
+
+
_
A

+

_

P
1

_
A

+

_ , (5)
where A is the incidence matrix of the graph G(V, E),
A
2
= A I
2
is an expanded form of the incidence
matrix, see [1], P

l = diag(P

l
1
, P

l
2
, . . . , P

l
m
), P

=
diag(P
1
, P
2
, . . . , P
m
), and R = R() is a block diagonal
matrix containing the rotation matrices that transform the
corresponding local measurements in the global frame, i.e., the
non-zero entries of R are in positions (2k 1, 2k 1), (2k
1, 2k), (2k, 2k 1), (2k, 2k), k = 1, . . . , m, and the k-th
diagonal block contains the rotation matrix converting the k-th
relative position measurement in the global frame.
Equation (5) can be further simplied by recalling the
choice of the reference frame
0
= [0 0]

and
0
= 0,
leading to the following relation:
f(P) =
_
A

2
R
l
_

_
RP

l R

_
1
_
A

2
R
l
_
+
+
_
A

P
1

_
A

_ , (6)
where and are the vectors obtained from
+
and
+
by
deleting the elements
0
and
0
, respectively, whereas A and
A
2
are reduced incidence matrices, obtained by deleting the
rows corresponding to
0
and
0
from A and A
2
, respectively.
We observe that the minimization of the cost function (6)
is equivalent to nd the solution (in the least-squares sense)
to the following system of equations:
_
A

2
= R()
l
A

=
. (7)
The system (7) includes m vector-valued relations (or con-
straints) on relative positions and m scalar constraints. When
a solution exactly satises all the constraints in (7), the
corresponding minimum of the cost function (6) is zero,
otherwise, the searched solution has to minimize the weighted
residual errors on the constraints. The nonlinear nature of the
problem at hand is connected with the matrix R(), which
contains trigonometrical functions of nodes orientation.
A. Solving SLAM with Linear Estimation Theory
We now provide a procedure that allows to retrieve an
approximation of the actual nonlinear solution of (6). The
procedure can be summarized in three phases:
1) Solve the following linear estimation problem:
A

= , (8)
from which the suboptimal orientation estimate

and
its covariance matrix P

can be obtained.
2) Estimate the relative position measurements in the global
reference frame:
z =
_

R 0
2mn
0

2mn
In
_ _

l

_
=
_
g
1
(
l
, )
g
2
()
_
=

, (9)
with

R = R(

); compute the corresponding uncertainty

(preserving the correlation with the orientation estimate):
P
z
= H
_
P

l 0
2mn
0

2mn
P

_
H

, (10)
where H is the Jacobian of the transformation in (9):
H
.
=
_
g1

l
g1

g2

l
g2

_
=
_

R J
0

2mn
I
n
_
. (11)
3) Solve the linear estimation problem in the unknown p =
[

, given z (9) and P

z
(10):
z =
_
A

2
0
2mn
0

2mn
I
n
_ _

_
= B

p, (12)
from which the solution p

= [(

and the
corresponding uncertainty can be retrieved.
In the rest of this section we will show how the proposed
procedure provides an approximate solution to SLAM with
graphical models. For sake of readability each phase of the
procedure will be discussed in detail, hence the proof of the
result will be clear at the end of Section IV-D.
B. Phase 1: Linear Orientation Estimation
Observing the structure of problem (7) it can be seen that
the second part of the system, including the relative orientation
measurements, is linear in the unknown variable . The rst
phase of the procedure requires the solution of such subset
of constraints; this is a standard linear estimation problem:
given the matrix A, the measurements and the corresponding
covariance matrix P

, the objective is to provide an estimate

of the unknown . According to well known results in linear

estimation theory [22], the Best Linear Unbiased Estimator
(BLUE) for in (8) and the corresponding covariance are:

= (AP
1

)
1
AP
1

, P

= (AP
1

)
1
. (13)
The following results establish existence and uniqueness of
the solution of problem (8). We recall that similar results can
be found in literature, see [1] and the references therein.
Lemma 1 (Connectivity of pose graph): The pose graph G
modeling robot poses, including odometric and loop closing
constraints is connected.
Proof. The proof is trivial since the path connecting all the
nodes is actually the trajectory traveled by the robot; in this
sense the edges corresponding to the odometric constraints
assure graph connectivity.
Proposition 1 (Existence of the solution for phase 1): The
solution of the rst step in the procedure IV-A is unique if G
is connected and a node orientation is supposed to be known.
Proof. The incidence matrix A of a connected graph G
has rank n (with |V| = n + 1) and becomes full rank
as soon as one row is deleted, see [4]. As we have just
shown in (6), assuming a node to have known orientation
(e.g., the rst node is assumed to be the absolute reference
frame) allows to reduce the incidence matrix by deleting
the corresponding row. Therefore, A is full rank and P
1

is positive denite by denition, then AP

1

is positive
denite, hence invertible, see [14]. If the matrix is invertible,
the solution of the linear problem can be uniquely determined
as

= (AP
1

)
1
AP
1

.
It is worth observing that once the absolute orientation of
the robot is known, also the rst equation in (7) becomes
linear in the unknown . Therefore, using

as the actual
nodes orientation, we can also compute an estimate for nodes
position , using linear estimation framework:
=
_
A2
_

RP

l

R

_
1
A

2
_
1
A2
_

RP

l

R

_
1

R
l
, (14)
where

R = R(

). Also in this case, the proof of existence

and uniqueness of the solution directly stems from Proposi-
tion 1. However, the rst equation in (7) also constraints the
orientations of the robot, thus the estimate p = [

constitutes a suboptimal solution, in which the inuence of

the rst equation on the estimated orientations is neglected.
C. Phase 2: First-order Error Propagation
The second phase simply provides an estimate of the relative
position measurements in the global reference frame: the
vector z will be in the form z = [(
g
)

where

g
=

R
l
is the vector containing the relative node position
expressed in the absolute reference frame R
0
. The corre-
sponding covariance matrix can be obtained by a rst-order
propagation of the uncertainty, hence we can rewrite (10) in
explicit form as:
Pz =
_
P
g + JP

JP

_
,
where P

g
.
=

RP

l

R

, and J and P

are dened as in (11)

and (13). The trick of including the orientation estimates in
z is useful for preserving the correlation between the relative
position measurements (expressed in the global frame) and
the corresponding angular information. As we will see in the
following section, such correlation terms play a fundamental
role in the outcome of the procedure.
D. Phase 3: Linear Position Estimation
We now want to show that the last phase allows to correct
the sub-optimal conguration estimate p, leading it towards a
minimum of the cost function, i.e.

=

+

,

= + ,
in which

and are rst-order correction terms. In order
to proceed in the demonstration we need to compute p

=
[(

as solution of the linear system (12):

p

=
_

_
= (BP
1
z
B

)
1
BP
1
z
z. (15)
Also in this case, the demonstration of uniqueness of the
solution (15) can be easily deduced from Proposition 1.
To write in explicit form

and

, we rst compute the

information matrix P
1
z
:
P
1
z
=
_
P
1

g P
1

g J
J

P
1

g P
1

+ J

P
1

g J
_
.
The previous inverse can be performed using blockwise matrix
inversion, see [14]. We can now compute the information
matrix I
p
= (BP
1
z
B

):
I
p
=
_
P
1

A
2
P
1

g
J
J

P
1

g
A

2
P
1

+ J

P
1

g
J
_
,
where P

.
= (A
2
P
1

g A

2
)
1
. The inverse of I
p
, namely P
p
,
is actually the covariance matrix of our estimated congura-
tion, and it is in the form:
P
p
=
_
P

,
P

_
,
where:
P

.
=
_
P
1

+ J

P
1

g
J J

P
1

g
A

2
P

A
2
P
1

g
J
_
1
P

= P

+ P

A
2
P
1

g
JP

P
1

g
A

2
P

P

,
= P

A
2
P
1

g
JP

.
From the expression of P
p
and P
1
z
it is straightforward
to compute the estimates (15) as:

= (AP
1

)
1
AP
1

+
+P

P
1

g
_
A

2
P

A
2
P
1

g I
2m
_

R
l
,
(16)
and:

=
_
A
2
_

RP

l

R

_
1
A

2
_
1
A
2
_

RP

l

R

_
1

R
l
+
+P

A
2
P
1

g JP

P
1

g
_
A

2
P

A
2
P
1

g I
2m
_

R
l
.
By simple inspection it is possible to verify that the obtained
estimate

is already in the form

=

+

, since the rst
summand in (16) coincides with (13). The same consideration
holds for

with respect to equation (14). Therefore we can

write

and as:

= P

P
1

g
_
A

2
P

A
2
P
1

g
I
2m
_

R
l
= P

A
2
P
1

g
JP

P
1

g
_
A

2
P

A
2
P
1

g
I
2m
_

R
l
.
(17)
We are now ready to state the following key result.
Theorem 1: The outcome of the three-phases procedure ap-
proximates the solution of the nonlinear optimization problem
(6). In particular, the third phase produces a local correction
of a sub-optimal conguration estimate computed in the rst
phase, leading it towards a minimum of the cost function.
Proof. We have already shown that the nal solution is
composed by the suboptimal solution plus a correction term.
Now the demonstration reduces to verify that p = [

is a local solution of our optimization problem around p =

[

. In order to prove this point we compute a rst-

order approximation of the residual errors in (6) around the
suboptimal solution p:
f(P)
_
A

2
+ A

2


R
l
J

_

RP

l

R

_
1
_
A

2
+ A

2


R
l
J

_
+
+
_
A

+ A

P
1

_
A

+ A

_
,
(18)
where

and

are the displacements from the lineariza-

tion point. This convex approximation can now be easily
minimized by taking the rst derivative with respect to the
optimization variables

and

and imposing it to be zero.

The minimum of (18) corresponds to:

= P

P
1

g
_
A

2
P

A
2
P
1

g
I
2m
_

R
l
= P

A
2
P
1

g
JP

P
1

g
_
A

2
P

A
2
P
1

g
I
2m
_

R
l
,
which can be easily seen to coincide, respectively, with

and
, see (17), thus proving our thesis.
Remark 1: A direct (iterative) method (see [7] and the
references therein) would require solving N linear problems
of size 3n (in which the iterations N may increase arbitrarily,
depending on the initial guess); our approach solves a smaller
problem (on robot orientations - size n) and a problem of the
same size of a single step of the direct method. The advantage,
now, is that the rst-order correction provided by the last phase
of the proposed approach, renes a sub-optimal solution which
in practice is already close to a global minimum. Therefore
the approach is expected to be accurate, while reducing the
risk of being trapped in a local minimum. Furthermore, the
sub-optimal solution needs not be computed explicitly: for the
purpose of proving Theorem 1 we reported , but it is not
actually computed in the three-phases procedure.
E. Regularization and Existence of the Correction Factors
We now discuss a crucial point of the proposed approach,
which is connected with the periodic nature of the angu-
lar information, i.e., robot orientations are dened up to
2k, k Z. Let us introduce the discussion with an example:
consider a simple scenario, in which a robot travels along
a circumference (in anticlockwise direction) coming back to
the starting point. In a noiseless case, summing up all the
relative orientation measurements from the one taken with
respect to the rst node, to the loop closing constraint, referred
to the last node, we obtain 2. This is because we sum small
angular measurements which are dened in (, ]. However,
the loop closing constraint is expected to link the last pose
with the initial pose, whose orientation was set by convention
to zero. The linear estimation framework presented so far
cannot recognize that the angles 0 and 2 actually correspond
to the same orientation, hence tries to impose contrasting
constraints, producing meaningless results. An easy solution
to the previous problem consists in adding a correction factor,
in the form 2k, k Z, to some orientation measurements.
These correction factors do not alter measurement content,
because of the periodicity of the angular information, but let
the relative orientation measurements sum up to zero (this
property will be lately referred to as zero-sum property). Hence
the input data provided to the problem solver are consistent
and the estimated conguration is correct.
In the rest of this section we will prove the existence of
suitable correction factors for any connected graph and we will
describe a methodology for retrieving such correction terms.
Before presenting the main result (Theorem 3) let us introduce
some specic concepts from graph theory.
A cycle is a subgraph in which every node appears in a even
number of edges. A circuit is a cycle in which every node
appears exactly in two edges. We can represent a (directed)
circuit as a vector c
i
of m elements in which the k-th element
is +1 or 1 if edge k belongs to the circuit and it is traversed
respectively forwards (from tail to head) or backwards, and 0
if it does not appear in the circuit (notice that the ordering of
the edges in c
i
is arbitrary).
Denition 1: Given a directed graph G and a spanning tree
T of G, a fundamental circuit is a circuit composed by a
chord (i, j) of G with respect to T and the unique path in
T connecting i and j.
A cycle basis of G is the smallest set of circuits such that
any cycle in the graph can be written as a combination of the
circuits in the basis. The space spanned by the vectors in the
basis is called cycle space.
Theorem 2: The set of the fundamental circuits of a di-
rected graph constitutes a cycle basis for G.
The proof of the previous theorem can be found in several
books of graph theory, see [4]. We already mentioned that a
spanning tree T of a connected graph G contains exactly n
edges. Accordingly, the number of chords, hence of funda-
mental circuits in G, is mn.
Corollary 1: The dimension of the cycle space of a con-
nected graph G is d = m n, and it is usually called
cyclomatic number of the graph [16].
Corollary 2: Ordering the edges of a connected directed
graph G from 1 to m, so that the rst mn elements are the
chords of G with respect to a given spanning tree T and the
last n elements are the edges of T, the matrix containing all
the vectors c
i
corresponding to the fundamental circuits can
be written as:
C = [c
1
c
2
. . . c
d
]

= [I
d
B] , (19)
where I
d
is the identity matrix of dimension d and B is a
matrix with elements in {1, 0, 1}. C is usually referred to as
cycle basis matrix.
The previous result is a direct consequence of the structure
of the fundamental circuits, each one containing a single chord
and a collection of edges in the spanning tree [4]. With slight
abuse of notation, in the following, C will denote both the
cycle basis and the cycle basis matrix.
According to the machinery introduced so far, we notice
that the zero-sum property essentially requires that the sum
of the relative orientation measurements along every cycle in
the graph is zero, instead of 2k, k Z\{0}. Hence we can
state the following theorem that holds under the assumption
of noiseless angular measurements.
Theorem 3 (Existence of correction factors): Given the
relative orientation measurements = [
1
,
2
, . . . ,
m
]

there
exists a correction vector = [
1
,
2
, . . . ,
m
]

so that the
corrected measurements

= [
1
+
1
,
2
+
2
, . . . ,
m
+
m
]

satisfy the zero-sum property.

The elements of

are referred to as regularized relative
orientation measurements. The process of compensating the
relative orientation measurements is named regularization.
Proof. Let us start by formulating the zero-sum property in
a more familiar way. A necessary and sufcient condition for
the zero-sum property to be satised for all the cycles in the
graph is that it is satised for the cycles in the cycle basis,
see [16]. Let us consider the cycle basis composed by the
fundamental circuits; for the zero-sum property to hold true,
the corrected measurements

have to satisfy:
c

= 0 c
i
C. (20)
Roughly speaking, if the sum of the relative orientation
measurements is zero for the edges in the fundamental circuits,
this property is true for every cycle in the graph. Equation (20)
can be written in compact form using the cycle basis matrix:
C

= 0
d
. (21)
According to the denition of regularized measurements we
can rewrite (21) as:
C( + ) = 0
d
= C = C. (22)
The right hand side will contain the sum of the original
measurements for each fundamental circuit. In a noiseless
case, the vector C contains terms in the form 2k, k Z.
Since the cycle basis matrix C can be computed from the graph
and is a given of the problem, the only unknown of (22)
is and the existence of a proper regularization is reduced
the demonstration of existence of a solution to system (22).
Recalling (19), it is easy to show that a solution to system
(22), is = [(C)

n
]

:
C = [I
d
B] [(C)

n
]

= C, (23)
hence proving our thesis.
We notice that the aforementioned solution only requires to
correct the angular measurements corresponding to the chords,
without any modication to the edges in the spanning tree. We
remark that the correction terms are in the form 2k, k Z,
hence the regulation procedure does not alter the information
content of the orientation measurement. It is now evident
that, in the case of noisy relative measurements, condition
(20) cannot be met exactly: our approach, in fact, will be in
charge of compensating the measurement errors by minimizing
a suitable cost function. Accordingly, the term C in (22) will
not contain exact multiples of 2. However, a simple rounding
to the closest multiple of 2 allows to retrieve the desired
correction factors. One may argue that, if the noise is large,
it is not possible to discern the desired correction factors,
since the rounding cannot compensate measurement errors;
however this issues was not found to be relevant in common
applications (see experimental section): as it will be clear in
a while, the impossibility to determine the correct multiple of
2 means that the amount of noise is so high that the robot,
revisiting a past pose, is not able to discern how many times
the robot turned around itself (i.e., completed 2 turns) since
the previous visit. Note that this result also sheds some light
on the so called orientation wraparound problem [11], which
is known to prevent convergence in iterative approaches.
We now tailor the previous formulation to the SLAM setup.
We state the following facts, whose demonstration is omitted,
since it can be easily inferred from the basic denitions.
Proposition 2: The edges corresponding to odometric con-
straints in the pose graph constitute a spanning tree, T, for the
connected graph G, describing the SLAM problem.
Corollary 3: The edges corresponding to loop closing con-
straints are chords of the pose graph G, with respect to the
spanning tree T.
Therefore, according to Corollary 3 and Theorem 3, we can
regularize the orientation measurements by simply correcting
loop closing constraints; in particular, a loop closing relation,
constraining two robot poses, has to be corrected taking into
account the number of complete turns (2 turns) the robot did
when traveling from the rst to the second pose.
V. EXPERIMENTAL VALIDATION
In this section we will show an application of the proposed
methodology to the Intel Research Lab dataset [18]. The
solution of the full SLAM problem from real data requires
three steps:
1) Odometric and loop closing constraints are extracted
from real sensor data;
2) Regularization is performed for making the orientation
measurements consistent;
3) The three-phases procedure is applied to solve the graph
embedding problem.
Number Number of Number of Number of
of odometric loop closing chords needing
nodes constraints constraints regularization
1228 1227 278 198
TABLE I
NUMBER OF ODOMETRIC CONSTRAINTS, LOOP CLOSING CONSTRAINTS,
AND REGULARIZED ORIENTATION MEASUREMENTS FOR INTEL DATASET.
10 5 0 5 10 15 20 25 30
45
40
35
30
25
20
15
10
5
0
5
x [m]
y
[m
]
Fig. 1. Intel Research Lab dataset: odometric constraints (solid lines),
corresponding to the odometric trajectory corrected with scan matching, and
loop closing constraints (dotted lines).
In the following, we will tailor the formulation to the
particular application scenario, in which a mobile robot is
equipped with wheel encoders and a laser range nder. The
odometric constraints are obtained by rening the wheel
odometry measurements with a scan matching procedure,
see [19]. The loop closing constraints are selected from the
relations available at [18]. The number of odometric and loop
closing constraints is reported in Table I.
In Figure 1 we show the odometric trajectory of the robot
(solid line) obtained from wheel encoders estimates and scan
matching. The gure also shows, as dotted lines, the edges
corresponding to loop closing constraints. The scan matching
algorithm is only able to enforce local consistency by aligning
the laser readings acquired at subsequent poses, thus failing
in producing a globally consistent map. Once the relative
pose information (,
l
) is available, it is possible to perform
regularization for orientation measurements. Computing the
cycle basis C and the correction terms = [(C)

n
]

,
it is possible to obtain the regularized measurements

. In
Table I we show the number of loop closing constraints
for which a correction factor 2k, k Z\{0} was needed.
The conguration estimated with the three-phases procedure
and the corresponding occupancy grid map are reported in
Fig. 2. Intel Research Lab dataset: (a) estimated node conguration, (b)
occupancy grid map obtained by associating the corresponding laser scan to
each node of the estimated conguration.
0 200 400 600 800 1000
2
4
6
8
10
12
14
x 10
4
Iterations

a
(b) (a)
0 200 400 600 800 1000
0
0.005
0.01
0.015
0.02
0.025
0.03
Iterations

c
Fig. 3. Errors versus iterations for TORO (solid lines) and errors for the
three-phases procedure (dashed line). (a) translation error squared (c); (b)
angular error squared (a).
P
ij
= diag(P

l
ij
, P

ij
) P
ij
= I
3
c [m
2
] a [rad
2
] c [m
2
] a [rad
2
]
TORO
2.60 10
3
2.30 10
4
6.16 10
4
1.62 10
4
(100 iter.)
TORO
1.90 10
3
2.18 10
4
5.83 10
4
1.62 10
4
(1000 iter.)
Proposed
0.91 10
3
2.25 10
4
3.36 10
4
2.00 10
4
approach
TABLE II
BENCHMARK METRICS [19] FOR THE INTEL DATASET: TRANSLATION
ERROR SQUARED (c) AND ANGULAR ERROR SQUARED (a).
Figure 2. For a quantitative evaluation we report the values
of the SLAM benchmark metrics proposed in [19]. Without
entering into details we mention that such metrics provide
a tool for comparing the SLAM approaches in terms of
accuracy. In Table II we show the values of the constraint
satisfaction metrics, comparing the proposed solution with the
Tree-based netwORk Optimizer (TORO), which is available
online, see [11]. We consider two scenarios, in which different
measurements covariance matrices are considered: in one case
the covariance matrix is chosen to be the identity matrix
(P
ij
= I
3
), whereas, in the other scenario, measurements
covariance is in the form P
ij
= diag(P

l
ij
, P
ij
), and the
corresponding uncertainties are assumed to be proportional
to the respective measurements, e.g. bigger displacements
correspond to higher uncertainty. For this last case, in Figure
3 we plot the errors versus iterations for TORO, compared
with the corresponding statistics obtained with the three-
phases procedure. It is now clear that the proposed approach
is accurate in practice. Further experiments and simulations
can be found in [2], conrming the results presented so far.
Remark 2: The assumption of independent position and
orientation measurements holds true when dealing with holo-
nomic platforms. For non-holonomic platforms it constitutes
an approximation, but several state-of-the-art techniques have
been demonstrated to produce excellent results, even under
stricter assumptions on the covariance structure (e.g. spherical
covariances in [11]).
VI. CONCLUSION
The contribution of this work is twofold: in primis we
combine tools of linear estimation and graph theory, to gain
a deep insight on SLAM with graphical models; then we
apply this theoretical analysis for retrieving an approximate
solution to the full SLAM problem, under mild assumptions
on the structure of the involved covariance matrices. The
proposed estimation process requires no initial guess and is
formally demonstrated to admit solution when applied to the
embedding of the pose graph. Experiments on a real dataset
conrm the validity of the theoretical analysis. It is possible
to consider the proposed approach as a linear initialization
method for iterative optimization or as a stand-alone tool. The
impact of the proposed methodology concerns different aspects
of the SLAM problem: (i) the solution only requires basic
linear algebra machinery hence it can be envisioned to apply
complexity reduction techniques (Cholesky decomposition,
QR factorization, blockwise inversion, etc.) or to use parallel
computational architectures (e.g., FPGA) making the approach
suitable for large scale mapping; (ii) the paper provides an
insight on the orientation wraparound problem: in large-scale
applications, one cannot expect to let the robot travel for a
long time without incurring in this issue; (iii) the linearity of
the framework provides a chance for devising an incremental
solution to graph-based SLAM.
ACKNOWLEDGMENTS
The authors wish to thank Frank Dellaert and four anony-
mous reviewers for comments that helped to improve the nal
version of this article. This work was partially funded by Min-
istero dellIstruzione, dellUniversit` a e della Ricerca (MIUR)
under MEMONET National Research Project, by Regione
Piemonte under MACP4log Grant (RU/02/26), by projects
MICINN-FEDER DPI2009-08126 and DPI2009-13710, and
grant MEC BES-2007-14772.
REFERENCES
[1] P. Barooah and J.P. Hespanha. Estimation on graphs from
relative measurements. IEEE Control Systems Magazine,
27(4):5774, 2007.
[2] L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona.
A rst-order solution to simultaneous localization and
mapping with graphical models. In Proc. of the IEEE
lnternational Conf. on Robotics and Automation, 2011.
[3] J.A. Castellanos, J. Neira, and J.D. Tard os. Limits to the
consistency of EKF-based SLAM. In 5th IFAC Symp. on
Intelligent Autonomous Vehicles, 2004.
[4] W. Chen. Graph Theory and Its Engineering Appli-
cations. Advanced Series in Electrical and Computer
Engineering, 1997.
[5] F. Dellaert and M. Kaess. Square root SAM: Simultane-
ous localization and mapping via square root information
smoothing. Int. J. Robot. Res., 25(12):11811203, 2006.
[6] F. Dellaert and A. Stroupe. Linear 2D localization and
mapping for single and multiple robots. In Proc. of the
IEEE Int. Conf. on Robotics and Automation, 2002.
[7] F. Dellaert, J. Carlson, V. Ila, K. Ni, and C. Thorpe.
Subgraph-preconditioned conjugate gradients for large
scale SLAM. In Proc. of the IEEE-RSJ Int. Conf. on
Intelligent Robots and Systems, 2010.
[8] A. Doucet, N. de Freitas, K. Murphy, and S. Russel. Rao-
Blackwellized particle ltering for dynamic bayesian
networks. In Proc. of the Conf. on Uncertainty in
Articial Intelligence, pages 176183, 2000.
[9] R.M. Eustice, H. Singh, and J.J. Leonard. Exactly sparse
delayed-state lters for view-based SLAM. Int. J. Robot.
Res., 22(6):11001114, 2006.
[10] U. Frese, P. Larsson, and T. Duckett. A multilevel
relaxation algorithm for simultaneous localization and
mapping. IEEE Trans. on Robotics, 21(2):196207,
2005.
[11] G. Grisetti, C. Stachniss, and W. Burgard. Non-linear
constraint network optimization for efcient map learn-
ing. IEEE Trans. on Intelligent Transportation Systems,
10(3):428439, 2009.
[12] J.L. Gross and T.W. Tucker. Topological graph theory.
Wiley Interscience, 1987.
[13] R.I. Hartley and A. Zisserman. Multiple View Geometry
in Computer Vision. Cambridge University Press, ISBN:
0521623049, 2000.
[14] R.A. Horn and C.R. Johnson. Matrix Analysis. Cam-
bridge University Press, UK, 1985.
[15] G. Huang, A.I. Mourikis, and S.I. Roumeliotis.
Observability-based rules for designing consistent EKF-
SLAM estimators. Int. J. Robot. Res., 29(5):502528,
2010.
[16] T. Kavitha, C. Liebchen, K. Mehlhorn, D. Michail,
R. Rizzi, T. Ueckerdt, and K. Zweig. Cycle bases in
graphs: Characterization, algorithms, complexity, and ap-
plications. Computer Science Rev., 3(4):199243, 2009.
[17] K. Konolige. Large-scale map-making. In Proc. of the
AAAI National Conf. on Articial Intelligence, 2004.
[18] R. K ummerle, B. Steder, C. Dornhege, M. Ruhnke,
G. Grisetti, C. Stachniss, and A. Kleiner. Slam
benchmarking webpage. http://ais.informatik.uni-
freiburg.de/slamevaluation, 2009.
[19] R. K ummerle, B. Steder, C. Dornhege, M. Ruhnke,
G. Grisetti, C. Stachniss, and A. Kleiner. On measuring
the accuracy of SLAM algorithms. Autonomous Robots,
27(4):387407, 2009.
[20] F. Lu and E. Milios. Globally consistent range scan align-
ment for environment mapping. Autonomous Robots, 4:
333349, 1997.
[21] R. Martinez-Cantin, N. de Freitas, and J. Castellanos.
Analysis of particle methods for simultaneous robot
localization and mapping and a new algorithm: Marginal-
SLAM. In Proc. of the IEEE lnternational Conf. on
Robotics and Automation, 2007.
[22] J.M. Mendel. Lessons in estimation theory for signal
processing, communications, and control. Englewood
Cliffs, NJ: Prentice-Hall, 1995.
[23] E. Olson, J.J. Leonard, and S. Teller. Fast iterative
optimization of pose graphs with poor initial estimates. In
Proc. of the IEEE Int. Conf. on Robotics and Automation,
pages 22622269, 2006.
[24] L. Quan and T. Kanade. Afne structure from line
correspondences with uncalibrated afne cameras. IEEE
Trans. on Pattern Analysis and Machine Intelligence, 19:
834845, 1997.
[25] D. Sabatta, D. Scaramuzza, and R. Siegwart. Improved
appearance-based matching in similar and dynamic envi-
ronments using a vocabulary tree. In Proc. of the IEEE
Int. Conf. on Robotics and Automation, pages 2262
2269, 2010.
[26] R. Smith and P. Cheesman. On the representation of
spatial uncertainty. Int. J. Robot. Res., 5(4):5668, 1987.
[27] S. Thrun and M. Montemerlo. The GraphSLAM algo-
rithm with applications to large-scale mapping of urban
structures. Int. J. Robot. Res., 25:403429, 2006.
[28] S. Thrun, D. Koller, Z. Ghahramani, H. Durrant-Whyte,
and A.Y. Ng. Simultaneous mapping and localization
with sparse extended information lters. In Proc. of
the 5th Int. Workshop on Algorithmic Foundations of
Robotics, 2002.
[29] S. Thrun, W. Burgard, and D. Fox. Probabilistic robotics.
MIT press, 2005.