This article presents normalizing flows for incremental smoothing and mapping (NF-iSAM), a novel algorithm for inferring thefullposterior distribution in SLAM problems with nonlinear measurement models and non-Gaussian factors. NF-iSAM exploits the expressive power of neural networks, and trains normalizing flows to model and sample the full posterior. By leveraging the Bayes tree, NF-iSAM enables efficient incremental updates similar to iSAM2, albeit in the more challengingnon-Gaussiansetting. We demonstrate the advantages of NF-iSAM over state-of-the-art point and distribution estimation algorithms using range-only SLAM problems with data association ambiguity. NF-iSAM presents superior accuracy in describing the posterior beliefs of continuous variables (e.g., position) and discrete variables (e.g., data association).

This paper presents a novel non-Gaussian inference algorithm, Normalizing Flow iSAM (NF-iSAM), for solving SLAM problems with non-Gaussian factors and/or non-linear measurement models. NF-iSAM exploits the expressive power of neural networks, and trains normalizing flows to draw samples from the joint posterior of non-Gaussian factor graphs. By leveraging the Bayes tree, NF-iSAM is able to exploit the sparsity structure of SLAM, thus enabling efficient incremental updates similar to iSAM2, albeit in the more challenging non- Gaussian setting. We demonstrate the performance of NF-iSAM and compare it against the state-of-the-art algorithms such as iSAM2 (Gaussian) and mm-iSAM (non-Gaussian) in synthetic and real range-only SLAM datasets.

This paper presents a method for processing sparse, non-Gaussian multimodal data in a simultaneous localization and mapping (SLAM) framework using factor graphs. Our approach demonstrates the feasibility of using a sum-product inference strategy to recover functional belief marginals from highly non-Gaussian situations, relaxing the prolific unimodal Gaussian assumption. The method is more focused than conventional multi-hypothesis approaches, but still captures dominant modes via multi-modality. The proposed algorithm exists in a trade space that spans the anticipated uncertainty of measurement data, task-specific performance, sensor quality, and computational cost. This work leverages several major algorithm design constructs, including clique recycling, to put an upper bound on the allowable computational expense - a major challenge in non-parametric methods. To better demonstrate robustness, experimental results show the feasibility of the method on at least two of four major sources of non-Gaussian behavior: i) the first introduces a canonical range-only problem which is always underdetermined although composed exclusively from Gaussian measurements; ii) a real-world AUV dataset, demonstrating how ambiguous acoustic correlator measurements are directly incorporated into a non-Gaussian SLAM solution, while using dead reckon tethering to overcome short term computational requirements.

Synthetic Aperture Sonar (SAS) is a technique to improve the spatial resolution from a moving set of receivers by extending the array in time, increasing the effective array length and aperture. This technique is limited by the accuracy of the receiver position estimates, necessitating highly accurate, typically expensive aided-inertial navigation systems for submerged platforms. We leverage simultaneous localization and mapping to fuse acoustic and navigational measurements and obtain accurate pose estimates even without the benefit of absolute positioning for lengthy underwater missions. We demonstrate a method of formulating the well-known SAS problem in a SLAM framework, using acoustic data from hydrophones to simultaneously estimate platform and beacon position. An empirical probability distribution is computed from a conventional beamformer to correctly account for uncertainty in the acoustic measurements. The non-parametric method relieves the familiar Gaussian-only assumption currently used in the localization and mapping discipline and fits effectively into a factor graph formulation with conventional factors such as ground-truth priors and odometry. We present results from field experiments performed on the Charles River with an autonomous surface vehicle which demonstrate simultaneous localization of an unknown acoustic beacon and vehicle positioning, and provide comparison to GPS ground truths.

The recent success of object detection systems motivates object-based representations for robot navigation; i.e. semantic simultaneous localization and mapping (SLAM). The semantic SLAM problem can be decomposed into a discrete inference problem: determining object class labels and measurement-landmark correspondences (the data association problem), and a continuous inference problem: obtaining the set of robot poses and object locations in the environment. A solution to the semantic SLAM problem necessarily addresses this joint inference, but under ambiguous data associations this is in general a non-Gaussian inference problem, while the majority of previous work focuses on Gaussian inference. Previous solutions to data association either produce solutions between potential hypotheses or maintain multiple explicit hypotheses for each association. We propose a solution that represents hypotheses as multiple modes of an equivalent non-Gaussian sensor model. We then solve the resulting non-Gaussian inference problem using nonparametric belief propagation. We validate our approach in a simulated hallway environment under a variety of sensor noise characteristics, as well as using real data from the KITTI dataset, demonstrating improved robustness to perceptual aliasing and odometry uncertainty.

We extend the core operation of non-parametric belief propagation (NBP), also known as multi-scale sequential Gibbs sampling, to approximate products of kernel density estimated beliefs that reside on some manifold. The original algorithm, though multidimensional, implicitly assumes the beliefs to reside on the Euclidean ℝdspace only. The proposed extension generalizes to any mixture of Riemannian manifolds, provided the primary operations - addition and subtraction- are defined. Our motivation is primarily focused on state-estimation using non-Gaussian factor graphs for multimodal simultaneous localization and mapping in robotics. The paper presents the method as well as simulation and experimental results for validation. Our implementation is publicly available and allows for expansion with user-defined manifold mixtures.

Typically, the reconstruction problem is addressed in three independent steps: first, sensor processing techniques are used to filter and segment sensor data as required by the front end. Second, the front end builds the factor graph for the problem to obtain an accurate estimate of the robot's full trajectory. Finally, the end product is obtained by further processing of sensor data, now re-projected from the optimized trajectory. In this paper we present an approach to model the reconstruction problem in a way that unifies the aforementioned problems under a single framework for a particular application: sonar-based inspection of underwater structures. This is achieved by formulating both the sonar segmentation and point cloud reconstruction problems as factor graphs, in tandem with the SLAM problem. We provide experimental results using data from a ship hull inspection test.

Robotic systems typically require memory recall mechanisms for a variety of tasks including localization, mapping, planning, visualization etc. We argue for a novel memory recall framework that enables more complex inference schemas by separating the computation from its associated data. In this work we propose a shared, centralized data persistence layer that maintains an ensemble of online, situationally-aware robot states. This is realized through a queryable graph-database with an accompanying key-value store for larger data. In turn, this approach is scalable and enables a multitude of capabilities such as experience-based learning and long-term autonomy. Using multi-modal simultaneous localization and mapping and a few example use-cases, we demonstrate the versatility and extensible nature that centralized persistence and SLAMinDB can provide. In order to support the notion of life-long autonomy, we envision robots to be endowed with such a persistence model, enabling them to revisit previous experiences and improve upon their existing task-specific capabilities.

We relax parametric inference to a nonparametric representation towards more general solutions on factor graphs. We use the Bayes tree factorization to maximally exploit structure in the joint posterior thereby minimizing computation. We use kernel density estimation to represent a wider class of constraint beliefs, which naturally encapsulates multi-hypothesis and non-Gaussian inference. A variety of new uncertainty models can now be directly applied in the factor graph, and have the solver recover a potentially multi-modal posterior. For example, data association for loop closure proposals can be incorporated at inference time without further modifications to the factor graph. Our implementation of the presented algorithm is written entirely in the Julia language, exploiting high performance parallel computing. We show a larger scale use case with the well known Victoria park mapping and localization data set inferring over uncertain loop closures.

A discontinuity seems to exist in present aided inertial navigation systems - the operational availability of high cost inertial navigation currently depends on the availability of a valid GPS lock. In general GPS solutions can be two or more orders cheaper than their inertial sensors counterparts. This paper addresses the north alignment by gyrocompassing in the context of more modern smoothing techniques. Incremental smoothing is becoming a serious contender for in-situ state estimation. The work here presents a probabilistic model for north alignment with an analysis towards the achievable accuracy of such a system. A pose graph formulation is used to estimate inertial sensor calibration terms over a long spanning trajectory, from which a true north alignment can be extracted. The preintegral method is used to condense high rate inertial data, but allow for post integration bias correction. Results show that the proposed alignment scheme is feasible, and would for gyrocompassing alignment relaxed sensor performance constraints.

The paper describes the system developed by researchers from MIT for the Defense Advanced Research Projects Agency's (DARPA) Virtual Robotics Challenge (VRC), held in June 2013. The VRC was the first competition in the DARPA Robotics Challenge (DRC), a program that aims to “develop ground robotic capabilities to execute complex tasks in dangerous, degraded, human-engineered environments”. The VRC required teams to guide a model of Boston Dynamics' humanoid robot, Atlas, through driving, walking, and manipulation tasks in simulation. Team MIT's user interface, the Viewer, provided the operator with a unified representation of all available information. A 3D rendering of the robot depicted its most recently estimated body state with respect to the surrounding environment, represented by point clouds and texture-mapped meshes as sensed by on-board LIDAR and fused over time.