Probabilistic Scheme for Laser Based Motion Detection

Roman Katz, Juan Nieto and Eduardo NebotARC Centre of Excellence for Autonomous Systems

Australian Centre for Field RoboticsThe University of Sydney

Sydney, NSW 2006, Australia{r.katz}@cas.edu.au

Abstract— This paper presents a motion detection schemeusing laser scanners mounted on a mobile vehicle. We proposea stable, yet simple motion detection scheme that can be usedand improved with tracking and classification procedures. Thesalient contribution of the developed architecture is twofold.It proposes a spatio-temporal correspondence procedure basedon a scan registration algorithm. The detection is cast as aprobability decision problem that accounts for sensor noise andachieves robust classification. Probabilistic occlusion checking isfinally performed to improve robustness. Experimental resultsshow the performance of the proposed architecture underdifferent settings in urban environments.

I. INTRODUCTION

Advanced perception modules that allow reliable movingobstacle detection from moving vehicles would provide au-tonomous systems with the appropriate environmental infor-mation to plan actions or avoid collisions. Such techniquesare at the core of navigation research, not only in thedeployment of completely autonomous vehicles, but also ofdriver assistance systems for commercial cars. In this context,the information provided to the driver should be able toidentify risky agents as other vehicles, pedestrians and bikes.

Most of the existing dynamic obstacle tracking and clas-sification architectures heavily rely on modules performingmotion detection [1], [2] or assume the initial tracks aregiven [3], [4]. Within the approaches based on laser, thestrategy presented in [5] is very related to our work. Motiondetection is based on detecting change in temporal maps.Assuming that an estimation of the vehicle motion is avail-able, a timestamp based representation is derived from theoccupancy grid. It is then used to perform classificationusing different thresholds for identifying static and dynamicclasses. Spatio-temporal data association in range data isalso studied in [6], with focus on obtaining accurate alignedmaps through non-rigid alignment and change detection. Thework presented in [7] integrates detection of moving objectsin the context of SLAM. The approach introduced in [8]proposes a Markov localization scheme to register scans,prior to perform detection and tracking of pedestrians. Amodel-based approach is chosen in [9], some models areproposed for foreground and background returns to performinitial detection from a static observer. Kalman filtering isused to filter the detected tracks assuming constant velocitymodels.

In this paper we present an architecture to perform motiondetection based only on range information provided by

laser scanners. The approach proposes a spatio-temporalcorrespondence procedure based on performing scan regis-tration using the Iterative Closest Point (ICP) algorithm [10].Using the obtained correspondences, a robust classification isperformed by casting the detection as a probability decisionproblem that considers sensor noise and then computes prob-abilistic occlusion checking. The proposed algorithm doesnot require dead-reckoning sensors and does not build normaintain any explicit map different from the scan registeringcapabilities. Neither explicit tracking nor temporal filteringis performed to achieve additional robustness. The systemis feature independent, since no prior knowledge is assumedabout the model of the objects, underlying classes, shape, etc.We aim at obtaining a stable, yet simple motion detectionstrategy that can be used and improved with tracking andclassification procedures.

This paper is organized as follows: the system is brieflyintroduced in Section II, presenting a general overview ofthe architecture and some of the basic concepts it relieson. Section III describes our proposed approach to per-form motion detection using an ICP based spatio-temporalcorrespondence procedure, together with the probabilisticrepresentation for the clusters and gating, multiple scanintegration, and occlusion checking. To illustrate the per-formance of the system, results of experiments undertakenin a large urban environment are provided and detailed inSection IV. On this basis, conclusions and future work arediscussed in Section V.

II. SYSTEM OVERVIEW

Changes in the environment sensed by a static observercan be almost immediately correlated to underlying dynamicbehavior. However, when the observer is instead dynamic,correspondence between the objects needs to be determinedfirst. This correspondence is defined as the association ofscans that belong to the same underlying object, and a mea-sure of its location displacement. Once the correspondence isestablished, the difference between subsequent snapshots intemporal sequence of spatial observations indicates potentialdynamic behavior. Nevertheless, this discrepancy cannot bedirectly used since it can be produced by other phenomenarather than dynamic objects. As presented in [5], it can becaused either by new information obtained about unknown,stationary objects due to the motion of the observer, by the

(a) (b) (c)

Fig. 1. (a) shows a scan segmented into clusters. The different clusters are indicated using different colors. Dashed ellipses and boxes show the Gaussianrepresentation (3σ contour) for some of the segmented clusters. (b) shows aligned scans after ICP registration. Different colors and markers indicatedifferent scans (magenta ‘•’ and yellow ‘◦’). Laser returns from (a) projected (yellow ‘◦’) onto the camera image are shown in (c).

motion and artifacts produced by already discovered dynamicobjects (occlusions), or by a combination of these effects.

We address these problems by first establishing corre-spondence between successive scans using a spatio-temporalprocedure based on a scan registration algorithm. Eventhough this scheme produces quite reliable hypotheses, itcannot identify dynamic behavior unambiguously. Hence,probabilistic criterion is used to perform a more robustclassification. The decision criterion considers a probabilisticrepresentation for the clusters in the scans that accounts forunderlying sensor uncertainty, and then evaluates a validationgate between the corresponding associates clusters. Finally,an occlusion checking is evaluated using also the probabilis-tic representation for the clusters. We next present some ofthe concepts that are salient in our approach.

A. Scan Segmentation

The goal of scan segmentation is to obtain simple entitiesfor the objects to simplify the representation, while captur-ing their salient characteristics. For laser scans, clusteringcan be efficiently done by comparing consecutive rangemeasurements which arrive in order of sweep angle. Fortwo successive returns, if the distance in either range orangle is greater than a threshold, the latter return formsthe beginning of a new cluster. However, we want to usea generic clustering procedure that can also handle disor-dered sequences (i.e., merged scans taken at different times).Therefore, the clustering is performed in an Euclidean spacewhere a distance threshold, defined by the desired behaviorfor dynamic obstacles detection, is used as the criterion forforming the clustering. An example of a laser scan segmentedinto clusters is shown in Figure 1(a). Figure 1(c) shows animage of part of the environment. The laser returns from (a)are projected (yellow ‘◦’) onto the image using the camera-laser registration procedure presented in [11].

B. Scan Registration

Unprocessed data correlation, also called scan alignmentor range-image registration, is the process of aligning a

measurement point set of (2D or 3D) points with a referencepoint set. There are several techniques to perform range-image registration, being ICP [10], [12] arguably the mostcommonly used, with its popularity mainly due to its sim-plicity and efficiency.

The standard ICP algorithm works as follows. Let P ={bi} be a measurement data point set containing m points,and a reference point set X = {ai} considered fixedand containing n points. The goal of the registration is tofind a transformation vector to align the measurement datapoints of P to the reference points of X. This transfor-mation is composed by a translation and rotation. ICP isan iterative algorithm that consists in two steps. The firststep involves finding the correspondence between points inthe measurement and reference scan. This is done using anearest neighbor algorithm. The second step is to obtain arotation and translation between the two scans, minimizingthe mean square error. The algorithm is initialized with aninitial pose guess and, until the estimated pose satisfiessome convergence criterion, it is iteratively refined by aprocess of point-to-point data association and least-squaresminimization. Each point bi ∈ P is first transformed to thereference coordinate frame using the current pose estimate,and then associated to its nearest neighbor in X. The originalpoint bi and its associate ai are added to an association setE. Finally, the pairs in E are used to calculate the relativepose that minimizes the mean-squared error between theassociated points. For the experiments in this paper we usea quaternion based method as presented in [13]. An exampleof ICP alignment is shown in Figure 1(b).

C. Probabilistic Cluster Representation

In order to be able to perform a probabilistic gatingfor the classification of the associated segments betweenscans, we need to represent clusters in a Bayesian way.This representation should consider the underlying clusteruncertainty given by the laser returns that form each of theclusters.

For a set of range-bearing measurements, we first convertthe laser measurements and their uncertainties to a sensor-centric Cartesian space. Each of the range-bearing returnszi = (ri, θi) with Gaussian uncertainty Ri is converted toCartesian coordinates:

xi = f (zi) =[ri cos θi

ri sin θi

]

Pi = ∇fziRi∇fTzi

, (1)

where the Jacobian ∇fzi = ∂f/∂zi. We can now obtain amean and a covariance for clusters composed of points withuncertainty as in (1). Considering the N Gaussian pointsobtained from the clusters’ returns expressed in Cartesiancoordinates, we compute the first and second moment of themixture for each of the clusters as in [14]:

x̄ =1N

N∑i

xi

P =1N

N∑i

(Pi + (xi − x̄) (xi − x̄)T

). (2)

This representation for the clusters allows as to consider thesensor noise while capturing the “dispersion” of their com-posing returns with respect to their mean. It also facilitatesthe process of gating clusters in order to perform motiondetection, as explained in the next section. The Gaussianrepresentation for some of the segmented clusters is shownin Figure 1(a).

III. MOTION DETECTION

In this section we discuss the main components of ourframework for motion detection. We present a simple spatio-temporal procedure based on ICP scan registration aimedat determining correspondences in laser scans. To solve thecorrespondence, we first establish the association of scansthat belong to same underlying objects (either static ordynamic) together with their corresponding displacement.The obtained correspondences are then used to classify theassociated clusters as dynamic or static using probabilisticcluster representation and gating. A stage of probabilisticocclusion checking is finally applied to reduce the numberof false positives.

In order to properly formulate an algorithmic approach,we initially identify the possible situations that might arisewhen associating objects in laser scans. Scans can be initiallycorrelated through a proximity criterion, since nearest neigh-bor clusters in aligned scans correspond to the same objects.Unfortunately, an incorrect correspondence can occur dueto several reasons. It can be produced by “unstable” scans,i.e. returns that vanish or lack sufficient structure and thendefine either incorrect neighboring associations or displace-ment measures. Different portions of the same objects mightbe represented in subsequent scans, with correct nearestneighboring association but inconsistent displacement com-putation. It can also be generated by the occlusion producedby moving targets. In this case, clusters might actuallybe well associated in terms of neighboring criterion and

displacement, but it might not be correlated with a real actualdiscrepancy. The range to the object might also degrade thequality of the association, where distant clusters normallybecome less dense and unreliable. Moreover, there might beno correspondence between clusters, due to limitations inthe visibility range or new moving targets appearing in thesensor field of view.

The images in Figure 2 show an example where incorrectdiscrepancies are encountered. Figure 2(a) and (b) show twosubsequent scans projected onto the corresponding images.Due to the to occlusion produced by a moving target (car),incorrect correspondences between clusters are establishedin the aligned scans shown in Figure 2(c). These incorrectassociations are indicated by the labeled boxes “A” and “B”in each of the images.

A. Spatio-temporal Correspondence

Our approach to perform correspondence is built upon ascan registration procedure. We assume that a large majorityof range information is static. Most of the laser returnsusually correspond to background and non moving objects,and only a small portion to dynamic ones. This is furtherstrengthened by the relatively wide field of view of theemployed sensor. This assumption has been validated byempirical evaluation using datasets taken in real world,outdoor settings under normal situations 1. Let’s consider twosubsequent laser scans that have been already segmentedinto clusters. We assume that egomotion information is notavailable. Therefore, ICP algorithm is an efficient and naturalmeans to initially align the scans in the same reference frame.Once these two scans have been aligned, the associationstrategy can be defined.

A nearest neighbor procedure could establish an initialset of associations. However, the associations of such astraightforward rule are not reliable since in many cases theremight be multiple hypotheses, as can be seen in Figure 2(d).A one-to-one association scheme is therefore chosen to avoidmultiple hypotheses situations. Since it is likely that sameobjects are the closest to each other between successivescans, then mutual nearest neighboring is also enforced tofurther improve the quality of the association. Moreover, allthose associations that link clusters that do not have sufficientstructure are eliminated. Associations are maintained if bothclusters contain more than a minimum number of points. It isimportant to note that this filtering is done only after the ICPalignment and the initial associations have been determinedand not before. The reason for this is that even when someclusters do not have enough structure, they might help tofill in gaps and reduce the chances of establishing incorrectcorrespondences that might occur if eliminated.

B. Probabilistic Gating

Once objects correspondence has been established usingthe scheme presented before, classification of the associatedsegments can be performed. Considering the mean andcovariance (2) for the mixture of Gaussians that represents

1A more thoroughly evaluation needs to be performed for extremelypopulated environments.

(a) (b) (c)

Fig. 2. Example situation with incorrect discrepancies generated by occlusion. (a) and (b) show two subsequent scans projected onto the correspondingimages, and (c) depicts the incorrect correspondences in the aligned scans. The incorrect associations are indicated by the labeled boxes “A” and “B” ineach of the images.

now each of the clusters, the classification of the associatedclusters can be based on standard gate validation techniques.This is done by setting up a gate for each of the associatedpair of clusters j, and classifying the association using thenormalized innovation squared (NIS) [15]:

νTj S−1

j νj < Md, (3)

with innovation ν = x̄k−p − x̄k and innovation covarianceS = Pk−p + Pk, for a defined separation p in the sequenceof scans, at time k. Since the NIS follows a chi-square PDF,then the threshold Md, for an innovation of dimension d, canbe chosen for a given level of certainty in the gate.

C. Multiple Scan Integration

Range requirements in the context of large, outdoor en-vironments can be very demanding, often deriving in lowspatial resolution and sparse density in the returns. We herebyexpand the proposed architecture to be able to integrate sev-eral consecutive scans. The increase in the density of pointscan actually determine more “stable” scans and clusters,where many gaps are filled in so that the robustness of theoverall procedure (mainly due to having obtained consistentcorrespondences) tends to increase notably. If we consideran arbitrary buffer of size m, we can iteratively performsegmentation (clustering) and ICP registration for the frames(k − p, k − p + m), and then compute correspondences sim-ilarly as before for two scans only.

D. Occlusion Checking

Due to the dynamic nature of the observer and obstaclesin the scene, some false positives can still be detected bythe classifier, hence a posterior occlusion checking analysiswould reduce some of the undesired false positives. Weaddress this issue by borrowing concepts from the approachpresented in [16] where occlusion checking is performed inorder to choose a camera configuration for optimal coverage.The aim here is to evaluate occlusion in a probabilistic way,

considering that both the targets’ and occluders’ locationsare represented by their likely locations.

Given a robot position R, the probability of occlusionfor the target T and the occluder Q can be evaluated ina sampled-based version as:

Poc(T, Q|R) ≈ 1n

∑p∈δT

Poc(p, Q|R), (4)

where p represents a point drawn from the distribution δT

that represents the target T location, and n is the number ofsamples. Poc(p, Q|R) is the probability of occlusion of thepoint p by the occluder Q. This can also be evaluated usingsamples as:

Poc(p, Q|R) ≈ 1m

∑o∈δQ

Poc(p, o|R), (5)

with m the number of samples drawn from the occluderQ, and δQ the distribution representing its location. Finally,merging(4) and (5) the probability of occlusion for the targetT and any occluder Q can be computed as:

Poc(T, Q|R) ≈ 1n · m

∑p∈δT

∑o∈δQ

Poc(p, o|R), (6)

where both δT and δQ are captured for the laser clustersusing (2), and Poc(p, o|R) is a simple visibility test.

IV. EXPERIMENTAL EVALUATION

In this section we present experimental results showing theperformance of the proposed architecture. The datasets wereobtained in an outdoor, urban environment at the Universityof Sydney campus, using the system provided by [17]. Theyconsist of a sequence of images temporally correlated withlaser scans provided by a Sick laser scanner LMS200 operat-ing at high speed (500kBps), with vehicle velocities varyingbetween 0-40km/h. The algorithmic procedure presented in[11] was used to compute the camera-laser calibration.

Using these datasets, we first computed the performanceof the probabilistic motion detection scheme, comparing

Fig. 3. ROC curves. Probabilistic versus deterministic motion detection.

results with a simplified scheme that applies deterministicthresholding. We chose the multiple scan version of thescheme, using a buffer size m = 4. The obtained im-provement is shown in Figure 3 by means of ReceiverOperating Characteristics (ROC) curves. ROC curves provideindication of the quality of the classifier as they plot truedetection rate vs. false detection rate, as the discriminationcriterion threshold varies. The criterion for constructing theROC curves were the certainty level to perform gatingin the probabilistic approach, and a distance threshold toclassify the correspondence in the deterministic one. Thetotal number of true dynamic obstacles for this evaluationwas 1763, manually labeled for 10mins of logged data. Ascan be seen, the better classification of the probabilisticscheme over the deterministic counterpart corresponds toa larger Area Under the Curve (AUC), with AUC=0.855and AUC=0.825, respectively. These results were obtainedwithout using the occlusion checking approach presented inSection III-D, to compare the motion detection schemes withno additional decision stages on top.

Additional evaluation was performed including the occlu-sion checking stage. Reasonable working conditions werechosen using the ROC curves presented in Figure 3, forboth the probabilistic and the deterministic approach. Forthe case of probabilistic gating, a threshold of Md = 0.75was chosen whereas a threshold of 0.3m was used for thedeterministic evaluation (being this value the same one usedin the segmentation stages). Table I shows the results for bothcases. The true positive and false negative rates (%) are verygood for both schemes using the probabilistic gating andits determinist implementation. A robust detection can beindeed performed assuming that a correspondence procedurehas established solid cluster associations, and identifying dis-crepancies. The probabilistic approach for motion detectionwas extremely accurate, with levels of true positive and falsepositive rates of 90.02% and 26.81%, respectively, out of atotal number of manually labeled dynamic obstacles of 1763.

Figure 4 shows images of the system performing motiondetection in different situations. Three different examples

TABLE I

CLASSIFICATION ACCURACY (IN %).

Probabilistic gating Deterministic gatingTrue Positive 90.02 85.2False Positive 26.81 28.73False Negative 9.98 14.80

are shown, one in each column, illustrating the detection ofcars, bicycles and pedestrians The top row (Figure 4(a)-(c))presents the laser scans and the associated clusters, whereasthe bottom row (Figure 4(d)-(f)) shows the correspondingscans projected onto images. The detection results are indi-cated using solid boxes for each detected object in both thetop and the bottom images.

V. CONCLUSIONS AND FURTHER WORK

The architecture presented in this paper introduces amotion detection procedure based on laser scans. Motiondetection is performed by first establishing spatio-temporalcorrespondence between clusters through an algorithm basedon a scan registration scheme. The detection is then achievedby obtaining probabilistic representation for each of the clus-ters, computing a validation gate and performing probabilis-tic occlusion checking. The proposed architecture is featureindependent, since no prior knowledge is assumed about themodel of the objects, underlying classes, shape, etc. Thesystem performs motion detection without dead-reckoningsensors, explicit maps and tracking. We have presentedexperimental evaluation under different settings in outdoor,urban environments showing the classification performanceof the proposed architecture. The system performs robustdetection of different moving objects with low false positiveand false negative rates.

We are currently considering further evaluation and im-provements of our architecture. The robustness of the ICPas a registration procedure under high speed (both in theobserver and targets) still needs to be evaluated. Boundariesdefinition in terms of ICP correspondence reliability giventhe speed of observer/targets will be clearly correlated to thelaser frequency. We are currently in the process of evaluatingits limitations in this regard. Since the system is dependenton segmentation, we intend to further improve it by includingmultiple hypothesis handling in the clustering procedure[18], and by fusing visual information as in [19].

VI. ACKNOWLEDGMENTS

This work is supported by the CRCMining, the AustralianResearch Council (ARC) Centre of Excellence program andthe New South Wales Government. Also special thanks toQSSL for donating copies of the QNX Momentics Profes-sional development system used to implement the real-timedata acquisition system.

REFERENCES

[1] Z. Sun, G. Bebis, and R. Miller, “On-Road Vehicle Detection: AReview,” IEEE Transactions on Pattern Analysis and Machine Intel-ligence, vol. 28, no. 5, pp. 694–711, 2006.

(a) (b) (c)

(d) (e) (f)

Fig. 4. System performing motion detection in different scenarios. The top row ((a)-(c)) shows the segmentation results and the associated clusters betweensubsequent scans. The bottom row ((d)-(f)) presents the corresponding laser scans projected onto the camera image for each of the situations in the upperrow. The obtained results are indicated using solid boxes with same colors for the detected objects in both top and bottom images.

[2] K. M. Krishna and P. K. Kalra, “Detection, Tracking and Avoidanceof Multiple Dynamic Objects,” Journal of Intelligent and RoboticSystems, vol. 33, pp. 371–408, 2002.

[3] G. Monteiro, C. Premebida, P. Peixoto, and U. Nunes, “Tracking andClassification of Dynamic Obstacles Using Laser Range Finder andVision,” in IEEE/RSJ International Workshop on Intelligent Robotsand Systems. Beijing, China: IEEE, 2006.

[4] O. Frank, J. Nieto, J. Guivant, and S. Scheding, “Multiple Tar-get Tracking using Sequential Monte Carlo Methods and StatisticalData Association,” in IEEE/RSJ International Workshop on IntelligentRobots and Systems. Las Vegas, USA: IEEE, 2003.

[5] E. Prassler, J. Scholz, and A. Elfes, “Tracking Multiple MovingObjects for Real-Time Robot Navigation,” Autonomous Robots, vol. 8,no. 2, pp. 105–116, 2000.

[6] R. Kaestner, S. Thrun, M. Montemerlo, and M. Whalley, “A Non-rigid Approach to Scan Alignment and Change Detection Using RangeSensor Data,” Springer Tracts in Advanced Robotics, vol. 25, pp. 179–194, 2006.

[7] B. Wang, “Simultaneous Localization, Mapping and Moving ObjectTracking,” PhD thesis, Robotics Institute, Carnegie Mellon University,USA, 2004.

[8] D. Schultz, W. Burgard, D. Fox, and A. Cremers, “People Trackingwith a Mobile Robot Using Sample-based Joint Probabilistic DataAssociation Filters,” The International Journal of Robotics Research,vol. 22, no. 2, pp. 99–116, 2003.

[9] A. Fod, A. Howard, and M. Mataric, “Laser-based People Tracking,”in International Conference on Robotics and Automation. WashingtonDC: IEEE, 2002.

[10] P. Besl and N. McKay, “A Method for Registration of 3-D Shapes,”

IEEE Transactions on Pattern Analysis and Machine Intelligence,vol. 14, no. 2, pp. 239–256, 1992.

[11] Q. Zhang and R. Pless, “Extrinsic Calibration for a Camera and LaserRanger Finder (improves camera intrinsic calibration),” in IEEE/RSJInternational Workshop on Intelligent Robots and Systems. Japan:IEEE, 2004.

[12] T. Bailey, “Mobile Robot Localisation and Mapping in ExtensiveOutdoor Environments,” PhD thesis, University of Sydney, Australia,2002.

[13] B. Horn, “Closed-form Solution of Absolute Orientation using UnitQuaternions,” Journal of the Optical Society of America A, vol. 4,no. 4, pp. 629–642, 1987.

[14] J. I. Nieto, T. Bailey, and E. Nebot, “Recursive Scan-MatchingSLAM,” Journal of Robotics and Autonomous Systems, vol. 55, no. 1,pp. 39–49, 2007.

[15] S. Blackman and R. Popoli, Design and Analysis of Modern TrackingSystems. New York: Artech House Radar Library, 1999.

[16] X. Chen and J. Davis, “ An Occlusion Metric for Selecting RobustCamera Configurations,” Machine Vision and Applications, acceptedMay 2007.

[17] ACFR, The University of Sydney and LCR, Universidad Nacionaldel Sur, “PAATV/UTE Projects,” Technical Report ACFR, Sydney,Australia, 2006.

[18] D. Streller and K. Dietmayer, “Object Tracking and Classificationusing a Multiple Hypothesis Approach,” in Intelligent Vehicles ’04Symposium, 2004.

[19] N. Kaempchen, M. Zocholl, and K. Dietmayer, “Spatio-temporal Seg-mentation using Laserscanner and Video Sequences,” Lecture Notesin Computer Science, vol. 3175, pp. 367–374, 2004.