Fusion Between Laser and Stereo Vision Data For Moving ObjectsTracking In Intersection Like Scenario

Qadeer Baig, Olivier Aycard, Trung Dung Vu and Thierry Fraichard

Abstract— Using multiple sensors in the context of environ-ment perception for autonomous vehicles is quite common thesedays. Perceived data from these sensors can be fused at differentlevels like: before object detection, after object detection andfinally after tracking the moving objects. In this paper we detailour object detection level fusion between laser and stereo visionsensors as opposed to pre-detection or track level fusion. Weuse the output of our previous laser processing [2] to get a listof objects with position and dynamic properties for each object.Similarly we use the stereo vision output of another team [7],[8] which consists of a list of detected objects with position andclassification properties for each object. We use Bayesian fusiontechnique on objects of these two lists to get a new list of fusedobjects. This fused list of objects if further used in trackingphase to track moving objects in an intersection like scenarioand has improved the data association and track managementsteps. The results obtained on data sets of INTERSAFE-2demonstrator vehicle show effectiveness of our techniques.

I. INTRODUCTION

Perceiving or understanding the environment surroundinga vehicle is a very important step in driving assistancesystems and for the functioning of autonomous vehicles.The task involves solving both simultaneous localization andmapping (SLAM) and detection and tracking of movingobjects (DATMO) problems. While SLAM provides thevehicle with a map of the environment, DATMO allows thevehicle being aware of dynamic entities around, trackingthem and predicting their future behaviors. If we are ableto accomplish both SLAM and DATMO reliably in realtime, we can detect critical situations to warn the driver inadvance and this will certainly improve driving safety andcan prevent traffic accidents. We use multiple sensors onthe vehicle to observe the surrounding environment to solvethese problems, since a single sensor can provide only alimited view of the environment.

A great deal of work has been done to solve theseproblems using different sensors, especially laser scanners[2], [12], [9], [5], [14]. In most of these works occupancygrids [4] framework has been used to represent the sur-rounding environment. Although using high resolution laserscanners (less than 1◦) we are able to obtain good mapsof the environment [12] but tracking moving objects in acomplex intersection like scenario using laser scanner onlyis quite challenging due to temporary occlusions and manydifferent directions of movements. So there is a strong needto use multiple sensors to solve this tracking problem. Butusing multiple sensors inherently requires to perform fusion

Authors are with University of Grenoble1 & INRIA Rhône-Alpes, Greno-ble, France, (e-mails: Qadeer.Baig@inrialpes.fr, Olivier.Aycard@inrialpes.frand Trung-Dung.Vu@inrialpes.f, Thierry.Fraichard@inria.fr)

between them at some appropriate level so as to get optimalresults. This fusion can take place at three different levels:i) before objects detection, also called low level fusion, forexample an occupancy grid is constructed for each sensorand fusion is performed to get a fused occupancy grid, ii)at objects detection level, output of each sensor is processedto extract lists of objects (or obstacles) in the environmentand information about corresponding objects in these listsare fused to get fused list of objects, and iii) at track level,output of each sensor is processed to do tracking of movingobjects and then fusion is performed on detected tracks (atrack is a moving object detected consistently in few previousframes), fusion at this level is usually performed to confirmtracks detected by a primary sensor.

In this work we have developed a generic architecture forobject detection level fusion between different sensors andtracking the fused list of objects (figure 1). This architecturehas two levels: the first level deals with the pre processingof sensors for detecting objects with properties specific tosensors and then performing fusion between these lists of ob-jects. The fusion at this level involves finding correspondingobjects in the lists and merging their properties, this gives afused list of objects with individual objects having more stateinformation than their pre fusion versions. Second level dealswith tracking of the fused objects. In this work we have usedthis architecture to perform fusion between objects detectedby laser and stereo vision in the context of a Europeanproject INTERSAFE-2 1 on Volkswagen demonstrator. Weperform the laser processing to construct local grid map andto detect moving objects in the environment, the output ofstereo vision processing consists of a list of 3D objects withclassification information. After fusion objects have position,dynamic state and classification information which result inmore precise tracking results. Due to this fusion we alsoget good tracks for occluded or transparent objects for lasersensor.

Fusion between laser and stereo vision on other two levels(pre detection and track levels) has some issues, for exampleoccupancy grids constructed for stereo vision for low levelfusion have many false positives and many filtrations needbe done to refine them [6], moreover due to high depthuncertainty for stereo sensor most of the weight is givento laser occupancy grid [1]. Baltzaksi et al. [3] have usedthis low level fusion technique for indoor robot navigation.Similarly at track level, due to small field of view andlimited range of stereo vision, there are many mis detections

1http://www.intersafe-2.eu

Fig. 1. Architecture of the perception system (SVS=Stereo Vision Sensorand LS=Laser Sensor).

because moving objects quickly go out of sight before theyare confirmed as tracks.

The rest of the paper is organized as follows. In the nextsection we present the demonstrator used for this work andsensors installed on it. We summarize our work [2] on laserprocessing to build a map of the environment, localize ourego vehicle inside this map and detect moving objects insection III. In section IV we introduce the stereo-visionprocessing done by University of Cluj. In section V, we detailour work on fusion with tracking in section VI. Experimentalresults are reported in section VII. We conclude this work insection VIII.

II. EXPERIMENTAL SETUP

The Volkswagen demonstrator vehicle used to get datasetsfor this work has multiple sensors installed on it. It has along range laser scanner (Lidar) with a field of view of 160◦

and a maximum range of 150 m. It has 161 laser beamscalled channels and resolution of 1◦. It has a measurementpulse width of 16ns. Each channel can detect a maximumof two targets with vertical divergence of 3◦. A channeldetects two targets because each laser beam scans two planes:horizontal and another with a vertical divergence of 3◦, if theobject detected in the second plane is not the same as thatof first plane then it is reported as second target (using adistance threshold to decide the second target). In our workwe only use the nearest target. Other sensors installed onthis demonstrator include a stereo vision camera, four shortrange radars (SRR) one at each corner of the vehicle anda long range radar (LRR) in front of the vehicle (figure. 2).Our work in this paper is only concerned with the processingand fusion of Lidar and stereo vision data. For both of thesesensors we have following two reference frames:

• Stereo (with origin on the ground plane exactly belowthe laser scanner, z-axis pointing in the direction ofdriving and x-axis towards right)

• Laser (with origin fixed on the laser scanner, y-axispointing in the direction of driving and x-towards right).

Fig. 2. Sensors installed on the demonstrator vehicle

These frames of reference are fixed w.r.t each other and theirtransformation matrices are known.

III. LASER PROCESSING

In this section we summarize our laser processing [2] thatwe have used for moving objects detection with laser datawith details of improvements done for current work. Thisprocess consists of following steps: first we construct a localgrid map and localize the vehicle in this map, then using thismap we classify individual laser beams in the current scanas belonging to moving or static parts of the environment,finally we segment the current laser scan to make objectsfrom individual laser beams.

A. Environment Mapping & Localization

We have used incremental mapping approach based onlaser scan matching algorithm to build a local vehicle map.Based on occupancy grid representation the environment isdivided into a two dimensional lattice of rectangular cellsand we keep track of probabilistic occupancy state of eachcell. We build a grid map of 90m × 108m with each cellhaving dimensions of 0.3m×0.3m. Environment mapping isessentially the estimate of posterior probability of occupancyP (m |x1:t, z1:t) for each cell of grid m, given observationsz1:t = {z1, ..., zt} from time 1 to time t at correspondingknown poses x1:t = {x1, ..., xt}, here zt = {Pi} wherePi = (x = ri cos θi, y = ri sin θi) is the impact point (ordetected target position) by ith laser beam (from its polarcoordinates ri, θi) for ∀i ≤ 161 expressed in laser frameof reference, and xt = (x, y, θ) is the vehicle pose. Toknow these pose values we need to solve the localizationproblem. We have used a particle filter based on importancesampling for this purpose (as explained in [11]), a total of300 particles are used. For the given previous pose xt−1 andcurrent odometry information ut = (ν, ω) (translational and

rotational velocities) we sample different possible positionsof the vehicle (ie, one position of the vehicle correspondsto one particle) from the motion model P (xt|ut, xt−1).Then we compute the probability of each position (ie, theprobability of each particle) using the laser data and asensor model. Practically, from the pose of each particle,we calculate the fraction of the laser beams terminating inoccupied cells of the grid map constructed so far. The poseof the particle getting highest probability is taken as truepose. The localization process takes on average 14ms on acomputer with 3.0GHz processor and 4GB of RAM.

B. Moving & Static Parts DistinctionUsing the local grid map constructed so far we classify

the laser impact points in the current laser scan as dynamicor static by comparing them with the map.The principal ideais based on the inconsistencies between observed free spaceand occupied space in the local map. Laser impacts observedin free space are classified as dynamic whereas the rest areclassified as static. More precisely, if we represent the localgrid map at time t as M t = {m} where m is a grid cell andif M t[Pi] gives the occupancy probability of the grid cellcorresponding to the laser impact point Pi then we classifythe laser impact points into following two types:

• MovingPoints = {Pi |M t[Pi] < 0.5}• StaticPoints = {Pi |M t[Pi] ≥ 0.5}

C. Laser Objects ExtractionFinally we perform segmentation to extract objects from

these laser impact points. We define an object as:

ÓL = {Pn | dist(Pn, Pn−1) < Sthr}

Here Pi is the impact point as defined above ,dist(Pn, Pn−1) is the euclidean distance between two ad-jacent points, and Sthr is the segment threshold distancewhich is equal to 2.0 meters in our experiments. An object ismarked as dynamic if ÓL ∩MovingPoints 6= φ, ie at leastone of its constituting laser points is classified as dynamic,otherwise it is considered a static object. Finally we calculatethe polar coordinates of center of gravity (centroid) (rL, θL)of each object using Cartesian coordinates of its constitutingpoints as:

rL =√x́2 + ý2

andθL = atan2(ý, x́)

where x́ =∑

i x/n and ý =∑

i y/n for ∀Pi(x, y) ∈ ÓLand n = |ÓL|.

D. Laser Processing OutputThe output of laser processing step at time t consists of a

local grid map M t, and a list of detected moving objectsLtobjects = {OL} where OL = (ÓL, rL, θL) is movingobject with centroid information (we do not include the staticobjects in this list because they need not be tracked). Gridmap is only used to display on the screen whereas list ofdynamic objects is used further for fusion. The results oflaser processing are shown in the figure 3.

Fig. 3. Mapping and moving objects detection results. Detected movingobjects (a bicycle in left image and two cars in right image) are shown asgreen rectangles.

IV. STEREO VISION PROCESSING

Another team from University of Cluj-Napoca Romaniaworking on INTERSAFE-2 project has performed the stereovision processing [7], [8]. Their output of stereo visionprocessing consists of a list of objects detected in each frameof the stereo images (figure 4). For each object in the listwe are given 3D coordinates of the four corners of the lowerrectangle of the object cuboid in stereo frame of reference,the height of the top rectangle and the class of the object(pedestrian, pole vehicle etc), but no information about thedynamic state of the object are available, objects can bedynamic or static. From this data we can easily calculatethe coordinates of the eight corners of the cuboid.

A. Pre-Fusion Processing

The first step before performing fusion between data fromlaser and stereo vision objects, is to project the objectsdetected by stereo processing onto the laser plane to achievethe common spatial reference using transformation matricesfrom stereo towards laser frame of reference. The results ofthis projection of stereo objects onto laser plane are shownin figure 4. For object level fusion between laser and stereowe need to represent the vision objects by their centroids.We take this centroid as the middle point of the front linesegment of the object rectangle obtained after projectioninto the laser plane (this point gives good results than thecenter of gravity of this rectangle because laser readings alsobelong to the front end of the object). We calculate the polarcoordinates of this centroid of each vision object (in a similar

Fig. 4. Stereo processing results, with objects projected on laser plane.

way as explained for laser objects) to make the representationcompatible to the laser objects for fusion.

B. Stereo Vision Processing Output

The output of the stereo vision processing at time tconsists of a list of objects V tobjects = {OV } where OV =(rV , θV , class). rV and θV are the polar coordinates of theobject centroid. We also save the rectangle of the object inthe laser plane just to show them as rectangle in the results.But only centroid and class information are used for fusion.

V. LASER AND STEREO DATA FUSION

In this section we give details of object detection levelfusion between laser and stereo vision sensors. As inputto the fusion process we have two lists of objects: list ofdynamic objects detected by laser and represented as centroidpoints, and list of objects (static or dynamic) detected bystereo vision represented as points along-with classificationinformation. We believe that an object detection level fusionbetween these two lists can complement each other thusgiving more complete information about the states of objectsin the environment. This fusion process consists of followingtwo steps:

A. Object Association

In this step we determine which stereo objects are to beassociated to which laser objects from the two object lists,using nearest neighbor technique. The positional uncertaintyof an object given by stereo vision increases with depth, so

we have defined a distance threshold function based on thedepth of the stereo object from the origin as:

Vthr = 5 ∗rV20

Vthr is the uncertainty in position of an object detectedat a distance of rV by stereo vision. Here 5 in meters isthe maximum depth uncertainty for an object detected at adistance of 20 meters. Stereo objects beyond this distanceare ignored because the effective range of stereo is limitedto 20m for this work. A stereo object OiV is associated to alaser object OjL if dist(O

iV , O

jL) < Vthr and O

jL is closest

to OiV .

B. Position information fusion

This step works on the pair of objects associated with eachother in the previous step and fuses their position (rangeand bearing) information. We model the position uncertaintyusing 2D Gaussian distribution for both objects. SupposePL = [rL, θL]

T is the centroid position of laser object andPV = [rV , θV ]

T is the centroid position of associated stereovision object. If X is the true position of the object then theprobability that laser detects this object at point PL is givenas:

P (PL|X) =e

−(PL−X)T R

−1L

(PL−X)2√

|RL|82πand similar probability for stereo object is given as:

P (PV |X) =e

−(PV −X)T R

−1V

(PV −X)2√

|RV |82πHere RL is the 2X2 covariance matrix of range and bearinguncertainty calculated from the uncertainty values providedby the vendor. Whereas RV is the covariance matrix forstereo vision and depends on the depth of the object fromorigin, we have empirically calculated its values for differentranges. In general range and bearing uncertainty for stereoobjects is much higher than the corresponding objects de-tected by laser and increases with distance from the origin.Also, range uncertainty for stereo is greater than the bearinguncertainty in general. Using Bayesian fusion the probabilityof fused position P = [rF , θF ]T is given as:

P (P |X) = e−(P−X)T R−1(P−X)

2√|R|82π

where P and R are given as:

P =PL/RL + PV /RV

1/RL + 1/RV

and1/R = 1/RL + 1/RV

respectively. This process of fusion is shown in figure 5. Pis taken as the position of the fused object.

The result of this fusion process is a new list of fusedobjects. This list also has all the laser objects which couldnot be associated with stereo objects and all the stereo

Fig. 5. Fusion process: red color shows the position uncertainty of laserobject, green color for corresponding stereo object and violet the fusionresult of the two.

objects which could not be associated with some laserobjects. We keep un associated stereo objects because theymay correspond to dynamic objects which may not havebeen detected by laser in current frame due to occlusion asexplained next.

Figure 6 shows an interesting fusion scenario, objectsshown in cyan color are the objects detected by stereo visionwhereas the objects shown by light violet rectangles are thelaser detected objects, red dots are raw laser impact points.An oncoming car that was being detected by laser until lastscan is now occluded by a cyclist whereas stereo systemwas able to detect this car. So this mis detection by laserwill be filled in by stereo during fusion hence giving asmooth track. The increased position uncertainty with depthfor stereo vision objects can also be seen in the figure (greenellipses).

C. Fusion Output

The output of fusion process consists of fusedlist of objects F tobjects = {OF } where OF =(rF , θF , class,DynamicState, SensorCount). For eachobject we have position (centroid) information, dynamicstate information (dynamic or unknown, unknown for unassociated stereo objects), classification information and acount for number of sensors detecting this object. For eachfused object we also have a pointer to the original laser orstereo object to use segment or rectangle information whiledisplaying the tracked object.

VI. TRACKING

In general, the multi objects tracking problem is complex:it includes the definition of tracking methods, but also associ-ation methods and maintenance of the list of objects currentlypresent in the environment. Bayesian filters are usually usedto solve tracking problem. These filters require the definitionof a specific motion model of tracked objects to predicttheir positions in the environment. Using the prediction

Fig. 6. Laser and Stereo vision objects fusion, see the text for details.

and observation update combination, position estimation foreach object is computed. In the following we explain thecomponents of our tracking module.

A. Data Association

This step consists of assigning new objects of fusedlist to the existing tracks. Since in the current work weare more concerned with tracking multiple objects in anintersection like scenario so it is important to choose a moreeffective technique of data association. In an intersectionlike scenario there may be many objects moving in differentdirections. They may be crossing or waiting to cross ina direction perpendicular to the oncoming vehicles, forexample a vehicle waiting to turn left etc. We have usedMHT [10] approach to solve the data association problem.An important optimization that we have achieved here dueto fusion process mentioned above is related to classificationinformation provided by stereo vision. While generatinghypotheses we ignore all those hypotheses which involveobjects from different classes. For example a hypothesistrying to involve a pedestrian with a vehicle in a track will beignored, this significantly reduces the number of hypotheses.To further control the growth of tracks trees we need to

use some pruning technique. We have chosen the N-Scanspruning technique to keep the track trees to a limit of N.

B. Track Management

In this step tracks are confirmed, deleted or created usingthe m-best hypotheses resulting from the data associationstep. New tracks are created if a new track creation hypoth-esis appears in the m-best hypothesis. A newly created trackis confirmed if it is updated by objects detected in currentframes after a variable number of algorithm steps (one stepif the object was detected by both laser and stereo visionotherwise in three steps). This implies that the spuriousmeasurements which can be detected as objects in the firststep of our method are never confirmed. To deal with non-detection cases, if a non-detection hypothesis appear (whichcan appear for instance when an object is occluded byan other one) tracks having no new associated objects areupdated according to their last associated objects and forthem next filtering stage becomes a simple prediction. Inthis way a track is deleted if it is not updated by a detectedobject for a given number of steps.

C. Filtering

Since in an intersection like scenario there may be dif-ferent types of objects (vehicles, motor bikes, pedestriansetc) moving in different directions using different motionmodes, a single motion model based filtering technique is notsufficient. To address the tracking problem in this scenariowe have used an on-line adapting version of InteractingMultiple Models (IMM) filtering technique. The details ofthis technique can be found in our other published work [13].We have seen that four motion models (constant velocity,constant acceleration left turn and right turn) are sufficientto successfully track objects on an intersection. We use fourKalman filters to handle these motion models. Finally themost probable trajectories are computed by taking the mostprobable branch and we select one unique hypothesis for onetrack tree.

Tracking results of two cars are shown in figure 7.

VII. RESULTS

Fusion and tracking results are shown in figures 8 and 9along-with the images of corresponding scenarios. Figure 8shows an interesting scenario in intersection for fusion, leftimage shows the tracking results based only on laser data,the car behind the cyclist was occluded in last few framesgiving insufficient impact points to be detected as a movingobject, the right image shows the tracking with fusion, carwas also partially detected by stereo vision and hence in thefused results it was tracked successfully. Figure 9 shows asimilar scenario, the ego vehicle is waiting for the signal, atruck turning left, a cyclist and a pedestrian crossing the roadin opposite directions are being tracked. Although truck inthis scenario is partially occluded by the cyclist but due tofusion it has been tracked successfully. A results video canbe found here2.

2https://sites.google.com/site/qadeerbaig/fusionresults

Fig. 7. Tracking results for two cars.

VIII. CONCLUSION

In this paper we have presented our approach for fusionbetween laser scanner and stereo-vision at the object detec-tion level as opposed to pre-detection or track level. Wehave demonstrated how the fused objects have more stateinformation than their pre-fusion versions. This fusion hasimproved the data association and track management stepsin the tracking phase. Experimental results on a Volkswagendemonstrator vehicle in the scope of the European projectINTERSAFE-2 show the effectiveness of the work presentedin this paper.

IX. ACKNOWLEDGEMENTS

This work was conducted within the research projectINTERSAFE-2. INTERSAFE-2 is part of the 7th FrameworkProgramme, funded by the European Commission. The part-ners of INTERSAFE-2 thank the European Commission forsupporting the work of this project.

REFERENCES

[1] Q. Baig and O. Aycard. Low level data fusion of laser and monocularcolor camera using occupancy grid framework. In InternationalConference on Control, Automation, Robotics and Vision (ICARCV),Singapore, December 2010.

[2] Q. Baig, TD. Vu, and O. Aycard. Online localization and mappingwith moving objects detection in dynamic outdoor environments. InIEEE Intelligent Computer Communication and Processing (ICCP),Cluj-Napoca, Romania, August 2009.

[3] H. Baltzakis, A. Argyros, and P. Trahanias. Fusion of laser and visualdata for robot motion planning and collision avoidance. Mach. VisionAppl., 15(2):92–100, 2003.

Fig. 8. Fusion and tracking results. Left(laser only): car occluded by cyclistis not being tracked. Right(laser and stereo): with fusion car was trackedsuccessfully.

Fig. 9. Tracking results for a Truck, a pedestrian and a cyclist.

[4] A. Elfes. Occupancy grids: a probabilistic framework for robotpercpetion and navigation. PhD thesis, Carnegie Mellon University,1989.

[5] D. Hähnel, R. Triebel, W. Burgard, and S. Thrun. Map building withmobile robots in dynamic environments. In Proceedings of the IEEEInternational Conference on Robotics and Automation (ICRA), 2003.

[6] D. Murray and J. Little. Using real-time stereo vision for mobile robotnavigation. In Autonomous Robotics, 2000.

[7] S. Nedevschi, R. Danescu, T. Marita, F. Oniga, and S. Bota.Stereovision-based sensor for intersection assistance. In AdvancedMicrosystems for Automotive Applications 2009, pages 129–163.Springer Berlin Heidelberg, 2009.

[8] S. Nedevschi, T. Marita, R. Danescu, F. Oniga, and S. Bota. On-board stereo sensor for intersection driving assistance. architectureand specification. In IEEE Intelligent Computer Communication andProcessing (ICCP), pages 409–416, Cluj-Napoca, Romania, August2009.

[9] E. Prassler, J. Scholz, and P. Fiorini. Navigating a robotic wheelchair ina railway station during rush hour. Int. Journal on Robotics Research,18(7):760–772, 1999.

[10] D. B. Reid. A multiple hypothesis filter for tracking multiple targetsin a cluttered environment. Technical Report D-560254, LockheedMissiles and Space Company Report, 1977.

[11] S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics (IntelligentRobotics and Autonomous Agents). The MIT Press, September 2005.

[12] TD. Vu, O. Aycard, and N. Appenrodt. Online localization and map-ping with moving objects tracking in dynamic outdoor environments.In IEEE International Conference on Intelligent Vehicles, 2007.

[13] TD. Vu, J. Burlet, and O. Aycard. Grid-based localization and localmapping with moving objects detection and tracking. InternationalJournal on Information Fusion, Elsevier, 2009. To appear.

[14] C.-C. Wang. Simultaneous Localization, Mapping and Moving ObjectTracking. PhD thesis, Robotics Institute, Carnegie Mellon University,Pittsburgh, PA, April 2004.