A particle filtering approach for joint vehiculardetection and tracking in lidar data

B. Fortin, R. Lherbier and J.C. NoyerUniv. Lille Nord-de-France

ULCO, LISICB.P. 719, 62228 Calais Cedex, France

Email: noyer@lisic.univ-littoral.fr

Abstract—This paper presents a method for joint detection andtracking of vehicles in scanning laser range data. Many methodsuse a solution that processes the raw data in a detection procedureand then tracks the detected object in an association/trackingprocedure. The proposed approach uses a preclustering stage(SIP) as an input of the tracking process that allows to managethe displacement of the center-of-gravity and the changes in theapparent shape from object and motion modeling. The globalproblem is then described using a state-space modeling which issolved by a nonlinear filtering method.

I. INTRODUCTION

In recent years, laser rangefinders have been employed inmany research works to detect and track moving objects [1],[2], [3]. The most popular approaches are composed of two in-dependent procedures: detection and tracking. Detection meth-ods need robust segmentation of telemetric data and eventuallythey model objects by building bounding-boxes from extractedfeatures [4], [5], [6]. Usually, tracking methods are basedon filtering techniques depending on a state-space modelingand involving an association procedure, like probabilistic dataassociation (PDA) or multi-hypothesis tracking (MHT) [7],[8], [9]. Unfortunately, imperfections in detection stage, dueto non detection and false alarms, lead to association errorsand disturb the tracking procedure.

The originality of the proposed method lies in a jointapproach of the detection and tracking problem deriving froman explicit model of the shape which leads to an unusualmeasurement equation. A difficulty in this type of approachis that it needs a detection approach compatible with thetracking process. We developed in [5] a detection method ofline segment in scanning laser range data (SIP: Segmentationusing Invariant Parameters) that consists in using invariantparameters of the shape. This approach has shown its interestin terms of detection rate but also in outliers rejection. Theseprevious works considered the detection problem only. Evenif the performances of the detector were significantly betterthan the usual methods, much more can be obtained by acoupling with the tracking process. It allows more particularlya better robustness of the processing to the presence ofoutliers in the measurements and a minimization of the nondetection of the objects. However, such an approach leads toa strongly nonlinear state-space modeling. To guarantee anoptimal solution of this problem, an algorithm based on the

sequential Monte Carlo methods is proposed, since they haveshown their efficiency to deal with nonlinear models. See [8],[6], [10] for implementations to the same applications.

II. DETECTION AND TRACKING OF EXTENDED OBJECT INSCANNING LASER RANGE DATA

A. Modeling of the detection problem: the SIP method

Basically, a scanning laser rangefinder (lidar) is a devicethat estimates the relative distance of objects from the measureof the time-of-flight of a laser beam (see Fig. 1). By nature,this sensor delivers information in polar coordinates (ρ, θ)about the environment of the ego-vehicle in an angular range[θmin, θmax] and between a distance interval [ρmin, ρmax].These intervals define the field of view .

Typically, a lidar scans the environment with a large aperture(180 degrees at least), a high measurement rate (usually 10/20Hz) and a small angular resolution α (0.25◦). In such a system,the error on range measurement is equal to several centimetersinside the interval [ρmin, ρmax] and those on bearing is usuallysupposed to be negligible.

In polar coordinates, a straight line is described with twoparameters: its orthogonal distance ρd to the origin of thereference frame and the related bearing θd. Hence, each pointPi = (ρi, θi) belonging to the same straight line verifies thefollowing polar equation:

ρd = ρi cos(θd − θi) (1)

Let us now consider 2k + 1 consecutive points{Pj}j=i−k,...,i,...,i+k of a scan, belonging to the samestraight line (i is the measurement number of the middlepoint Pi in the line segment). One defines the cumulativefunction Ak(i) of the inverse range measurements in theneighborhood k from this central point Pi:

Ak(i) =

j=i+k∑j=i−k

1

ρj

1

ρi

(2)

It can be shown [5] that Eq. 2 can be written as follows:

Ak(i) = 1 + 2

k∑j=1

cos(jα) ≡ Cthk (3)

X LRF

Y LRF

Z LRF

0LRF

Rotating mirror

Measurement system

3 consecutive laser beams

0 LRF X LRF

Y LRF

i

i+1

i-1

3 consecutive laserbeams (i-1, i, i+1)

Field of view

Angular resolution

Fig. 1. (a) Laser rangefinder system and geometry. (b) Basic principles of a scanning laser rangefinder: α defines the angular resolution.

where α is the angular resolution of the scanning sensor.Cthk is a geometric invariant that only depends on the neigh-borhood k of Pi.

A scanning laser telemeter measure, for each angle nα(n ∈ N), the radial distance ρ to the object:

ρmi = ρi + νi ∀i = {1, . . . , nmax} (4)

νi is an additive white Gaussian noise with zero mean andσ2ρ variance. nmax denotes the number of points in a scan

(nmax = 1 + (θmax − θmin)/α).Due to the presence of measurement noise, Eq. 3 is no

longer valid (Ak(i) 6= Cthk ). Hence, the geometric invariantmust be recalculated using the statistics of the measurementnoise ν. Ak is a random variable and its two first momentsare equal to: E(Ak(i)) = Cthk (5)

V ar(Ak(i)) = σ2Ak'σ2ρ

ρ2i

(Cthk − 1

)2(6)

The first order moment of Ak is equal to the theoreticalinvariant Cthk . As a consequence, one can build a membershiptest of 2k + 1 successive scan points Pi to the same linesegment by deriving statistical properties, especially on thesecond order moment:

|Ak(i)− Cthk | ≤ 3σAk = 3σρρi

(Cthk − 1) (7)

The membership criterion does not here depend on the inter-impact distance (as it is the case for the usual methods [4])which cannot be derived straightforwardly since it is relatedto the shape orientation and distance.

In order to guarantee the most compact description of thescene, the longest line segment must be detected in the scene.To this end, one defines the neighborhood function V (i, k)which denotes all the possible neighborhoods k of a linesegment centered on Pi, k = {1, . . . , kmax(i)}:V (i, k) = k if and only if Eq. 7 is valid

for the considered neighborhood k= 0 elsewhere

Hence, one derives a cumulative function CF (i) thatrepresents for each scan point Pi, the maximum number of

Fig. 2. a) Synthetic measurements. The scene is composed of three vehicles(a,b and c) and of some surrounding elements - b) Cumulative Function CF ofthe whole scan. The three vehicles are labeled and described by two triangles.

potential neighbors belonging to the same Pi-centered linesegment: CF (i) = max

kV (i, k) ∀i (8)

Figs. 2 shows an example of the global aspect of thefunction CF for an ideal synthetic case. Each line segmentis here described by a triangle in CF . In such a modeling,the detection problem can be summed up in identifying thepeaks. It can be shown that the peak position defines themiddle point Pε of the line segment (ε is the scan pointnumber) and the corresponding value of CF allows to estimatethe widest neighborhood {Pε−kmax , . . . , Pε+kmax} of Pε withkmax = CF (ε). In practice, the envelope of CF is not soperfect. This is mainly due to the presence of outliers, themeasurement noise and possible non detection.

B. Proposed solution for a joint detection and tracking

One of the difficulties lies in the detection of local maximain the function CF . Indeed, this step is more complex as it

seems, mainly because of measurement noise and outliers. Anefficient solution has been proposed in [5] to detect the objectsin the scene. The tracking stage can be done a posterioriusing the output of the detector. The philosophy of the methoddeveloped in this paper is rather different: we aim at avoidingthe detection stage by using directly the function CF in thetracking process, instead of using the output of the detector.Doing so, the detection problems can be lessened. Anotherhard point in an usual detection procedure of vehicles isthat the object modeling must be deduced from the detectedsegments (with the risk of biased estimation of the model).In a bounding-box modeling indeed, the estimation of thecenter-of-gravity is done from a disjointed detection of theline segments related to the object.

In the top-down approach proposed here, the vehicle model(described here by a boxcar model) is directly embedded inthe filter in order to detect it in the function CF . Let us noteX = (x, vx, γx, y, vy, γy)T the state vector that characterizesthe tracked object. The relative motion is described by a thirdorder kinematic model (position, velocity, acceleration) alongthe components x and y:

Xt = ΦXt−1 +Wt (9)

where Wt is a six-dimensional white Gaussian noise withzero mean and Q covariance. The dynamics flow of the systemcan be written as follows (O3x3 is the three-dimensional nullmatrix, ∆t is the full scan period):

Φ =

(Φx O3x3

O3x3 Φy

)with Φx,y =

1 ∆t ∆t2/20 1 ∆t0 0 1

The ego-vehicle evolves according to a motion that is

estimated at each time instant (using a GPS sensor, forinstance). Its velocity is denoted as V egot = (V ego,xt , V ego,yt ).A main difficulty here lies in the definition of the measurementequation. In our approach, the tracked vehicle is no longermeasured by the position of its center-of-gravity, deducedfrom the detected line segments and the chosen modeling.The proposed method uses the n-dimensional cumulativefunction CFt, built from the scan at time t with an angularaperture [θmin, θmax] and composed of n scan points{CFt(i)}i=1,...,n. Hence, the measurement equation is:

Zt = H(Xt) + νt = CFt + νt (10)

νt is a n-dimensional white Gaussian noise with zero meanand covariance R = σ2

νIn (In is the n-dimensional identitymatrix). The measurement function H is strongly nonlinear.

The proposed solution uses a nonlinear filter based onthe sequential Monte Carlo techniques [11]. Applied to thisproblem, they allow from the chosen modeling, to jointlysolve the vehicle detection and tracking problem. In thispaper, the particles are described by the state vector Xj

t =(xjt , v

x,jt , γx,jt , yjt , v

y,jt , γy,jt )T and their related importance

weights wjt . Each particle describes the motion of the center-of-gravity of the vehicle. The basic structure of our methodcan be summarized as:

1) Initialization: each particle j = 1, . . . , Np is drawn asXj

0 ∼ N (X0, Q0) where X0 is the initial state vector

and Q0 the related covariance matrix:Q0 = diag(σ2

0,x, σ20,vx , σ

20,γx , σ

20,y, σ

20,vy , σ

20,γy )

The importance weights are set to wj0 = 1/Np.2) Evolution: the prior density is used as proposal distri-

bution. The particles evolve in the state-space accordingto the dynamics equation Eq. 9, i.e. Np samples Xj

t aredrawn according to p(Xt|Xj

t−1) via the law p(Wt).3) Weighting: the difficulty point here lies in the calculus

of the particles weights since they are strongly linkedto the modeling of the measurement equation. For eachXjt , a particle-based measure Zjt must be deduced.

This measure is based on the particle-based cumulativefunction CF jt . In our model-based approach, CF jt isestimated as (for each particle j):

a) The particle-based orientation of the trackedvehicle is computed using the state Xj

t of theparticle. This orientation depends on the absolutevelocity of the vehicle (written in a Galileanreference frame):

θjt = tan−1(V ego,y + V y,j

V ego,x + V x,j

)(11)

b) The angle θjt gives the orientation of the objectmodel linked to the particle Xj

t . Using this model,the cumulative function CF jt of the particle j isbuilt. This function depends on the position of theparticle and the orientation of the related model.

The importance weight is recursively calculated:

wjt =p(Zt|Xj

t )Np∑l=1

p(Zt|X lt)w

lt−1

wjt−1 (12)

In this case, the log-likelihood of the particle j is givenby

log p(Zt|Xjt ) ∝ −1

2

(Zt − CF jt

)TR−1

(Zt − CF jt

)The importance weight uses the cumulative function CFthat indicates membership assumptions of consecutivescan points to the same line segment. At this step, onedoes not dispose of any range information of the linesegment but only preclustering information. To providea robust tracking of the shape, the measurement equationmust be augmented with the raw measurement of theradial distance ρ of the middle point of the particle-based segment (which corresponds to the peak of CF ).This second equation is given by Eq. 4.For each particle, the related range measure is

Zρ,jt = ρuj ,t (13)

where uj = argmaxCF jt denotes the scan number ofthe middle of the line segment. Eq. 12 is then modified

wjt =p(Zt|Xj

t )p(Zρt |Xjt )

Np∑l=1

p(Zt|X lt)p(Z

ρt |X

jt )wlt−1

wjt−1 (14)

and the log-likelihood of this new term is

log p(Zρt |Xjt ) ∝ − 1

2σ2ρ

(ρmuj ,t − Z

ρ,jt

)T (ρmuj ,t − Z

ρ,jt

)where ρmuj ,t is the range measure of the uj-th impact attime t. This additive term allows to take into accountthe raw range measurement of the middle point of theline segment. This distance is indeed not included in thecumulative function CF .

4) Estimation: using the importance weights, the estimateis then:

Xt|t =

Np∑j=1

wjtXjt (15)

5) Resampling: according to an estimation of the effectivesample size Neff = 1/

(∑Npj=1(wjt )

2)

, a resamplingprocedure is applied [11].

III. EXPERIMENTAL RESULTS

A. Simulated data

The presented method has been evaluated on laserrangefinder simulated data. To have the same theoretical basis,a comparison to a popular target tracking method [9] based onProbabilistic Data Association and a particle filtering (PDA-PF) is proposed.

The sensor has the following typical characteristics: range= 100m, angular resolution = 0.25◦, field-of-view [θmin =10◦, θmax = 170◦], measurement noise with σρ = 0.03 andσν = 1. The ego-vehicle evolves with a speed of 13m/s(approx. 47km/h).

The following common parameters are chosen for the algo-rithms of PDA-PF and SIP tracking:• number of particles: Np = 10000;• resampling threshold: Nrt = 20%Np;• dynamic noise: σγx = 0.15m/s2 and σγy = 0.3m/s2

The initial parameters of the particles are:• X0: a line segment detection and a modeling of the

vehicle are proceeded to estimate the center of gravity(x0, y0). The other components are set to 0:

X0 = (x0, 0, 0, y0, 0, 0)T

• Q0: σ0,x = σ0,y = 0.15m, σ0,vx = σ0,vy = 0.25m/sand σ0,γx = σ0,γy = 1m/s2.

For visibility reasons, only results on the position estimationalong x-axis are given. PDA-PF tracking uses a detection stagewith the Split-and-Merge algorithm and a modeling stage tocreate measurements of the vehicle position: Fig. 4a showsthese measurements (red +) and theoretical positions (blackdot). The presence in Fig. 4a of several measurements for thesame vehicle is here due to false alarm and/or mis-modeling.

The PDA-PF tracking uses the detected positions shown inFig. 4a. As it can be seen in Fig. 4b, presence of severalmeasurements leads to the creation of additional tracks. Toomany creations may cause errors in the data association whichdisturb the tracking process. On the other hand, the proposedmethod avoids this drawback as shown in Fig. 4c. Indeed, thefilter does not use the detection stage and processes directly

(a)

(b)Fig. 3. Comparison of errors: (a) Position error (m) is lower with the proposedmethod. The increase of error for PDA-PF Tracking method is due to themoving away of vehicle from the sensor. (b)Velocity error (m/s): our methodlet also a better estimate of the velocity.

TABLE IESTIMATION ERRORS

Standard deviation σx σVx σγx σy σVy σγySIP tracking 0.0077 0.0202 0.0383 0.0105 0.0305 0.0783PDA-PF 0.0741 0.1227 0.0816 0.0144 0.0470 0.0857

the cumulative function CF . As a consequence, the resultingfalse alarm rate is much lower than in the sequential PDA-PF.

Concerning the estimation accuracies, the position and ve-locity errors are computed at each time (Fig. 3). The standarddeviation on estimated dynamics parameters errors are alsoderived in Tab. I. In the PDA-PF approach, one focuses onthe most favorable case: it means that the correct tracks onlyare used to compute these errors, excluding the false alarmgenerated tracks. When the vehicle moves away from thesensor (y component of the state vector), usual detectionmethods are less precise. On the other hand, the proposedmethod has the benefit of being insensitive to the distancebetween the sensor and the vehicle. Tab. I shows the benefitof the proposed approach with an improvement in estimationaccuracies up to a factor 10 for σx.

B. Real data

The method is then evaluated on real data from an IBEO LDAutomotive scanning laser telemeter. This sensor is mounted

(a)

(b)

(c)

Fig. 4. (a) Detected positions by S&M algorithm after modeling (red crosses) and theoretical positions (black dots). Several crosses for one time stepindicate a case of mis-modeling or false alarm.(b) Results of PDA-PF tracking: each colored star represent a trajectory created by the method. Due to multipledetections, more than one trajectory are sometimes tracked. (c) Results of SIP tracking: the process is not disturbed by mismodelings and is more accurate.

on a moving vehicle. Its characteristics are: range = 250m, an-gular resolution = 0.25◦, field-of-view [θmin = 10◦, θmax =169.75◦], measurement noise with σρ = 0.05m.

The parameters Np, Nrt and σν are the same than them inthe synthetic measurements. The initial parameters X0 and Q0

are set in the same manner as for synthetic data. In order toaccount for various possible dynamics of the tracked vehicles,the dynamic noise parameters are σγx = 0.5m/s2 and σγy =0.8m/s2.

Fig. 5 shows a scan sample of the real scenario. One of thedifficulty here lies in the evaluation of the methods due to theabsence of ground truth. Fig. 6a shows estimation results of theproposed method along the x component which are comparedto the detection only results. Figs. 6b plot the vehicle trackingresults for both methods at two time instants. It can be seenthat the SIP tracking perfectly fits with the raw data whereasthe PDA-PF method is perturbed by the detection step (biasedestimation of the center of gravity, multiple detections, etc.)

Fig. 5. Example of a real scan: the scene is composed of two vehicles andof some surrounding elements. One tracks the first vehicle (black circle).

(a)

(b)Fig. 6. (a) SIP Tracking: the estimated trajectory (red star) is not disturbed by multiple detections (blue plus). (b) Tracking results for PDA-PF Trackingand SIP tracking at time steps #42 and #164: scan impacts in black dots, centers of gravity detection (multiple at time step 64) in red squares and estimatedposition of the tracked vehicle in blue.

IV. CONCLUSION

This paper deals with a new method of vehicle trackingin scanning laser range data. The originality lies in a jointapproach of the detection and tracking problems, whereaspopular methods use a predetection stage prior to any trackingprocedure. The proposed method deals directly with raw mea-surements which avoids the usual drawbacks of the detectionstage, caused by mis-modeling and false alarms. This methodis very effective and robust towards these false alarms. Exper-iments on synthetic and real data show that the estimationaccuracies are significantly higher than those of the PDA-PF tracking method and also confirm the robustness of theapproach, especially in the areas leading to problematicaldetection.

REFERENCES

[1] H. Dyckmanns, R. Matthaei, M. Maurer, B. Lichte, J. Effertz, andD. Stuker, “Object tracking in urban intersections based on active useof a priori knowledge: Active interacting multi model filter,” in IEEEIntelligent Vehicles Symposium, june 2011, pp. 625 –630.

[2] R. MacLachlan and C. Mertz, “Tracking of moving objects from amoving vehicle using a scanning laser rangefinder,” in IEEE IntelligentTransportation Systems Conference, sept. 2006, pp. 301 –306.

[3] A. Petrovskaya and S. Thrun, “Model based vehicle detection andtracking for autonomous urban driving,” Autonomous Robots, vol. 26,no. 2–3, pp. 123–139, apr 2009.

[4] V. Nguyen, S. Gatcher, A. Martinelli, N. Tomatis, and R. Siegwart, “Acomparison of line extraction algorithms using 2d range data for indoormobile robotics,” Autonomous Robot, vol. 23, pp. 97–111, 2007.

[5] J. Noyer, R. Lherbier, and B. Fortin, “Automatic feature extraction inlaser rangefinder data using geometric invariance,” in IEEE Forty FourthAsilomar Conference on Signals, Systems and Computers, Pacific Grove,California, USA, nov. 2010, pp. 199 –203.

[6] M. Thuy and F. Leon, “Non-linear, shape independent object trackingbased on 2d lidar data,” in IEEE Intelligent Vehicles Symposium, june2009, pp. 532 –537.

[7] Y. Bar-Shalom and T. E.Fortmann, Tracking and Data Association.Academic Press, Inc., 1987.

[8] M. Ekman, “Particle filters and data association for multi-target track-ing,” in 11th International Conference on Information Fusion, 30 2008-july 3 2008, pp. 1 –8.

[9] A. Gorji and M. Menhaj, “Multiple target tracking for mobile robotsusing the jpdaf algorithm,” in 19th IEEE International Conference onTools with Artificial Intelligence, vol. 1, oct. 2007, pp. 137 –145.

[10] T.-D. Vu and O. Aycard, “Laser-based detection and tracking movingobjects using data-driven markov chain monte carlo,” in IEEE Inter-national Conference on Robotics and Automation, may 2009, pp. 3800–3806.

[11] A. Doucet, N. de Freitas, and N. Gordon, Eds., Sequential Monte-CarloMethods in Practice. Springer Verlag, 2002.