Change detection is important for autonomous perception systems that operate in dynamic environments. Mapping and tracking components commonly handle two ends of the dynamic spectrum: stationarity and rapid motion. This paper presents a fast algorithm for 3D change detection from LIDAR or equivalent optical range sensors, that can operate from arbitrary viewpoints and can detect fast and slow dynamics. Distinct from prior work, the method explicitly detects changes in the world, and suppresses apparent changes in the data due to exploration at frontiers or behind occlusions. Comprehensive experimentation is performed to assess the performance in several application domains. Sample data and source code are provided.

This paper presents a method for pairwise 3D alignment which solves data association by matching scan segments across scans. Generating accurate segment associations allows to run a modified version of the Iterative Closest Point (ICP) algorithm where the search for point-to-point correspondences is constrained to associated segments. The novelty of the proposed approach is in the segment matching process which takes into account the proximity of segments, their shape, and the consistency of their relative locations in each scan. Scan segmentation is here assumed to be given (recent studies provide various alternatives [10], [19]). The method is tested on seven sequences of Velodyne scans acquired in urban environments. Unlike various other standard versions of ICP, which fail to recover correct alignment when the displacement between scans increases, the proposed method is shown to be robust to displacements of several meters. In addition, it is shown to lead to savings in computational times which are potentially critical in real-time applications.

This paper presents a distributed algorithm for performing joint localisation of a team of robots. The mobile robots have heterogeneous sensing capabilities, with some having high quality inertial and exteroceptive sensing, while others have only low quality sensing or none at all. By sharing information, a combined estimate of all robot poses is obtained. Inter-robot range-bearing measurements provide the mechanism for transferring pose information from well-localised vehicles to those less capable. In our proposed formulation, high frequency egocentric data (e.g., odometry, IMU, GPS) is fused locally on each platform. This is the distributed part of the algorithm. Inter-robot measurements, and accompanying state estimates, are communicated to a central server, which generates an optimal minimum mean-squared estimate of all robot poses. This server is easily duplicated for full redundant decentralisation. Communication and computation are efficient due to the sparseness properties of the information-form Gaussian representation. A team of three indoor mobile robots equipped with lasers, odometry and inertial sensing provides experimental verification of the algorithms effectiveness in combining location information.

This paper presents a probabilistic framework for road geometry estimation using a millimetre wave radar. It aims at estimating the geometry of roads without assuming any particular infrastructure such as lane marks. It provides also the vehicle location with respect to the edges of the road. This system employs a radar sensor in view of its robustness to weather conditions such as fog, dust, rain and snow. The proposed approach is robust to noisy measurements since the radar target locations are modelled as Gaussian distributions. These observations are integrated into a Kalman Particle filter to estimate the posterior distribution of the parameters that best describe the geometry of the road. Experimental results using data acquired on a highway road are presented. The effectiveness of the proposed approach is demonstrated by a qualitative analysis of the results.

This paper presents a new technique for sparsification of the information matrix of a multi-dimensional Gaussian distribution. We call this technique Conservative Sparsification (CS) and show that it produces estimates which are consistent with respect to an optimal filter. This technique was applied to the Simultaneous Localisation and Mapping (SLAM) problem, and compared with two existing sparsification approaches; the Sparse Extended Information Filter (SEIF) and the Data Discarding Sparse Extended Information Filter (DDSEIF). Simulation demonstrates that CS is a consistent approach and provides a tighter upper bound than existing conservative methods.

Novel lazy Lauritzen-Spiegelhalter (LS), lazy Hugin and lazy Shafer-Shenoy (SS) algorithms are devised for Gaussian Bayesian networks (BNs). In the lazy algorithms, the clique potentials and separator potentials are kept in combinable decomposed form instead of combined to be a single valuation in conventional junction tree algorithms. By employing decomposed form potentials, the independence relations between variables are explored online and the directed graph information is utilized in the message calculations. In the proposed algorithms, a consistent junction tree with the evidence entered can be obtained by a single round of message passing. The moments form parametrization of Gaussian distributions allows the deterministic relationships between variables. Preliminary analysis shows that the lazy LS algorithm and the lazy Hugin algorithm are more computationally efficient than the lazy SS algorithm, especially when there are multiple items of evidence to be incorporated.

This paper develops a general theory of validation gating for non-linear non-Gaussian models. Validation gates are used in target tracking to cull very unlikely measurement-to-track associations, before remaining association ambiguities are handled by a more comprehensive (and expensive) data association scheme. The essential property of a gate is to accept a high percentage of correct associations, thus maximising track accuracy, but provide a sufficiently tight bound to minimise the number of ambiguous associations. For linear Gaussian systems, the ellipsoidal validation gate is standard, and possesses the statistical property whereby a given threshold will accept a certain percentage of true associations. This property does not hold for non-linear non-Gaussian models. As a system departs from linear-Gaussian, the ellipsoid gate tends to reject a higher than expected proportion of correct associations and permit an excess of false ones. In this paper, the concept of the ellipsoidal gate is extended to permit correct statistics for the non-linear non-Gaussian case. The new gate is demonstrated by a bearing-only tracking example

This paper presents an analysis of FastSLAM - a Rao-Blackwellised particle filter formulation of simultaneous localisation and mapping. It shows that the algorithm degenerates with time, regardless of the number of particles used or the density of landmarks within the environment, and would always produce optimistic estimates of uncertainty in the long-term. In essence, FastSLAM behaves like a non-optimal local search algorithm; in the short-term it may produce consistent uncertainty estimates but, in the long-term, it is unable to adequately explore the state-space to be a reasonable Bayesian estimator. However, the number of particles and landmarks does affect the accuracy of the estimated mean and, given sufficient particles, FastSLAM can produce good non-stochastic estimates in practice. FastSLAM also has several practical advantages, particularly with regard to data association, and would probably work well in combination with other versions of stochastic SLAM, such as EKF-based SLAM

This paper presents an analysis of the extended Kalman filter formulation of simultaneous localisation and mapping (EKF-SLAM). We show that the algorithm produces very optimistic estimates once the "true" uncertainty in vehicle heading exceeds a limit. This failure is subtle and cannot, in general, be detected without ground-truth, although a very inconsistent filter may exhibit observable symptoms, such as disproportionately large jumps in the vehicle pose update. Conventional solutions - adding stabilising noise, using an iterated EKF or unscented filter, etc., - do not improve the situation. However, if "small" heading uncertainty is maintained, EKF-SLAM exhibits consistent behaviour over an extended time-period. Although the uncertainty estimate slowly becomes optimistic, inconsistency can be mitigated indefinitely by applying tactics such as batch updates or stabilising noise. The manageable degradation of small heading variance SLAM indicates the efficacy of submap methods for large-scale maps

Although full 3D navigation and mapping is recognized as one of the most important challenges for autonomous navigation, the lack of robust sensors, providing 3D information in real time, has burdened the progress in this direction. This paper presents our ongoing work towards the deployment of an integrated sensing system for 3D mapping in outdoor environments. We first describe a 3D data acquisition architecture based on a standard 2D laser. Techniques for registering scans using a scan matching procedure and for estimating the errors are then introduced. We finally present results showing the performance of the proposed architecture in real outdoor environments by means of the integration of the 3D scans with dead reckoning and inertial measurement unit (IMU) information

This paper presents a decentralised particle filtering algorithm that enables multiple vehicles to jointly track 3D features under limited communication bandwidth. This algorithm, applied within a decentralised data fusion (DDF) framework, deals with correlated estimation errors due to common past information when fusing two discrete particle sets. Our solution is to transform the particles into Gaussian mixture models (GMMs) for communication and fusion. Not only can decentralised fusion be approximated by GMMs, but this representation also provides summaries of the particle set. Less bandwidth per communication step is required to communicate a GMM than the particle set itself hence conversion to GMMs for communication is an advantage. Real airborne data is used to demonstrate the accuracy of our decentralised particle filtering algorithm for airborne tracking and mapping

Simultaneous Localisation And Mapping (SLAM) is a stochastic map building method which permits consistent robot navigation without requiring an a priori map. The map is built incrementally as the robot observes the environment with its on-board sensors and, at the same time, is used to localise the robot. Typically, SLAM has been performed using range-bearing sensors, but the development of a SLAM implementation using only bearing measurements is desirable as it permits the use of sensors such as CCD cameras, which are small, reliable and cheap. However, bearing-only SLAM is hindered by the feature initialisation problem, where the estimated location of a new map landmark cannot be determined from a single measurement, and combined information from multiple measurements may be ill-conditioned. This paper presents a solution to the feature initialisation problem called constrained initialisation, which defers the use of sensor information until initialisation becomes well-conditioned. Measurements may be used out-of-sequence and all the available information can be incorporated without inconsistency. Furthermore, this method operates within the conventional extended Kalman filter (EKF) framework of the SLAM algorithm.

E-intelligence for integrated product design, manufacturing and service is becoming the core of manufacturing technology. This paper presents the current efforts at the McMaster Manufacturing Research Institute (MMRI) to implement an Internet-based computer aided engineering (CAE) facility for the development of new manufacturing processes. The facility, termed The Centre for Online Manufacturing Optimization (COMO), integrates advanced process simulation software, a remote machine monitoring system, and state of the art multimedia and Internet technologies, to realize a virtual manufacturing process optimization environment. This environment leads the manufacturing engineer through a set of procedures in which physics based process simulations allow process parameters and operational sequences to be evaluated and optimized with respect to desired product features. Once the process parameters are established, the part can be prototyped at the MMRI laboratory or the industrial facility. The Internet-based computer engineering facility will provide companies with predictive intelligence tools for process development, and production monitoring. A case study is presented.

Data association is the process of relating features observed in the environment to features viewed previously or to features in a map. This paper presents a graph theoretic method that is applicable to data association problems where the features are observed via a batch process. Batch observations detect a set of features simultaneously or with sufficiently small temporal difference that, with motion compensation, the features can be represented with precise relative coordinates. This data association method is described in the context of two possible navigation applications: metric map building with simultaneous localisation, and topological map based localisation. Experimental results are presented using an indoor mobile robot with a 2D scanning laser sensor. Given two scans from different unknown locations, the features common to both scans are mapped to each other and the relative change in pose (position and orientation) of the vehicle between the two scans is obtained.

The theoretical basis of the solution to the simultaneous localisation and map building (SLAM) problem where an autonomous vehicle starts in an unknown location in an unknown environment and then incrementally build a map of landmarks present in this environment while simultaneously using this map to compute absolute vehicle location is well understood. Although a number of SLAM implementations have appeared in the literature, the need to maintain the knowledge of the relative relationships between all the landmark location estimates contained in the map makes SLAM computationally intractable in implementations containing more than few tens of landmarks. In this paper, the theoretical basis and a practical implementation of a computationally efficient solution to SLAM is presented. The paper shows that it is indeed possible to remove a large percentage of the landmarks from the map without making the map-building process statistically inconsistent. Furthermore, it is shown that the efficiency of the SLAM can be maintained by judicious selection of landmarks, to be preserved in the map, based on their information content.