An Integrated Probabilistic Model for Scan-Matching, Moving Object Detection and Motion Estimation

Transcription

1 2 IEEE International Conference on Robotics and Autoation Anchorage Convention District May 3-8, 2, Anchorage, Alaska, USA An Integrated Probabilistic Model for Scan-Matching, Moving Object Detection and Motion Estiation Joop van de Ven, Fabio Raos and Gian Diego Tipaldi Abstract This paper presents a novel fraework for integrating fundaental tasks in robotic navigation through a statistical inference procedure. A probabilistic odel that jointly reasons about scan-atching, oving object detection and their otion estiation is developed. Scan-atching and oving object detection are two iportant probles for full autonoy of robotic systes in coplex dynaic environents. Popular techniques for solving these probles usually address each task in turn disregarding iportant dependencies. The odel developed here jointly reasons about these tasks by perforing inference in a probabilistic graphical odel. It allows different but related probles to be expressed in a single fraework. The experients deonstrate that jointly reasoning results in better estiates for both tasks copared to solving the tasks individually. I. INTRODUCTION Deployent of robotic systes in coplex environents, such as the DARPA Grand Challenge, requires robust techniques for localisation and apping. Accurate position estiation is one of the priary requireents for achieving robust operation. Additionally, urban environents are characterised by the existence of dynaic objects such as vehicles or people, whose detection and otion estiation is crucial for safety. These localisation, detection and tracking probles can be forulated as two well-known tasks in robotics: 1) data association for estiating the robot oveent fro ranging sensors and 2) detection and otion estiation of oving objects. Both probles have been studied in great detail in obile robotics, as they are central to a nuber of iportant probles. To date, however, these tasks have generally been considered separately. This paper describes an approach that perfors both tasks siultaneously, as a joint statistical inference process. It builds on graphical odel techniques and specifically CRF- Matching [19] and CRF-Clustering [23]. The advantage of forulating the proble as a joint inference process is that it allows the transission of inforation, fro the odel perforing scan-atching, to the odel perforing otion detection and vice-versa. There are two ain contributions in this paper. First, it provides a new forulation for the probles of scanatching, oving object detection and otion estiation as a joint statistical inference procedure. Second, it presents an J. van de Ven and F. Raos are with the ARC Centre of Excellence for Autonoous Systes, Australian Centre for Field Robotics, University of Sydney 26, NSW, Australia {j.vandeven, f.raos}@acfr.usyd.edu.au G. D. Tipaldi is with the Social Robotics Lab, Departent of Coputer Science, University of Freiburg, 791 Freiburg, Gerany tipaldi@inforatik.uni-freiburg.de algorith to perfor Maxiu a posteriori (MAP) inference in a graphical odel with both discrete and continuous rando variables. This paper is organised as follows: Section II presents an overview of existing solutions to the association and oving object detection probles. Section III discusses the basics of Conditional Rando Fields (CRF). This is followed by the definition of the joint graphical odel in Section IV. Section V then copares the results of the odel with current state of the art scan-atching and oving object detection ipleentations ost notably, CRF-Matching and CRF- Clustering. Finally, conclusions and directions for future work are presented in Section VI. II. RELATED WORK Matching of laser range scans is generally perfored using the Iterative Closest Point (ICP) algorith or one of its any variants [14]. The algorith iteratively iniises the distance between laser points in one scan and corresponding laser points in a consecutive scan by rotating and translating one of the scans. Usually a nearest neighbour approach is used to associate laser points. Despite having good coputational perforance, ICP has several probles. Firstly it is sensitive to the initial estiated scan offset. Secondly it does not provide a probabilistic interpretation for rotation and translation or even for point correspondences. Finally, it does not take into account other properties of the scans such as shape, or intensity. Various variants of the ICP algorith have dealt with one or ore of the above liitations (see for exaple [8] or []). However, there is no variant of ICP that address all these issues. To overcoe these liitations, a probabilistic data association odel was proposed in [19]. The odel is based on Conditional Rando Fields (CRF) where features representing different properties of the scans can be incorporated and learned fro data. Experiental results deonstrated the benefits of the approach for large transforations, with no prior initialisation. The current paper therefore extends the CRF-Matching approach by incorporating otion detection for each laser point and allowing for direct coputation of the translation and rotation of objects (sets of laser points). The detection and tracking of oving objects (DATMO) proble has been extensively studied [2], in different scenarios, and using different sensors. Related work is discussed with an ephasis on detecting oving objects fro a oving platfor using a laser range finder. The related work is divided into two categories; algoriths that perfor detection and algoriths that perfor segentation and tracking //$26. 2 IEEE 887

2 The detection algoriths address the proble in ters of separating the data into static and dynaic clusters. Dynaic points are used for tracking of dynaic objects while the static points are used to obtain better otion estiates for the oving platfor. Hähnel et al. [9] detect oving points in range data using an Expectation-Maxiisation (EM) based approach. The algorith axiises the likelihood of the data using a hidden variable expressing the nature of the points (static or dynaic). In [26], two separate aps are aintained; one for the static part of the environent and one for the dynaic. The aps eploy a odified occupancy grid fraework which also infers if points are static or dynaic. Rodriguez-Losada and Minguez [2] iprove data association for the ICP algorith. They introduce a new etric which odels dynaic objects to better reflect the real otion of the robot. Their approach however, does not distinguish oving objects fro outliers. The second category of algoriths focuses on object segentation and tracking. Anguelov et al. [1], [4] detect oving points using siple differencing and then apply a odified EM algorith for clustering the different objects. In [2] an integrated solution to the apping and tracking proble is defined: static points are used for apping and dynaic points for tracking. Data differencing and consistency-based otion detection [24] is used for the detection and segentation of dynaic points. Points are classified as static or dynaic and clustered into segents; when a segent contains enough dynaic points is considered dynaic. Montesano et al. [16] subsequently iprove the classification procedure of [2] by jointly solving it in a sequential anner. Schulz et al. [22] use a feature based approach to detect oving objects; the features used are the local inia of the laser data. The objects are then tracked using a joint probabilistic data association filter (JPDAF). The focus for ost of these approaches is on tracking different objects under different hypothesis. The detection part, however, is ainly based on heuristics such as scan differencing. The detection routines observe the actual position of the object and the velocities are coputed by the tracking algorith. CRF-Clustering [23] on the other hand, applies a feature based graphical odel to the data. This allows it to reason about the underlying otion of points; it detects and tracks the objects in a single fraework, iteratively iproving the estiates. The work presented in [] allows different, state-ofthe-art algoriths, to be cobined in a cascaded/sequential fraework. The approach treats each algorith as a blackbox though it allows liited unidirectional interaction through feature vectors. The odel proposed in this paper on the other hand, integrates and extends the CRF-Clustering and CRF-Matching approaches into a single fraework. Motion estiates are coputed in-line with otion detection and data association, allowing for joint reasoning enhanced estiates. III. PRELIMINARIES A. Conditional Rando Fields Conditional Rando Fields [12] (CRFs) are undirected graphical odels that represent probability distributions; the vertices of the graph index rando variables while the edges of the graph capture relationships between variables. In the case of CRFs the distribution is conditional (a probabilistic discriinative odel); the hidden variables x = x 1,x 2,...,x n are globally conditioned on the observations z; p(x z). Let C be the set of all cliques of the graph. The distribution ust then factorise as a product of clique potentials φ c (x c,z), where c C and x c are the hidden variables of the clique. Clique potentials are coonly expressed by a log-linear cobination of feature functions, φ c (x c,z) = exp ( w T c f c (x c,z) ), resulting in the definition of a CRF as: p(x z) = 1 Z(z) exp ( w T c f c (x c,z) c C ). (1) Here Z(z) = x exp( c C w T c f c (x c,z)) is the partition function; it noralises the exponential ensuring Equation 1 is a proper distribution. C is again the set of all cliques in the graph. w c are paraeters (or weights). The feature functions f c extract feature vectors given the value of the clique variables x c and observations z. B. Paraeter Learning The weights w c express the relative iportance of each feature function. As such they play an iportant role in deterining the shape of the distribution. The weights are learnt by axiising the conditional likelihood (Equation 1) given labelled training data. In our case, this is coputationally intractable as the partition function Z(z) sus over the entire state space of all hidden variables. A typical graph in the experients contains on average 6 hidden variables with an approxiate total nuber of states of. We therefore use axiu pseudo-likelihood learning [3]. Maxiu pseudo-likelihood learning approxiates the joint by considering, for each hidden variable, only its neighbours in the graph: the Markov blanket. As a result, the partition function is approxiated by suing over the state space of individual hidden variables. C. Inference The odel defined in the next section is a joint distribution over data association, otion detection and otion clustering hidden variables. The desired outcoe of inference is a configuration of the hidden variables for which the joint distribution achieves its axiu; a axiu a posteriori (MAP) inference proble. The proposed algorith is a variation on Max-Product Loopy Belief Propagation. Belief Propagation (BP) [18] is a class of inference algoriths in which each node sends essages to each of its neighbours in the graph. The essages convey what a node believes its neighbours state should be given its own state. The received essages together with a node s own belief are then used to copute the MAP configuration. 888

3 Construction of the essages is perfored using the Max- Product algorith [18] as follows: i j (x i ) = ax x j ( φ(x j,z)φ(x i,x j,z) k N ( j)\i k j (x j ) ), (2) where i j (x i ) is the essage node j sends to node i. φ(x j,z) is the local clique potential for node j. Local clique potentials represent what a node believes its state is, given the observations. Coputation of the local potential can be visualised as passing a essage fro the observation to the node, see Z in the left hand side of Figure 2. φ(x i,x j,z) is the pairwise clique potential. Pairwise potentials relate nodes i and j on either end of an edge. They transfor the belief of node j into a for suitable for node i and vice versa. Lastly a product of all incoing essages is coputed, except fro the node to which the essage is being sent. In the left hand side of Figure 2, the incoing essages are represented by a/c and RT, while the outgoing essages i j is visualised as out. BP generates exact solution for graphs such as trees or polytrees. The proposed odel is neither a tree nor a polytree. We therefore use Loopy Belief Propagation (LBP) [17], an approxiation to BP. In LBP, essage propagation continuous until the difference in essages falls below soe threshold, or until a axiu nuber of iterations has been reached. Once essage propagation is finished, the axial configuration can be found by applying Equation 3 to each node in turn, ( ) x i argax x i φ(x i,z) ji (x i ) j N (i). (3) Here x i indicates the node while xi is its axial configuration. As can be seen, axiisation is perfored on a node s own belief (local potential) cobined with all incoing essages. The axial configuration for the node is then siply a state for which the cobined belief is axial. IV. JOINT MODEL FOR SCAN MATCHING AND CLUSTERING A. Model Definition As discussed in Section II, solutions for otion detection/estiation and scan atching are generally defined independently. In reality though, both scan atching and oving object detection are strongly interdependent; laser points belonging to the sae object will have siilar associations and vice versa. Following [23], otion detection is forulated as a clustering proble. The environent consists of one or ore dynaic objects (one cluster for each dynaic object) together with a cluster representing the background. Laser points are clustered based on the object they belong to (i.e. those with siilar otion patterns). Additionally, here we also include an outlier cluster for those points which can not reasonably be considered part of an object. Given the clusters for static x c 1 x c 2 x c N x RT z x a N x a 2 x a 1 Fig. 1. Graphical odel for cobined scan atching, otion clustering and otion estiation. and dynaic objects, otion estiation then deterines the rotation and translation of each object. A odel (Equation 4), is proposed to solve oving object detection, otion estiation and data association siultaneously. p(x z) = p(x a,x c,x RT z). (4) Equation 4 expresses a conditional distribution of three sets of hidden variables (x a, x c, x RT ) given an observation z. The observation consists of two laser scans for which association and otion detection/estiation is to be deterined. The variables x a consists of N association nodes, where N is the nuber of points in the first scan. Each of these nodes is discrete with M+1 states; M being the nuber of points in the second scan. The states of x a at node i have the following interpretation: The first state indicates the likelihood that point i in the first scan associates to point 1 in the second scan. The second state is the likelihood of association to the second point in the second scan, etc. Finally, the M+1 state represents the likelihood that point i is an outlier. The variables x c consists of N otion detection nodes, with D discrete states each. Here D represents the nuber of clusters a point can belong to. The interpretation is analogous to that of association. The first state represents the likelihood that point i belongs to the first object (cluster), etc. By convention, the last state D represents the cluster for outliers. The choice on nuber of clusters D ay be guided using a priori knowledge. If no such knowledge is available then D can be set to M+1, i.e. start with a cluster for each point. Once the inference algorith has clustered the points, any vacuous clusters can safely be discarded. The inference algorith will assign probability ass to each node according to the likelihood of belonging to a cluster. Clusters that have no significant probability ass for all nodes are therefore not likely, and can be reoved. For coputational reasons it is advisable to reduce the nuber of clusters. Lastly, the variable x RT represents the rotational R and translational T paraeters of each object in the syste except the outlier cluster. As such this is a ulti-diensional continuous variable. Note that the robot otion can be obtained fro the rotation and translation paraeters of the background object. 889

4 The graphical odel corresponding to Equation 4 is shown in Figure 1. The odel consists of two chains, one for otion clustering on the left, and one for data association on the right. The two chains are connected using a variable representing the otion estiates of the syste, and a variable for the observations. The otivation for using a chain for both association and clustering stes fro the way scan data is obtained. The laser scanner obtains data points sequentially and in a single plane; hence a chain to represent this acquisition. Note that any correlation in the data is not lost as there is a path fro any one point in a chain to any other point. B. Inference The inclusion of the variable x RT adds coplexity. First and foreost, rotation and translation are continuous quantities whereas the nodes for association and clustering are discrete in nature. This could result in a ixing of sus and integrals in the inference procedure; in turn leading to difficulties in obtaining a solution. However, as explained in Section III-C, we forulate the proble as a MAP inference procedure defined as: x MAP = argax p(x z), () x where x = x a,x c,x RT. Max-product inference operates on the axia of the hidden variable; it therefore does not suffer fro any ixed su integral proble - provided closed for solutions exist. The closed for solution to rotation and translation can efficiently be coputed by iniising the error function in Equation 6 [13]: R,T argin R,T N i=1 (z 1,i R+T z 2,x a i ) 2, (6) where z 1,i is the i-th point in the first scan and z 2,x a i is the point in the second scan corresponding to the i-th association. The essage passing schedule is, in our case, dictated by the structure of the graph. In our odel, the x a and x c nodes are indirectly linked through the otion estiate node, x RT. This is done for practical reasons, as it allows inference to run as two chains siultaneously influencing each other. It does require a flooding schedule [11]; each node always sends essages to all of its neighbours. This ensures any change in one chain is always propagated to the other. In addition, the schedule visits x RT as every second node in the schedule to facilitate joint reasoning, i.e. a schedule such as: x1 c,xrt,x2 c,xrt,...,xn c,xrt,x1 a,xrt,...,xn a. In order to reduce the coputational cost of sending essages to and fro x RT, the essage passing algorith is odified according to the right hand side of Figure 2. Our odified belief propagation algorith treats the value for x RT as if it was observed. Outgoing essages out contain the sae inforation as long as we ensure no inforation (belief) is lost due to this change. In order to guarantee this, local features that depend on x RT are recoputed each tie a node is visited. Any change to either rotation or translation is then incorporated into the outgoing essage out. The out a/c x i Z Z RT a/c out a/c x i Z Z,RT Fig. 2. Message passing for a single node. Left node shows standard belief propagation, right side uses proposed essage propagation observations z do not change, so any change in the essage Z,RT is solely due to changes in x RT. In order for the odified algorith to behave as if essages are being passed in both directions, the value of x RT needs to be updated inline with the essage passing schedule. This requires x RT be recoputed after each node has been visited. Algorith 1 Pseudo-code of the odified inference algorith. 1: x RT InitialiseRT() 2: for iteration = 1 to MaxIterations do 3: for node = 1 to NuNodes do 4: Z,RT CoputeLocalFeature(node,z,x RT ) : for nb = 1 to Neighbours(node) do 6: a/c CollectIncoingMessage(node,nb) 7: out ConstructMessage(node,nb, Z,RT, a/c ) 8: SendMessage(node,nb, out ) 9: end for : x RT CoputeRT() 11: end for 12: if CheckConvergence() = true then 13: break 14: end if : end for 16: x RT CoputeRT() 17: for node = 1 to NuNodes do 18: Z,RT CoputeLocalFeature(node,z,x RT ) 19: a/c CollectIncoingMessage(node) 2: x a/c CoputeState(node, Z,RT, a/c ) 21: end for The resulting algorith is shown in Algorith 1. To start, x RT is initialised on line 1 to reflect that it is observed. Belief propagation is perfored on lines 2-. Before a node propagates belief to one of its neighbours it coputes its own belief (line 4) fro the observations z and the value for x RT. For each of its neighbours Equation 2 is coputed (lines -9) and the output essage is sent to its neighbour. After sending essages to all neighbours, the crucial step of updating x RT is executed on line ; iniising Equation 6. After each iteration of the algorith convergence is checked, only if the algorith has converged can it be terinated early. Finally, lines copute the state of each node analogous to standard LBP inference (Equation 3). a/c 89

5 C. Feature Description CRFs owe uch of their popularity to the feature functions f c in Equation 1. These encapsulate doain specific knowledge which can subsequently be used in the probabilistic fraework of the CRF. For copleteness a brief description of each feature is provided below. The reader is referred to [19], [23] for ore detailed descriptions. 1) Association Local Features: A first class of association local features use geoetric properties of the data to identify patterns (shapes) around a single point, say point i, in the first scan. They then try to find the sae pattern around each point in the second scan. The resulting difference is an error etric; points in the second scan that have a siilar pattern (a sall value for the error etric) are ore likely candidates for association to point i in the first scan. The pattern finding features are: Distance: Uses the distance to its first, third or fifth neighbour. Angular: Uses the angle between the first, third or fifth neighbours on either side. Geodesic: Uses the accuulated distance to its first, third or fifth neighbour; visiting all points in between. Radius: Uses the range value. In addition to the shape based features above, two other features are used. These use the structure of the data and optionally contextual inforation. Translation: Uses the distance between a point in the first scan with every point in the second scan - analogous to ICP. Registered Translation: Transfors each point in the first scan according to its otion estiate. Then uses the distance between the transfored point and every point in the second scan. The above features are not directly used in Equation 1. They are, instead, used as inputs to boosting [7], [19]. The outputs of boosting (the experients eploy AdaBoost [6] with decision stups) are used as local feature values in Equation 1. Using boosting results in better estiates for the local potentials. There are two boosting features used for association: 1) Data Boosting coputes the likely associations and 2) Outlier Boosting deterines the likelihood that a point is an outlier. 2) Association Pairwise Features: These features fall roughly into two categories. Those that use the structure of the scan acquisition and those that use the observations. The first category of features are those that use scan acquisition structure. In an ideal world, without noisy easureents and outliers, it would be straight forward to relate the association of node i with that of node i + 1. If node i associates to point j then node i + 1 can reasonably be expected to associate to point j+ 1. Sequence: Expresses the sequential nature of association by an identity atrix with the diagonal shifted. Pairwise Outlier: Expresses how outliers ipact on association transitions - fro inlier to outlier and vice versa. The second category of pairwise features operate siilarly to the pattern finding local features. They use a easure between two points on an edge in the first scan and copare this easure with all possible cobinations of this easure in the second scan. The coparison produces a etric of how pairs of points are related between the two scans (i.e. a transition). Pairwise Distance: Uses the distance between points on either end of an edge. Pairwise Translation: Uses the vector between two points corresponding to an edge. 3) Clustering Local Features: The purpose of these features is to deterine whether points are part of the static background, part of a dynaic object or are outliers. Cluster Distance: Uses distance between two scans to cluster points based on their otion. Outlier: Uses a threshold value to indicate if the point is an outlier. Cluster Inlier Enforces inlier consistency between association and clustering. 4) Clustering Pairwise Features: With the exception of the outlier feature, all these features cluster by incorporating neighbourhood inforation. Sequence: Enforces if neighbouring points belong to the sae cluster; an identity atrix. Neighbour Weight: Captures (non-)neighbouring relationships; scaled by the distance between the points. Neighbour Stiffness: Captures (non-)neighbouring relationships; scaled by the distance between associated pairs of neighbouring points. Neighbour Distance: Expresses that points belonging to the sae cluster should preserve their relative distance after transforation. Pairwise Outlier: Expresses how outliers ipact on cluster transitions - fro inlier to outlier and vice versa. V. EXPERIMENTS We perfor experients using data collected with a laser scanner ounted on a car travelling around the University of Sydney capus [23]. 31 pairs of scans containing both static and dynaic objects were anually annotated. The 31 scan pairs can be divided into 8 subsets; each subset consisting of sequential scan pairs. A single scan contains on average about 3 points. As a result the proposed odel consists of a graph of 6 nodes (3 association + 3 clustering). The clustering nodes are initialised with states for each node while the association nodes will have on average 31 states. The proposed joint odel is copared to three techniques that copute laser point association and otion clustering as separate tasks. The first technique eploys ICP and the output associations provide translation and rotation paraeters fro K-Means clustering []. The second technique uses CRF-Matching [19] instead of ICP for association and again K-Means for clustering. Note that K-Means requires the nuber of clusters to be defined in advance. In the experients the nuber of clusters is set to 4 - the axiu 891

6 Fig. 3. A sequence of 4 scan association results with agnified dynaic object associations. Top row, left to right: ICP, CRF-Matching. Botto row, left to right: Proposed Joint Model, Ground Truth. Lines indicate MAP associations between laser points (outlier points have no connecting lines). All techniques perfor well on the background. Looking at the dynaic object, ICP is unable to associate the dynaic object. CRF-Matching does reasonably well but struggles in the encircled area. The proposed odel correctly associates points in the direction of otion. Technique Accuracy(%) SED Outlier(%) (true/ratio) ICP (11.7/89.79) CRF-Matching (11.7/7.93) Cobined Model (11.7/73.37) TABLE I ASSOCIATION METRICS FOR THE DIFFERENT TECHNIQUES. based on the ground truth. Finally, the third technique uses CRF-Matching and CRF-Clustering in turn. CRF-Clustering is essentially the clustering layer in Figure 1 while CRF- Matching is the association layer. This technique equates to reoving all the links connecting the two chains through x RT in Figure 1 - this allows for a clear coparison and shows the advantage of joint reasoning about otion detection and scan atching. Experients are conducted in a leave-one-out cross validation fashion. All CRF based techniques require learning of the weights (18/12/3 weights for CRF-Matching/CRF- Clustering/proposed odel respectively). As such, 3 scan pairs are used to training the odel and 1 scan pair is used for testing. This process is then repeated for each of the 31 scan pairs and averaging the results. This schee ensures that the results represent the odel s ability to deal with each of the scan pairs individually. ICP and K-Means do not require training, for these techniques the average over the 31 scans is coputed for fair coparison. Figure 3 shows exaples of association results for each of the different association techniques together with the association result of the proposed odel and ground truth. The figures contain 4 sequential scans, with the blue scan being the ost recently acquired scan. The dynaic object is agnified in all figures. Looking at the results it is clear that all perfor quite well on the static points. The difference is how the different techniques deal with the dynaic object. ICP does poorly, this is to be expected since ICP uses only nearest neighbour inforation for deterining associations. CRF-Matching does significantly better due to the fact that it perfors associations based on local shape inforation. It is, however, unable to correctly associate the points in the lower section (circled) of the dynaic object as the local shape of these points is siilar. The proposed odel on the other hand has clustering inforation available. This allows it to correctly associate in the direction of otion. The output of otion detection is analogous. Figure 4 shows the results for a single scan pair - for clarity. As can be seen, the results for K-Means clustering depend very uch on the quality of the associations. Using ICP for the associations, the results are inconsistent. K-Means with CRF-Matching does significantly better but is adversely affected by K-Means requiring the nuber of clusters to be deterined in advance. CRF-Clustering does better, because like CRF-Matching, CRF-Clustering is able to infer ore fro the scans. The proposed odel does exceptionally well. It correctly deterines the nuber of clusters in the data and there are very few is-classifications - only a few in the corner of the dynaic object. In addition to the visual coparison, a quantitative easure of siilarity between the association/clustering results and the ground-truth is given. Tables I and II present a perforance coparison in ters of accuracy, String Edit Distance (SED) [21] and outlier detection. The accuracy is 892

7 KMeans (ICP) Results KMeans (CRF Matching) Results CRF Clustering Results y coordinate () y coordinate () y coordinate () x coordinate () Cobined Model Results x coordinate () Ground Truth Results x coordinate () y coordinate () y coordinate () x coordinate () x coordinate () Fig. 4. Motion detection results. Top row, left to right; K-Means initialised with ICP, K-Means initialised with CRF-Matching, CRF-Clustering. Botto row, left to right: Proposed Joint Model, Ground Truth. Different colours indicate different clusters (dark green for the static background), outlier points have no coloured circle. K-Means (ICP) has very poor results ainly due the poor associations. CRF-Clustering and K-Means (CRF-Matching) have siilar results, though CRF-Clustering results are a little ore consistent. The proposed odel has near perfect result. Technique Accuracy(%) SED Outlier(%) (true/ratio) K-Means (ICP) (11.7/89.79) K-Means (CRF) (11.7/7.93) CRF-Clustering (11.7/.64) Cobined Model (11.7/73.37) TABLE II MOTION DETECTION METRICS FOR THE TECHNIQUES. Technique Rotation Error (radians) Translation Error () K-Means (ICP) K-Means (CRF).4.77 CRF-Clustering..88 Cobined Model.3.6 TABLE III MOTION ESTIMATION METRICS FOR THE TECHNIQUES. the percentage of correctly associated/clustered points 1. The SED is a easure of how any perutations are required on a string to ake it atch another. Here the two strings are the association/clustering results on the one hand and the ground-truth on the other. Therefore, the less the SED the better. Finally the percentage of outliers gives a easure of how useful the solution is. Solutions with a high percentage of outliers ay give a reasonable accuracy/sed but they are not practical. Outliers are presented as the percentage of outliers found, together with the true percentage of outliers (fro the ground-truth), and a ratio (percentage) of groundtruth outliers that were found. Table I confirs the visual results of Figure 3 for the coplete data set. The cobined odel outperfors both ICP and CRF-Matching. A note on the results of ICP. Based on the etrics provided, accuracy & SED, one ight be inclined to conclude that ICP did rather well. To a certain extent this 1 A good association accuracy etric is difficult to define; associations near to the true associations require a different penalty fro those further away. is true; the different scans were reasonably closely aligned and there are relatively few dynaic points (ratio of dynaic over static points is.14). ICP s solution however, consists (on average) of ore than a third outliers. It should have found around 11 percent of outliers. This high percentage of outliers subsequently results in poor estiates for otion detection. Metric coparison for the otion paraeters is given in Tables II and III. The coparison shows again that joint reasoning about association and otion produces better results. Table III shows average results for correctly clustered objects only. The relatively good perforance of the K-Means based techniques is priarily due to correct clustering of the background. VI. CONCLUSIONS AND FUTURE WORKS This paper presented a probabilistic odel that jointly reasons about laser point association, otion clustering and otion estiation. The experiental results have deonstrated that uch can be gained by odelling different (but dependent) probles in the sae statistical fraework. We 893

8 believe the ethods presented here are an initial step towards integrating ultiple tasks in obile robotics. In its present for, the clustering and association chain odels are (indirectly) linked using the rotation and translation node. In future these two odels will be directly connected thus foring a clustering and association lattice. The lattice ore closely represents the dependencies between the two odels. The drawback of this is an increent in the coputational coplexity. Note that a different graph structure also allows for different essage passing schedules with different perforance characteristics (see coent on perforance next). Inference in the cobined odel is slow. For laser scans with 361 points, this can take up to 4 inutes in our Matlab ipleentation - depending on convergence. Analysis has shown 3 perforance bottlenecks. First, soe of the pairwise features are poorly ipleented. This is exacerbated by the second bottleneck; the fact that a flooding schedule is used for essage passing. Lastly the cost of coputing Equation 6. There are, however, several alternatives to speed up inference. One possibility is to constrain the nuber of states in the association odel. For exaple, a particular laser point can only be associated to one of its nearest neighbours rather than to all 361 points of the other scan. This would significantly reduce the cost of searching for an association. As well as a reduction in the coputational cost of essage propagation - the size of the pairwise feature functions will be quadratically reduced. Additionally, we plan to investigate better inference algoriths, especially ones with strong convergence guarantees. In this case, Linear Prograing (LP) relaxations appear as a potential candidate. ACKNOWLEDGEMENTS This work has been supported by the Rio Tinto Centre for Mine Autoation and the ARC Centre of Excellence prograe, funded by the Australian Research Council (ARC) and the New South Wales State Governent. REFERENCES [1] D. Anguelov, R. Biswas, D. Koller, B. Liketkai, S. Sanner, and S. Thrun. Learning hierarchical object aps of non-stationary environents with obile robots. In Proc. of the Conference on Uncertainty in Artificial Intelligence (UAI), 22. [2] Y. Bar-Shalo and X.-R. Li. Multitarget-Multisensor Tracking: Principles and Techniques. YBS Publishing, 199. [3] J. Besag. Statistical analysis of non-lattice data. The Statistician, 24, 197. [4] R. Biswas, B. Liketkai, S. Sanner, and S. Thrun. Towards object apping in dynaic environents with obile robots. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systes (IROS), 22. [] M. Bosse and R. Zlot. Continuous 3d scan-atching with a spinning 2d laser. In Proc. of the IEEE International Conference on Robotics & Autoation (ICRA), 29. [6] Y. Freund and R.E. Schapire. Experients with a new boosting algorith. In Proc. of the International Conference on Machine Learning (ICML), [7] S. Friedan, D. Fox, and H. Pasula. Voronoi rando fields: Extracting the topological structure of indoor environents via place labeling. In Proc. of the International Joint Conference on Artificial Intelligence (IJCAI), 27. [8] S. Granger and X. Pennee. Multi-scale EM-ICP: A fast and robust approach for surface registration, 22. Internal research report, INRIA. [9] D. Hähnel, R. Triebel, W. Burgard, and S. Thrun. Map building with obile robots in dynaic environents. In Proc. of the IEEE International Conference on Robotics & Autoation (ICRA), 23. [] G. Heitz, S. Gould, A. Saxena, and D. Koller. Cascaded classification odels: Cobining odels for holistic scene understanding. In Advances in Neural Inforation Processing Systes (NIPS 28), 28. [11] F. Kschischang, B. J. Frey, and H. Loeliger. Factor graphs and the su-product algorith. IEEE Transactions on Inforation Theory, 47:498 19, 21. [12] J. Lafferty, A. McCallu, and F. Pereira. Conditional rando fields: Probabilistic odels for segenting and labeling sequence data. In Proc. of the International Conference on Machine Learning (ICML), 21. [13] F. Lu and E. Milios. Robot pose estiation in unknown environents by atching 2D range scans. In IEEE Coputer Vision and Pattern Recognition Conference (CVPR), [14] F. Lu and E. Milios. Robot pose estiation in unknown environents by atching 2D range scans. Journal of Intelligent and Robotic Systes, 18, [] D. J. C. MacKay. Inforation Theory, Inference, and Learning Algoriths. Cabridge University Press, 23. [16] L. Montesano, J. Minguez, and L. Montano. Modeling the static and the dynaic parts of the environent to iprove sensor-based navigation. In Proc. of the IEEE International Conference on Robotics & Autoation (ICRA), 2. [17] K. Murphy, Y. Weiss, and M. Jordan. Loopy belief propagation for approxiate inference: An epirical study. In Proc. of the Conference on Uncertainty in Artificial Intelligence (UAI), [18] J. Pearl. Probabilistic Reasoning in Intelligent Systes: Networks of Plausible Inference. Morgan Kaufann Publishers, Inc., [19] F. Raos, D. Fox, and H. Durrant-Whyte. CRF-atching: Conditional rando fields for feature-based scan atching. In Proc. of Robotics: Science and Systes, 27. [2] D. Rodriguez-Losada and J. Minguez. Iproved data association for icp-based scan atching in noisy and dynaic environents. In Proc. of the IEEE International Conference on Robotics & Autoation (ICRA), 27. [21] D. Sankoff and J. Kruskal, editors. Tie Warps, String Edits, and Macroolecules: The Theory and Practice of Sequence Coparison. Addison-Wesley, [22] D. Schulz, W. Burgard, D. Fox, and A. B Creers. Tracking ultiple oving targets with a obile robot using particle filters and statistical data association. In Proc. of the IEEE International Conference on Robotics & Autoation (ICRA), 21. [23] G. Tipaldi and F. Raos. Motion clustering and estiation with conditional rando fields. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systes (IROS), 29. [24] C. Wang and C. Thorpe. Siultaneous localization and apping with detection and tracking of oving objects. In Proc. of the IEEE International Conference on Robotics & Autoation (ICRA), 22. [2] C. Wang, C. Thorpe, M. Hebert, S. Thrun, and H. Durrant-Whyte. Siultaneous localization, apping and oving object tracking. The International Journal of Robotics Research, 26(6), June 27. [26] D. Wolf and G. Sukhate. Mobile robot siultaneous localization and apping in dynaic environents. Autonoous Robots, 19:3 6,