See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/224244939

Modeling and Tracking the Driving Environment With a Particle-Based

Occupancy Grid

Article  in  IEEE Transactions on Intelligent Transportation Systems · January 2012

DOI: 10.1109/TITS.2011.2158097 · Source: IEEE Xplore

CITATIONS READS
103 475

3 authors, including:

Radu Danescu Sergiu Nedevschi

Universitatea Tehnica Cluj-Napoca Universitatea Tehnica Cluj-Napoca
78 PUBLICATIONS   1,075 CITATIONS    337 PUBLICATIONS   2,891 CITATIONS   

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

PAN-Robots View project

Intelligent System For The Noninvasive Detection and Evaluation of Liver Fibrosis, Restructuration and Dysplastic Nodules, Using 2d/3D Ultrasound and Molecular Markers
View project

All content following this page was uploaded by Radu Danescu on 04 June 2014.

The user has requested enhancement of the downloaded file.

 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

Modeling and Tracking the Driving Environment

with a Particle Based Occupancy Grid
Radu Danescu, Florin Oniga, and Sergiu Nedevschi, Member, IEEE

Abstract— Modeling and tracking the driving environment is a Also, sometimes a driving assistance application needs to have
complex problem, due to the heterogeneous nature of the real static and dynamic information about the environment before a
world. In many situations, modeling the obstacles and the driving model can be instantiated and tracked, or it may use this
surfaces can be achieved by the use of geometrical objects, and additional information in model fitting and model-based
tracking becomes the problem of estimating the parameters of
these objects. In the more complex cases, the scene can be
tracking. For these reasons, solutions for intermediate level
modeled and tracked as an occupancy grid. This paper presents a representation and tracking are devised. These intermediate
novel occupancy grid tracking solution, based on particles, for representation and tracking solutions can be based on
tracking the dynamic driving environment. The particles will occupancy grids, or directly on the 3D points (the 6D vision
have a dual nature – they will denote hypotheses, as in the technique, presented in [1]), on compact dynamic obstacle
particle filtering algorithms, but they will also be the building primitives called stixels [2], or they can be replaced with
blocks of our modeled world. The particles have position and
speed, and they can migrate in the grid from cell to cell depending
specialized techniques of detecting critical motion [3]. In what
on their motion model and motion parameters, but they will also follows, we’ll focus on the works related to occupancy grids.
be created and destroyed using a weighting-resampling Maybe one of the first uses of occupancy grids, under the
mechanism specific to particle filter algorithms. The tracking name of probabilistic local maps, is presented by Elfes in [4],
algorithm will be centered on particles, instead of cells. An in the context of sonar based robot navigation. Another paper
obstacle grid derived from processing a stereovision-generated by the same author [5] names the occupancy maps occupancy
elevation map is used as measurement information, and the
measurement model takes into account the uncertainties of the grids, and describes the probability inference mechanism for
stereo reconstruction. The resulted system is a flexible, real-time handling the uncertainty of a range sensor in computing the
tracking solution for dynamic unstructured driving environments. probability of each cell’s occupancy state. In the same
reference we find a definition of the occupancy grid: “the
Index terms-Occupancy grids, environment modeling, tracking, occupancy grid is a multi-dimensional random field that
particle filtering, stereovision. maintains stochastic estimates of the cells in a spatial lattice”.
The initial occupancy grids, such as those presented in [4]
I. INTRODUCTION and [5], are simple 2D maps of the environment, each cell

T he tasks of modeling and perceiving the driving

environment are a continuous challenge, because there are
multiple types of scenarios, of different degrees of order and
describing the probability of it being occupied or free.
However, for many tracking applications, especially in the
driving assistance field, there is a need for estimating the
complexity. Some environments are well-regulated, and the dynamic parameters of the environment, namely the speed of
types of static and dynamic objects are easily modeled and each grid cell. By adding the speed factor in the environment
tracked using geometrical models and their parameters. The estimation, the complexity increases significantly, as the cells
obstacles can be modeled as cuboids having position, size and are now strongly interconnected. The work of Coué et al,
speed, and the driving surface delimiters can be modeled as presented in [6], uses a 4D occupancy grid, where each cell
parametrical curves. The highway and most of the urban and has a position and two speed components along each axis. By
rural sections of road are usually suitable for geometrical estimating the occupancy of each cell in the 4D grid, the
modeling and tracking. speeds for the classical cells in the 2D grid can be computed.
The conditions change when the environment to be tracked Another solution for the representation of speeds is
is an intersection, a busy urban center, or an off-road scenario. presented by Chen et al, in [7]. Instead of having a 4D grid,
Even if parts of this environment can be tracked by estimating this solution comes back to 2D, but uses for each cell a
the parameters of a geometrical model, many essential parts of distribution of speeds, in the form of a histogram. The
the environment will not fulfill the constraints of the models. Bayesian inference mechanism relies on sensor data and
antecedent cells, the list of antecedents being decided by the
Manuscript received May 31, 2010. This work was supported by CNCSIS speed hypotheses.
–UEFISCSU, project number PNII – IDEI 1522/2008, and by the POSDRU A simpler, but limited way of handling the dynamic aspects
program, financing contract POSDRU/89/1.5/S/62557.
Radu Danescu, Florin Oniga and Sergiu Nedevschi are with the Technical
of the environment is presented in [8]. Instead of estimating
University of Cluj-Napoca, Computer Science Department (e-mail: the speed of each cell, this solution relies on “occupancy
[email protected]). Department address: Computer Science trails”, which are specific patterns, similar to the motion blur
Department, Str. Memorandumului, Nr. 28, Cluj-Napoca, Romania. Phone:
of the camera, which can be used to derive the trajectory and
+40 264 401457. The authors contributed equally to this work.
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

therefore the speed of the moving objects. A more CONDENSATION [16], but can also be regarded as physical
sophisticated method is presented in [9], where the building blocks of our modeled world. The tracking algorithm
inconsistencies in the static grid are detected as soon as they described in this paper is particle-oriented, not cell oriented.
appear, and a multi-model Kalman filter tracker is initialized The particles have position and speed, and they can migrate
to track the dynamic object. from cell to cell depending on their motion model and motion
We can attempt a first classification of the dynamic parameters, but they are also created and destroyed using the
occupancy grid solutions (not the grids themselves) into fully same logic as the weighting-resampling mechanism described
dynamic, as those presented in [6], [7] and [10], and static- in [16]. The measurement data is the raw obstacle grid
dynamic hybrids, as those presented in [8] and [9]. obtained by processing the elevation map, as described in [17].
One of the most important features of an occupancy grid Building a sufficiently dense elevation map requires accurate
tracking solution is the way the sensor model is used for grid dense stereo information, which is computed using the
update. The most time efficient way of updating a grid is to techniques described in [18]. Other techniques for dense stereo
rely on the inverse sensor model, which derives the probability processing are presented in [19].
of a cell being occupied directly from sensor readout, Based on the surveyed literature, the occupancy grid
assuming the occupancy of each cell is independent of its tracking solution presented in this paper can be classified as
neighbors. This solution is maybe still the most popular, having a Cartesian representation, using a forward sensor
mainly in static grids [9]. However, the work of Thrun [11] probability model, and producing a fully dynamic grid. The
proved that forward sensor probability models are preferable proposed method is most closely related to the works
even in the case of static grids, even if this significantly presented in [7] and [10], which use a speed probability
increases the complexity of computation. distribution for each cell in the grid, instead of modeling the
The occupancy grids can have multiple spatial dynamic grid as a high dimensional space, as in [6]. We
representations, and in [12] we are shown a comparison believe that our solution comes as an improvement over these
between three types of grids, the Cartesian (classic), the polar techniques, because due to the use of moving particles the
(distance and angle) and the column/disparity grids. All these representation of the speed probability distribution and the
grids have advantages and drawbacks. A Cartesian grid is estimation of this distribution are no longer a concern. We do
closer to the real world representation, and can handle not have to approximate the velocity as a histogram [7] or as a
velocities easier, while the other types of grids are more mixture of Gaussians [10], we don’t have to assume that one
“sensor-friendly”, making the computation of the sensor cell belongs to only one object with only one velocity, and
uncertainties easier. neither are we concerned with estimation of this speed, as this
The occupancy grid is a flexible representation of the results naturally from the survival or elimination of the
environment, and this flexibility allows powerful integration of particles. The particles in a cell can have different speeds, and
multiple information sources. For instance, map information therefore they can handle the situation of overlapping objects,
can be mapped on the grid, when available, as presented in or the most likely situation when the objects are too close and
[10]. The map can associate to each cell a terrain type (such as the uncertainty of one overlaps over the uncertainty of the
road, curb or sidewalk), and the terrain type is translated into a other. The complexity of the algorithm is linear with the
reachability probability for the cell. The use of terrain number of cells in the grid and with the maximum number of
information can greatly improve the prediction of the position particles in a cell, a tradeoff between accuracy and response
of dynamic objects on the road. The flexibility of the time being always available as a simple parameter. Also,
occupancy grid makes it well suited for collaborative updating, integrating other motion parameters, such as acceleration, does
using the information from multiple sensors or multiple not increase the complexity of the tracking algorithm, because
observers. A solution which uses the occupancy grid (named it only alters the way the position of the particles in time is
obstacle map) to integrate laser and radar information is computed.
presented in [13], and in [14] the grids are used to fuse stereo The remainder of this paper is organized as follows: first,
and optical flow information. A solution that integrates the the particle grid model is presented, and then the steps of the
observations of multiple mobile observers into a unified filtering algorithm are detailed: prediction, measurement and
description of the environment is presented in [15]. initialization. Then, the paper describes the way the particle
This paper presents a driving environment tracking solution grid results can be used to extract 3D cuboids that have
based on a particle occupancy grid. This solution is defined by position, size and speed. The paper ends with the testing and
a new and original approach for the representation of the results section, followed by conclusions.
occupancy and velocity probability distribution of each grid
cell, and by the original updating algorithm derived from the II. THE WORLD MODEL
proposed representation. The occupancy probability of each The world is represented by a 2D grid, mapping the bird-eye
grid cell is described by the number of particles in that cell, view 3D space into discrete 20 cm x 20 cm cells. The size of
and the particles have a dual nature – they describe occupancy the grid is 250 rows x 120 columns (this corresponds to a
hypotheses, as in the particle filtering algorithms such as scene size of 50x24 meters). The aim of the tracking algorithm
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

is to estimate the occupancy probability of each grid cell, and update and destroy particles such that they accurately represent
the speed components on each axis. The tracking goals are the real world.
achieved by the use of a particle-based filtering mechanism.
Considering a coordinate system where the z axis points III. ALGORITHM OVERVIEW
towards the direction of the ego-vehicle, and the x axis points The first step of the algorithm is the prediction, which is
to the right, the obstacles in the world model are represented applied to each particle in the set. The positions of the
by a set of particles particles are altered according to their speed, and to the motion
S = { pi | pi = (ci , ri , vci , vri , ai ), i = 1...N S } , each particle i parameters of the ego vehicle. Also, a random amount is added
having a position in the grid, described by the row ri (a to the position and speed of each particle, for the effect of
discrete value of the distance in the 3D world z) and the stochastic diffusion. The second step is the processing of
column ci (discrete value of the lateral position x), and a speed, measurement information. This step is based on the raw
described by the speed components vci and vri. An additional occupancy cells provided by dense stereo processing, and
parameter, ai, describes the age of the particle, since its provides the measurement model for each cell. The
creation. The purpose of this parameter is to facilitate the measurement model information is used to weight the
validation process, which will be described in a subsequent particles, and resample them in the same step. By weighting
section of the paper. The total number of particles in the scene and resampling, the particles in a cell can be multiplied or
NS is not fixed. This number depends on the occupancy degree reduced. The final step is to estimate the occupancy and
of the scene, that is, the number of obstacle cells. Having the speeds for each cell, and to group the cells into 3D oriented
population of particles in place, the occupancy probability of a objects, for result evaluation.
cell C is estimated as the ratio between the number of particles
whose position coincides with the position of the cell C and IV. PREDICTION
the total number of particles allowed for a single cell, NC. This step will derive the present particle distribution from
the past information, preparing the particle set for
| { pi ∈ S | ri = rc , ci = cc } | measurement. The prediction equations will use odometry and
PO (C ) = (1) motion model information.
NC The basic odometry information available through the CAN
bus of a modern car is the speed v and the yaw rateψ& .
The number of allowed particles per cell NC is a constant of
the system. In setting its value, a tradeoff between accuracy Together with the time interval ∆t elapsed between
and time performance should be considered. A large number measurements, these parameters can be used to compensate for
means that on a single cell multiple speed hypotheses can be the ego-motion, and separate it from the independent motion
maintained, and therefore the tracker can have a better speed of the objects in the scene. Between measurements, the ego-
estimation, and can handle fast moving objects better. vehicle rotates with an angleψ , and travels a distance d.
However, the total number of particles in the scene will be ψ = ψ&∆t (3)
directly proportional with NC, and therefore the time
ψ
consumption will increase. 2v∆t sin
The speed estimation of a grid cell can be estimated as the d= 2 (4)
average speed of its associated particles, if we assume that ψ
only one obstacle is present in that cell. Of course, the particle The origin of the grid representation is displaced along the
population can handle the situation when multiple obstacles, two coordinate axes by dc and dr.
having different speeds, share the same cell, and in this case ψ
the speed estimate of the cell must be computed by clustering. d c = d sin / DX (5)
2
ψ
∑ (vc , vr ) i
p i ∈S , x i = x c , z i = z c
i d r = d cos
2
/ DZ (6)
(vcC , vrC ) = (2)
| { pi ∈ S | ri = rc , ci = cc } | We denote by DX and DZ the cell size of the grid (in the
current implementation, 0.2 m). A point in the grid, at row r
and column c, is displaced by the following equation:
Thus, the population of particles is sufficiently
representative for the probability density of occupancy and
speed for the whole grid. Multiple speed hypotheses can be cn  cosψ − sinψ  c  d c 
maintained simultaneously for a single cell, and the occupancy  r  =  sinψ −
cosψ  r  d r 
(7)
uncertainty is represented by the varying number of particles
 n 
associated to the cell. The goal of the tracking algorithm can The prediction is achieved using equation 8, which
now be stated: using the measurement information to create, combines the deterministic drift caused by the ego-motion
compensation and the particle’s own speed, with the stochastic
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

diffusion caused by the uncertainties in the motion model. The the focal distance in pixels, and σ d is the error in disparity
quantities δc , δr , δvc and δvr are randomly drawn from a computation (usually about 0.25 pixels, for a good stereo
Gaussian distribution of zero mean and a covariance matrix Q reconstruction engine).
equivalent to the state transition covariance matrix of a The error in lateral positioning (usually much smaller than
Kalman filter. The covariance matrix is diagonal, with the the error in z), can be derived from the distance error. This
standard deviations for the speed components corresponding to error depends on the lateral position x (in the real world
a real-world amount of 1 m/s, and the standard deviations for coordinates) and the distance z.
the position corresponding to a real-world value of 0.1 m.
xσ z
These values will ensure that the system is able to cope with σx = (10)
fast-moving objects even at a 10 fps frame rate. z
The 3D errors are mapped into grid cell errors, by dividing
them with the grid cell size on x and z.
 c  1 0 ∆t 0  cn   δc 
 r  0 σz
 = 1 0 ∆t   rn   δr  σ row =
+ (8) DZ
vc  0 0 1 0  vc  δvc  σx
(11)
       σ column =
vr  0 0 0 1  vr  δvr  DX
The values of σ row and σ column are computed offline, at the
From the grid model point of view, the prediction has the
initialization phase, for each cell in the grid.
effect of moving particles from one cell to another, as seen in
figure 1. The occupancy probability is thus dynamically B. The raw occupancy density cue
adjusted using the particle’s motion model and the vehicle In order to compute the conditional probability of the
odometry. measurement cell, under the occupied or free assumption, we
have to take into account a reality that is specific to
stereovision sensors. The stereo sensor does not perform a
scan of the scene, and therefore it does not output a single
bird-eye view point for a real-world obstacle cell. We’ll take
as example a pillar, which has almost no width, and no depth
spread. The representation of a pillar in the occupancy grid
should be a single cell. If the pillar were observed by a
scanner-type sensor, this sensor will output a cell, displaced
from the true position by an amount specific to the sensor
error. For the stereo sensor, things are different, because the
Fig. 1. Particles in the grid, before and after prediction. camera observes the whole height of the pillar, and therefore
each pillar pixel will get a distance and a lateral position. This
V. MEASUREMENT MODEL means that once we “collapse” the pillar information in the 2D
The measurement model will relate the measurement data, grid representation, each part of the pillar may fall in a
which is a binary occupied/free condition derived from the different cell, and the pillar will generate a spread of cells. The
stereovision-generated elevation map [10], to the conditional size of the spread area is controlled by the grid uncertainties
probabilities p(measurement | occupied) and p(measurement | on the c and r axes (real world x and z).
free), which will weight the particles. In order to compute This property leads us to find a good cue, which will
these probability values, we have to pass through several steps. contribute to the conditional probabilities of the measurement
cells under the occupied/free assumption. We’ll count the
A. The uncertainty of the stereo measurement
obstacle cells in the measurement grid around the current cell
In order to compute these probabilities, we start by
computing the uncertainty of the stereo reconstruction. First,
position, in an area of σ row height and σ column width, and
the uncertainty of the distance reconstruction, in the case of a divide the number of found obstacle cells by the total number
rectified system, is given by: of cells in the uncertainty area. We’ll denote this ratio as
pdensity(m(r,c) | occupied).
z 2σ d
σz = (9) row=r +σ row col=c+σ column
bf ∑ ∑O(row, col)
row=r −σ row col=c−σ column
pdensity(m(r, c) | occupied) =
In the above equation, z denotes the distance (in the real (2σ row + 1)(2σ column + 1)
world coordinates), b is the baseline of the stereo system, f is (12)
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

By O(row, col) we denote the “occupied” value of the Once each cell has an obstruction value, the final analysis is
measurement grid, at position row and col. This value is 1 performed. Each cell that has an obstruction value higher than
when an obstacle cell is present and 0 when not. 10 is considered obstructed and considered as such in the
The density cue for the “free” assumption is: particle weighting and resampling phase (to be described in the
next chapter). However, this is not the only way we use the
pdensity (m(r , c) | free) = 1 − pdensity (m(r , c) | occupied ) obstruction property. If a raw measurement cell is marked as
“occupied”, but from the obstruction analysis it is found to be
(13)
obstructed, the occupied cell is removed. This will make the
A graphic comparison between the raw measurement data
raw occupancy map look more like a scanner-derived map.
and the density cue (conditional probability) of the
This reduction of measurement information must be performed
measurement under the “occupied” assumption is given in the
before the computation of the other particle weighing cue,
following figure.
which relies on the distance from measurement.
The obstruction-related processing steps are illustrated in
figure 3. The left panel shows the raw measurement data, the
middle panel shows the obstruction value for each cell (the
lighter, the more obstructed), and the right panel shows the
measurement data that remains after the obstructed cells are
removed. This data set is used for the next cue computation.

Fig. 2. From the raw occupancy grid to the raw measurement density cues. Fig. 3. Handling the occlusions. Left – original measurement information,
Bottom-left: raw occupancy grid, bottom-right: density cue for the occupied middle – obstruction value for each cell, right – unobstructed measurement.
cell hypothesis.
D. The distance from measurement cue
C. Handling the occlusions For each cell in the grid, we need to compute the distance to
Not all cells in the grid can be observed directly, and this the nearest occupied cell in the measurement grid. For that,
fact must be taken into consideration by the tracking we’ll use a modified version of the distance transform
algorithm. Due to the limitations of the primary source of algorithm presented in [20]. The main issue is that we need to
information, the stereovision-based raw occupancy grid, some know not only the distance to the nearest measurement point,
of the cells are never observed. The raw occupancy grid only but the distance components on the two coordinate axes, row
covers a longitudinal distance from 0 to 40 meters, a lateral and column. The reason for this requirement is that the
span of 13 meters. Also, the field of view of the camera standard deviations for the positioning errors are different on
(angular span) limits the areas that are visible at close distance. the row and on the column, and therefore one cannot be
The cells that are excluded by the field of view and distance substituted for another.
limitations are marked as obstructed (unobservable) by default. Our distance transform algorithm performs like the classical
Another way for a cell to become unobservable is if it is two-pass L1 norm one, but instead of updating only the cell
obstructed by an obstacle cell that is located between it and the distance to the nearest measurement, the position of the nearest
observation origin (camera position). In order to decide if a measurement is updated along. The following algorithm
cell is in such a situation, we switch to polar coordinates. Each updates a distance matrix D(r,c), initialized with zero for
cell is mapped to a polar grid. Then, for each angle, the cells measurement occupancy cells, and with 255 for the free cells,
are scanned in the order of their distance. Once a raw occupied and two position matrices Mr and Mc that hold the row and the
cell is found, an obstruction counter is incremented for every column of the nearest occupied measurement cell. The values
cell that is behind the first occupied one. Then, the obstruction of Mr and Mc are initialized to the current row and column of
values are re-mapped into the Cartesian grid. each cell.
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

Algorithm DistanceTransform approximates an updated probability density function over a

For r=1 to max_r space of parameters. However, this approach works when the
For c=1 to max_c particles are hypotheses of the state of a system, not when the
Update (r, c, -1, 0) particles are the system itself (we can see our tracked world as
Update (r, c, 0, -1) physically composed of particles).
End For Our algorithm tries to use the particles in a dual form – as
End For hypotheses, and as building blocks of the world that we track.
For r = max_r to 1 Their role as building blocks has been already explained.
For c = max_c to 1 However, if we restrict our reasoning to a single cell in the
Update (r, c, 1, 0) grid world, we can see that the particle is also a hypothesis. A
Update (r, c, 0, 1) particle in a grid cell is a hypothesis that this cell is occupied,
End For and that the cell has the speed equal to the speed of the
End For particle. More particles in the cell mean that the hypothesis of
occupancy is strongly supported. Less particles in the cell
Function Update(r, c, n, k) means that the hypothesis of the cell being free is supported.
If D(r, c) > D(r+n, c+k) + 1 We can regard the difference between the number of particles
D(r, c) = D(r+n, c+k) + 1 in a cell and the total number of particles allowed in a cell as
Mr(r, c) = Mr(r+n, c+k) the number of particles having the occupancy hypothesis zero.
Mc(r, c) = Mc(r+n, c+k)
A. Weighting the particles
End If
If we regard the number of particles in the cell to be
After the distance transform algorithm is applied, the constant, and some of them having the occupancy value “true”
distance-to-measurement-occupied on rows and on columns, while some having it “false”, we can apply the mechanism of
for each cell can be found by: weighting and resampling.
If we assume that the measurement data does not contain
d occupied
row
(r , c) =| r − M r (r , c) | speed information, the weight of the particle depends only on
(14)
occupied
d column (r , c) =| c − M c (r , c) | the “occupied” hypothesis. Also, this means that all the
particles having the same occupied hypothesis will have the
The distance to measurement-free-cell is computed as the same weight.
difference between the double of the distance standard For each cell at position r, c in the grid, the weights for the
deviation and the distance-to-occupied, saturated to zero. free and for the occupied hypotheses is obtained by fusing the
cues computed from the measurement data using the methods
free
d row (r , c) = max(2σ row (r , c) − d occupied
row
(r , c),0) described in section V.
(15)
free
d column (r , c) = max(2σ column (r , c) − d occupied
column
(r , c),0) woccupied (r , c) = pdensity (m(r , c) | occupied ).
These distances are converted to a probability density value (17)
using the multivariate Gaussian equation (equation 16). We pdistance (m(r , c) | occupied )
have removed the row and column arguments for all the values w free (r , c) = pdensity (m(r , c) | free).
involved, for the sake of readability. The same equation is (18)
applied for both free and occupied distances, and therefore the
pdistance (m(r , c) | free)
condition status is a placeholder for both situations.
1 The equations 17 and 18 hold if the cell in the grid is not
pdistance (m | status ) = . marked as obstructed, as described in section V.C. If the cell is
2πσ rowσ column obstructed, the weights of the occupied and free hypotheses
(16)
 2 2  will be equal, woccupied (r , c) = w free (r , c) = 0.5 .
1 d   d column 
status status

−   row  + 


2   σ row
status   σ status  
   column   The number of particles having the “occupied” hypothesis
e
At the end of this step, we have, for each cell, the values true is the number of “real” particles in the cell.
pdistance (m(r , c) | occupied ) and pdistance (m(r , c) | free) .
N OC (r , c) =| { pi ∈ S | ri = r , ci = c} | (19)
VI. WEIGHTING AND RESAMPLING
The number of particles (hypotheses) having the “occupied”
The classical steps of a particle filter based tracker are
value false is the complement of NOC. We remind the reader
resampling, drift, diffusion, and measurement (weighting).
that NC is the maximum number of particles allowed in a cell,
This behavior replaces a population of a fixed number of
and this number is a constant of the algorithm.
particles with an equal number of particles, which
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

N FC (r , c) = N C − N OC (r , c) (20) r = random value between 0 and 1

If (r<Ff)
S.Add(pi.MakeCopy)
The total posterior probability of a cell being occupied and
End if
of a cell being free can be computed from the number of
End if
free/occupied hypotheses, and their corresponding weights. In
the following equations we have removed the row and column
If (fC <1) – number of particles will decrease
parameters, but they are implied.
r = random value between 0 and 1
If (r> fC)
woccupied N OC S.Remove(pi)
POC = (21)
woccupied N OC + w free ( N C − N OC ) End if
End if
w free ( N C − N OC )
PFC = (22)
woccupied N OC + w free ( N C − N OC ) End For

The system will compute the number of particles that each

The aggregate particle weights POC and PFC are used for
cell should have after the process of resampling has been
particle resampling. The resampling of the particle population
completed. The ratio fC between this number and the existing
is done at the end of the measurement step, so that the next
number of particles in the cell will tell us if the particles have
cycle can start again with an updated population of particles
to be duplicated or removed. If fC is higher than 1, the number
without concerning about their weight.
of particles has to be increased. The integer part of the
B. Resampling difference between fC and 1 tells us the number of certain
A classical resampling algorithm would make NC random duplications a particle must undergo (for instance, if fC is 2,
draws from the previous particle population of a cell, while the each particle will be doubled). The fractional part of the
weight of each particle controls its chances of being selected. difference is used for chance duplication: each particle will
Because we don’t care for the “cell free” hypothesis particles, have a probability of being duplicated equal to the fractional
our resampling will instead decide for each real particle part of this difference.
(particle having the occupied hypothesis true) whether it is If f is lower than 1, the number of particles has to be
destroyed or multiplied (and, if multiplied, how many copies decreased, by removing some of the particles. Each particle
of it are created). has 1- fC chance of being eliminated.
The following algorithm describes the process of At this point the cycle is complete, and the tracking
resampling, which is materialized as duplication or removal of algorithm can process a new frame. Secondary estimations for
particles from the particle set. The key solution for a real-time occupancy, speed, or clustering the cells into objects can be
operation is that all the heavy computing tasks are executed at performed at the end of this step.
cell level, mostly by the use of LUT’s, while the particle level
processing is kept very light.

Algorithm Resample
For each cell C
Compute NOC and POC
Compute resampled number of particles NRC
NRC=POCNC
Compute ratio between actual number of particles and the
number of resampled particles
Fig. 4. Weighting and resampling. The weight of the occupied hypothesis is
N RC
fC = encoded in the darkness of the cell of the left grid.
N OC
End For VII. INITIALIZATION
For each particle pi Although the measurement step takes care of particle
Find corresponding cell C creation and deletion, this step only works if there are particles
If (fC>1) – number of particles will increase to be duplicated or deleted. For the prediction-measurement
Fn = Int(fC) Integer part cycle to work, the particle population has to be initialized.
Ff = fC -Int(fC) Fractional part From a strictly probabilistic point of view, each cell’s state
For k=1 to Fn-1 is unknown at startup, which means that the cell has equal
S.Add(pi.MakeCopy) probability of being occupied or free. In our tracking system,
End For this would mean that each cell should be assigned a number of
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

particles equal to half the total number of particles allowable and a new label (which implies a new object) is generated. The
in a cell. However, this approach would significantly reduce difference between our labeling and a classical labeling
the speed of the system, and would require permanent re- algorithm is the way the neighborhood relationship is defined.
initialization. Two cells are neighbors if the following conditions are
Our solution is to use the measurement occupancy grid to fulfilled:
create particles. If a measurement cell is of type obstacle, its - The distance between them in the grid is less than 3,
p(m(r,c) | occupied) is high, and there are no particles in the meaning that a one cell gap is allowed.
corresponding tracked grid cell, a small number of particles - The difference in the orientation of the speed vectors in
will be created. The initial speed components vr and vc of the the two cells is less than 30 degrees.
created particles will be sampled randomly from an initial - The difference in speed vector magnitudes is less than
range of possible values, and the initial position is confined to 30% of the value of the largest magnitude of the two
the creation cell. In this way, the initialization is a continuous cells.
process. The labeling process is shown in figure 5, middle panel,
Particles are automatically removed when they go outside where each color marks a different object. We can see that by
the grid area, in the prediction phase. Another case of applying vicinity criteria only, the moving vehicle will be
“administrative” removal (removal not caused by the connected to the stationary structure. However, this does not
probability mechanism described in section VI) is when, due happen due to the fact that we can use dynamic information
to particle drifting, the number of particles in a cell exceeds provided by the grid to successfully discriminate the two
the allowed value. objects.

VIII. CELL STATE ESTIMATION AND OBJECT EXTRACTION

The result of the tracking algorithm is the particle
population itself. However, for testing and validation purposes,
and for using the tracking results in further stages of
processing, we will estimate the occupancy state and the speed
of each cell in the grid.
The occupancy probability of each grid cell is approximated
by the ratio between the number of particles in that cell and the
total number of allowed particles in a cell (equation 1).
The components of the speed vector for each cell are
estimated using equation 2. However, due to the fact that the
speed of a newly created particle is completely random, these Fig. 5. Cell labeling and extraction of objects.
particles are excluded from the speed estimation of a grid cell.
For this purpose, we can use the age property of the particle. The labeled connected components in the grid are used to
The age of the particle is set to 1 when the particle is created, generate the 3D objects in the form of oriented cuboids (figure
and increased each time the particle’s state (position and 5, third panel). The objects are grouped into two categories,
speed) is altered by prediction. Basically, the age of the based on their average speed, computed from the speeds of
particle tell us how many tracking cycles the particle has each component cell: static (shown in green) and dynamic
“survived” in the system. (shown in red). Only the dynamic objects receive orientation,
All the particles in a cell that have an age higher than two which is the orientation of their average speed.
become part of the speed estimation. They are counted, and the
speed components on row and column are averaged. Also, the IX. TESTS AND RESULTS
standard deviation of these speed components is computed. If
A. Qualitative assessment
both the estimated speed components are lower in absolute
value than the double of their standard deviations, the cell is The qualitative tests, which allow us to monitor the general
declared static, because it means that either the speed is too behavior of the system in complex situations, are performed on
low, or it is too dispersed to draw a definite conclusion. video sequences recorded in real urban traffic. These tests
For further testing and evaluation, a subset of the grid cells show how the occupancy grid is computed, how the speed
is grouped into 3D cuboids. A cell is considered for object vector for each cell is estimated, and how the grid results are
grouping if its occupancy probability is at least 0.5, meaning grouped into cuboidal objects having position, size, orientation
that the particle count in the cell is at least NC/2. The and oriented speed vector. The speed of the cells is displayed
individual objects are identified by a generic algorithm of in color, using Hue for orientation and Saturation for
connected component labeling. The algorithm starts from a magnitude. Due to the need for compact representation of the
valid cell, and recursively propagates a unique label to the grid results, we have also encoded the occupancy probability
cell’s occupied neighbors, until no more connections are found as the color’s Intensity, making full use of the whole HSI color
space.
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

Fig. 6. Color coding for speed vectors (full and half occupancy).

Video files, describing results in different traffic situations,

can be downloaded from this page:
http://users.utcluj.ro/~rdanescu/gridtrackingtests.htm . The
main qualitative test is the sequence
http://users.utcluj.ro/~rdanescu/long_sequence.avi , which
shows the results over a significant distance through Cluj-
Napoca. Some highlights of this sequence are presented in
figure 7:
a) Crossing pedestrian, mixed with lateral traffic and static
distant objects.
b) Incoming vehicle, static lateral scenery.
c) Two incoming vehicles, the most distant one only visible
for a couple of frames.
d) Moving vehicle against static wall, ego vehicle
performing a sharp turn left.
e) Distant object, accurately tracked.
f) Moving object against static background. The protrusion
from the static background near the moving object is actually
an occluded stationary car. The ego vehicle is performing a Fig. 7. Extended sequence in urban traffic – highlights.
sharp right turn, which causes the instability in the estimation
of the static nature of the background in the top right corner.
Also, that area was previously occluded by the moving vehicle,
which means that the static nature of the cells has not yet been
detected, due to the short observation time.
g) Distant crossing vehicle going through stationary
vehicles. The ego vehicle is turning right.
h) Tracking a moving target through a narrow corridor of
stationary vehicles.
The behavior of the system in the case of occlusions is
highlighted by the sequence
http://users.utcluj.ro/~rdanescu/cluj-occlusion.avi . Key points
from the sequence are presented in figure 8. While the ego
vehicle is performing a sharp left turn, a vehicle comes from
our right, and is occluded by a vehicle coming from our left.
The occluded vehicle is also maneuvering, changing its
heading to its left. While occluded, its particle distribution
becomes diffuse, accounting for possible exit trajectories, and
the correct heading is quickly identified as the object becomes
observable again.
An extensive sequence, recorded while observing an
intersection with the ego vehicle standing still, produced the
results that are available in the file
http://users.utcluj.ro/~rdanescu/wob-occlusion.avi . A
Fig. 8. Dynamic occlusion.
highlight of this sequence is shown in figure 9. A vehicle
comes from our right, then turns left and proceeds to exit the
During this maneuver it occludes the static object near its
scene.
left side, but does not become joined with this structure due to
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

the speed-sensitive nature of the cell clustering algorithm. We

can see how the occupancy becomes diffused as the object is
occluded by a large truck, which then again occludes the static
objects on the right.
B. Numerical evaluation in controlled environment
The numerical evaluation was performed on sequences
acquired in controlled scenarios, with known target speed and
orientation. We have performed four tests, with the same
orientation, -45 degrees, but different speeds, 30 km/h, 40
km/h, 50 km/h, 60 km/h. The results that were evaluated are
the estimated speed and orientation of the 3D cuboid resulted Fig. 10. Controlled test sequence.

from clustering the occupied grid cells. These results are

compared to the ground truth, and they are also compared to
the results of another means of intermediate extraction of 3D
dynamic information, the optical flow combined with
stereovision. The results of optical flow that are taken into
consideration are the speed and orientation of the 3D cuboid
obtained from grouping the points having 3D and speed
information [21]. The controlled test sequence is highly
favorable to the optical flow approach, as the vehicle is clearly
visible, has plenty of features that can be matched from one
frame to another, a situation which provides plenty of good
speed vectors to be averaged into an accurate vector of the
cuboid.

Fig. 11. Speed and orientation estimation, 30 km/h test.

Fig. 9. Turning near a stationary object and occlusion.

The results of speed and orientation estimation are Fig. 12. Speed and orientation estimation, 40 km/h test.
displayed in the graphs shown in figures 11 to 14. The grid
tracking results are shown with the red dotted line. We can see
that both methods quickly converge towards the ground truth,
but the grid tracking results are more stable (lower error
standard deviation) and more accurate (lower mean absolute
error). This fact is confirmed by the tables I and II.
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

typical urban scene, and a total number of particles in a cell

NC=50, the total running time is about 40 ms per frame, on an
Intel Core 2 Duo processor at 2.1 GHz. Due to the fact that the
particle tracking system shares the processor with other
sensorial processing algorithms such as lane detection, object
classification and so on, the total frame rate is about 10 fps.
Note: video files showing results in multiple traffic
situations can be downloaded from the address:
http://users.utcluj.ro/~rdanescu/gridtrackingtests.htm.

X. CONCLUSION AND FUTURE WORK

We have presented a solution for driving environment
modeling and tracking, which employs particles in order to
estimate the occupancy and speed of the cells of an occupancy
grid. This flexible and real-time solution is capable of
correctly track dynamic environments even at high relative
speeds, without the need of a very high frame rate from the
Fig. 13. Speed and orientation estimation, 50 km/h test.
measurement system. The test sequences prove that the method
is sensitive enough to detect and estimate the speed of a
pedestrian, but also the speed of a fast moving vehicle. The
accuracy of the speed and orientation estimation is proven by
the tests conducted in controlled situations.
The particle grid tracking solution is an elegant extension of
the dynamic occupancy grid solutions that were surveyed. The
particle population approach relieves the designer of the
choice of a speed probability distribution for each cell, and can
handle multiple divergent speed hypotheses. Also, the speed
distribution does not have to be estimated, and the
measurement data only controls the creation or deletion of
particles. We believe that the proposed technique is a new
view of the occupancy grid problem, a view oriented towards
practical implementation, and a view that can open the door to
interesting extensions.
The presented technique is not a substitute for model-based
tracking, but a method for intermediate representation and
processing of sensorial data. The occupancy probability and
Fig. 14. Speed and orientation estimation, 60 km/h test.
dynamic parameters of each cell can subsequent algorithms of
TABLE 1 feature grouping, model-based object tracking, or even sensor
NUMERICAL RESULTS – SPEED ESTIMATION ACCURACY fusion. The advantages of having a good dynamic intermediate
Speed of Particle
Particle
Optical
Optical representation are proven by the results of the experimental
grid flow step of model-based object reconstruction. The quality of the
target grid MAE flow MAE
STDEV STDEV
30 km/h 0.9016 0.9731 2.0141 2.3087 particle grid tracking results as intermediate representation
40 km/h 1.0184 0.9730 2.1181 1.9017 towards object detection and tracking are also proven by the
50 km/h 2.4989 2.3370 3.7329 4.4966 comparison with the most used source of intermediate
60 km/h 2.1279 1.3858 3.0677 2.2725
representation in computer vision, the Lucas-Kanade optical
TABLE 2
flow mixed with stereo 3D information, and the comparison
NUMERICAL RESULTS – ORIENTATION ESTIMATION ACCURACY was made in the most favorable case for the optical flow
Particle Optical technique.
Speed of Particle Optical
grid flow The solution leaves plenty of room for future work. For
target grid MAE flow MAE
STDEV STDEV
example, many of the calculations performed by the algorithm
30 km/h 0.9728 0.8376 1.8219 2.0122
40 km/h 1.0321 0.8616 1.1962 1.0146 can be subjected to parallelization, for significant speed
50 km/h 0.4695 0.2659 1.2775 1.1095 improvement. The particle-related computations, such as the
60 km/h 0.9343 0.6739 1.4554 1.1634 prediction of the new position, can be subjected to massive
parallelization, while the grid-related computations can be
The time performance depends on the obstacle load of the parallelized at region level. Further work will be dedicated to
scene, which influences the total number of particles. For a
 Accepted for IEEE Transactions on Intelligent Transportation Systems, © IEEE

the issue of optimization through parallelization. [16] M. Isard, A. Blake, “CONDENSATION -- conditional density
propagation for visual tracking”, International Journal of Computer
We believe that the most important development for the Vision, Vol. 29, No. 1, 1998, pp. 5-28.
future would be to use the capability of the particle to carry [17] F. Oniga, S. Nedevschi, “Processing Dense Stereo Data Using Elevation
additional information. For example, the age information may Maps: Road Surface, Traffic Isle, and Obstacle Detection”, IEEE
be used for more than validation. One use of age is to adjust Transactions on Vehicular Technology, Vol. 59, No. 3, March 2010,
pp. 1172-1182.
the variances of the random alterations of speed and position [18] I. Haller, C. Pantilie, F. Oniga, S. Nedevschi, “Real-time semi-global
that are applied in the prediction phase – once a particle is dense stereo solution with improved sub-pixel accuracy”, in proc of
older, its randomness can be decreased. The particles can be IEEE Intelligent Vehicles Symposium 2010 (IV 2010), pp. 369-376.
[19] W. van der Mark, D. M. Gavrila, “Real-Time Dense Stereo for
tagged with a unique ID, allowing us to reconstruct the Intelligent Vehicles”, IEEE Transactions on Intelligent Transportation
trajectory of an object. Other parameters, such as height, or the Systems, Vol. 7, No. 1, March 2006, pp. 38-50.
class of the object from which the particle is a part, can be [20] A. Rosenfeld, J. L. Pfaltz, “Sequential Operations in Digital Picture
Processing”, Journal of the Association for Computing Machinery, Vol.
added to the particle, and used by the tracking mechanism or 13, No. 4, October 1966, pp. 471-494.
by the applications developed on top of it. [21] C. Pantilie, S. Nedevschi, “Real-time Obstacle Detection in Complex
Scenarios Using Dense Stereo Vision and Optical Flow”, IEEE
REFERENCES Conference on Intelligent Transportation Systems (IEEE-ITSC), 2010,
pp. 439-444.
[1] U. Franke, C. Rabe, H. Badino, and S. Gehrig, “6d-vision: Fusion of
stereo and motion for robust environment perception,” in proc of 27th
Annual Meeting of the German Association for Pattern Recognition Radu Danescu received the Diploma Engineer
DAGM ’05, Vienna, October, 2005, pp. 216-223. degree in Computer Science in 2002 from the
[2] D. Pfeiffer, U. Franke, "Efficient Representation of Traffic Scenes by Technical University of Cluj-Napoca, Romania,
Means of Dynamic Stixels", in proc of IEEE Intelligent Vehicles followed by the M.S. degree in 2003 and the PhD
Symposium (IEEE-IV), 2010, pp. 217-224. (Computer Science) degree in 2009, from the same
[3] S. Cherng, C. Y. Fang, C. P. Chen, S. W. Chen, “Critical Motion university. He is a Senior Lecturer with the
Detection of Nearby Moving Vehicles in a Vision-Based Driver-
Computer Science Department, TUCN, teaching
Assistance System”, IEEE Transactions on Intelligent Transportation
Image Processing, Pattern Recognition, and design
Systems, Vol. 10, No. 1, March 2009, pp. 70-82.
with microprocessors. His main research interests
[4] A. Elfes, “A Sonar-Based Mapping and Navigation System”, in proc of
IEEE International Conference on Robotics and Automation, April are stereovision and probability based tracking, with
1986, pp. 1151-1156. applications in driving assistance. He is a member of the Image Processing
[5] A. Elfes, “Using Occupancy Grids for Mobile Robot Perception and and Pattern Recognition Research Laboratory at TUCN.
Navigation”, Computer, vol. 22, No. 6, June 1989, pp. 46-57.
[6] C. Coue, C.Pradalier, C.Laugier, T.Fraichard, P.Bessiere, “Bayesian Florin Oniga received the Diploma Engineer degree
Occupancy Filtering for Multitarget Tracking: An Automotive in Computer Science in 2002 from the Technical
Application”, The International Journal of Robotics Research, Vol 25, University of Cluj-Napoca, Romania, followed by
No 1, 2006, pp. 19-30. the M.S. degree in 2003 from the same university.
[7] C. Chen, C. Tay, K. Mekhnacha, C. Laugier, “Dynamic environment He is currently working towards the Ph.D. degree in
modeling with gridmap: a multiple-object tracking application”, in proc Computer Science at Technical University of Cluj-
of International Conference on Automation, Robotics and Computer Napoca, specializing in Computer Vision. He is a
Vision (ICARCV) 2006, pp. 1-6. Lecturer with the Computer Science Department,
[8] T. Weiss, B. Schiele, K. Dietmayer, “Robust Driving Path Detection in Technical University of Cluj-Napoca, teaching
Urban and Highway Scenarios Using a Laser Scanner and Online Image Processing, Pattern Recognition, and Computer Architecture. His
Occupancy Grids”, in proc of IEEE Intelligent Vehicles Symposium research interests include stereovision, digital elevation maps processing, and
2007, pp. 184-189. vision based automotive applications. He is a member of the Image
[9] S. Pietzch, T. D. Vu, J. Burtlet, O. Aycard, T. Hackbarth, N. Appenrodt, Processing and Pattern Recognition Research Laboratory at TUCN.
J. Dickmann, B. Radig, "Results of a Precrash Application based on
Laser Scanner and Short Range Radars", IEEE Transactions on
Sergiu Nedevschi (M’99) received the M.S. and
Intelligent Transportation Systems, Vol. 10, No. 4, 2009, pp. 584-593.
[10] T. Gindele, S. Brechtel, J. Schroeder, R. Dillmann, “Bayesian PhD degrees in Electrical Engineering from the
Occupancy Grid Filter for Dynamic Environments Using Prior Map Technical University of Cluj-Napoca (TUCN), Cluj-
Knowledge”, in proc of IEEE Intelligent Vehicles Symposium 2009, pp. Napoca, Romania, in 1975 and 1993, respectively.
669 - 676 From 1976 to 1983, he was with the Research
[11] S. Thrun, "Learning Occupancy Grids With Forward Sensor Models", Institute for Computer Technologies, Cluj-Napoca,
Autonomous Robots, Vol. 15, No 2, 2003, pp. 111-127. as researcher. In 1998, he was appointed Professor in
[12] H. Badino, U. Franke, R. Mester, “Free Space Computation Using computer science and founded the Image Processing
Stochastic Occupancy Grids and Dynamic Programming”, Workshop on and Pattern Recognition Research Laboratory at the
Dynamical Vision, ICCV, 2007, pp. 1-12. TUCN. From 2000 to 2004, he was the Head of the
[13] M. S. Darms, P. E. Rybski, C. Baker, C. Urmson, “Obstacle Detection Computer Science Department, TUCN, and is currently the Dean of the
and Tracking for the Urban Challenge”, IEEE Transactions on Faculty of Automation and Computer Science. He has published more than
Intelligent Transportation Systems, Vol. 10, No. 3, September 2009, pp. 200 scientific papers and has edited over ten volumes, including books and
475-485. conference proceedings. His research interests include Image Processing,
[14] C. Braillon, K. Usher, C. Pradalier, J. Crowley, C. Laugier, “Fusion of Pattern Recognition, Computer Vision, Intelligent Vehicles, Signal
stereo and optical flow data using occupancy grids”, in proc of IEEE Processing, and Computer Architecture.
International Conference on Intelligent Transportation Systems, 2006,
pp. 1240-1245.
[15] J. Y. Chen, J. Hu, “Probabilistic Map Building by Coordinated Mobile
Sensors”, in proc of IEEE International Conference on Networking,
Sensing and Control, 2006, pp. 807-812.

View publication stats