Although Point Clouds Registration is a very well studied problem, with many different solutions, most of the approaches in the literature aims at aligning two dense point clouds. Instead, we tackle the problem of aligning a dense point cloud with a sparse one: a problem that has to be solved, for example, to merge maps produced by different sensors, such as a vision-based sensor and laser scanner or two different laser-based sensors. The most used approach to point clouds registration, Iterative Closest Point (ICP), is also applicable to this sub-problem. We propose an improvement over the standard ICP data association policy and we called it Probabilistic Data Association. It was derived applying statistical inference techniques on a fully probabilistic model. In our proposal, each point in the source point cloud is associated with a set of points in the target point cloud; each association is then weighted so that the weights form a probability distribution. The result is an algorithm similar to ICP but more robust w.r.t. noise and outliers. While we designed our approach to deal with the problem of dense-sparse registration, it can be successfully applied also to standard point clouds registration.

M-estimators are the de-facto standard method of robust estimation in robotics. They are easily incorporated into iterative non-linear least-squares estimation and provide seamless and effective handling of outliers in data. However, every M-estimator's robust loss function has one or more tuning parameters that control the influence of different data. The choice of M-estimator and the manual tuning of these parameters is always a source of uncertainty when applying the technique to new data or a new problem. In this paper we develop the concept of self-tuning M-estimators. We first make the connection between many common M-estimators and elliptical probability distributions. This connection shows that the choice of M-estimator is an assumption that the residuals belong to a well-defined elliptical distribution. We exploit this implication in two ways. First, we develop an algorithm for tuning the M-estimator parameters during iterative optimization. Second, we show how to choose the correct M-estimator for your data by examining the likelihood of the data given the model. We fully derive these algorithms and show their behavior on a representative example of visual simultaneous localization and mapping.

Intelligent transportation systems are able to collect large volumes of high-resolution data. The amount of data collected by these systems can quickly overwhelm the ability of human analysts to draw meaningful conclusions from the data, particularly in large-scale multivehicle field trials. As advanced driver assistance systems develop, they will also be required to form a rich and high-level understanding of the world from the data they receive, including the behavior of the driver. These applications motivate the need for unsupervised tools capable of forming a high-level summary of low-level driving data. This paper presents an unsupervised method for converting naturalistic driving data into high-level behaviors. The proposed method works in two steps. In the first step, inertial data are automatically decomposed into linear segments. In the second step, the segments are assigned to high-level driving behaviors. The proposed method is computationally efficient and completely unsupervised and requires minimal preprocessing. Although the method is unsupervised, the clusters produced exhibit high-level patterns that can easily be associated with driving behaviors such as braking, turning, accelerating, and coasting. The effectiveness of the proposed algorithms is demonstrated in an offline application where the objective is to summarize inertial data into driving behaviors. The method is also demonstrated in an online application where the aim is to infer the current driving behavior using only inertial data. Both experiments were conducted using driving data collected in natural driving conditions.

Vehicle-to-vehicle (V2V) communication systems allow vehicles to share state information with one another to improve safety and efficiency of transportation networks. One of the key applications of such a system is in the prediction and avoidance of collisions between vehicles. If a method to do this is to succeed it must be robust to measurement uncertainty. The method should also be general enough that it does not rely on constraints on vehicle motion for the accuracy of its predictions. It should work for all interactions between vehicles and not just a select subset. This paper presents a method for collision probability calculation that addresses these problems.

This paper presents a statistical method for detecting anomalous driving behavior by analyzing cross-sectional profiles of the road. A profile captures the way vehicles normally traverse the road; a statistical hypothesis test determines whether the observed behavior is anomalous. Experimental results on genuine data collected by a fleet of vehicles demonstrates the potential of this new method.

This paper presents a robust, non-linear smoothing algorithm and develops the theory behind it. This algorithm is extremely robust to outliers and missing data and handles state-dependent noise. Implementing it is straightforward as it consists mainly of two sub-routines: (a) the Rauch-Tung-Striebel recursions, or Kalman smoother; and (b) a backtracking line search strategy. The computational load grows linearly with the number of data because the algorithm preserves the underlying structure of the problem. Global convergence to a local optimum is guaranteed, under mild assumptions.

Learning motion patterns in dynamic environments is a key component of any context-aware robotic system, and probabilistic mixture models provide a sound framework for mining these patterns. This paper presents an approach for learning motion models from trajectories provided by the tracking system of a moving platform. We present a learning approach in which a Linear Dynamical System (LDS) is augmented with a discrete hidden variable that has a number of states equal to the number of behaviours in the environment. As a result, a mixture of linear dynamical systems (MLDSs) capable of explaining several motion behaviours is developed. The model is learned by means of the Expectation Maximization (EM) algorithm.

This paper presents results of the deployment of vehicle safety systems developed by the Intelligent Vehicles and Safety Systems Group at the Australian Centre for Field Robotics. The technology was deployed within an active mine site in Australia undergoing standard open-cut mining operations. Data were collected from the vehicles and analysed in order to assess the overall safety and performance of the vehicle operations within the mine. Metrics calculated include distributions of vehicle paths along stretches of road for driving line analysis, and statistics around vehicle events such as overspeed and proximity to other vehicles.

An increasing number of intelligent transportation applications require robust and reliable wireless communication. To achieve the required quality of service it is necessary to implement redundancy in the critical path which includes the radio software and hardware. In a real-world application there are many things that can cause the communication between two vehicles to degrade or stop completely. This paper describes a novel technique for detecting degradation or failure of communication links by comparing the performance of the radios to a probabilistic model built using data collected in the field. The results show that this techinique can successfully detect when there is partial or complete failure to communicate due to damage to the external components such as antennas, connectors and cables.

Human drivers are endowed with an inborn ability to put themselves in the position of other drivers and reason about their behavior and intended actions. State-of-the-art driving-assistance systems, on the other hand, are generally limited to physical models and ad hoc safety rules. In order to drive safely amongst humans, autonomous vehicles need to develop an understanding of the situation in the form of a high-level description of the state of traffic participants. This paper presents a probabilistic model to estimate the state of vehicles by considering interactions between drivers immersed in traffic. The model is defined within a probabilistic filtering framework; estimation and prediction are carried out with statistical inference techniques. Memory requirements increase linearly with the number of vehicles, and thus, it is possible to scale the model to complex scenarios involving many participants. The approach is validated using real-world data collected by a group of interacting ground vehicles.

We introduce a novel approach for processing sequential data in the presence of outliers. The outlier-robust Kalman filter we propose is a discrete-time model for sequential data corrupted with non-Gaussian and heavy-tailed noise. We present efficient filtering and smoothing algorithms which are straightforward modifications of the standard Kalman filter Rauch-Tung-Striebel recursions and yet are much more robust to outliers and anomalous observations. Additionally, we present an algorithm for learning all of the parameters of our outlier-robust Kalman filter in a completely unsupervised manner. The potential of our approach is borne out in experiments with synthetic and real data.

One of the most important problems in robot localisation is the detection of previously visited places (loops). When a robot closes a loop, the association between observed features and present ones can be used to update its position. The computational cost involved in the association process makes exhaustive loop search intractable. Most of the current techniques use observations of the environment as their main features to produce loop hypotheses. In this paper, we investigate the feasibility of producing loop candidates from features of the robot trajectory. We propose a new method for selecting loop-closure candidates based on an alignment likelihood function, which measures similarity between trajectory sequences. The algorithm is validated with data gathered in the city with our experimental platform. Positive results show that the trajectory has, indeed, features that can be extracted and applied to robot localisation. The resulting loop hypotheses may be regarded, for example, as a initialisation step to aid current methods.

Human drivers are endowed with an inborn ability to put themselves in the position of other drivers and reason about their behaviors and intended actions. State-of-the-art driving assistance systems, on the other hand, are generally limited to physical models and ad-hoc safety rules. In order to drive safely amongst humans, autonomous vehicles require a high-level description of the state of traffic participants. This paper presents a probabilistic model for estimating and predicting the behavior of drivers immersed in traffic. The model is defined within a stochastic filtering framework and estimation and prediction are carried out with statistical inference techniques. The approach is validated with real data from a fleet of mining vehicles.

Over the last few years, electronic vehicle guidance systems have become increasingly more popular. However, despite their ubiquity, performance will always be subject to availability of detailed digital road maps. Most current digital maps are still inadequate for advanced applications in unstructured environments. Lack of up-to-date information and insufficient refinement of the road geometry are among the most important shortcomings. The massive use of inexpensive Global Positioning System (GPS) receivers, combined with the rapidly increasing availability of wireless communication infrastructure, suggests that large amounts of data combining both modalities will be available in the near future. The approach presented here draws on machine-learning techniques and processes logs of position traces to consistently build a detailed and fine-grained representation of the road network by extracting the principal paths followed by the vehicles. Although this work addresses the road-building problem in dynamic environments such as open-pit mines, it is also applicable to urban environments. New contributions include a fully unsupervised segmentation method for sampling roads and inferring the network topology, which is a general technique for extracting detailed information about road splits, merges, and intersections, as well as a robust algorithm that articulates these two. Experimental results with data from large mining operations are presented to validate the new algorithm.

Over the last ten years, electronic vehicle guidance systems have become increasingly popular. However, their performance is subject to the availability and accuracy of digital road maps. Most current digital maps are still inadequate for advanced applications in unstructured environments. Lack of detailed up-to-date information and insufficient accuracy and refinement of the road geometry are among the most important shortcomings. The massive use of inexpensive GPS receivers, combined with the rapidly increasing availability of wireless communication infrastructure, suggests that large volumes of data combining both modalities will be available in a near future. The approach presented here draws on machine learning techniques to process logs of position traces to consistently build a detailed and accurate representation of the road network and, more importantly, extract the actual paths followed by vehicles. Experimental results with data from large mining operations are presented to validate the algorithm.

This paper addresses the problem of safety in mining applications. It presents new metrics that can be used to determine dangerous situations during mine operation in real time. It also presents a fast and robust algorithm for extracting significant places from information logged by a state-of-the-art collision avoidance system. Determining significant places provides valuable context information in a variety of applications such as map building, vehicle tracking and user assistance. In our case, we are interested in obtaining context information as a preliminary step towards improving mining safety. The algorithm presented here is validated with experimental data obtained from a fleet of haulage vehicles operating in various open pit mines.