(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.
(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.