JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

1

Map-Based Probabilistic Visual Self-Localization
Marcus A. Brubaker, Member, IEEE, Andreas Geiger, Member, IEEE, and Raquel Urtasun, Member, IEEE
Abstract—Accurate and efficient self-localization is a critical problem for autonomous systems. This paper describes an affordable
solution to vehicle self-localization which uses odometry computed from two video cameras and road maps as the sole inputs. The
core of the method is a probabilistic model for which an efficient approximate inference algorithm is derived. The inference algorithm isable to utilize distributed computation in order to meet the real-time requirements of autonomous systems in some instances. Becauseof the probabilistic nature of the model the method is capable of coping with various sources of uncertainty including noise in the visual
odometry and inherent ambiguities in the map (e.g., in a Manhattan world). By exploiting freely available, community developed maps
and visual odometry measurements, the proposed method is able to localize a vehicle to 4m on average after 52 seconds of driving on
maps which contain more than 2,150km of drivable roads.
Index Terms—localization, visual odometry, OpenStreetMaps, map

F

1

I NTRODUCTION

One of the fundamental tasks necessary for autonomous systems is to be able to accurately determine its position at all
times. This is necessary not only for core tasks like path
planning [1] and navigation [2], but also to simplify other tasks
such as scene understanding [3], [4], [5]. Despite decades ofresearch, self-localization is still an exciting open problem. In
this paper we are interested in building affordable and robust
solutions to this problem in the context of autonomous driving.
Currently, the most common self-localization technology is
the Global Positioning System (GPS) which utilizes satellites
in medium Earth orbit to provide location information. While
popular due to its low cost, GPS has limitations whichmake it potentially unsuitable as a stand-alone technology forautonomous systems. Notably, its signal is not always available
and its localization can become imprecise, e.g., in the presenceof skyscrapers, tunnels or jammed signals. While this may not
be problematic when helping a human navigate, it can havecatastrophic consequences for autonomous devices like selfdriving cars. Further, in applications involving undergroundor indoor navigation where GPS is completely unavailable analternative approach is required.
Several alternatives have been developed in the past few
years, which frame self-localization as a retrieval task. Towards this goal, the “world” is represented in terms of visualfeatures [6], [7], [8], [9], [10], [11] or 3D point clouds [12],[13], [14], [15], and localization is performed by retrieving
images or point clouds which are similar to the current
scene. In combination with GPS, impressive results havebeen demonstrated, e.g., at the DARPA Urban Challenge[16] or the Google self-driving car which has autonomously
driven over 500,000 km without incident as of August 20121 .
• This work was supported by a postdoctoral fellowship from the Natural
Science and Engineering Research Council of Canada (NSERC).

1. http://googleblog.blogspot.ca/2012/08/the-self-driving-car-logs-moremiles-on.html

However it remains unclear if maintaining an up-to-date world
representation will be feasible given the computation, memoryand communication requirements. Furthermore, privacy issues
need to be considered as recording and storing such data might
be illegal in some countries.
All aforementioned approaches have in common that theyrequire significant knowledge of the appearance of the world
for localization to be possible. In contrast, in this paper
we propose an approach which is able to localize a vehicle
in places that have never before been seen. We take ourinspiration from humans, which are able to perform this task
while having only access to a rough cartographic descriptionof the environment.
In particular, we propose to exploit OpenStreetMap (OSM),
a free online community-driven map, which is free and constantly updated, making our approach inexpensive. Towards
this goal, we derive a probabilistic map localization approachthat uses visual odometry estimates and OSM data as only
inputs. We demonstrate the effectiveness of our approach on
the KITTI visual odometry benchmark sequences [17]. As our
experiments show, we are able to self-localize our vehicle after
52 seconds of driving with an accuracy of 4 meters on a 18km2
map containing 2, 150km of drivable roads. A preliminary
version of this work was presented in [18].

2

R ELATED W ORK

The problem of map localization (sometimes known as the
kidnapped robot problem) has been traditionally approachedusing Monte Carlo methods [12], [13], [14], [15]. Here,the Markov assumption is used to maintain a particle-based
posterior representation of an agent’s pose, integrating laser orvisual observations. Typically, these methods operate only in asmall environment without providing any global (geographic)
positioning information. Furthermore, and most significantly,
they rely on more specific measurement sources (e.g., depth
measurements, sonar) and are restricted to small-scale environments (e.g., office scenes) where accurate 2D floor plans areavailable. While the approach presented here is similar to these

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

2

Fig. 1. The OpenStreetMap project: Free geographic data for the world. Left: Development and coverage of
OpenStreetMap from 2006-2012. Right: Screenshot of JOSM, a popular OSM editor, showing the level of annotation
detail for the intersection ’Mendelssohnplatz’ in Karlsruhe, Germany.

methods in that a recursive Bayesian filtering algorithm is
proposed, these methods use particle-based filtering techniques
which do not handle persistent uncertainty due to the wellknown issue of particle depletion. In contrast, the proposed
approach leverages analytic approximations, making it morestable when ambiguities persist over long periods (e.g., see
Figure 5). Maps have been widely used to improve upon GPS
measurements as well as to fill in when signals are unavailable
or degraded [19], [20], [21], [22]. However, these methods
assume that a relatively accurate initial position has been
provided, e.g., from a recently lost GPS signal. In contrast,
this work assumes no knowledge of the position of the vehicle
beyond the gross region.
In computer vision place recognition methods attempt tolocalize [6], [8], [10], [11], [23], [24], [25] or categorize[26], [27], [28] an image, given a large database of georeferenced images. Often, hashing methods in combination
with a geometric verification step are employed to register
the camera view to a 3D model of the scene which has beenobtained a-priori. 3D input information is used in [29], whereregression forests are applied to localize an RGB-D camera
within an indoor scene. When video streams or time stamped
image sequences are available, temporal dependencies can bemodelled as well [7], [9], [30]. While some of the proposed
methods have been applied successfully to single landmarks
or even at city-scale, they either can’t provide the required
accuracy [8], [31] or they require a large amount of reference
images for building the models. However, creating an up-todate “world database” seems at best costly if not impractical.
While services such as Google Street View have been successful at capturing images for many locations, much of the world
has still not been captured and maintaining updated images
will likely remain a challenge. Furthermore, vision-based place
recognition methods must be robust against changes in lighting
across day times and seasons [32] to achieve reliable and failsafe localization, a requirement hard to fulfill in practice. In
contrast, the maps used by our localization approach take up
only a couple of gigabytes for storing maps for the entiretyof planet earth2 and can be updated relatively quickly andinexpensively.
When one assumes that the initial position is known, visual
odometry [33] yields relative motion estimates that can be
integrated to obtain an estimate for the agent’s current location.
2. http://wiki.openstreetmap.org/wiki/planet.osm

While impressive performance on the KITTI visual odometry
benchmark [17] has been demonstrated for example in [34],[35], [36], the incremental nature of these methods inevitably
leads to drift, limiting the error of the leading methods ataround 1% in terms of translation and 0.003 deg/m in termsof rotation.
Simultaneous Localization And Mapping (SLAM) methods[37], [38], [39], [40], [41], [42], [43] are able to reduce thisdrift by using landmarks and jointly optimizing over all ora selection of poses and landmarks. Efficient optimization
strategies using incremental sparse matrix factorization [44] orrelative representations [45] have been proposed to make thesealgorithms tractable and scalable. Due to the lack of texture
in indoor scenes, RGB-D approaches leveraging Microsoft’s
Kinect sensor have been proposed recently [46], [47], [48],[49], [50], [51]. Drift can be further reduced by revisiting
places for several times and detecting loop closures in thetraveled trajectory [52], [53], [54], [55]. Incorporating these
constraints into the optimization leads to improved maps andreduces the localization error. Unfortunately, SLAM methods
can only localize in maps that have been recorded a-priori
with a sensor setup similar to the one used at localization time
and their performance strongly depends on the frequency withwhich places are revisited. In contrast, the proposed approachenables geographic localization without knowledge of theinitial location, relies only on freely available OpenStreetMap
information and doesn’t assume loopy trajectories.
The task of localization in road network maps has also
been approached using so-called map matching techniques[56], [57], [58], [59], [60], [61], [62]. With a goal similar toours, these methods are able to efficiently localize a querymap within a larger map region. While in principle such
query maps can be obtained by integrating visual odometry
measurements over time [63], neglecting drift can heavily
impact performance. A notable exception is [64] which aims
at localizing non-rigid subgraphs in a larger graph. However,their algorithm applies only to small graphs with up to 100
nodes. By proposing a probabilistic map localization modelthat explicitly models visual odometry measurements and theirnoise characteristics we mitigate these problems and obtain
fast localization times even on very large road maps with morethan 2, 000km of drivable roads.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

3

d

SINK

θ

6
1

r

5
6

5

β
2

4

1

3

4

2

3

Fig. 2. Map Representation: This figure shows a simple
street map (left) and its corresponding graph representation (right). Directed street segments are represented as
nodes and edges define their connectivity.

3

V ISUAL L OCALIZATION

The goal of this paper is to use one or two roof-mounted
cameras as well as a cartographic map of the environment
to self-localize a driving vehicle. The map contains streetsas line segments as well as intersection points, but no visualappearance information. As such, the only information that canbe used for self-localization is the trajectory of the vehicle
and as such we propose the use of visual odometry. Notethat other sources of information such as the speedometercan also be used, as they provide similar information. As the
trajectory resulting from the integration of visual odometry
is too noisy for direct shape matching in large maps due
to drift, here we propose a probabilistic approach to selflocalization that employs visual odometry measurements inorder to determine the instantaneous position and orientationof the vehicle in a given map. We define a graph-based
representation of the map as well as a probabilistic model of
how a vehicle can traverse the graph. Furthermore, we derivea filtering algorithm to perform inference in the probabilistic
model, which exploits the structure of the graph and propertiesof the model to efficiently perform approximate inference. In
order to keep running times reasonable, we propose to manage
the complexity of the mixture models using a novel algorithm
for simplifying Gaussian Mixture models which are used torepresent the posterior distribution. In the remainder of the section, we first discuss the employed map information, followedby our probabilistic model and the inference algorithm.

3.1

Origin p0

�� p1

ψ0
ψ1

c

p1
θ �
d
β
p
Origin 0

Fig. 3. Street Segment: Each street segment has a startand end position p0 and p1 , a length `, an initial headingof the street segment β and a curvature parameter α =ψ1 −ψ0. For arc segments c is the circle center, r is the`radius and ψ0 and ψ1 are the start and end angles of thearc. For linear segments, α = 0.
by providing local information. So far, more than 1.5 million3
users have registered and provided geographic information toOSM. As of December 2013, more than 3.7 billion GPS track
points have been submitted corresponding to over 2.1 billion
waypoints and 210 million roads in total.
The growth of OSM for the region of Europe is illustrated in
Figure 1 (left). Compared to commercial products like Google
Maps, the provided data is more up-to-date, includes many details and – most importantly – can be freely accessed and used
under the Open Database License. The startup GeoFabrik4 ,for example, provides daily snapshots for certain regions or
the whole globe (29 GB) which can be downloaded in XML
format. Figure 1 (right) shows a screenshot of JOSM5 , oneof the most popular OSM editors. Note that the annotations
do not only include streets, but also cycling and hiking trails,
traffic lights, signs, parking lots, building outlines, shops, trees,
postboxes, bars, restaurants, cinemas, recreational areas andmore. While the main focus of this work is on leveraging roadmaps for localization, we believe that the wealth of OSM data
will become useful in other areas of computer vision, such asurban scene understanding [3] or semantic image segmentationof road scenes [65].
For our purposes, we extract all crossings and all drivableroads (represented as piece-wise linear segments) connecting
them. We additionally extract the type of each street (i.e.,highway or rural) and the direction of traffic. By splitting eachbi-directional street into two one-way streets and “smoothing”
intersections using circular arcs, we obtain a lane-based map
representation, on which the state of the vehicle is defined.

The OpenStreetMap ProjectIn 2004, Steve Coast started the OpenStreetMap project withthe goal of creating a free editable map of the world much
like Wikipedia provides a free community-based encyclopedia.
While initial efforts focused mainly on mapping the United
Kingdom, volunteers around the globe quickly helped in
making OSM a worldwide success. Users can contribute to
OSM in various ways, for example by supplying GPS tracks
using portable GPS devices (e.g., while driving, cycling or
hiking), labeling objects such as buildings in aerial imagery or

3.2

Map Representation

The map itself is represented by a directed graph where nodes
represent street segments and edges define the connectivity of
those segments. Without loss of generality, all street segments
are assumed to be one-way and two-way streets are converted
to two one-way streets to fit this framework. Roads which
3. http://wiki.openstreetmap.org/wiki/stats
4. http://www.geofabrik.de/
5. http://josm.openstreetmap.de/JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 20074

dead-end or run off the edge of the map are connected to a“sink” node. Figure 2 represents an example of a map and thecorresponding graph representation.
We find it convenient to represent every street segment as
either a linear or a circular arc segment, which can bothbe described using the same parametric representation as
detailed below. Note that more complex road shapes can beapproximated with these primitives by joining multiple, shortsegments. As illustrated in Figure 3, we define the shape ofa street segment by its starting and ending points p0 and p1
respectively, initial heading angle β and a curvature parameter
α. The curvature is zero for linear segments (i.e., α = 0) and0
for circular arc segments where ψ0 and ψ1 are
α = ψ1 −ψ`
the starting and ending angle of the arc and ` is the arc lengthof the segment.
The position and orientation of a vehicle restricted to alocation on the map is then defined by the street segment uthat the vehicle is driving on, the distance from the origin ofthat street segment d and the angular offset θ of the vehicle
heading from the local street heading. Thus, the global headingof the vehicle is given byθ̄ = θ + β + αd(1)

and its position is
p̄ =

d
`−d
p0 + p1``

for a linear segment andp̄ = c + r d



`−d
d
ψ0 + ψ1
`
`

(2)


(3)

for a circular arc segment where r is the radius of the circle,
c denotes the circle center and d(ψ) = (cos ψ, sin ψ)T . We
refer the reader to Figure 3 for an illustration of all parametersof the street segment geometry. Important to note is that thisparameterization of heading allows the rate of global headingchange ∂ θ̄ to be dependent on the linear velocity ∂d/∂t whilestill allowing the dynamics to be modelled with a simple linear
diffusion model described below.
Given this map representation, we next present a state-spacemodel which describes the the motion of the vehicle in themap. A probabilistic inference algorithm is then derived torecursively compute the posterior over position. This filtering
algorithm is summarized in Algorithm 1.
3.3

State-Space Model

As visual odometry measurements are obtained at discrete timeintervals, we choose a discrete time model where the subscript
t represents the index of a data timestamp or – equivalently –the frame number. However, for brevity, we will call t simply’time’ in the following. Based on the map representation
introduced above, the state of the vehicle at time t is describedby xt = (ut , st ) where st = (dt , dˆt−1 , θt , θ̂t−1 )T and dˆt−1 ,θ̂t−1 are the distance and angle at the previous time andthe hat indicates that both are defined relative to the current
street ut which changes when the vehicle transitions to a newstreet segment. The visual odometry measurement computed
at time t is denoted by yt and directly measures the linear

Algorithm 1 Filter
1: Input: Posterior at t − 1, {Put−1 , Mt−1u }2: Input: Observation at t, yt
3: Initialize mixtures, Mtu ← ∅, for all u
4: for all streets ut−1 do5:for all streets ut reachable from ut−1 do
6:
M0 ← ∅
7:for all (ω, µ, Σ) ∈ Mt−1
ut−1 do
8:Compute cpred N (µpred , Σpred ) using Alg 2
9:Compute cupd
S N (µupd , Σupd ) using Alg 3
10:
M0 ← M0 {(cupd , µupd , Σupd )}11:
if ut 6= ut−1 then12:
Compute (c, µ,SΣ) to approximate M0
13:
Mtut ← Mtut {(c, µ, Σ)}
14:
else
S
15:
Mtut ← Mtut M0
16: for all streets u do
P17:
Put ← (c,µ,Σ)∈Mt cu

Mtu ← {( Pct , µ, Σ) | (c, µ, Σ) ∈ Mtu }
u
`u
19:
if |M
t | < 10 meters thenu20:
Simplify Mtu with Algorithm 4
P t
21: Normalize Put so thatu Pu = 1.
22: For all u, if Put < 10−50 set Put ← 0 and Mtu ← ∅23: Return: Posterior at t, {Put , Mtu }18:

and angular displacement from time t − 1 to time t. We
assume that the map data available is planar so visual odometry
measurements are projected onto the road plane which can beeasily recovered using SfM or stereo information. While height
information is sometimes available in OSM, it is typicallynoisy and unreliable so it was not used. Note, however, that themodel presented here could be extended to the 3D case should
reliable height data be available. The odometry observations
are modelled asp(yt |xt ) = N (yt |Mut st , Σyut )(4)p(xt |xt−1 ) = p(ut |xt−1 )p(st |ut , xt−1 )(5)where Mu = [md , mθ ]T , md = (1, −1, 0, 0)T and mθ =(αu , −αu , 1, −1)T . Note that the curvature of the street, αu ,is necessary because the global heading of the vehicle, θ̄,
depends on both d and θ, see (1). The state transition modelis factorized aswith street transition probability p(ut |xt−1 ), and state transition distribution p(st |ut , xt−1 ). The latter is assumed to be theresult of a linear transformation corrupted by Gaussian noisep(st |ut , xt−1 ) = N (st |Aut ,ut−1 st−1 + but ,ut−1 , Σsut )(6)with Σsut the covariance matrix for a given ut which is learned
from data as discussed in Section 4. A second-order, constantvelocity model is used to represent the change in d
dt = dt−1 + (dt−1 − dˆt−2 )(7)and a first order autoregressive model, i.e., AR(1), for the
angular offset θθt = γut−1 θt−1
(8)

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

where γut−1 ∈ [0, 1] is the parameter of the AR(1) model
which controls the correlation between θt and θt−1 . Even
though these models are relatively simple, they were foundto be very effective in practice. Combining (6), (7) and (8),one obtains


2
−1
0 0
1
0
0 0

(9)
Aut ,ut−1 = 00
γut 0 0 αut−1 − αut 1 0(−(`ut−1 , `ut−1 , 0, θut ,ut−1 )T ut 6= ut−1
but ,ut−1 =(10)(0, 0, 0, 0)T
ut = ut−1
where θut ,ut−1 = βut − (βut−1 + αut `ut−1 ) is the angle
between the end of ut and the beginning of ut−1 .
As noted above and illustrated in Figure 3, the componentsof st are relative to the current street, ut . When ut 6= ut−1 thestate transition model must be adjusted so that st is relative
to ut . The length of the ut−1 must be subtracted from both,and θ̂t−1 must be updated so that θ̂t−1 relative to ut has thesame global heading as θt−1 relative to ut−1 .
The street transition probability p(ut |xt−1 ) defines the probability of transitioning onto street ut given the previous statext−1 . To define the probability of changing street segments
we make use of the Gaussian state transition model in (6) togetZ `u +`ut−1p(ut |xt−1 ) ∝ ξut ,ut−1
N (x|aTd st−1 , aTd Σsut−1 ad )dx
`ut−1

(11)
where ad = (2, −1, 0, 0) is the first row of Aut ,ut−1 which
represents the predicted position according to (7),ut = ut−1

1
1
(12)
ξut ,ut−1 = |N(ut−1 )| ut ∈ N(ut−1 )0otherwiseis the prior probability of transitioning onto ut from ut−1 , andN(u) is the set of streets to which u connects.
Extremely short street segment can be problematic, as theymust be skipped entirely when a vehicle is traveling quickly
enough. To handle this, we add “leapfrog” edges to the graphwhich allow the vehicle to move from ut−1 to any ut towhich there exists a path in the base graph. As the speedof the vehicle is assumed to be limited, not all possible edges
need to be added. Instead, only edges representing paths of
30m or less are added which handles vehicles traveling up
to approximately 110km/h with observations every second. If
faster travel is necessary or observations are less frequent, thenlonger edges can easily be added. To incorporate “leapfrog”
edges into the model, the entries of but ,ut−1 are updated toconsider also transitioning over multiple street segments andξut ,ut−1 becomes the product of all ξ along the path.

3.4

Inference

Given the model described above, inference consists of computing the filtering distribution, p(xt |y1:t ). Like most recursive
filtering algorithms (e.g., the particle or Kalman filters) we will

5

Algorithm 2 Prediction Step
1: Input: Parameters of current mode µ, Σ
2: Input: Street nodes ut , ut−1d
3: if k dµg(µ, Σ)k < η then4:
Analytically approximate cpred N (µpred , Σpred )5:cpred ← p(ut |ut−1 , st−1 = µ)6:µpred ← Aut ,ut−1 µ + but ,ut−1
7:
Σpred ← Σsut + Aut ,ut−1 ΣATut ,ut−1
8: else9:
Sample to compute cpred N (µpred , Σpred )10:
for j = 1, . . . , M do(j)
11:st−1 ∼ N (µ, Σ)
(j)
(j)
12:
st ← Aut ,ut−1 st−1 + but ,ut−1(j)13:w(j) ← p(ut |ut−1 , st−1 = st−1 )PM14:cpred ← M −1 j=1 w(j)
PM
(j)
15:
µpred ← (M cpred )−1 j=1 w(j) st
(j)
(j)
P
(s −µpred )(st −µpred )TM
16:
Σpred ← Σsut + j=1 w(j) t
M cpred
17: Return: Predicted mode (cpred , µpred , Σpred )divide the algorithm into a prediction step where uncertainty is
propagated using the state transition model (5) and an update
step where the uncertainty is updated using the observation
model (4). The overall inference algorithm is described in
Algorithm 1 and the prediction and update steps are derived
next.
Remembering that xt = (ut , st ), the posterior can befactored using the product rule asp(xt |y1:t ) = p(st |ut , y1:t )p(ut |y1:t )(13)

where p(ut |y1:t ) is a discrete distribution over streets andp(st |ut , y1:t ) is a continuous distribution over the positionand orientation on a given street. The discrete distributionp(ut |y1:t ) is easily represented as a multinomial distribution
over street labels. For representing the continuous distributionp(st |ut , y1:t ) a Gaussian mixture model is used, i.e.,p(st |ut , y1:t ) =

Nut
X
i=1

(i)
πu(i)t N (st |µ(i)ut , Σut )(14)

where Nut is the number of components for the mixture(i)(i)(i) Nut
associated with ut and Mtut = {πut , µut , Σut }i=1
are theparameters of the mixture for ut . This is a general andpowerful representation and allows for efficient and accurateinference.
Assuming independent observations given the states andfirst order Markov state transitions, the filtering distribution
can then be written recursively asZp(yt |xt )p(xt |xt−1 )p(xt |y1:t ) =p(xt−1 |y1:t−1 )dxt−1
p(yt |y1:t−1 )(15)which, after factoring p(xt−1 |y1:t−1 ) yields
Z
X Put−1p(xt |y1:t ) =p(yt |xt ) p(st |ut , ut−1 , st−1 )
Zt
(16)
ut−1
× p(ut |ut−1 , st−1 )p(st−1 |ut−1 , y1:t−1 )dst−1

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

6
Analytic
Approximation

Algorithm 3 Update Step
1: Input: Current mode (cpred , µpred , Σpred ) on street ut
2: Input: Observation yt



1
0.9

p(ut |ut−1 , st−1 )MTut Σyut −1 Mut + Σ−1
pred


−1
T
y −1
4: µupd ← Σupd Mut Σut yt + Σpred µpredT
5: Σ0upd ← Σyutut + Mut Σpred Mωcpred |Σupd |0.5
6: cupd ← |Σ0.5 |Σy |0.5 exp
pred |
ut

0.7
0.6

0.4
0.3



where Put−1 = p(ut−1 |y1:t−1 ) and Zt = p(yt |y1:t−1 ).
Substituting the mixture model (14) and the state transition
dynamics (6) into (16), the integrand becomes
Nut−1

X
i=1

π

(i)

Z

p(ut |ut−1 ,st−1 )N (st |Ast−1 + b, Σs )

(17)

0.1
0

dt−1

Fig. 4. Approximate Inference: The sigmoidal shape ofthe street node transition probability allows for efficient
inference as analytic approximations are effective in thetails and a Monte Carlo approximation can be applied forthe central section which is very small in practice.

with respect to the given mode and the integral in (17) isapproximated analytically as
cpred N (st |Aµ + b, Σs + AΣAT )× N (st−1 |µ(i) , Σ(i) )dst−1

where the subscripts have been dropped for clarity. In general,
the integral in (17) is not analytically tractable due to the nonlinear form of p(ut |ut−1 , st−1 ). However, if p(ut |ut−1 , st−1 )was constant the integral could be solved easily. In the model
presented above p(ut |ut−1 , st−1 ) from (11) is the Gaussian
CDF and has sigmoidal shape (see Figure 4). Consequently, it
is approximately constant everywhere except near the transition point of the sigmoid. In the following this is exploited inorder to create an efficient but accurate approximate inference
algorithm.
3.4.1

Prediction Step

For each mode i representing a distribution over st−1 we
aim to determine whether p(ut |ut−1 , st−1 ) can be considered
constant and whether an analytic approximation can be used.
Looking at Figure 4, the goal is to determine whether themode, N (st−1 |µ(i) , Σ(i) ) lies in either tail (i.e., the blue
portion) or if it is close to the changepoint of the sigmoid.
To that end, consider the functionZg(µ, Σ) = p(ut |ut−1 , st−1 )N (st−1 |µ, Σ)dst−1(18)
and the norm of its gradient with respect to µ, i.e.,dk dµg(µ, Σ)k where we dropped the mode index i for notational clarity. Substituting (11) in for p(ut |ut−1 , st−1 ), andsimplifying givesg(µ, Σ) = ξut ,ut−1 Φ(`ut−1 + `u |m, s2 ) − Φ(`ut−1 |m, s2 )(19)
where m = aTd µ, s2 = aTd (Σsut−1 + Σ)ad and Φ(·|m, s2 ) is
the CDF of a univariate normal distribution with mean m andvariance s2 .
Using this simplification, there are now two cases based ondd
k dµg(µ, Σ)k. If k dµg(µ, Σ)k < η for some threshold6 , thenp(ut |ut−1 , st−1 ) is considered to be approximately constant
6. In practice, we used η = 10−8 .

≈c

≈0

0.5

0.2

− 12 kyt − Mut µpred k2Σ0
upd

7: Return: Updated mode (cupd , µupd , Σupd ).

�ut−1

0.8

−1

3: Σupd ←Analytic
Approximation

Monte Carlo
Approximation(20)

where cpred = p(ut |ut−1 , µ). This update is equivalent
to the prediction step of a Kalman filter. Note that moststreets we consider are relatively long, thus the section wherep(ut |ut−1 , st−1 ) can be considered constant occurs frequently
and we benefit from the analytical approximation. Furthermore, in those cases when cpred ≈ 0 (see left side of Figure4) the mode can simply be dropped.
d
If k dµg(µ, Σ)k ≥ η then the mode overlaps the inflection
point of p(ut |ut−1 , st−1 ) and the analytic approximation will
not suffice. Instead, we use a Monte Carlo approximation,(j)drawing a set of M = 400 samples st−1 ∼ N (µ, Σ) for
j = 1, . . . , M and the integral is then approximated witha single component cpred N (st |µpred , Σpred ) where cpred =
PM
1
(j)
, and
j=1 w
M
µpred =

MX
1
(j)w(j) stM cpred j=1

Σpred = Σs +

(21)

M
X
1
(j)
(j)
w(j) (st − µpred )(st − µpred )T
M cpred j=1

are found by matching moments to the Monte Carlo approximation
M
X
j=1

(j)

(j)

(j)

p(ut |ut−1 , st−1 )N (st |Ast−1 + b, Σs )
(j)

(22)
(j)

with st = Ast−1 + b and w(j) = p(ut |ut−1 , st−1 = st−1 ).
The prediction step is summarized in Algorithm 2.
3.4.2

Update Step

The result of the prediction step is a single mode
cpred N (st |µpred , Σpred ) which approximates the integral in(17). This mode must then be updated according to (16)in order to take account for observation yt . Because theobservation distribution p(yt |xt ) is Gaussian and linear in st ,this corresponds to multiplying two Gaussian densities and canJOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

Algorithm 4 GMM Simplification
1: Input: Mixture model parameters M
2: Initialize M0 = M
3: loop4:
Select a component to remove b̂ = arg minb0 ωb0
5:M̂ ← M0 \ {(ωb̂ , µb̂ , Σb̂ )}6:
Initialize the variational parameters φ and ψ
7:
while D̂(φ, ψ, M, M̂) ≥  and not converged do
8:
Minimize D̂(φ, ψ, M, M̂) using (29-33)
9:if D̂(φ0 , ψ 0 , M, M̂) ≥  then10:
Return: M0
11:
else
12:
M0 ← M̂
be solved in closed form. The resulting update can be written
as cupd N (st |µupd , Σupd ) whereωcpred |Σupd |0.5 − 12 kyt −Mut µpred k2Σ0upde(23)|Σpred |0.5 |Σyut |0.5−1
Σupd = MTut Σyut −1 Mut + Σ−1
(24)
pred


−1
µupd = Σupd MTut Σyut yt + Σ−1(25)
pred µpred
cupd =

with Σ0upd = Σyut +Mut Σpred MTut . We summarize this update
step in Algorithm 3.
Performing the prediction and update steps for each component and each pair of connected nodes produces a set of
mixture model components for each u, the weights of which
are proportional to Put . After normalizing the mixtures for eachstreet, normalizing across streets yields Put , the probabilityof being located on a given street. The filtering process issummarized in Algorithm 1. Note that this algorithm canbe performed in parallel by assigning subsets of streets todifferent workers, a fact which we exploit to achieve real-time
performance.
3.5

Managing Posterior Complexity

The previous section provides a basic algorithm to perform
inference by computing the filtering distribution recursively.
Unfortunately, straightforward application of this algorithm isintractable as the complexity of the posterior (i.e., the numberof mixture components) grows exponentially with time asmodes are duplicated at transitions with multiple options (e.g.,intersections). This can be seen by noting the two loops in
Algorithm 1 over ut and ut−1 . This means that every modein the current posterior can, in the next posterior, result in
multiple modes, yielding a worst case exponential growth. To
alleviate this, three approximations are used which limit theresulting complexity of the posterior. These approximations
have been found to work well in practice and to significantly
reduce computational costs
First, for each pair of connected streets, the modes thattransition from ut−1 to ut when ut−1 6= ut are all likely
similar. As such, all of the transitioned modes are replacedwith a single component using moment matching. Second,
after running the algorithm for a while most streets will have7

negligible probability but may still be associated with mixtureswith a large number of components. To avoid this, mixtures onstreets whose probability p(ut |y1:t ) is below a threshold arepruned such that these streets will have zero probability. To
avoid mistakes, we make use of a very conservative threshold,p(ut |y1:t ) ≤ 10−50 , in our experiments. Finally, even with theabove steps, the number of components in the posterior grows
with t. Many of the components will have small weight andbe redundant. To mitigate this, a mixture model simplification
procedure is run when the number of modes on a street
segment gets too large. This procedure, which we describe
next, removes components and updates others while keeping
the KL divergence below a specified threshold . The valuefor  was determined experimentally, as described in Section
4.4.
3.5.1

Mixture Model Simplification

Given a Gaussian mixture model with N = |M| components
and parameters denoted by M = {(πa , µa , Σa )}N
a=1 andprobability density functionXf (x) =
πa N (x|µa , Σa )
(26)
a

0

we seek a mixture model M0 =P{(ωb , µb , Σb )}N
b=1 withprobability density function g(x) = b ωb N (x|µb , Σb ) which
has the least number of components (i.e., minimal N 0 = |M0 |)such that D(f kg) <  where D(f kg) is the KL divergence. We
begin with M0 = M and successively remove the component
with the lowest weight from M0 . At the same time, we update
the remaining components to minimize the KL divergence
D(f kg). This process, of removing components and updating
the remaining ones, continues so long as M0 remains a good
approximation in the sense that D(f kg) < .
To minimize the KL divergence D(f kg), we use a variational upper bound [66]. Introducing the variationalparameters
P
φ
≥
0
and
ψ
≥
0
such
that
φ
=
πa and
a,b
a,b
a,b
b
P
ψ
=
ω
,
we
can
bound
the
KL
divergence
by
a,b
b
a
D(f kg) ≤ D̂(φ, ψ, M, M0 )whereD̂(φ, ψ, M, M0 ) =

X
a,b

(27)



φa,b
+ D(fa kgb )(28)
φa,b log
ψa,b

and D(fa kgb ) is the KL divergence between the Gaussian
distributions N (x|µa , Σa ) and N (x|µb , Σb ). In order to simply compute the upper bound of D(f kg), D̂(φ, ψ, M, M0 ) is
minimized with respect to the variational parameters φ and ψ.
Similarly, to update the components of M0 to better approximate M, D̂(φ, ψ, M, M0 ) is minimized with respect toboth the variational parameters φ and ψ and the component
parameters ωb , µb and Σb . While this objective function is
non-convex, the exact minima for each set of parameters canbe found conditioned on the others. This provides the basis
for an efficient coordinate-descent algorithm which, while notglobally optimal, was found to be effective in practice. Using
the method of Lagrange multipliers to enforce the constraints

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 20078

Driving Time

Heading

Position

TABLE 1
Quantitative Evaluation: Average position and heading error and driving time until localization. “M” and “S” indicate
monocular and stereo odometry, “G” GPS-based odometry and “O” is the oracle error, i.e., the error from projecting
the GPS positions onto the map. Stereo and GPS-based odometry results run on small maps as well as the fullregion map. Position and heading errors are computed over frames which have been localized. Sequences 04 and 06
are ambiguous and cannot be localized, see text and Figure 5.

P

M
S
G
S (full)G (full)O
M
SG
S (full)G (full)M
SG
S (full)G (full)b ωb = 1,00
16 ± 67m
2.1 ± 1.5m
1.8 ± 1.1m
2.2 ± 1.8m
1.9 ± 1.1m
0.8 ± 0.6m
2.0 ± 3.9◦
1.2 ± 1.3◦
1.0 ± 1.0◦
1.3 ± 1.2◦
1.0 ± 1.0◦
22s
22s
21s
41s
41s

P

01
893 ± 22m
5.1 ± 6.7m
3.6 ± 5.0m
2.6 ± 2.2m
3.8 ± 4.8m
1.3 ± 1.2m
5.2 ± 3.5◦
2.7 ± 1.8◦
0.9 ± 0.8◦
1.4 ± 0.9◦
1.3 ± 0.8◦
88s
88s
88s
90s
88s

b φa,b = πa and02
8.1 ± 7.6m
4.1 ± 2.9m
2.2 ± 1.3m
4.3 ± 3.0m
2.1 ± 1.3m
1.0 ± 0.8m
1.5 ± 1.2◦
1.3 ± 1.0◦
0.8 ± 0.9◦
1.2 ± 1.1◦
0.9 ± 0.9◦
26s
26s
26s
27s
27s

03
19 ± 7m
4.8 ± 2.0m
6.9 ± 2.9m
6.8 ± 1.6m
8.4 ± 2.3m
2.5 ± 1.8m
2.4 ± 1.6◦
1.6 ± 1.2◦
1.4 ± 0.9◦
1.2 ± 0.6◦
1.4 ± 0.6◦
41s
34s
34s
66s
64s

05
5.6 ± 4.6m
2.6 ± 1.4m
2.7 ± 1.6m
3.7 ± 2.4m
4.6 ± 3.3m
1.3 ± 1.1m
2.0 ± 1.8◦
1.4 ± 1.2◦
1.2 ± 1.1◦
1.3 ± 1.1◦
1.2 ± 1.1◦
199s
45s
44s
63s
55s

P

a ψa,b = ωb , the minima of(28) with respect to the individual parameters are achieved by
X
ωb =
φa,b
(29)
a

φa,b
0
a0 φa ,bψa,b exp (−D(fa kgb ))φa,b = πa P
ψ 0 exp (−D(fa kgb0 ))P b0 a,ba φa,b µaµb = Pφa,b

P aTa φa,b Σa + (µa − µb )(µa − µb )PΣb =a φa,b

ψa,b = ωb P

(30)
(31)
(32)
(33)

These equations are used iteratively to simplify the mixture
model and this procedure is summarized in Algorithm 4. The
variational parameters φ and ψ are initialized based on theweights of the mixture before components are removed and,after an individual component is removed, it’s φ and ψ values
are uniformly distributed to the remaining components.

4

E XPERIMENTAL E VALUATION

To evaluate the localization method in realistic situations,experiments were performed using the KITTI visual odometry dataset [17]. The 11 training sequences were used forquantitative evaluation as ground truth data7 is available. The
dataset consists of two video streams from a left and rightcamera. Visual odometry was computed from these videos
using LIBVISO2 [35], a freely available library for monocularand stereo visual odometry. To reduce computational time, thevisual odometry is subsampled to a rate of one frame persecond. Slower data rates are possible and may be faster but
were found to suffer from accumulated odometry error. To
7. An OxTS RT 3003 GPS unit was used which integrates GPS, RTK
corrections, IMU, and wheel odometry and has a rated accuracy of 0.02m.

07
08
15 ± 15m 45 ± 106m
1.8 ± 1.1m 6.0 ± 21.4m
1.5 ± 0.8m 2.4 ± 7.4m
1.7 ± 1.0m 6.0 ± 21.4m
1.5 ± 0.8m 1.9 ± 1.1m
0.6 ± 0.4m 1.1 ± 0.9m
1.3 ± 1.0◦
10 ± 30◦
1.9 ± 1.3◦ 1.2 ± 1.5◦
1.5 ± 1.1◦ 1.0 ± 1.3◦
2.3 ± 1.5◦ 1.2 ± 1.6◦
1.3 ± 1.0◦ 1.0 ± 1.3◦
34s
79s
26s
70s
26s
78s
34s
70s
32s
79s

09
5.4 ± 4.7m
4.2 ± 3.1m
3.8 ± 3.0m
4.4 ± 3.3m
4.0 ± 3.1m
1.2 ± 1.2m
1.6 ± 1.4◦
1.3 ± 1.1◦
0.9 ± 0.8◦
1.3 ± 1.4◦
1.0 ± 0.9◦
25s
24s
25s
44s
44s

10
534 ± 25m
3.9 ± 1.9m
2.5 ± 1.6m
4.9 ± 2.0m
2.8 ± 1.3m
1.1 ± 0.8m
116 ± 8◦1.3 ± 1.2◦
1.0 ± 1.0◦
1.4 ± 1.2◦
1.0 ± 0.9◦
94s
18s
18s
32s
32s

Average
39 ± 135m
3.7 ± 9.5m
2.4 ± 3.6m
4.0 ± 9.8m
2.6 ± 2.2m
1.2 ± 1.0m
5.4 ± 20.3◦
1.3 ± 1.3◦
1.0 ± 1.1◦
1.3 ± 1.3◦
1.0 ± 1.1◦
68 ± 54s
39 ± 23s
40 ± 24s
52 ± 20s
51 ± 21s

better illustrate the results, mid-size regions of OpenStreetMap
data were extracted for each sequence which included thetrue trajectory and the surrounding region. On average, thesesubregions covered an area of 2km2 and contain 47km of drivable roads. Unless otherwise specified, all experiments were
initialized with regularly placed mixture components whichapproximated a uniform probability of being at any point on
the map. The method is also able to localize successfully onmuch larger maps, see results in Figures 11 and 12, where
the map covers an 18km2 region and contains 2, 150km ofdrivable roads.
4.1

Quantitative Evaluation

Quantitative results can be found in Table 1, with corresponding qualitative results shown in Figures 9 and 10. Here,“M” and “S” indicate results using monocular and stereo
visual odometry respectively with localization performed on
the subregion maps. In addition, “gold-standard” odometry
measurements were computed based on the GPS trajectories(entry “G” in the table) and the algorithm was run using theparameters learned from the stereo data. Note that this does not
use the absolute positions from the GPS signal, only relative
position and orientation with respect to the previous frame.
Results on the full 18km2 map are indicated with “(full)”.
Finally, the method was also run using KITTI test visual
odometry sequences. These do not have publicly available
ground-truth so cannot be quantitatively assessed but results
can be viewed online: http://www.cs.toronto.edu/∼mbrubake.
To determine the accuracy of the maps and GPS data,
the ground truth GPS tracks were projected onto the nearest
street segment on the map and the position reprojection error
was computed. These errors, reported as “O” for oracle, areindicative of the best possible error which can be achieved
using the given map data. In some cases this error canbe significant, as the map data does not account for lane

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

65
60
55
50
45
40
35
30
25
200.0

350

Monocular
Stereo1.5
0.5
1.0Initial Region Size (km)Distance to Localize (m)Time to Localize (s)

9

2.0

300
250

Monocular
Stereo

200
150
100
500.0

1.5
0.5
1.0
Initial Region Size (km)2.0

Fig. 7. Map Size: Driving time (left) and distance travelled(right) before localization as a function of the map size.

Position Error (m)2520

Monocular
Stereo

15
10
5
0
10-5

10-4
10-3
10-2
Simplify Threshold (nats)

10-1

Computation Time Per Frame (s)Fig. 5. Ambiguous Sequences: Both 04 and 06 cannot
be localized due to fundamental ambiguities. Sequence
04 consists of a short, straight driving sequence and 06
traverses a symmetric part of the map, resulting in two
equally likely modes.
1.0
0.8

Monocular
StereoReal-time

0.6
0.4
0.2
0.0 -5
10

10-4
10-3
10-2
Simplify Threshold (nats)

10-1

Fig. 6.
Simplification Threshold: Impact of the simplification threshold  on localization accuracy (left) andcomputation time (right). See Section 4.4.

widths, number of lanes or intersection sizes. Finally, chance
performance was computed to be 397m by measuring the
average distance of the GPS data to the mean road position ofeach subregion map.
The projected GPS data was also used to learn the small
number of model parameters. In particular, the street stateevolution noise covariance Σsu , the angular AR(1) parameter γu and the observation noise Σyu were estimated using
maximum likelihood. Different parameters were learned for
highways and city/rural roads as the visual odometry performs
significantly worse at higher speeds. The data from the last halfof each sequence was used to learn the parameters.
The accuracy of position and heading estimates is notwell defined until the posterior has converged to a single
mode. Thus, accuracy is only computed once a sequence has
been localized. All results are divided into two temporally
contiguous parts: unlocalized and localized. A sequence isdefined to be localized when, for at least ten seconds, there is asingle mode in the posterior. Once the criteria for localizationis met, all subsequent frames are considered localized. Errors
in global position and heading of the MAP state for localized
frames were computed using the GPS data as ground truth.
Overall, the position and heading were estimated to an
accuracy of 3.7m and 1.3◦ using stereo visual odometry on
the subregion maps. Note that this is within the standard
deviation of the oracle error of 2.6 ± 2.2m, which is a lower
bound on the achievable error induced by inaccuracies in the
map and GPS data. These results outperform typical consumer
grade navigation systems which offer accuracy of around 10m
at best and is better than the 5.19m reported by [63] on

the same sequences. Furthermore, errors are comparable tothose achieved using the GPS-based odometry, suggesting theapplicability and utility of low-cost vision-based sensors for
localization. Using monocular odometry as input performs
worse and suffers catastrophic failures on sequences 01, 08
and 10. However, excluding these sequences, it was accurate
to 11.5m. Sequence 01 is particularly challenging for the
monocular odometry as it is exclusively highway driving
where high speeds and sparse visual features results in an
accumulated odometry errors of more than 500m, making
accurate localization impossible. Sequence 08 localizes wellinitially, but a large outlier in the monocular odometry after
localization results in significant errors at the end of thesequence. In contrast the stereo visual odometry performs wellon all sequences shown in Figures 9 and 10 and is able tolocalize successfully in all but sequences 04 and 06 which are
discussed next.
4.2 Identifiable Sequences
A natural question to ask is what makes localization of avehicle difficult. Part of this answer comes from two sequences
which the system was unable to localize, sequences 04 and
06. As shown in Figure 5, these sequences are fundamentallyambiguous and cannot be localized with monocular, stereo or
even GPS-based odometry. Sequence 04 is a short sequence ona straight road segment and, in the absence of any turns, cannot
be localized beyond somewhere on the long road segment.
Sequence 06 is longer and has turns, but traverses a symmetricpath which results in a fundamental bimodality. In both cases
our approach correctly indicates the set of probable locations.
More generally though, one can look at how long unambiguous sequences took to localize. Consider the two sequences
which localized most quickly on the full maps, sequences 02
and 10. Looking at these sequences in Figures 11 and 12,shows that both sequences traversed curved roads, making
them quickly uniquely identifiable. In contrast, consider thetwo sequences which took the longest to localize on the fullmaps, sequences 01 and 08. These sequences demonstrate thatlong straight stretches are difficult to localize and even withturns, as in sequence 08, can be problematic. However, alsointeresting to note is that both of these sequences showedno practical difference between the subregion and full map
localization time.
4.3 Simplification Threshold
To determine the value used for the simplification threshold multiple values were tried. Figure 6 depicts the average

60
50
40
30
20
10
0
10-1

Angular Error (deg)Position Error (m)JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

101
100
102
Signal to Noise Ratio

103

4.5
4.0
3.5
3.0
2.5
2.0
1.5
1.0
0.5 -1
10

101
100
102
Signal to Noise Ratio

10

103

Fig. 8. Localization Accuracy with Noise: Position andheading error with different noise levels, averaged over
five independent samples of noise.

computation time per frame and localized position error as afunction of the threshold, with values ranging from 10−5 to 0.1
nats. Sequences 04 and 06 were excluded from this experimentbecause, as discussed above, they are inherently ambiguous.
As expected, computation time decreases and error increases
with more simplification (i.e., larger threshold). However, there
is a point of diminishing returns for computation time around10−2 nats, and little difference in error for smaller values.
Thus a simplification threshold of  = 10−2 was selected andused for all other experiments.
4.4

Map Size

To understand how the map region size impacts performance,experiments were run where uniform initial probability was
restricted to a square subregion of the map centred at the
ground truth initial position and all other parts of the map
were given zero initial probability. The size of the square wasvaried from 100m up to the typical subregion map size of
2km, resulting in an average of 300m to 47km of drivableroad. For this range of sizes, the average time to localization
was computed for all non-ambiguous sequences (i.e., all but
04, 06) and was plotted as a function of region size in Figure
7. As expected, small initial regions localized faster. However,somewhat surprisingly, after the region becomes sufficiently
large, the impact of region size on localization diminishes.
Indeed, average localization time (right column of Table 1)on the full maps was only 13 seconds longer on average thanon the subregion maps. This suggests that most paths quickly
become unique as they get longer, even in very large regions.
4.5

Noise

To study the impact of noise on the localization accuracy,
odometry measurements were synthesized by adding Gaussian
noise to the GPS-based odometry. Five different samples ofnoisy odometry were created for each sequence with signalto-noise ratios (SNR) ranging from 0.1 to 1000. Error inposition and heading after localization is plotted in Figure 8
as a function of SNR. As expected, error increases as the SNR
decreases, however the performance scales well, showing littlechange in error until the SNR drops below 1.
4.6

Scalability

Running on 16 cores with a basic Python implementation, weare able to achieve real time results on the subregion maps asshown in Figure 6 (right). To test the methods ability to scale tolarge maps we ran the sequences using stereo odometry with amap covering the entire urban district of Karlsruhe, Germany.
The map is around 18km2 and contains over 2, 150km ofdrivable road. Errors and localization times are shown in Table
1 for both stereo and GPS-based odometry, indicated by “S(full)” and “G (full)” respectively. The errors are effectively
equivalent to those with the smaller maps and the localization
time is only slightly longer. Computation was slower, taking
around 20 seconds per frame on average. However, it isexpected that this could be greatly improved with suitable
optimization and further parallelization.

5

C ONCLUSIONSIn this paper we have described an approach to selflocalization which employs (one or two) cameras mounted on
the vehicle as well as crowd sourcing in the form of free online
maps. The effectiveness of our approach was demonstrated in avariety of scenarios including highway, suburbs and crowded
urban scenes. Furthermore, the approach has been validated
on the KITTI visual odometry benchmark and shown to beable to localize the vehicle with an average precision of 4m
after less than a minute of driving. This is a new and exciting
problem for computer vision and we believe there is much
more to do. Experiments with monocular odometry suggestthat noise in the observations is likely heavy tailed, as there
are periodic, large outliers, and extending the model to handle
this would improve its robustness. The probabilistic framework
here could be easily extended to incorporate other visual cuesof location, for instance, learning to classify street types fromvideo frames. In particular, OpenStreetMaps contains othersalient pieces of information to aid in localization such as
speed limits, street names, and more; we plan to exploit thisinformation in the future. Code and videos are available at
http://www.cs.toronto.edu/∼mbrubake.

R EFERENCES[1]

D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning forautonomous vehicles in unknown semi-structured environments,” IJRR,
vol. 29, no. 5, pp. 485–501, 2010.
[2] D. Delling, P. Sanders, D. Schultes, and D. Wagner, “Engineering route
planning algorithms,” in Algorithmics of large and complex networks.
Springer, 2009, pp. 117–139.
[3] A. Geiger, M. Lauer, C. Wojek, C. Stiller, and R. Urtasun, “3d traffic
scene understanding from movable platforms,” PAMI, 2014.
[4] S. Sengupta, E. Greveson, A. Shahrokni, and P. H. Torr, “Semantic
modelling of urban scenes,” in ICRA, 2013.
[5] D. Munoz, J. A. Bagnell, and M. Hebert, “Stacked hierarchical labeling,”
in ECCV, 2010.
[6] G. Baatz, K. Köser, D. Chen, R. Grzeszczuk, and M. Pollefeys,“Leveraging 3D City Models for Rotation Invariant Place-of-Interest
Recognition,” IJCV, 2012.
[7] H. Badino, D. Huber, and T. Kanade, “Real-time topometric localization,” in ICRA, May 2012.
[8] J. Hays and A. A. Efros, “im2gps: estimating geographic informationfrom a single image,” in CVPR, 2008.
[9] J. Levinson, M. Montemerlo, and S. Thrun, “Map-based precisionvehicle localization in urban environments,” in RSS, 2007.
[10] Y. Li, N. Snavely, D. Huttenlocher, and P. Fua, “Worldwide pose
estimation using 3d point clouds,” in ECCV, 2012.
[11] T. Sattler, B. Leibe, and L. Kobbelt, “Fast image-based localization using
direct 2d-to-3d matching,” in ICCV, 2011.
[12] F. Dellaert, W. Burgard, D. Fox, and S. Thrun, “Using the condensation
algorithm for robust, vision-based mobile robot localization,” CVPR,
1999.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

11

Fig. 9. Selected Frames on the Subregion Maps: Inference results for unambiguous sequences. The left most column
shows the full map for each sequence, followed by zoomed in sections of the map showing the posterior distribution
over time. The black line is the GPS trajectory and the concentric circles indicate the current GPS position. Grid lines
are every 500m. High probability is indicated with red while low probability regions are shown in blue.
[13] D. Fox, W. Burgard, F. Dellaert, and S. Thrun, “Monte carlo localization:
Efficient position estimation for mobile robots,” in AAAI, 1999.
[14] J.-S. Gutmann, W. Burgard, D. Fox, and K. Konolige, “An experimental
comparison of localization methods,” in ICIRS, 1998.
[15] S. M. Oh, S. Tariq, B. N. Walker, and F. Dellaert, “Map-based priors
for localization,” in ICIRS, 2004.
[16] M. Buehler, K. Iagnemma, and S. Singh, Eds., The DARPA Urban
Challenge, ser. Advanced Robotics, vol. 56, 2009.
[17] A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for Autonomous
Driving? The KITTI Vision Benchmark Suite,” in CVPR, 2012.
[18] M. A. Brubaker, A. Geiger, and R. Urtasun, “Lost! Leveraging the
Crowd for Probabilistic Visual Self-Localization,” in CVPR, 2013.
[19] M. E. El Najjar and P. Bonnifait, “A road-matching method for
precise vehicle localization using belief theory and kalman filtering,”
Autonomous Robots, vol. 19, no. 2, pp. 173–191, 2005.
[20] J. Guivant and R. Katz, “Global urban localization based on road maps,”in IROS, 2007, pp. 1079–1084.
[21] S. Bonnabel and E. Salaün, “Design and prototyping of a low-cost

vehicle localization system with guaranteed convergence properties,”
Control Engineering Practice, vol. 19, no. 6, pp. 591–601, 2011.
[22] C. Fouque and P. Bonnifait, “Matching raw gps measurements on anavigable map without computing a global position,” IEEE Transactions
on Intelligent Transportation Systems, vol. 13, no. 2, pp. 887–898, 2012.
[23] F. Li and J. Kosecka, “Probabilistic location recognition using reduced
feature set,” in ICRA, 2006.
[24] W. Zhang and J. Kosecka, “Image based localization in urban environments,” in 3DPVT, 2006.
[25] G. Schindler, M. Brown, and R. Szeliski, “City-scale location recognition,” in CVPR, 2007.
[26] A. Pronobis, B. Caputo, P. Jensfelt, and H. Christensen, “A discriminative approach to robust visual place recognition,” in IROS, 2006.
[27] A. Rottmann, O. Martı́nez Mozos, C. Stachniss, and W. Burgard, “Placeclassification of indoor environments with mobile robots using boosting,”
in AAAI, 2005.
[28] J. Wu and J. M. Rehg, “Where am I: Place instance and category
recognition using spatial PACT,” in CVPR, 2008.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

12

Fig. 10. Selected Frames on the Subregion Maps: Inference results for unambiguous sequences. The left most column
shows the full map for each sequence, followed by zoomed in sections of the map showing the posterior distribution
over time. The black line is the GPS trajectory and the concentric circles indicate the current GPS position. Grid lines
are every 500m. High probability is indicated with red while low probability regions are shown in blue.
[29] J. Shotton, B. Glocker, C. Zach, S. Izadi, A. Criminisi, and A. Fitzgibbon, “Scene coordinate regression forests for camera relocalization in
rgb-d images,” in CVPR, 2013.
[30] E. Kalogerakis, O. Vesselova, J. Hays, A. A. Efros, and A. Hertzmann,“Image sequence geolocation with human travel priors,” in ICCV, 2009.
[31] R. Dewri, P. Annadata, W. Eltarjaman, and R. Thurimella, “Inferring
trip destinations from driving habits data,” in WPES, 2013.
[32] W. S. Churchill and P. Newman, “Experience-based navigation for longterm localisation,” IJRR, 2013.
[33] D. Nister, O. Naroditsky, and J. R. Bergen, “Visual odometry,” in CVPR,
2004.
[34] P. Alcantarilla, L. Bergasa, and F. Dellaert, “Visual odometry priors forrobust EKF-SLAM,” in ICRA, 2010.
[35] A. Geiger, J. Ziegler, and C. Stiller, “Stereoscan: Dense 3d reconstruction in real-time,” in IV, 2011.
[36] M. Kaess, K. Ni, and F. Dellaert, “Flow separation for fast and robust
stereo odometry,” in ICRA, 2009.
[37] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM 2.0:
An improved particle filtering algorithm for simultaneous localizationand mapping that provably converges,” in IJCAI, 2003.
[38] S. Se, D. Lowe, and J. Little, “Mobile robot localization and mappingwith uncertainty using scale-invariant visual landmarks,” IJRR, vol. 21,
pp. 735–758, 2002.
[39] H. Durrant-Whyte and T. Bailey, “Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms,” RAM, 2006.
[40] A. Davison, I. Reid, N. Molton, and O. Stasse, “MonoSLAM: Real-timesingle camera slam,” PAMI, vol. 29, 2007.

[41] R. A. Newcombe, S. J. Lovegrove, and A. J. Davison, “Dtam: Dense
tracking and mapping in real-time.” in CVPR, 2011.
[42] A. Ranganathan and F. Dellaert, “Online probabilistic topological mapping,” IJRR, vol. 30, no. 6, pp. 755–771, 2011.
[43] A. Ranganathan, E. Menegatti, and F. Dellaert, “Bayesian inference inthe space of topological maps,” Transactions on Robotics, 2006.
[44] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, andF. Dellaert, “iSAM2: Incremental smoothing and mapping using theBayes tree,” IJRR, vol. 31, pp. 217–236, 2012.
[45] C. Mei, G. Sibley, M. Cummins, P. Newman, and I. Reid, “Real: A
system for large-scale mapping in constant-time using stereo,” IJCV,
pp. 1–17, 2010.
[46] J. Engel, J. Sturm, and D. Cremers, “Semi-dense visual odometry for a
monocular camera,” in ICCV, 2013.
[47] E. Bylow, J. Sturm, C. Kerl, F. Kahl, and D. Cremers, “Real-time camera
tracking and 3d reconstruction using signed distance functions,” in RSS,
2013.
[48] C. Kerl, J. Sturm, and D. Cremers, “Dense visual slam for rgb-d
cameras,” in IROS, 2013.
[49] R. A. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J.
Davison, P. Kohli, J. Shotton, S. Hodges, and A. Fitzgibbon, “Kinectfusion: Real-time dense surface mapping and tracking,” in ISMAR, 2011.
[50] F. Endres, J. Hess, N. Engelhard, J. Sturm, D. Cremers, and W. Burgard,“An evaluation of the rgb-d slam system,” in ICRA, 2012.
[51] T. Whelan, M. Kaess, J. Leonard, and J. McDonald, “Deformation-based
loop closure for large scale dense RGB-D SLAM,” in IROS, Tokyo,
Japan, November 2013.
[52] M. Cummins and P. Newman, “FAB-MAP: Probabilistic Localization

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

13

Fig. 11. Selected Frames on the Full Map: Inference results for unambiguous sequences. The left most column
shows the full map for each sequence, followed by zoomed in sections of the map showing the posterior distribution
over time. The black line is the GPS trajectory and the concentric circles indicate the current GPS position. Grid lines
are every 2km. High probability is indicated with red while low probability regions are shown in blue.

and Mapping in the Space of Appearance,” IJRR, vol. 27, no. 6, pp.
647–665, 2008.
[53] R. Paul and P. Newman, “FAB-MAP 3D: Topological mapping withspatial and visual appearance,” in ICRA, 2010.
[54] B. Williams, M. Cummins, J. Neira, P. Newman, I. Reid, and J. Tardós,“A comparison of loop closing techniques in monocular SLAM,” RAS,
2009.
[55] B.Williams, G. Klein, and I. Reid, “Automatic relocalization and loop
closing for real-time monocular slam,” PAMI, vol. 33(9), 2011.
[56] A. Javanmard, M. Haridasan, and L. Zhang, “Multi-track map matching,”
ArXiv e-prints, Sep 2012.
[57] D. Chen, A. Driemel, L. J. Guibas, A. Nguyen, and C. Wenk, “Approximate map matching with respect to the fréchet distance,” in ALENEX,
2011, pp. 75–83.
[58] R. Raymond, T. Morimura, T. Osogami, and N. Hirosue, “Map matchingwith hidden markov model on sampled road network,” in ICPR, 2012.
[59] Y. Li, Q. Huang, M. Kerber, L. Zhang, and L. Guibas, “Large-scale joint

map matching of gps traces,” in SIGSPATIAL, 2013.
[60] C. Goh, J. Dauwels, N. Mitrovic, M. T. Asif, A. Oran, and P. Jaillet,“Online map-matching based on hidden markov model for real-timetraffic sensing applications,” in ITSC, 2012.
[61] P. Newson and J. Krumm, “Hidden markov map matching through noiseand sparseness,” in GIS, 2009.
[62] S. Se, D. Lowe, and J. Little, “Multi-track map matching,” Transactions
on Robotics, vol. 21(3), 2005.
[63] G. Floros, B. van der Zander, and B. Leibe, “OpenStreetSLAM: Global
vehicle localization using OpenStreetMaps,” in ICRA, 2013.
[64] E. Serradell, P. Glowacki, J. Kybic, F. Moreno-Noguer, and P. Fua,
“Robust Non-Rigid Registration of 2D and 3D Graphs,” in CVPR, 2012.
[65] B. Micusk, J. Koseck, and G. Singh, “Semantic parsing of street scenes
from video,” IJRR, vol. 31, no. 4, pp. 484–497, 2012.
[66] J. Hershey and P. Olsen, “Approximating the Kullback-Leibler Divergence Between Gaussian Mixture Models,” in ICASSP, vol. 4, 2007.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007

14

Fig. 12. Selected Frames on the Full Map: Inference results for unambiguous sequences. The left most column
shows the full map for each sequence, followed by zoomed in sections of the map showing the posterior distribution
over time. The black line is the GPS trajectory and the concentric circles indicate the current GPS position. Grid lines
are every 2km. High probability is indicated with red while low probability regions are shown in blue.

Marcus A. Brubaker received his Ph.D. in Computer Science from the University of Toronto in2011 and from 2011 to 2014 he was a postdoctoral fellow at TTI-Chicago. He is currently a
postdoctoral fellow at the University of Toronto
and consults with Cadre Research Labs. His research interests span statistics, machine learning and computer vision and includes applications in computational biology and forensics.

Andreas Geiger received his Diploma in computer science and his Ph.D. degree from Karlsruhe Institute of Technology in 2008 and 2013.
Currently, he is a research scientist in the Perceiving Systems group at the Max Planck Institute for Intelligent Systems in Tübingen. His
research interests include computer vision, machine learning and scene understanding.

Raquel Urtasun is an Assistant Professor at
the Department of Computer Science, Universityof Toronto. From 2009-2014 she was an Assistant Professor at TTI-Chicago, a philanthropically endowed academic institute located in thecampus of the University of Chicago. She wasa visiting professor at ETH Zurich during thespring semester of 2010. Previously, she wasa postdoctoral research scientist at UC Berkeley and ICSI and a postdoctoral associate at
the Computer Science and Artificial Intelligence
Laboratory (CSAIL) at MIT. Raquel Urtasun completed her PhD at theComputer Vision Laboratory, at EPFL, Switzerland in 2006 working with
Pascal Fua and David Fleet at the University of Toronto. She has beenarea chair of multiple machine learning and vision conferences (i.e.,
NIPS, UAI, ICML, CVPR, ECCV, ICCV), she is on the editorial boardof the International Journal of Computer Vision (IJCV), and servedin the committee of numerous international conferences. Her majorinterests are statistical machine learning, computer vision and robotics,
with a particular interest in structured prediction and their application toautonomous driving.