Person:

Dellaert, Frank

Permanent Link

https://hdl.handle.net/1853/71202

Associated Organization(s)

Organizational Unit

School of Interactive Computing

Full item page

Publication Search Results

Now showing 1 - 10 of 10

Concurrent Filtering and Smoothing: A Parallel Architecture for Real-Time Navigation and Full Smoothing

(Georgia Institute of Technology, 2014) Williams, Stephen ; Indelman, Vadim ; Kaess, Michael ; Roberts, Richard ; Leonard, John J. ; Dellaert, Frank

We present a parallelized navigation architecture that is capable of running in real-time and incorporating long-term loop closure constraints while producing the optimal Bayesian solution. This architecture splits the inference problem into a low-latency update that incorporates new measurements using just the most recent states (filter), and a high-latency update that is capable of closing long loops and smooths using all past states (smoother). This architecture employs the probabilistic graphical models of Factor Graphs, which allows the low-latency inference and high-latency inference to be viewed as sub-operations of a single optimization performed within a single graphical model. A specific factorization of the full joint density is employed that allows the different inference operations to be performed asynchronously while still recovering the optimal solution produced by a full batch optimization. Due to the real-time, asynchronous nature of this algorithm, updates to the state estimates from the high-latency smoother will naturally be delayed until the smoother calculations have completed. This architecture has been tested within a simulated aerial environment and on real data collected from an autonomous ground vehicle. In all cases, the concurrent architecture is shown to recover the full batch solution, even while updated state estimates are produced in real-time.
Information Fusion in Navigation Systems via Factor Graph Based Incremental Smoothing

(Georgia Institute of Technology, 2013-08) Indelman, Vadim ; Williams, Stephen ; Kaess, Michael ; Dellaert, Frank

This paper presents a new approach for high-rate information fusion in modern inertial navigation systems, that have a variety of sensors operating at different frequencies. Optimal information fusion corresponds to calculating the maximum a posteriori estimate over the joint probability distribution function (pdf) of all states, a computationally-expensive process in the general case. Our approach consists of two key components, which yields a flexible, high-rate, near-optimal inertial navigation system. First, the joint pdf is represented using a graphical model, the factor graph, that fully exploits the system sparsity and provides a plug and play capability that easily accommodates the addition and removal of measurement sources. Second, an efficient incremental inference algorithm over the factor graph is applied, whose performance approaches the solution that would be obtained by a computationally-expensive batch optimization at a fraction of the computational cost. To further aid high-rate performance, we introduce an equivalent IMU factor based on a recently developed technique for IMU pre-integration, drastically reducing the number of states that must be added to the system. The proposed approach is experimentally validated using real IMU and imagery data that was recorded by a ground vehicle, and a statistical performance study is conducted in a simulated aerial scenario. A comparison to conventional fixed-lag smoothing demonstrates that our method provides a considerably improved trade-off between computational complexity and performance.
Autonomous Flight in GPS-Denied Environments Using Monocular Vision and Inertial Sensors

(Georgia Institute of Technology, 2013-04) Wu, Allen D. ; Johnson, Eric N. ; Kaess, Michael ; Dellaert, Frank ; Chowdhary, Girish

A vision-aided inertial navigation system that enables autonomous flight of an aerial vehicle in GPS-denied environments is presented. Particularly, feature point information from a monocular vision sensor are used to bound the drift resulting from integrating accelerations and angular rate measurements from an Inertial Measurement Unit (IMU) forward in time. An Extended Kalman filter framework is proposed for performing the tasks of vision-based mapping and navigation separately. When GPS is available, multiple observations of a single landmark point from the vision sensor are used to estimate the point’s location in inertial space. When GPS is not available, points that have been sufficiently mapped out can be used for estimating vehicle position and attitude. Simulation and flight test results of a vehicle operating autonomously in a simplified loss-of-GPS scenario verify the presented method.
Concurrent Filtering and Smoothing

(Georgia Institute of Technology, 2012-07) Kaess, Michael ; Williams, Stephen ; Indelman, Vadim ; Roberts, Richard ; Leonard, John J. ; Dellaert, Frank

This paper presents a novel algorithm for integrating real-time filtering of navigation data with full map/trajectory smoothing. Unlike conventional mapping strategies, the result of loop closures within the smoother serve to correct the real-time navigation solution in addition to the map. This solution views filtering and smoothing as different operations applied within a single graphical model known as a Bayes tree. By maintaining all information within a single graph, the optimal linear estimate is guaranteed, while still allowing the filter and smoother to operate asynchronously. This approach has been applied to simulated aerial vehicle sensors consisting of a high-speed IMU and stereo camera. Loop closures are extracted from the vision system in an external process and incorporated into the smoother when discovered. The performance of the proposed method is shown to approach that of full batch optimization while maintaining real-time operation.
Factor Graph Based Incremental Smoothing in Inertial Navigation Systems

(Georgia Institute of Technology, 2012-07) Indelman, Vadim ; Williams, Stephen ; Kaess, Michael ; Dellaert, Frank

This paper describes a new approach for information fusion in inertial navigation systems. In contrast to the commonly used filtering techniques, the proposed approach is based on a non-linear optimization for processing incoming measurements from the inertial measurement unit (IMU) and any other available sensors into a navigation solution. A factor graph formulation is introduced that allows multi-rate, asynchronous, and possibly delayed measurements to be incorporated in a natural way. This method, based on a recently developed incremental smoother, automatically determines the number of states to recompute at each step, effectively acting as an adaptive fixed-lag smoother. This yields an efficient and general framework for information fusion, providing nearly-optimal state estimates. In particular, incoming IMU measurements can be processed in real time regardless to the size of the graph. The proposed method is demonstrated in a simulated environment using IMU, GPS and stereo vision measurements and compared to the optimal solution obtained by a full non-linear batch optimization and to a conventional extended Kalman filter (EKF).
iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree

(Georgia Institute of Technology, 2012-02) Kaess, Michael ; Johannsson, Hordur ; Roberts, Richard ; Ila, Viorela ; Leonard, John ; Dellaert, Frank

We present a novel data structure, the Bayes tree, that provides an algorithmic foundation enabling a better understanding of existing graphical model inference algorithms and their connection to sparse matrix factorization methods. Similar to a clique tree, a Bayes tree encodes a factored probability density, but unlike the clique tree it is directed and maps more naturally to the square root information matrix of the simultaneous localization and mapping (SLAM) problem. In this paper, we highlight three insights provided by our new data structure. First, the Bayes tree provides a better understanding of the matrix factorization in terms of probability densities. Second, we show how the fairly abstract updates to a matrix factorization translate to a simple editing of the Bayes tree and its conditional densities. Third, we apply the Bayes tree to obtain a completely novel algorithm for sparse nonlinear incremental optimization, named iSAM2, which achieves improvements in efficiency through incremental variable re-ordering and fluid relinearization, eliminating the need for periodic batch steps. We analyze various properties of iSAM2 in detail, and show on a range of real and simulated datasets that our algorithm compares favorably with other recent mapping algorithms in both quality and efficiency.
iSAM2: Incremental Smoothing and Mapping with Fluid Relinearization and Incremental Variable Reordering

(Georgia Institute of Technology, 2011) Kaess, Michael ; Johannsson, Hordur ; Roberts, Richard ; Ila, Viorela ; Leonard, John ; Dellaert, Frank

We present iSAM2, a fully incremental, graphbased version of incremental smoothing and mapping (iSAM). iSAM2 is based on a novel graphical model-based interpretation of incremental sparse matrix factorization methods, afforded by the recently introduced Bayes tree data structure. The original iSAM algorithm incrementally maintains the square root information matrix by applying matrix factorization updates. We analyze the matrix updates as simple editing operations on the Bayes tree and the conditional densities represented by its cliques. Based on that insight, we present a new method to incrementally change the variable ordering which has a large effect on efficiency. The efficiency and accuracy of the new method is based on fluid relinearization, the concept of selectively relinearizing variables as needed. This allows us to obtain a fully incremental algorithm without any need for periodic batch steps. We analyze the properties of the resulting algorithm in detail, and show on various real and simulated datasets that the iSAM2 algorithm compares favorably with other recent mapping algorithms in both quality and efficiency.
The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping

(Georgia Institute of Technology, 2010-12) Kaess, Michael ; Ila, Viorela ; Roberts, Richard ; Dellaert, Frank

We present a novel data structure, the Bayes tree, that provides an algorithmic foundation enabling a better understanding of existing graphical model inference algorithms and their connection to sparse matrix factorization methods. Similar to a clique tree, a Bayes tree encodes a factored probability density, but unlike the clique tree it is directed and maps more naturally to the square root information matrix of the simultaneous localization and mapping (SLAM) problem. In this paper, we highlight three insights provided by our new data structure. First, the Bayes tree provides a better understanding of batch matrix factorization in terms of probability densities. Second, we show how the fairly abstract updates to a matrix factorization translate to a simple editing of the Bayes tree and its conditional densities. Third, we apply the Bayes tree to obtain a completely novel algorithm for sparse nonlinear incremental optimization, that combines incremental updates with fluid relinearization of a reduced set of variables for efficiency, combined with fast convergence to the exact solution. We also present a novel strategy for incremental variable reordering to retain sparsity.We evaluate our algorithm on standard datasets in both landmark and pose SLAM settings.
Probabilistic Structure Matching for Visual SLAM with a Multi-Camera Rig

(Georgia Institute of Technology, 2010-02) Kaess, Michael ; Dellaert, Frank

We propose to use a multi-camera rig for simultaneous localization and mapping (SLAM), providing flexibility in sensor placement on mobile robot platforms while exploiting the stronger localization constraints provided by omni-directional sensors. In this context, we present a novel probabilistic approach to data association, that takes into account that features can also move between cameras under robot motion. Our approach circumvents the combinatorial data association problem by using an incremental expectation maximization algorithm. In the expectation step we determine a distribution over correspondences by sampling. In the maximization step, we find optimal parameters of a density over the robot motion and environment structure. By summarizing the sampling results in so-called virtual measurements, the resulting optimization simplifies to the equivalent optimization problem for known correspondences. We present results for simulated data, as well as for data obtained by a mobile robot equipped with a multi-camera rig.
The Bayes Tree: Enabling Incremental Reordering and Fluid Relinearization for Online Mapping

(Georgia Institute of Technology, 2010-01-29) Kaess, Michael ; Ila, Viorela ; Roberts, Richard ; Dellaert, Frank

In this paper we present a novel data structure, the Bayes tree, which exploits the connections between graphical model inference and sparse linear algebra. The proposed data structure provides a new perspective on an entire class of simultaneous localization and mapping (SLAM) algorithms. Similar to a junction tree, a Bayes tree encodes a factored probability density, but unlike the junction tree it is directed and maps more naturally to the square root information matrix of the SLAM problem. This makes it eminently suited to encode the sparse nature of the problem, especially in a smoothing and mapping (SAM) context. The inherent sparsity of SAM has already been exploited in the literature to produce efficient solutions in both batch and online mapping. The graphical model perspective allows us to develop a novel incremental algorithm that seamlessly incorporates reordering and relinearization. This obviates the need for expensive periodic batch operations from previous approaches, which negatively affect the performance and detract from the intended online nature of the algorithm. The new method is evaluated using simulated and real-world datasets in both landmark and pose SLAM settings.