John J. Leonard

ICRA Conference 2025 Conference Paper

Computational Teaching for Driving via Multi-Task Imitation Learning

• Deepak E. Gopinath
• Xiongyi Cui
• Jonathan A. DeCastro
• Emily Sumner
• Jean Costa
• Hiroshi Yasuda
• Allison Morgan
• Laporsha Dees

Learning motor skills for sports or performance driving is often done with professional instruction from expert human teachers, whose availability is limited. Our goal is to enable automated teaching via a learned model that interacts with the student similar to a human teacher. However, training such automated teaching systems is limited by the availability of highquality annotated datasets of expert teacher and student interactions as they are difficult to collect at scale. To address this data scarcity problem, we propose an approach for training a coaching system for complex motor tasks such as high performance driving via a Multi-Task Imitation Learning (MTIL) paradigm. MTIL allows our model to learn robust representations by utilizing self-supervised training signals from more readily available non-interactive datasets of humans performing the task of interest. We validate our approach with (1) a semi-synthetic dataset created from real human driving trajectories, (2) a professional track driving instruction dataset, (3) a track-racing driving simulator human-subject study, and (4) a system demonstration on an instrumented car at a race track. Our experiments show that the right set of auxiliary machine learning tasks improves prediction of teaching instructions. Moreover, in the human subjects study, students exposed to the instructions from our teaching system improve their ability to stay within track limits, and show favorable perception of the model's interaction with them, in terms of usefulness and satisfaction.

ICRA Conference 2025 Conference Paper

Open-Set Semantic Uncertainty Aware Metric-Semantic Graph Matching

• Kurran Singh
• John J. Leonard

Underwater object-level mapping requires incorporating visual foundation models to handle the uncommon and often previously unseen object classes encountered in marine scenarios. In this work, a metric of semantic uncertainty for open-set object detections produced by visual foundation models is calculated and then incorporated into an object-level uncertainty tracking framework. Object-level uncertainties and geometric relationships between objects are used to enable robust object-level loop closure detection for unknown object classes. The above loop closure detection problem is formulated as a graph matching problem. While graph matching, in general, is NP-Complete, a solver for an equivalent formulation of the proposed graph matching problem as a graph editing problem is tested on multiple challenging underwater scenes. Results for this solver as well as three other solvers demonstrate that the proposed methods are feasible for real-time use in marine environments for the robust, open-set, multi-object, semantic-uncertainty-aware loop closure detection. Further experimental results on the KITTI dataset demonstrate that the method generalizes to large-scale terrestrial scenes.

ICRA Conference 2025 Conference Paper

SeaSplat: Representing Underwater Scenes with 3D Gaussian Splatting and a Physically Grounded Image Formation Model

• Daniel Yang
• John J. Leonard
• Yogesh A. Girdhar

We introduce SeaSplat, a method to enable real-time rendering of underwater scenes leveraging recent advances in 3D radiance fields. Underwater scenes are challenging visual environments, as rendering through a medium such as water introduces both range and color dependent effects on image capture. We constrain 3D Gaussian Splatting (3DGS), a recent advance in radiance fields enabling rapid training and real-time rendering of full 3D scenes, with a physically grounded under-water image formation model. Applying SeaSplat to the real-world scenes from SeaThru-NeRF dataset, a scene collected by an underwater vehicle in the US Virgin Islands, and simulation-degraded real-world scenes, not only do we see increased quantitative performance on rendering novel viewpoints from the scene with the medium present, but are also able to recover the underlying true color of the scene and restore renders to be without the presence of the intervening medium. We show that the underwater image formation helps learn scene structure, with better depth maps, as well as show that our improvements maintain the significant computational improvements afforded by leveraging a 3D Gaussian representation. Code, data, and visualizations are available at https://seasplat.github.io

IROS Conference 2025 Conference Paper

Semantic Enhancement for Object SLAM with Heterogeneous Multimodal Large Language Model Agents

• Jungseok Hong
• Ran Choi
• John J. Leonard

Object Simultaneous Localization and Mapping (SLAM) systems struggle to correctly associate semantically similar objects in close proximity, especially in cluttered indoor environments and when scenes change. We present Semantic Enhancement for Object SLAM (SEO-SLAM), a novel framework that enhances semantic mapping by integrating heterogeneous multimodal large language model (MLLM) agents. Our method enables scene adaptation while maintaining a semantically rich map. To improve computational efficiency, we propose an asynchronous processing scheme that significantly reduces the agents’ inference time without compromising semantic accuracy or SLAM performance. Additionally, we introduce a multi-data association strategy using a cost matrix that combines semantic and Mahalanobis distances, formulating the problem as a Linear Assignment Problem (LAP) to alleviate perceptual aliasing. Experimental results demonstrate that SEO-SLAM consistently achieves higher semantic accuracy and reduces false positives compared to baselines, while our asynchronous MLLM agents significantly improve processing efficiency over synchronous setups. We also demonstrate that SEO-SLAM has the potential to improve downstream tasks such as robotic assistance. Our dataset is publicly available at: jungseokhong.com/SEO-SLAM.

IROS Conference 2024 Conference Paper

Opti-Acoustic Semantic SLAM with Unknown Objects in Underwater Environments

• Kurran Singh
• Jungseok Hong
• Nicholas R. Rypkema
• John J. Leonard

Despite recent advances in semantic Simultaneous Localization and Mapping (SLAM) for terrestrial and aerial applications, underwater semantic SLAM remains an open and largely unaddressed research problem due to the unique sensing modalities and the object classes found underwater. This paper presents an object-based semantic SLAM method for underwater environments that can identify, localize, classify, and map a wide variety of marine objects without a priori knowledge of the object classes present in the scene. The method performs unsupervised object segmentation and object-level feature aggregation, and then uses opti-acoustic sensor fusion for object localization. Probabilistic data association is used to determine observation to landmark correspondences. Given such correspondences, the method then jointly optimizes landmark and vehicle position estimates. Indoor and outdoor underwater datasets with a wide variety of objects and challenging acoustic and lighting conditions are collected for evaluation and made publicly available. Quantitative and qualitative results show the proposed method achieves reduced trajectory error compared to baseline methods, and is able to obtain comparable map accuracy to a baseline closed-set method that requires hand-labeled data of all objects in the scene.

ICRA Conference 2023 Conference Paper

Data-Association-Free Landmark-based SLAM

• Yihao Zhang 0003
• Odin Severinsen
• John J. Leonard
• Luca Carlone
• Kasra Khosoussi

We study landmark-based SLAM with unknown data association: our robot navigates in a completely unknown environment and has to simultaneously reason over its own trajectory, the positions of an unknown number of landmarks in the environment, and potential data associations between measurements and landmarks. This setup is interesting since: (i) it arises when recovering from data association failures or from SLAM with information-poor sensors, (ii) it sheds light on fundamental limits (and hardness) of landmark-based SLAM problems irrespective of the front-end data association method, and (iii) it generalizes existing approaches where data association is assumed to be known or partially known. We approach the problem by splitting it into an inner problem of estimating the trajectory, landmark positions and data associations and an outer problem of estimating the number of landmarks. Our approach creates useful and novel connections with existing techniques from discrete-continuous optimization (e. g. , k-means clustering), which has the potential to trigger novel research. We demonstrate the proposed approaches in extensive simulations and on real datasets and show that the proposed techniques outperform typical data association baselines and are even competitive against an “oracle” baseline which has access to the number of landmarks and an initial guess for each landmark.

IROS Conference 2023 Conference Paper

GAPSLAM: Blending Gaussian Approximation and Particle Filters for Real-Time Non-Gaussian SLAM

• Qiangqiang Huang
• John J. Leonard

Inferring the posterior distribution in SLAM is critical for evaluating the uncertainty in localization and mapping, as well as supporting subsequent planning tasks aiming to reduce uncertainty for safe navigation. However, real-time full posterior inference techniques, such as Gaussian approximation and particle filters, either lack expressiveness for representing non-Gaussian posteriors or suffer from performance degeneracy when estimating high-dimensional posteriors. Inspired by the complementary strengths of Gaussian approximation and particle filters-scalability and non-Gaussian estimation, respectively-we blend these two approaches to infer marginal posteriors in SLAM. Specifically, Gaussian approximation provides robot pose distributions on which particle filters are conditioned to sample landmark marginals. In return, the maximum a posteriori point among these samples can be used to reset linearization points in the nonlinear optimization solver of the Gaussian approximation, facilitating the pursuit of global optima. We demonstrate the scalability, generalizability, and accuracy of our algorithm for real-time full posterior inference on realworld range-only SLAM and object-based bearing-only SLAM datasets.

IROS Conference 2023 Conference Paper

NeRF-SLAM: Real-Time Dense Monocular SLAM with Neural Radiance Fields

• Antoni Rosinol
• John J. Leonard
• Luca Carlone

We propose a novel geometric and photometric 3D mapping pipeline for accurate and real-time scene reconstruction from casually taken monocular images. To achieve this, we leverage recent advances in dense monocular SLAM and real-time hierarchical volumetric neural radiance fields. Our insight is that dense monocular SLAM provides the right information to fit a neural radiance field of the scene in real-time, by providing accurate pose estimates and depth-maps with associated uncertainty. Our proposed pipeline achieves better geometric and photometric accuracy than competing approaches (up to 178% better PSNR and 75% better L1 depth), while working in real-time and using only monocular images.

ICRA Conference 2023 Conference Paper

SCORE: A Second-Order Conic Initialization for Range-Aided SLAM

• Alan Papalia
• Joseph Morales
• Kevin J. Doherty 0001
• David M. Rosen
• John J. Leonard

We present a novel initialization technique for the range-aided simultaneous localization and mapping (RA-SLAM) problem. In RA-SLAM we consider measurements of point-to-point distances in addition to measurements of rigid transformations to landmark or pose variables. Standard formulations of RA-SLAM approach the problem as non-convex optimization, which requires a good initialization to obtain quality results. The initialization technique proposed here relaxes the RA-SLAM problem to a convex problem which is then solved to determine an initialization for the original, non-convex problem. The relaxation is a second-order cone program (SOCP), which is derived from a quadratically constrained quadratic program (QCQP) formulation of the RA-SLAM problem. As a SOCP, the method is highly scalable. We name this relaxation Second-order COnic RElaxation for RA-SLAM (SCORE). To our knowledge, this work represents the first convex relaxation for RA-SLAM. We present real-world and simulated experiments which show SCORE initialization permits the efficient recovery of quality solutions for a variety of challenging single- and multi-robot RA-SLAM problems with thousands of poses and range measurements.

ICRA Conference 2022 Conference Paper

HYPER: Learned Hybrid Trajectory Prediction via Factored Inference and Adaptive Sampling

• Xin Huang 0018
• Guy Rosman
• Igor Gilitschenski
• Ashkan Jasour
• Stephen G. McGill
• John J. Leonard
• Brian Williams 0001

Modeling multi-modal high-level intent is important for ensuring diversity in trajectory prediction. Existing approaches explore the discrete nature of human intent before predicting continuous trajectories, to improve accuracy and support explainability. However, these approaches often assume the intent to remain fixed over the prediction horizon, which is problematic in practice, especially over longer horizons. To overcome this limitation, we introduce HYPER, a general and expressive hybrid prediction framework that models evolving human intent. By modeling traffic agents as a hybrid discrete-continuous system, our approach is capable of predicting discrete intent changes over time. We learn the probabilistic hybrid model via a maximum likelihood estimation problem and leverage neural proposal distributions to sample adaptively from the exponentially growing discrete space. The overall approach affords a better trade-off between accuracy and coverage. We train and validate our model on the Argoverse dataset, and demonstrate its effectiveness through comprehensive ablation studies and comparisons with state-of-the-art models.

ICRA Conference 2022 Conference Paper

Performance Guarantees for Spectral Initialization in Rotation Averaging and Pose-Graph SLAM

• Kevin J. Doherty 0001
• David M. Rosen
• John J. Leonard

In this work we present the first initialization methods equipped with explicit performance guarantees that are adapted to the pose-graph simultaneous localization and mapping (SLAM) and rotation averaging (RA) problems. SLAM and rotation averaging are typically formalized as large-scale nonconvex point estimation problems, with many bad local minima that can entrap the smooth optimization methods typically applied to solve them; the performance of standard SLAM and RA algorithms thus crucially depends upon the quality of the estimates used to initialize this local search. While many initialization methods for SLAM and RA have appeared in the literature, these are typically obtained as purely heuristic approximations, making it difficult to determine whether (or under what circumstances) these techniques can be reliably deployed. In contrast, in this work we study the problem of initialization through the lens of spectral relaxation. Specifically, we derive a simple spectral relaxation of SLAM and RA, the form of which enables us to exploit classical linear-algebraic techniques (eigenvector perturbation bounds) to control the distance from our spectral estimate to both the (unknown) ground-truth and the global minimizer of the estimation prob lem as a function of measurement noise. Our results reveal the critical role that spectral graph-theoretic properties of the measurement network play in controlling estimation accuracy; moreover, as a by-product of our analysis we obtain new bounds on the estimation error for the maximum likelihood estimators in SLAM and RA, which are likely to be of independent interest. Finally, we show experimentally that our spectral estimator is very effective in practice, producing initializations of comparable or superior quality at lower computational cost compared to existing state-of-the-art techniques.

ICRA Conference 2022 Conference Paper

Prioritized Planning for Cooperative Range-Only Localization in Multi-Robot Networks

• Alan Papalia
• Nicole Thumma
• John J. Leonard

We present a novel path-planning algorithm to reduce localization error for a network of robots cooperatively localizing via inter-robot range measurements. The quality of localization with range measurements depends on the configuration of the network, and poor configurations can cause substantial localization errors. To reduce the effect of network configuration on localization error for moving networks we consider various optimality measures of the Fisher information matrix (FIM), which have well-known relationships with localization error. We pose a trajectory planning problem with constraints on the FIM optimality measures. By constraining these optimality measures we can control the statistical properties of the localization error. To efficiently generate trajectories which satisfy these FIM constraints we present a prioritized planner which leverages graph-based planning and properties of the range-only FIM. We demonstrate in simulations that the trajectories generated by our algorithm reduce worst-case localization error by up to 42% in comparison to existing planners and can scalably plan distance-efficient trajectories in complicated environments for large numbers of robots.

IROS Conference 2022 Conference Paper

Robust Change Detection Based on Neural Descriptor Fields

• Jiahui Fu 0002
• Yilun Du
• Kurran Singh
• Joshua B. Tenenbaum
• John J. Leonard

The ability to reason about changes in the environment is crucial for robots operating over extended periods of time. Agents are expected to capture changes during operation so that actions can be followed to ensure a smooth progression of the working session. However, varying viewing angles and accumulated localization errors make it easy for robots to falsely detect changes in the surrounding world due to low observation overlap and drifted object associations. In this paper, based on the recently proposed category-level Neural Descriptor Fields (NDFs), we develop an object-level online change detection approach that is robust to partially overlapping observations and noisy localization results. Utilizing the shape completion capability and SE(3)-equivariance of NDFs, we represent objects with compact shape codes encoding full object shapes from partial observations. The objects are then organized in a spatial tree structure based on object centers recovered from NDFs for fast queries of object neighborhoods. By associating objects via shape code similarity and comparing local object-neighbor spatial layout, our proposed approach demonstrates robustness to low observation overlap and localization noises. We conduct experiments on both synthetic and real-world sequences and achieve improved change detection results compared to multiple baseline methods. Project web-page: ?http://yilundu.github.io/ndf_change

IROS Conference 2022 Conference Paper

SLAM-Supported Self-Training for 6D Object Pose Estimation

• Ziqi Lu
• Yihao Zhang 0003
• Kevin J. Doherty 0001
• Odin Severinsen
• Ethan Yang
• John J. Leonard

Recent progress in object pose prediction provides a promising path for robots to build object-level scene representations during navigation. However, as we deploy a robot in novel environments, the out-of-distribution data can degrade the prediction performance. To mitigate the domain gap, we can potentially perform self-training in the target domain, using predictions on robot-captured images as pseudo labels to fine-tune the object pose estimator. Unfortunately, the pose predictions are typically outlier-corrupted, and it is hard to quantify their uncertainties, which can result in low-quality pseudo-labeled data. To address the problem, we propose a SLAM-supported self-training method, leveraging robot understanding of the 3D scene geometry to enhance the object pose inference performance. Combining the pose predictions with robot odometry, we formulate and solve pose graph optimization to refine the object pose estimates and make pseudo labels more consistent across frames. We incorporate the pose prediction covariances as variables into the optimization to automatically model their uncertainties. This automatic covariance tuning (ACT) process can fit 6D pose prediction noise at the component level, leading to higher-quality pseudo training data. We test our method with the deep object pose estimator (DOPE) on the YCB video dataset and in real robot experiments. It achieves respectively 34. 3% and 17. 8% accuracy enhancements in pose prediction on the two tests. Our code is available at https://github.com/520xyxyzq/slam-super-6d.

IROS Conference 2022 Conference Paper

Spectral Measurement Sparsification for Pose-Graph SLAM

• Kevin J. Doherty 0001
• David M. Rosen
• John J. Leonard

Simultaneous localization and mapping (SLAM) is a critical capability in autonomous navigation, but in order to scale SLAM to the setting of “lifelong” SLAM, particularly under memory or computation constraints, a robot must be able to determine what information should be retained and what can safely be forgotten. In graph-based SLAM, the number of edges (measurements) in a pose graph determines both the memory requirements of storing a robot's observations and the computational expense of algorithms deployed for performing state estimation using those observations; both of which can grow unbounded during long-term navigation. To address this, we propose a spectral approach for pose graph sparsification which maximizes the algebraic connectivity of the sparsified measurement graphs, a key quantity which has been shown to control the estimation error of pose graph SLAM solutions. Our algorithm, MAC (for maximizing algebraic connectivity), which is based on convex relaxation, is simple and computationally inexpensive, and admits formal post hoc performance guarantees on the quality of the solutions it provides. In experiments on benchmark pose-graph SLAM datasets, we show that our approach quickly produces high-quality sparsification results which retain the connectivity of the graph and, in turn, the quality of corresponding SLAM solutions, as compared to a baseline approach which does not consider graph connectivity.

IROS Conference 2022 Conference Paper

TIP: Task-Informed Motion Prediction for Intelligent Vehicles

• Xin Huang 0018
• Guy Rosman
• Ashkan Jasour
• Stephen G. McGill
• John J. Leonard
• Brian Williams 0001

When predicting trajectories of road agents, motion predictors often approximate the future distribution by a limited number of samples. This constraint requires the predictors to generate samples that best support the task given task specifications. However, existing predictors are often optimized and evaluated via task-agnostic measures without accounting for the use of predictions in downstream tasks, and thus could result in sub-optimal task performance. In this paper, we propose a task-informed motion prediction model that better supports the tasks through its predictions by jointly reasoning about prediction accuracy and the utility of the downstream tasks during training. The task utility function is commonly used to evaluate task performance. It does not require the full task information, but rather a specification of the utility of the task, resulting in predictors that are tailored to different downstream tasks. We demonstrate our approach on two use cases of common decision making tasks and their utility functions, in the context of autonomous driving and parallel autonomy. Experiment results show that our predictor produces accurate predictions that improve the task performance by a large margin in both tasks when compared to task-agnostic baselines on the Waymo Open Motion dataset.

ICRA Conference 2022 Conference Paper

Trajectory Prediction with Linguistic Representations

• Yen-Ling Kuo
• Xin Huang 0018
• Andrei Barbu
• Stephen G. McGill
• Boris Katz
• John J. Leonard
• Guy Rosman

Language allows humans to build mental models that interpret what is happening around them resulting in more accurate long-term predictions. We present a novel trajectory prediction model that uses linguistic intermediate representations to forecast trajectories, and is trained using trajectory samples with partially-annotated captions. The model learns the meaning of each of the words without direct per-word supervision. At inference time, it generates a linguistic description of trajectories which captures maneuvers and interactions over an extended time interval. This generated description is used to refine predictions of the trajectories of multiple agents. We train and validate our model on the Argoverse dataset, and demonstrate improved accuracy results in trajectory prediction. In addition, our model is more interpretable: it presents part of its reasoning in plain language as captions, which can aid model development and can aid in building confidence in the model before deploying it.

ICRA Conference 2021 Conference Paper

A Front-End for Dense Monocular SLAM using a Learned Outlier Mask Prior

• Yihao Zhang 0003
• John J. Leonard

Recent achievements in depth prediction from a single RGB image have powered the new research area of combining convolutional neural networks (CNNs) with classical simultaneous localization and mapping (SLAM) algorithms. The depth prediction from a CNN provides a reasonable initial point in the optimization process in the traditional SLAM algorithms, while the SLAM algorithms further improve the CNN prediction online. However, most of the current CNN-SLAM approaches have only taken advantage of the depth prediction but not yet other products from a CNN. In this work, we explore the use of the outlier mask, a by-product from unsupervised learning of depth from video, as a prior in a classical probability model for depth estimate fusion to step up the outlier-resistant tracking performance of a SLAM front-end. On the other hand, some of the previous CNN-SLAM work builds on feature-based sparse SLAM methods, wasting the per-pixel dense prediction from a CNN. In contrast to these sparse methods, we devise a dense CNN-assisted SLAM frontend that is implementable with TensorFlow and evaluate it on both indoor and outdoor datasets.

IROS Conference 2021 Conference Paper

A Multi-Hypothesis Approach to Pose Ambiguity in Object-Based SLAM

• Jiahui Fu 0002
• Qiangqiang Huang
• Kevin J. Doherty 0001
• Yue Wang
• John J. Leonard

In object-based Simultaneous Localization and Mapping (SLAM), 6D object poses offer a compact representation of landmark geometry useful for downstream planning and manipulation tasks. However, measurement ambiguity then arises as objects may possess complete or partial object shape symmetries (e. g. , due to occlusion), making it difficult or impossible to generate a single consistent object pose estimate. One idea is to generate multiple pose candidates to counteract measurement ambiguity. In this paper, we develop a novel approach that enables an object-based SLAM system to reason about multiple pose hypotheses for an object, and synthesize this locally ambiguous information into a globally consistent robot and landmark pose estimation formulation. In particular, we (1) present a learned pose estimation network that provides multiple hypotheses about the 6D pose of an object; (2) by treating the output of our network as components of a mixture model, we incorporate pose predictions into a SLAM system, which, over successive observations, recovers a globally consistent set of robot and object (landmark) pose estimates. We evaluate our approach on the popular YCB-Video Dataset and a simulated video featuring YCB objects. Experiments demonstrate that our approach is effective in improving the robustness of object-based SLAM in the face of object pose ambiguity. 1

IROS Conference 2021 Conference Paper

Bootstrapped Self-Supervised Training with Monocular Video for Semantic Segmentation and Depth Estimation

• Yihao Zhang 0003
• John J. Leonard

For a robot deployed in the world, it is desirable to have the ability of autonomous learning to improve its initial pre-set knowledge. We formalize this as a bootstrapped self-supervised learning problem where a system is initially bootstrapped with supervised training on a labeled dataset and we look for a self-supervised training method that can subsequently improve the system over the supervised training baseline using only unlabeled data. In this work, we leverage temporal consistency between frames in monocular video to per-form this bootstrapped self-supervised training. We show that a well-trained state-of-the-art semantic segmentation network can be further improved through our method. In addition, we show that the bootstrapped self-supervised training framework can help a network learn depth estimation better than pure supervised training or self-supervised training.

IROS Conference 2021 Conference Paper

Consensus-Informed Optimization Over Mixtures for Ambiguity-Aware Object SLAM

• Ziqi Lu
• Qiangqiang Huang
• Kevin J. Doherty 0001
• John J. Leonard

Building object-level maps can facilitate robot-environment interactions (e. g. planning and manipulation), but objects could often have multiple probable poses when viewed from a single vantage point, due to symmetry, occlusion or perceptual failures. A robust object-level simultaneous localization and mapping (object SLAM) algorithm needs to be aware of this pose ambiguity. We propose to maintain and subsequently disambiguate the multiple pose interpretations to gradually recover a globally consistent world representation. The max-mixtures model is applied to implicitly and efficiently track all pose hypotheses, but the resulting formulation is non-convex, and therefore subject to local optima. To mitigate this problem, temporally consistent hypotheses are extracted, guiding the optimization into the global optimum. This consensus-informed inference method is applied online via landmark variable re-initialization within an incremental SLAM framework, iSAM2, for robust real-time performance. We demonstrate that this approach improves SLAM performance on both simulated and real object SLAM problems with pose ambiguity.

ICRA Conference 2021 Conference Paper

Lidar-Monocular Surface Reconstruction Using Line Segments

• Victor Amblard
• Timothy P. Osedach
• Arnaud Croux
• Andrew Speck
• John J. Leonard

Structure from Motion (SfM) often fails to estimate accurate poses in environments that lack suitable visual features. In such cases, the quality of the final 3D mesh, which is contingent on the accuracy of those estimates, is reduced. One way to overcome this problem is to combine data from a monocular camera with that of a LIDAR. This allows fine details and texture to be captured while still accurately representing featureless subjects. However, fusing these two sensor modalities is challenging due to their fundamentally different characteristics. Rather than directly fusing image features and LIDAR points, we propose to leverage common geometric features that are detected in both the LIDAR scans and image data, allowing data from the two sensors to be processed in a higher-level space. In particular, we propose to find correspondences between 3D lines extracted from LIDAR scans and 2D lines detected in images before performing a bundle adjustment to refine poses. We also exploit the detected and optimized line segments to improve the quality of the final mesh. We test our approach on the recently published dataset, Newer College Dataset. We compare the accuracy and the completeness of the 3D mesh to a ground truth obtained with a survey-grade 3D scanner. We show that our method delivers results that are comparable to a state-of-the-art LIDAR survey while not requiring highly accurate ground truth pose estimates. We plan to release our code before publication.

ICRA Conference 2021 Conference Paper

NF-iSAM: Incremental Smoothing and Mapping via Normalizing Flows

• Qiangqiang Huang
• Can Pu
• Dehann Fourie
• Kasra Khosoussi
• Jonathan P. How
• John J. Leonard

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.

ICRA Conference 2020 Conference Paper

Probabilistic Data Association via Mixture Models for Robust Semantic SLAM

• Kevin J. Doherty 0001
• David P. Baxter
• Edward Schneeweiss
• John J. Leonard

Modern robotic systems sense the environment geometrically, through sensors like cameras, lidar, and sonar, as well as semantically, often through visual models learned from data, such as object detectors. We aim to develop robots that can use all of these sources of information for reliable navigation, but each is corrupted by noise. Rather than assume that object detection will eventually achieve near perfect performance across the lifetime of a robot, in this work we represent and cope with the semantic and geometric uncertainty inherent in object detection methods. Specifically, we model data association ambiguity, which is typically non-Gaussian, in a way that is amenable to solution within the common nonlinear Gaussian formulation of simultaneous localization and mapping (SLAM). We do so by eliminating data association variables from the inference process through max-marginalization, preserving standard Gaussian posterior assumptions. The result is a max-mixture-type model that accounts for multiple data association hypotheses. We provide experimental results on indoor and outdoor semantic navigation tasks with noisy odometry and object detection and find that the ability of the proposed approach to represent multiple hypotheses, including the "null" hypothesis, gives substantial robustness advantages in comparison to alternative semantic SLAM approaches.

IROS Conference 2020 Conference Paper

Towards Real-Time Non-Gaussian SLAM for Underdetermined Navigation

• Dehann Fourie
• Nicholas R. Rypkema
• Pedro Vaz Teixeira
• Samuel Claassens
• Erin M. Fischell
• John J. Leonard

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.

IROS Conference 2020 Conference Paper

Variational Filtering with Copula Models for SLAM

• John D. Martin
• Kevin J. Doherty 0001
• Caralyn Cyr
• Brendan J. Englot
• John J. Leonard

The ability to infer map variables and estimate pose is crucial to the operation of autonomous mobile robots. In most cases the shared dependency between these variables is modeled through a multivariate Gaussian distribution, but there are many situations where that assumption is unrealistic. Our paper shows how it is possible to relax this assumption and perform simultaneous localization and mapping (SLAM) with a larger class of distributions, whose multivariate dependency is represented with a copula model. We integrate the distribution model with copulas into a Sequential Monte Carlo estimator and show how unknown model parameters can be learned through gradient-based optimization. We demonstrate our approach is effective in settings where Gaussian assumptions are clearly violated, such as environments with uncertain data association and nonlinear transition models.

IROS Conference 2019 Conference Paper

Dense, Sonar-based Reconstruction of Underwater Scenes

• Pedro Vaz Teixeira
• Dehann Fourie
• Michael Kaess
• John J. Leonard

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.

ICRA Conference 2019 Conference Paper

Multimodal Semantic SLAM with Probabilistic Data Association

• Kevin J. Doherty 0001
• Dehann Fourie
• John J. Leonard

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.

ICRA Conference 2019 Conference Paper

Non-Gaussian SLAM utilizing Synthetic Aperture Sonar

• Mei Yi Cheung
• Dehann Fourie
• Nicholas R. Rypkema
• Pedro Vaz Teixeira
• Henrik R. Schmidt
• John J. Leonard

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.

IROS Conference 2019 Conference Paper

Non-parametric Mixed-Manifold Products using Multiscale Kernel Densities

• Dehann Fourie
• Pedro Vaz Teixeira
• John J. Leonard

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 $\mathbb{R}^{d}$ space 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.

IROS Conference 2018 Conference Paper

Multibeam Data Processing for Underwater Mapping

• Pedro Vaz Teixeira
• Franz S. Hover
• John J. Leonard
• Michael Kaess

From archaeology to the inspection of subsea structures, underwater mapping has become critical to many applications. Because of the balanced trade-off between range and resolution, multibeam sonars are often used as the primary sensor in underwater mapping platforms. These sonars output an image representing the intensity of the received acoustic echos over space, which must be classified into free and occupied regions before range measurements are determined and spatially registered. Most classifiers found in the underwater mapping literature use local thresholding techniques, which are highly sensitive to noise, outliers, and sonar artifacts typically found in these images. In this paper we present an overview of some of the techniques developed in the scope of our work on sonar-based underwater mapping, with the aim of improving map accuracy through better segmentation performance. We also provide experimental results using data collected with a DIDSON imaging sonar that show that these techniques improve both segmentation accuracy and robustness to outliers.

ICRA Conference 2017 Conference Paper

Duckietown: An open, inexpensive and flexible platform for autonomy education and research

• Liam Paull
• Jacopo Tani
• Heejin Ahn
• Javier Alonso-Mora
• Luca Carlone
• Michal Cáp
• Yu Fan Chen
• Changhyun Choi

Duckietown is an open, inexpensive and flexible platform for autonomy education and research. The platform comprises small autonomous vehicles (“Duckiebots”) built from off-the-shelf components, and cities (“Duckietowns”) complete with roads, signage, traffic lights, obstacles, and citizens (duckies) in need of transportation. The Duckietown platform offers a wide range of functionalities at a low cost. Duckiebots sense the world with only one monocular camera and perform all processing onboard with a Raspberry Pi 2, yet are able to: follow lanes while avoiding obstacles, pedestrians (duckies) and other Duckiebots, localize within a global map, navigate a city, and coordinate with other Duckiebots to avoid collisions. Duckietown is a useful tool since educators and researchers can save money and time by not having to develop all of the necessary supporting infrastructure and capabilities. All materials are available as open source, and the hope is that others in the community will adopt the platform for education and research.

ICRA Conference 2017 Conference Paper

SLAMinDB: Centralized graph databases for mobile robotics

• Dehann Fourie
• Samuel Claassens
• Sudeep Pillai
• Roxana Mata
• John J. Leonard

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.

IROS Conference 2017 Conference Paper

Towards visual ego-motion learning in robots

• Sudeep Pillai
• John J. Leonard

Many model-based Visual Odometry (VO) algorithms have been proposed in the past decade, often restricted to the type of camera optics, or the underlying motion manifold observed. We envision robots to be able to learn and perform these tasks, in a minimally supervised setting, as they gain more experience. To this end, we propose a fully trainable solution to visual ego-motion estimation for varied camera optics. We propose a visual ego-motion learning architecture that maps observed optical flow vectors to an ego-motion density estimate via a Mixture Density Network (MDN). By modeling the architecture as a Conditional Variational Autoencoder (C-VAE), our model is able to provide introspective reasoning and prediction for ego-motion induced scene-flow. Additionally, our proposed model is especially amenable to bootstrapped ego-motion learning in robots where the supervision in ego-motion estimation for a particular camera sensor can be obtained from standard navigation-based sensor fusion strategies (GPS/INS and wheel-odometry fusion). Through experiments, we show the utility of our proposed approach in enabling the concept of self-supervised learning for visual ego-motion estimation in autonomous robots.

IROS Conference 2016 Conference Paper

A nonparametric belief solution to the Bayes tree

• Dehann Fourie
• John J. Leonard
• Michael Kaess

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.

ICRA Conference 2016 Conference Paper

A unified resource-constrained framework for graph SLAM

• Liam Paull
• Guoquan Huang 0001
• John J. Leonard

Graphical methods have proven an extremely useful tool employed by the mobile robotics community to frame estimation problems. Incremental solvers are able to process incoming sensor data and produce maximum a posteriori (MAP) estimates in realtime by exploiting the natural sparsity within the graph for reasonable-sized problems. However, to enable truly longterm operation in prior unknown environments requires algorithms whose computation, memory, and bandwidth (in the case of distributed systems) requirements scale constantly with time and environment size. Some recent approaches have addressed this problem through a two-step process - first the variables selected for removal are marginalized which induces density, and then the result is sparsified to maintain computational efficiency. Previous literature generally addresses only one of these two components. In this work, we attempt to explicitly connect all of the aforementioned resource constraint requirements by considering the node removal and sparsification pipeline in its entirety. We formulate the node selection problem as a minimization problem over the penalty to be paid in the resulting sparsification. As a result, we produce node subset selection strategies that are optimal in terms of minimizing the impact, in terms of Kullback-Liebler divergence (KLD), of approximating the dense distribution by a sparse one. We then show that one instantiation of this problem yields a computationally tractable formulation. Finally, we evaluate the method on standard datasets and show that the KLD is minimized as compared to other commonly-used heuristic node selection techniques.

ICRA Conference 2016 Conference Paper

High-performance and tunable stereo reconstruction

• Sudeep Pillai
• Srikumar Ramalingam
• John J. Leonard

Traditional stereo algorithms have focused their efforts on reconstruction quality and have largely avoided prioritizing for run time performance. Robots, on the other hand, require quick maneuverability and effective computation to observe its immediate environment and perform tasks within it. In this work, we propose a high-performance and tunable stereo disparity estimation method, with a peak frame-rate of 120Hz (VGA resolution, on a single CPU-thread), that can potentially enable robots to quickly reconstruct their immediate surroundings and maneuver at high-speeds. Our key contribution is a disparity estimation algorithm that iteratively approximates the scene depth via a piece-wise planar mesh from stereo imagery, with a fast depth validation step for semi-dense reconstruction. The mesh is initially seeded with sparsely matched keypoints, and is recursively tessellated and refined as needed (via a resampling stage), to provide the desired stereo disparity accuracy. The inherent simplicity and speed of our approach, with the ability to tune it to a desired reconstruction quality and runtime performance makes it a compelling solution for applications in high-speed vehicles.

IROS Conference 2016 Conference Paper

SLAM with objects using a nonparametric pose graph

• Beipeng Mu
• Shih-Yuan Liu
• Liam Paull
• John J. Leonard
• Jonathan P. How

Mapping and self-localization in unknown environments are fundamental capabilities in many robotic applications. These tasks typically involve the identification of objects as unique features or landmarks, which requires the objects both to be detected and then assigned a unique identifier that can be maintained when viewed from different perspectives and in different images. The data association and simultaneous localization and mapping (SLAM) problems are, individually, well-studied in the literature. But these two problems are inherently tightly coupled, and that has not been well-addressed. Without accurate SLAM, possible data associations are combinatorial and become intractable easily. Without accurate data association, the error of SLAM algorithms diverge easily. This paper proposes a novel nonparametric pose graph that models data association and SLAM in a single framework. An algorithm is further introduced to alternate between inferring data association and performing SLAM. Experimental results show that our approach has the new capability of associating object detections and localizing objects at the same time, leading to significantly better performance on both the data association and SLAM problems than achieved by considering only one and ignoring imperfections in the other.

ICRA Conference 2016 Conference Paper

Towards lifelong feature-based mapping in semi-static environments

• David M. Rosen
• Julian Mason
• John J. Leonard

The feature-based graphical approach to robotic mapping provides a representationally rich and computationally efficient framework for an autonomous agent to learn a model of its environment. However, this formulation does not naturally support long-term autonomy because it lacks a notion of environmental change; in reality, “everything changes and nothing stands still, ” and any mapping and localization system that aims to support truly persistent autonomy must be similarly adaptive. To that end, in this paper we propose a novel feature-based model of environmental evolution over time. Our approach is based upon the development of an expressive probabilistic generative feature persistence model that describes the survival of abstract semi-static environmental features over time. We show that this model admits a recursive Bayesian estimator, the persistence filter, that provides an exact online method for computing, at each moment in time, an explicit Bayesian belief over the persistence of each feature in the environment. By incorporating this feature persistence estimation into current state-of-the-art graphical mapping techniques, we obtain a flexible, computationally efficient, and information-theoretically rigorous framework for lifelong environmental modeling in an ever-changing world.

IROS Conference 2016 Conference Paper

Underwater inspection using sonar-based volumetric submaps

• Pedro Vaz Teixeira
• Michael Kaess
• Franz S. Hover
• John J. Leonard

We propose a submap-based technique for mapping of underwater structures with complex geometries. Our approach relies on the use of probabilistic volumetric techniques to create submaps from multibeam sonar scans, as these offer increased outlier robustness. Special attention is paid to the problem of denoising/enhancing sonar data. Pairwise submap alignment constraints are used in a factor graph framework to correct for navigation drift and improve map accuracy. We provide experimental results obtained from the inspection of the running gear and bulbous bow of a 600-foot, Wright-class supply ship.

ICRA Conference 2015 Conference Paper

A convex relaxation for approximate global optimization in simultaneous localization and mapping

• David M. Rosen
• Charles DuHadway
• John J. Leonard

Modern approaches to simultaneous localization and mapping (SLAM) formulate the inference problem as a high-dimensional but sparse nonconvex M-estimation, and then apply general first- or second-order smooth optimization methods to recover a local minimizer of the objective function. The performance of any such approach depends crucially upon initializing the optimization algorithm near a good solution for the inference problem, a condition that is often difficult or impossible to guarantee in practice. To address this limitation, in this paper we present a formulation of the SLAM M-estimation with the property that, by expanding the feasible set of the estimation program, we obtain a convex relaxation whose solution approximates the globally optimal solution of the SLAM inference problem and can be recovered using a smooth optimization method initialized at any feasible point. Our formulation thus provides a means to obtain a high-quality solution to the SLAM problem without requiring high-quality initialization.

ICRA Conference 2015 Conference Paper

Appearance-based SLAM in a network space

• Padraig Corcoran
• Ted J. Steiner
• Michela Bertolotto
• John J. Leonard

The task of Simultaneous Localization and Mapping (SLAM) is regularly performed in network spaces consisting of a set of corridors connecting locations in the space. Empirical research has demonstrated that such spaces generally exhibit common structural properties relating to aspects such as corridor length. Consequently there exists potential to improve performance through the placement of priors over these properties. In this work we propose an appearance-based SLAM method which explicitly models the space as a network and in turn uses this model as a platform to place priors over its structure. Relative to existing works, which implicitly assume a network space and place priors over its structure, this approach allows a more formal placement of priors. In order to achieve robustness, the proposed method is implemented within a multi-hypothesis tracking framework. Results achieved on two publicly available datasets demonstrate the proposed method outperforms a current state-of-the-art appearance-based SLAM method.

IROS Conference 2015 Conference Paper

Bridging text spotting and SLAM with junction features

• Hsueh-Cheng Wang
• Chelsea Finn
• Liam Paull
• Michael Kaess
• Ruth Rosenholtz
• Seth J. Teller
• John J. Leonard

Navigating in a previously unknown environment and recognizing naturally occurring text in a scene are two important autonomous capabilities that are typically treated as distinct. However, these two tasks are potentially complementary, (i) scene and pose priors can benefit text spotting, and (ii) the ability to identify and associate text features can benefit navigation accuracy through loop closures. Previous approaches to autonomous text spotting typically require significant training data and are too slow for real-time implementation. In this work, we propose a novel high-level feature descriptor, the “junction”, which is particularly well-suited to text representation and is also fast to compute. We show that we are able to improve SLAM through text spotting on datasets collected with a Google Tango, illustrating how location priors enable improved loop closure with text features.

ICRA Conference 2015 Conference Paper

Communication-constrained multi-AUV cooperative SLAM

• Liam Paull
• Guoquan Huang 0001
• Mae L. Seto
• John J. Leonard

Multi-robot deployments have the potential for completing tasks more efficiently. For example, in simultaneous localization and mapping (SLAM), robots can better localize themselves and the map if they can share measurements of each other (direct encounters) and of commonly observed parts of the map (indirect encounters). However, performance is contingent on the quality of the communications channel. In the underwater scenario, communicating over any appreciable distance is achieved using acoustics which is low-bandwidth, slow, and unreliable, making cooperative operations very challenging. In this paper, we present a framework for cooperative SLAM (C-SLAM) for multiple autonomous underwater vehicles (AUVs) communicating only through acoustics. We develop a novel graph-based C-SLAM algorithm that is able to (optimally) generate communication packets whose size scales linearly with the number of observed features since the last successful transmission, constantly with the number of vehicles in the collective, and does not grow with time even the case of dropped packets, which are common. As a result, AUVs can bound their localization error without the need for pre-installed beacons or surfacing for GPS fixes during navigation, leading to significant reduction in time required to complete missions. The proposed algorithm is validated through realistic marine vehicle and acoustic communication simulations.

IROS Conference 2015 Conference Paper

Lagrangian duality in 3D SLAM: Verification techniques and optimal solutions

• Luca Carlone
• David M. Rosen
• Giuseppe Carlo Calafiore
• John J. Leonard
• Frank Dellaert

State-of-the-art techniques for simultaneous localization and mapping (SLAM) employ iterative nonlinear optimization methods to compute an estimate for robot poses. While these techniques often work well in practice, they do not provide guarantees on the quality of the estimate. This paper shows that Lagrangian duality is a powerful tool to assess the quality of a given candidate solution. Our contribution is threefold. First, we discuss a revised formulation of the SLAM inference problem. We show that this formulation is probabilistically grounded and has the advantage of leading to an optimization problem with quadratic objective. The second contribution is the derivation of the corresponding Lagrangian dual problem. The SLAM dual problem is a (convex) semidefinite program, which can be solved reliably and globally by off-the-shelf solvers. The third contribution is to discuss the relation between the original SLAM problem and its dual. We show that from the dual problem, one can evaluate the quality (i. e. , the suboptimality gap) of a candidate SLAM solution, and ultimately provide a certificate of optimality. Moreover, when the duality gap is zero, one can compute a guaranteed optimal SLAM solution from the dual problem, circumventing non-convex optimization. We present extensive (real and simulated) experiments supporting our claims and discuss practical relevance and open problems.

ICRA Conference 2015 Conference Paper

Location utility-based map reduction

• Ted J. Steiner
• Guoquan Huang 0001
• John J. Leonard

Maps used for navigation often include a database of location descriptions for place recognition (loop closing), which permits bounded-error performance. A standard pose-graph SLAM system adds a new entry for every new pose into the location database, which grows linearly and unbounded in time and thus becomes unsustainable. To address this issue, in this paper we propose a new map-reduction approach that pre-constructs a fixed-size place-recognition database amenable to the limited storage and processing resources of the vehicle by exploiting the high-level structure of the environment as well as the vehicle motion. In particular, we introduce the concept of location utility - which encapsulates the visitation probability of a location and its spatial distribution relative to nearby locations in the database - as a measure of the value of potential loop-closure events to occur at that location. While finding the optimal reduced location database is NP-hard, we develop an efficient greedy algorithm to sort all the locations in a map based on their relative utility without access to sensor measurements or the vehicle trajectory. This enables pre-determination of a generic, limited-size place-recognition database containing the N best locations in the environment. To validate the proposed approach, we develop an open-source street-map simulator using real city-map data and show that an accurate map (pose-graph) can be attained even when using a place-recognition database with only 1% of the entries of the corresponding full database.

IROS Conference 2015 Conference Paper

Real-time manhattan world rotation estimation in 3D

• Julian Straub
• Nishchal Bhandari
• John J. Leonard
• John W. Fisher III

Drift of the rotation estimate is a well known problem in visual odometry systems as it is the main source of positioning inaccuracy. We propose three novel algorithms to estimate the full 3D rotation to the surrounding Manhattan World (MW) in as short as 20 ms using surface-normals derived from the depth channel of a RGB-D camera. Importantly, this rotation estimate acts as a structure compass which can be used to estimate the bias of an odometry system, such as an inertial measurement unit (IMU), and thus remove its angular drift. We evaluate the run-time as well as the accuracy of the proposed algorithms on groundtruth data. They achieve zerodrift rotation estimation with RMSEs below 3. 4° by themselves and below 2. 8° when integrated with an IMU in a standard extended Kalman filter (EKF). Additional qualitative results show the accuracy in a large scale indoor environment as well as the ability to handle fast motion. Selected segmentations of scenes from the NYU depth dataset demonstrate the robustness of the inference algorithms to clutter and hint at the usefulness of the segmentation for further processing.

IROS Conference 2015 Conference Paper

Shape and pose recovery from planar pushing

• Kuan-Ting Yu
• John J. Leonard
• Alberto Rodriguez 0003

Tactile exploration refers to the use of physical interaction to infer object properties. In this work, we study the feasibility of recovering the shape and pose of a movable object from observing a series of contacts. In particular, we approach the problem of estimating the shape and trajectory of a planar object lying on a frictional surface, and being pushed by a frictional probe. The probe, when in contact with the object, makes observations of the location of contact and the contact normal.

IROS Conference 2014 Conference Paper

Decentralized cooperative trajectory estimation for autonomous underwater vehicles

• Liam Paull
• Mae L. Seto
• John J. Leonard

Autonomous agents that can communicate and make relative measurements of each other can improve their collective localization accuracies. This is referred to as cooperative localization (CL). Autonomous underwater vehicle (AUV) CL is constrained by the low throughput, high latency, and unreliability of of the acoustic channel used to communicate when submerged. Here we propose a CL algorithm specifically designed for full trajectory, or maximum a posteriori, estimation for AUVs. The method is exact and has the advantage that the broadcast packet sizes increase only linearly with the number of AUVs in the collective and do not grow at all in the case of packet loss. The approach allows for AUV missions to be achieved more efficiently since: 1) vehicles waste less time surfacing for GPS fixes, and 2) payload data is more accurately localized through the smoothing approach.

ICRA Conference 2014 Conference Paper

Efficient incremental map segmentation in dense RGB-D maps

• Ross Finman
• Thomas Whelan
• Michael Kaess
• John J. Leonard

In this paper we present a method for incrementally segmenting large RGB-D maps as they are being created. Recent advances in dense RGB-D mapping have led to maps of increasing size and density. Segmentation of these raw maps is a first step for higher-level tasks such as object detection. Current popular methods of segmentation scale linearly with the size of the map and generally include all points. Our method takes a previously segmented map and segments new data added to that map incrementally online. Segments in the existing map are re-segmented with the new data based on an iterative voting method. Our segmentation method works in maps with loops to combine partial segmentations from each traversal into a complete segmentation model. We verify our algorithm on multiple real-world datasets spanning many meters and millions of points in real-time. We compare our method against a popular batch segmentation method for accuracy and timing complexity.

IROS Conference 2014 Conference Paper

Experimental evaluation of on-board, visual mapping of an object spinning in micro-gravity aboard the International Space Station

• Brent E. Tweddle
• Timothy P. Setterfield
• Alvar Saenz-Otero
• David W. Miller
• John J. Leonard

Spacecraft proximity operations near an unknown and uncooperative and spinning target object is a challenging problem. One of the key steps in ensuring safe operations is building a map of the target object. Additionally, it is useful to estimate the relative position, orientation, linear and angular velocities of the object as well as its center of mass, principal axes of inertia and ratios of inertia so that the object's pose can be accurately propagated forward in time. This paper reviews an approach for building a map of an unknown and spinning object, and presents experimental results of this algorithm when it was run onboard the Synchronized Position Hold Engage Reorient Experimental Satellites (SPHERES) and Visual Estimation for Relative Tracking and Inspection of Generic Objects (VERTIGO) Goggles that are operating within the International Space Station. To the best of the authors' knowledge, this is the first time a solution to a Simultaneous Localization and Mapping (SLAM) problem has been run on a computer that is in space.

ICRA Conference 2014 Conference Paper

Inference over heterogeneous finite-/infinite-dimensional systems using factor graphs and Gaussian processes

• David M. Rosen
• Guoquan Huang 0001
• John J. Leonard

The ability to reason over partially observable networks of interacting states is a fundamental competency in probabilistic robotics. While the well-known factor graph and Gaussian process models provide flexible and computationally efficient solutions for this inference problem in the special cases in which all of the hidden states are either finite-dimensional parameters or real-valued functions, respectively, in many cases we are interested in reasoning about heterogeneous networks whose hidden states are comprised of both finite-dimensional parameters and functions. To that end, in this paper we propose a novel probabilistic generative model that incorporates both factor graphs and Gaussian processes to model these heterogeneous systems. Our model improves upon prior approaches to inference within these networks by removing the assumption of any specific set of conditional independences amongst the modeled states, thereby significantly expanding the class of systems that can be represented. Furthermore, we show that inference within this model can always be performed by means of a two-stage procedure involving inference within a factor graph followed by inference over a Gaussian process; by exploiting fast inference methods for the individual factor graph and Gaussian process models to solve each of these subproblems in succession, we thus obtain a general framework for computationally efficient inference over heterogeneous finite-/infinite-dimensional systems.

IROS Conference 2014 Conference Paper

Optimized visibility motion planning for target tracking and localization

• Hongchuan Wei
• Wenjie Lu 0005
• Pingping Zhu
• Guoquan Huang 0001
• John J. Leonard
• Silvia Ferrari

This paper presents a visibility-based method for planning the motion of a mobile robotic sensor with bounded field-of-view to optimally track a moving target while localizing itself. The target and robot states are estimated from online sensor measurements and a set of a priori known landmarks, using an extended Kalman filter (EKF), and thus the proposed method is applicable to robots without a global positioning system. It is shown that the problem of optimizing the target tracking and robot localization performance is equivalent to optimizing the visibility or probability of detection in the EKF framework under mild assumptions. The control law that maximizes the probability of detection for a robotic sensor with a sector-shaped field-of-view (FoV) is derived as a function of the robot heading and aperture. Simulations have been conducted on synthetic experiments and the results show that the optimized-visibility approach is effective at avoiding target loss, and outperforms a state-of-the-art potential method based on robot trailer models [1].

ICRA Conference 2014 Conference Paper

Towards consistent visual-inertial navigation

• Guoquan Huang 0001
• Michael Kaess
• John J. Leonard

Visual-inertial navigation systems (VINS) have prevailed in various applications, in part because of the complementary sensing capabilities and decreasing costs as well as sizes. While many of the current VINS algorithms undergo inconsistent estimation, in this paper we introduce a new extended Kalman filter (EKF)-based approach towards consistent estimates. To this end, we impose both state-transition and obervability constraints in computing EKF Jacobians so that the resulting linearized system can best approximate the underlying nonlinear system. Specifically, we enforce the propagation Jacobian to obey the semigroup property, thus being an appropriate state-transition matrix. This is achieved by parametrizing the orientation error state in the global, instead of local, frame of reference, and then evaluating the Jacobian at the propagated, instead of the updated, state estimates. Moreover, the EKF linearized system ensures correct observability by projecting the most-accurate measurement Jacobian onto the observable subspace so that no spurious information is gained. The proposed algorithm is validated by both Monte-Carlo simulation and real-world experimental tests.

IROS Conference 2013 Conference Paper

Deformation-based loop closure for large scale dense RGB-D SLAM

• Thomas Whelan
• Michael Kaess
• John J. Leonard
• John McDonald 0001

In this paper we present a system for capturing large scale dense maps in an online setting with a low cost RGB-D sensor. Central to this work is the use of an “as-rigid-as-possible” space deformation for efficient dense map correction in a pose graph optimisation framework. By combining pose graph optimisation with non-rigid deformation of a dense map we are able to obtain highly accurate dense maps over large scale trajectories that are both locally and globally consistent. With low latency in mind we derive an incremental method for deformation graph construction, allowing multi-million point maps to be captured over hundreds of metres in real-time. We provide benchmark results on a well established RGB-D SLAM dataset demonstrating the accuracy of the system and also provide a number of our own datasets which cover a wide range of environments, both indoors, outdoors and across multiple floors.

ICRA Conference 2013 Conference Paper

Robust incremental online inference over sparse factor graphs: Beyond the Gaussian case

• David M. Rosen
• Michael Kaess
• John J. Leonard

Many online inference problems in robotics and AI are characterized by probability distributions whose factor graph representations are sparse. While there do exist some computationally efficient algorithms (e. g. incremental smoothing and mapping (iSAM) or Robust Incremental least-Squares Estimation (RISE)) for performing online incremental maximum likelihood estimation over these models, they generally require that the distribution of interest factors as a product of Gaussians, a rather restrictive assumption. In this paper, we investigate the possibility of performing efficient incremental online estimation over sparse factor graphs in the non-Gaussian case. Our main result is a method that generalizes iSAM and RISE by removing the assumption of Gaussian factors, thereby significantly expanding the class of distributions to which these algorithms can be applied. The generalization is achieved by means of a simple algebraic reduction that under relatively mild conditions (boundedness of each of the factors in the distribution of interest) enables an instance of the general maximum likelihood estimation problem to be reduced to an equivalent instance of least-squares minimization that can be solved efficiently online by application of iSAM or RISE. Through this construction we obtain robust, computationally efficient, and mathematically correct incremental online maximum likelihood estimators for non-Gaussian distributions over sparse factor graphs.

ICRA Conference 2013 Conference Paper

Robust real-time visual odometry for dense RGB-D mapping

• Thomas Whelan
• Hordur Johannsson
• Michael Kaess
• John J. Leonard
• John McDonald 0001

This paper describes extensions to the Kintinuous [1] algorithm for spatially extended KinectFusion, incorporating the following additions: (i) the integration of multiple 6DOF camera odometry estimation methods for robust tracking; (ii) a novel GPU-based implementation of an existing dense RGB-D visual odometry algorithm; (iii) advanced fused realtime surface coloring. These extensions are validated with extensive experimental results, both quantitative and qualitative, demonstrating the ability to build dense fully colored models of spatially extended environments for robotics and virtual reality applications while remaining robust against scenes with challenging sets of geometric and visual features.

ICRA Conference 2013 Conference Paper

Temporally scalable visual SLAM using a reduced pose graph

• Hordur Johannsson
• Michael Kaess
• Maurice F. Fallon
• John J. Leonard

In this paper, we demonstrate a system for temporally scalable visual SLAM using a reduced pose graph representation. Unlike previous visual SLAM approaches that maintain static keyframes, our approach uses new measurements to continually improve the map, yet achieves efficiency by avoiding adding redundant frames and not using marginalization to reduce the graph. To evaluate our approach, we present results using an online binocular visual SLAM system that uses place recognition for both robustness and multi-session operation. Additionally, to enable large-scale indoor mapping, our system automatically detects elevator rides based on accelerometer data. We demonstrate long-term mapping in a large multi-floor building, using approximately nine hours of data collected over the course of six months. Our results illustrate the capability of our visual SLAM system to map a large are over extended period of time.

ICRA Conference 2012 Conference Paper

An incremental trust-region method for Robust online sparse least-squares estimation

• David M. Rosen
• Michael Kaess
• John J. Leonard

Many online inference problems in computer vision and robotics are characterized by probability distributions whose factor graph representations are sparse and whose factors are all Gaussian functions of error residuals. Under these conditions, maximum likelihood estimation corresponds to solving a sequence of sparse least-squares minimization problems in which additional summands are added to the objective function over time. In this paper we present Robust Incremental least-Squares Estimation (RISE), an incrementalized version of the Powell's Dog-Leg trust-region method suitable for use in online sparse least-squares minimization. As a trust-region method, Powell's Dog-Leg enjoys excellent global convergence properties, and is known to be considerably faster than both Gauss-Newton and Levenberg-Marquardt when applied to sparse least-squares problems. Consequently, RISE maintains the speed of current state-of-the-art incremental sparse least-squares methods while providing superior robustness to objective function nonlinearities.

IROS Conference 2012 Conference Paper

Dynamic pose graph SLAM: Long-term mapping in low dynamic environments

• Aisha Walcott-Bryant
• Michael Kaess
• Hordur Johannsson
• John J. Leonard

Maintaining a map of an environment that changes over time is a critical challenge in the development of persistently autonomous mobile robots. Many previous approaches to mapping assume a static world. In this work we incorporate the time dimension into the mapping process to enable a robot to maintain an accurate map while operating in dynamical environments. This paper presents Dynamic Pose Graph SLAM (DPG-SLAM), an algorithm designed to enable a robot to remain localized in an environment that changes substantially over time. Using incremental smoothing and mapping (iSAM) as the underlying SLAM state estimation engine, the Dynamic Pose Graph evolves over time as the robot explores new places and revisits previously mapped areas. The approach has been implemented for planar indoor environments, using laser scan matching to derive constraints for SLAM state estimation. Laser scans for the same portion of the environment at different times are compared to perform change detection; when sufficient change has occurred in a location, the dynamic pose graph is edited to remove old poses and scans that no longer match the current state of the world. Experimental results are shown for two real-world dynamic indoor laser data sets, demonstrating the ability to maintain an up-to-date map despite long-term environmental changes.

IROS Conference 2012 Conference Paper

Dynamic positioning of beacon vehicles for cooperative underwater navigation

• Alexander Bahr
• John J. Leonard
• Alcherio Martinoli

Autonomous Underwater Vehicles (AUVs) are used for an ever increasing range of applications due to the maturing of the technology. Due to the absence of the GPS signal underwater, the correct estimation of its position is a challenge for submerged vehicles. One promising strategy to mitigate this problem is to use a group of AUVs where one or more assume the role of a beacon vehicle which has a very accurate position estimate due to an expensive navigation suite or frequent surfacings. These beacon vehicles broadcast their position and the remaining survey vehicles can use this position information and intra-vehicle ranges to update their position estimate. The effectiveness of this approach strongly depends on the geometry between the beacon vehicles and the survey vehicles. The trajectories of the beacon vehicles should thus be planned with the goal to minimize the position uncertainty of the survey vehicles. We propose a distributed algorithm which dynamically computes the locally optimal position for a beacon vehicle using only information obtained from broadcast communication of the survey vehicles. It does not need prior information about the survey vehicles' trajectory and can be used for any group size of beacon and survey vehicles.

ICRA Conference 2012 Conference Paper

Efficient scene simulation for robust monte carlo localization using an RGB-D camera

• Maurice F. Fallon
• Hordur Johannsson
• John J. Leonard

This paper presents Kinect Monte Carlo Localization (KMCL), a new method for localization in three dimensional indoor environments using RGB-D cameras, such as the Microsoft Kinect. The approach makes use of a low fidelity a priori 3-D model of the area of operation composed of large planar segments, such as walls and ceilings, which are assumed to remain static. Using this map as input, the KMCL algorithm employs feature-based visual odometry as the particle propagation mechanism and utilizes the 3-D map and the underlying sensor image formation model to efficiently simulate RGB-D camera views at the location of particle poses, using a graphical processing unit (GPU). The generated 3D views of the scene are then used to evaluate the likelihood of the particle poses. This GPU implementation provides a factor of ten speedup over a pure distance-based method, yet provides comparable accuracy. Experimental results are presented for five different configurations, including: (1) a robotic wheelchair, (2) a sensor mounted on a person, (3) an Ascending Technologies quadrotor, (4) a Willow Garage PR2, and (5) an RWI B21 wheeled mobile robot platform. The results demonstrate that the system can perform robust localization with 3D information for motions as fast as 1. 5 meters per second. The approach is designed to be applicable not just for robotics but other applications such as wearable computing.

IROS Conference 2012 Conference Paper

Sensor fusion for flexible human-portable building-scale mapping

• Maurice F. Fallon
• Hordur Johannsson
• Jonathan Brookshire
• Seth J. Teller
• John J. Leonard

This paper describes a system enabling rapid multi-floor indoor map building using a body-worn sensor system fusing information from RGB-D cameras, LIDAR, inertial, and barometric sensors. Our work is motivated by rapid response missions by emergency personnel, in which the capability for one or more people to rapidly map a complex indoor environment is essential for public safety. Human-portable mapping raises a number of challenges not encountered in typical robotic mapping applications including complex 6-DOF motion and the traversal of challenging trajectories including stairs or elevators. Our system achieves robust performance in these situations by exploiting state-of-the-art techniques for robust pose graph optimization and loop closure detection. It achieves real-time performance in indoor environments of moderate scale. Experimental results are demonstrated for human-portable mapping of several floors of a university building, demonstrating the system's ability to handle motion up and down stairs and to organize initially disconnected sets of submaps in a complex environment.

ICRA Conference 2011 Conference Paper

Efficient AUV navigation fusing acoustic ranging and side-scan sonar

• Maurice F. Fallon
• Michael Kaess
• Hordur Johannsson
• John J. Leonard

This paper presents an on-line nonlinear least squares algorithm for multi-sensor autonomous underwater vehicle (AUV) navigation. The approach integrates the global constraints of range to and GPS position of a surface vehicle or buoy communicated via acoustic modems and relative pose constraints arising from targets detected in side-scan sonar images. The approach utilizes an efficient optimization algorithm, iSAM, which allows for consistent on-line estimation of the entire set of trajectory constraints. The optimized trajectory can then be used to more accurately navigate the AUV, to extend mission duration, and to avoid GPS surfacing. As iSAM provides efficient access to the marginal covariances of previously observed features, automatic data association is greatly simplified - particularly in sparse marine environments. A key feature of our approach is its intended scalability to single surface sensor (a vehicle or buoy) broadcasting its GPS position and simultaneous one-way travel time range (OWTT) to multiple AUVs. We discuss why our approach is scalable as well as robust to modem transmission failure. Results are provided for an ocean experiment using a Hydroid REMUS 100 AUV co-operating with one of two craft: an autonomous surface vehicle (ASV) and a manned support vessel. During these experiments the ranging portion of the algorithm ran online on-board the AUV. Extension of the paradigm to multiple missions via the optimization of successive survey missions (and the resultant sonar mosaics) is also demonstrated.

ICRA Conference 2011 Conference Paper

iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering

• Michael Kaess
• Hordur Johannsson
• Richard Roberts 0001
• Viorela Ila
• John J. Leonard
• Frank Dellaert

We present iSAM2, a fully incremental, graph-based 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.

ICRA Conference 2011 Conference Paper

Using prioritized relaxations to locate objects in points clouds for manipulation

• Robert Truax
• Robert Platt 0001
• John J. Leonard

This paper considers the problem of identifying objects of interest in laser range point clouds for the purposes of manipulation. One of the characteristics of perception for manipulation is that while it is unnecessary to label all objects in the scene, it may be very important to maximize the likelihood of correctly locating a desired object. This paper leverages this and proposes an approach for locating the most likely object configurations given an object parameterization and a point cloud. While many other approaches to object localization need to explicitly associate points with hypothesized objects, our proposed method avoids this by optimizing relaxations of the likelihood function rather than the exact likelihood. The result is a simple, efficient, and robust method for locating objects that makes few assumptions beyond the desired object parameterization and with few parameters that require tuning.

ICRA Conference 2010 Conference Paper

A measurement distribution framework for cooperative navigation using multiple AUVs

• Maurice F. Fallon
• Georgios Papadopoulos 0003
• John J. Leonard

In recent years underwater survey and surveillance missions with more than a single Autonomous Underwater Vehicle (AUV) have become more common thanks to more reliable and cheaper platforms, as well as the addition of remote command and control communications using, for example, the WHOI acoustic modem. However cooperative navigation of AUVs has thus far been limited to a single AUV supported by a dedicated surface vehicle with access to GPS. In this paper a scalable and modular framework is presented in which any number of vehicles can broadcast, forward and acknowledge range, dead-reckoning, feature and GPS measurements so that the full fleet of AUVs can navigate and cooperate in a consistent and accurate manner. The approach is independent of the resultant application-such as recursive state estimation or full pose optimization. Trade-offs between the number of vehicles, the condition of the communication channel and rate at which updates are available are also discussed. Finally performance is illustrated in a realistic experiment.

IROS Conference 2010 Conference Paper

Cooperative localization of marine vehicles using nonlinear state estimation

• Georgios Papadopoulos 0003
• Maurice F. Fallon
• John J. Leonard
• Nicholas M. Patrikalakis

This paper investigates the problem of cooperative navigation of autonomous marine vehicles using range-only acoustic measurements. We consider the use of a single maneuvering autonomous surface vehicle (ASV) to aid the navigation of one or more submerged autonomous underwater vehicles (AUVs), using acoustic range measurements combined with position measurements for the ASV when data packets are transmitted. The AUV combines the data from the surface vehicle with its proprioceptive sensor measurements to compute its trajectory. In previous work, we presented an experimental demonstration of this approach, using an extended Kalman filter (EKF) for state estimation. In the present paper, we analyze the observability properties of the cooperative ASV/AUV localization problem and present experimental results comparing several different state estimators. Using the weak observability theorem for nonlinear systems, we demonstrate that this cooperative localization problem is best attacked using nonlinear least squares (NLS) optimization. We present experimental results for this new approach and compare it to alternative state estimators, demonstrating superior performance.

IROS Conference 2010 Conference Paper

Imaging sonar-aided navigation for autonomous underwater harbor surveillance

• Hordur Johannsson
• Michael Kaess
• Brendan J. Englot
• Franz S. Hover
• John J. Leonard

In this paper we address the problem of drift-free navigation for underwater vehicles performing harbor surveillance and ship hull inspection. Maintaining accurate localization for the duration of a mission is important for a variety of tasks, such as planning the vehicle trajectory and ensuring coverage of the area to be inspected. Our approach only uses onboard sensors in a simultaneous localization and mapping setting and removes the need for any external infrastructure like acoustic beacons. We extract dense features from a forward-looking imaging sonar and apply pair-wise registration between sonar frames. The registrations are combined with onboard velocity, attitude and acceleration sensors to obtain an improved estimate of the vehicle trajectory. We show results from several experiments that demonstrate drift-free navigation in various underwater environments.

ICRA Conference 2010 Conference Paper

Multiple relative pose graphs for robust cooperative mapping

• Been Kim
• Michael Kaess
• Luke Fletcher
• John J. Leonard
• Abraham Bachrach
• Nicholas Roy
• Seth J. Teller

This paper describes a new algorithm for cooperative and persistent simultaneous localization and mapping (SLAM) using multiple robots. Recent pose graph representations have proven very successful for single robot mapping and localization. Among these methods, incremental smoothing and mapping (iSAM) gives an exact incremental solution to the SLAM problem by solving a full nonlinear optimization problem in real-time. In this paper, we present a novel extension to iSAM to facilitate online multi-robot mapping based on multiple pose graphs. Our main contribution is a relative formulation of the relationship between multiple pose graphs that avoids the initialization problem and leads to an efficient solution when compared to a completely global formulation. The relative pose graphs are optimized together to provide a globally consistent multi-robot solution. Efficient access to covariances at any time for relative parameters is provided through iSAM, facilitating data association and loop closing. The performance of the technique is illustrated on various data sets including a publicly available multi-robot data set. Further evaluation is performed in a collaborative helicopter and ground robot experiment.

ICRA Conference 2009 Conference Paper

Consistent cooperative localization

• Alexander Bahr
• Matthew R. Walter
• John J. Leonard

In cooperative navigation, teams of mobile robots obtain range and/or angle measurements to each other and dead-reckoning information to help each other navigate more accurately. One typical approach is moving baseline navigation, in which multiple Autonomous Underwater Vehicles (AUVs) exchange range measurements using acoustic modems to perform mobile trilateration. While the sharing of information between vehicles can be highly beneficial, exchanging measurements and state estimates can also be dangerous because of the risk of measurements being used by a vehicle more than once; such data re-use leads to inconsistent (overconfident) estimates, making data association and outlier rejection more difficult and divergence more likely. In this paper, we present a technique for the consistent cooperative localization of multiple AUVs performing mobile trilateration. Each AUV establishes a bank of filters, performing careful bookkeeping to track the origins of measurements and prevent the use any of the measurements more than once. The multiple estimates are combined in a consistent manner, yielding conservative covariance estimates. The technique is illustrated using simulation results. The new method is compared side-by-side with a naive approach that does not keep track of the origins of measurements, illustrating that the new method keeps conservative covariance bounds whereas state estimates obtained with the naive approach become overconfident and diverge.

ICRA Conference 2009 Conference Paper

Simultaneous local and global state estimation for robotic navigation

• David C. Moore
• Albert S. Huang
• Matthew R. Walter
• Edwin Olson
• Luke Fletcher
• John J. Leonard
• Seth J. Teller

Recent applications of robotics often demand two types of spatial awareness: 1) A fine-grained description of the robot's immediate surroundings for obstacle avoidance and planning, and 2) Knowledge of the robot's position in a large-scale global coordinate frame such as that provided by GPS. Although managing information at both of these scales is often essential to the robot's purpose, each scale has different requirements in terms of state representation and handling of uncertainty. In such a scenario, it can be tempting to pick either a body-centric coordinate frame or a globally fixed coordinate frame for all state representation. Although both choices have advantages, we show that neither is ideal for a system that must handle both global and local data. This paper describes an alternative design: a third coordinate frame that stays fixed to the local environment over short time-scales, but can vary with respect to the global frame. Careful management of uncertainty in this local coordinate frame makes it well-suited for simultaneously representing both locally and globally derived data, greatly simplifying system design and improving robustness. We describe the implementation of this coordinate frame and its properties when measuring uncertainty, and show the results of applying this approach to our 2007 DARPA Urban Challenge vehicle.

ICRA Conference 2008 Conference Paper

SLAM for ship hull inspection using exactly sparse extended information filters

• Matthew R. Walter
• Franz S. Hover
• John J. Leonard

Many important missions for autonomous underwater vehicles (AUVs), such as undersea inspection of ship hulls, require integrated navigation, control, and motion planning in complex, 3D environments. This paper describes a SLAM implementation using forward-looking sonar (FLS) data from a highly maneuverable, hovering AUV performing a ship hull inspection mission. The exactly sparse extended information filter (ESEIF) algorithm is applied to perform SLAM based upon features manually selected within FLS images. The results demonstrate the ability to effectively map a ship hull in a challenging marine environment. This provides a foundation for future work in which real-time SLAM will be integrated with motion planning and control to achieve autonomous coverage of a complete ship hull.

IROS Conference 2007 Conference Paper

Feature tracking for underwater navigation using sonar

• John Folkesson
• John J. Leonard
• Jacques Leederkerken
• Rob Williams

Tracking sonar features in real time on an underwater robot is a challenging task. One reason is the low observability of the sonar in some directions. For example, using a blazed array sonar one observes range and the angle to the array axis with fair precision. The angle around the axis is poorly constrained. This situation is problematic for tracking features in world frame Cartesian coordinates as the error surfaces will not be ellipsoids. Thus Gaussian tracking of the features will not work properly. The situation is similar to the problem of tracking features in camera images. There the unconstrained direction is depth and its errors are highly non-Gaussian. We propose a solution to the sonar problem that is analogous to the successful inverse depth feature parameterization for vision tracking, introduced by [1]. We parameterize the features by the robot pose where it was first seen and the range/bearing from that pose. Thus the 3D features have 9 parameters that specify their world coordinates. We use a nonlinear transformation on the poorly observed bearing angle to give a more accurate Gaussian approximation to the uncertainty. These features are tracked in a SLAM framework until there is enough information to initialize world frame Cartesian coordinates for them. The more compact representation can then be used for a global SLAM or localization purposes. We present results for a system running real time underwater SLAM/localization. These results show that the parameterization leads to greater consistency in the feature location estimates.

IROS Conference 2006 Conference Paper

Adaptive Control of Heterogeneous Marine Sensor Platforms in an Autonomous Sensor Network

• Donald P. Eickstedt
• Michael R. Benjamin
• Henrik Schmidt
• John J. Leonard

This paper describes an investigation into the control of autonomous mobile sensor platforms in a marine sensor network used to provide monitoring of transitory phenomenon over a wide area. A distributed network of small, inexpensive vehicles with heterogeneous sensors allows us to build a robust monitoring network capable of real-time response to rapidly changing sensor data. The major objective of this paper is to describe a framework for adaptive and cooperative control of the autonomous sensor platforms in such a network. This framework has two major components, a sensor that provides high-level state information to a behavior-based autonomous vehicle control system and a new approach to behavior-based control of autonomous vehicles using multiple objective functions that allow reactive control in complex environments with multiple constraints. Experimental results are presented for a 2-D target tracking application using a network of autonomous surface craft in which one platform with a simulated bearing sensor tracks a moving target and relays the target state information to a second vehicle that is moving in a classification mode. From these results, it is readily seen that there is the potential for potent synergy from the cooperation of multiple sensor platforms which can each view an event of interest from a different vantage point

ICRA Conference 2006 Conference Paper

Fast Iterative Alignment of Pose Graphs with Poor Initial Estimates

• Edwin Olson
• John J. Leonard
• Seth J. Teller

A robot exploring an environment can estimate its own motion and the relative positions of features in the environment. Simultaneous localization and mapping (SLAM) algorithms attempt to fuse these estimates to produce a map and a robot trajectory. The constraints are generally non-linear, thus SLAM can be viewed as a non-linear optimization problem. The optimization can be difficult, due to poor initial estimates arising from odometry data, and due to the size of the state space. We present a fast non-linear optimization algorithm that rapidly recovers the robot trajectory, even when given a poor initial estimate. Our approach uses a variant of stochastic gradient descent on an alternative state-space representation that has good stability and computational properties. We compare our algorithm to several others, using both real and synthetic data sets

ICRA Conference 2006 Conference Paper

Navigation of Unmanned Marine Vehicles in Accordance with the Rules of the Road

• Michael R. Benjamin
• Joseph A. Curcio
• John J. Leonard
• Paul Newman 0001

This paper is concerned with the in-field autonomous operation of unmanned marine vehicles in accordance with convention for safe and proper collision avoidance as prescribed by the coast guard collision regulations (COLREGS). These rules are written to train and guide safe human operation of marine vehicles and are heavily dependent on human common sense in determining rule applicability as well as rule execution, especially when multiple rules apply simultaneously. To capture the flexibility exploited by humans, this work applies a novel method of multi-objective optimization, interval programming, in a behavior-based control framework for representing the navigation rules, as well as task behaviors, in a way that achieves simultaneous optimal satisfaction. We present experimental validation of this approach using multiple autonomous surface craft. This work represents the first in-field demonstration of multiobjective optimization applied to autonomous COLREGS-based marine vehicle navigation

ICRA Conference 2005 Conference Paper

Exactly Sparse Delayed-State Filters

• Ryan M. Eustice
• Hanumant Singh
• John J. Leonard

This paper presents the novel insight that the SLAM information matrix is exactly sparse in a delayed-state framework. Such a framework is used in view-based representations of the environment which rely upon scan-matching raw sensor data. Scan-matching raw data results in virtual observations of robot motion with respect to a place its previously been. The exact sparseness of the delayed-state information matrix is in contrast to other recent feature based SLAM information algorithms like Sparse Extended Information Filters or Thin Junction Tree Filters. These methods have to make approximations in order to force the feature-based SLAM information matrix to be sparse. The benefit of the exact sparseness of the delayed-state framework is that it allows one to take advantage of the information space parameterization without having to make any approximations. Therefore, it can produce equivalent results to the “full-covariance” solution.

IROS Conference 2005 Conference Paper

Sparse extended information filters: insights into sparsification

• Ryan M. Eustice
• Matthew R. Walter
• John J. Leonard

Recently, there have been a number of variant simultaneous localization and mapping (SLAM) algorithms that have made substantial progress towards large-area scalability by parameterizing the SLAM posterior within the information (canonical/inverse covariance) form. Of these, probably the most well known and popular approach is the sparse extended information filter (SEIF) by Thrun et al. While SEIFs have been successfully implemented with a variety of challenging real world datasets and have led to new insights into scalable SLAM, open research questions remain regarding the approximate sparsification procedure and its effect on map error consistency. In this paper, we examine the constant time SEIF sparsification procedure in depth and offer new insight into issues of consistency. In particular, we show that exaggerated map inconsistency occurs within the global reference frame where estimation is performed, but that empirical testing shows that relative local map relationships are preserved. We then present a slightly modified version of their sparsification procedure, which is shown to preserve sparsity while also generating both local and global map estimates comparable to those obtained by the nonsparsified SLAM filter. While this modified approximation is no longer constant time, it does serve as a theoretical benchmark against which to compare SEIFs constant time results. We demonstrate our findings by benchmark comparison of the modified and original SEIF sparsification rule using simulation in the linear Gaussian SLAM case and real world experiments for a nonlinear dataset.

ICRA Conference 2003 Conference Paper

An atlas framework for scalable mapping

• Michael Bosse
• Paul Newman 0001
• John J. Leonard
• Martin Soika
• Wendelin Feiten
• Seth J. Teller

This paper describes Atlas, a hybrid metrical/topological approach to SLAM that achieves efficient mapping of large-scale environments. The representation is a graph of coordinate frames, with each vertex in the graph representing a local frame, and each edge representing the transformation between adjacent frames. In each frame, we build a map that captures the local environment and the current robot pose along with the uncertainties of each. Each map's uncertainties are modeled with respect to its own frame. Probabilities of entities with respect to arbitrary frames are generated by following a path formed by the edges between adjacent frames, computed via Dijkstra's shortest path algorithm. Loop closing is achieved via an efficient map matching algorithm. We demonstrate the technique running in real-time in a large indoor structured environment (2. 2 km path length) with multiple nested loops using laser or ultrasonic ranging sensors.

ICRA Conference 2003 Conference Paper

Autonomous feature-based exploration

• Paul Newman 0001
• Michael Bosse
• John J. Leonard

This paper presents an algorithm for feature-based exploration of a priori unknown environments. We aim to build a robot that, unsupervised, plans its motion such that it continually increases both the spatial extent and detail of its world model - its map. We present a method by which the planned motion at any instant is motivated by the geometric, spatial and stochastic characteristics of the current map. In particular each feature within the map is responsible for determining nearby unexplored areas that if visited are likely to constitute exploration. We assume that the location of the features is uncertain and represented by a set of probability distribution functions (pdfs). These distributions are used in conjunction with the robot path history to determine a robot trajectory suited to exploration. We show results that demonstrate the algorithm providing real-time exploration of a mobile robot in an unknown environment.

ICRA Conference 2003 Conference Paper

Pure range-only sub-sea SLAM

• Paul Newman 0001
• John J. Leonard

This paper is about using range-only data to navigate an autonomous underwater vehicle (AUV). We assume the vehicle is equipped with conventional long base line (LBL) transceiver which measures acoustic time of flights (TOFs) between vehicle and small submerged transponders. Using only range data and no prior information other than approximate water column depth, we solve for both transponder location and vehicle trajectory. Results are given using data from a AUV operating in shallow water. A ground truth comparison is made with surveyed transponder locations and trajectory estimates from an on-board Doppler/Compass/LBL derived navigation filter.

ICRA Conference 2003 Conference Paper

Trajectory sonar perceptio

• Richard J. Rikoski
• John J. Leonard

This paper describes a new data association technique for the interpretation of wide-beam sonar measurements. The goal is to group sets of returns that originate from the same surface of an object. We consider the case of a moving observer that obtains range and bearing measurements of curved and/or faceted objects with binaural sonar operating in the specular wavelength regime. Rather than projecting sensor data into a Cartesian space before processing, we operate on a "raw" representation in terms of range, bearing, amplitude, and time. A binary geometric constraint is applied to pairs of consecutive measurements to rule out impossible assignments. A measurement flow model is proposed for validating triples of measurement. The performance of these techniques is illustrated via a set of experiments using a wide-beam 500 kHz binaural sonar system, demonstrating effective perceptual grouping and mapping with sonar echoes originating from a set of objects, despite the presence of navigation error.

ICRA Conference 2002 Conference Paper

Cooperative Concurrent Mapping and Localization

• John W. Fenwick
• Paul Newman 0001
• John J. Leonard

Autonomous vehicles require the ability to build maps of an unknown environment while concurrently using these maps for navigation. Current algorithms for this concurrent mapping and localization (CML) problem have been implemented for single vehicles, but do not account for extra positional information available when multiple vehicles operate simultaneously. Multiple vehicles have the potential to map an environment more quickly and robustly than a single vehicle. This paper presents a cooperative CML algorithm that merges sensor and navigation information from multiple autonomous vehicles. The algorithm presented is based on stochastic estimation and uses a feature-based approach to extract landmarks from the environment. The theoretical framework for the collaborative CML algorithm is presented, and a convergence theorem central to the cooperative CML problem. is proved for the first time. This theorem quantifies the performance gains of collaboration, allowing for determination of the number of cooperating vehicles required to accomplish a task. A simulated implementation of the collaborative CML algorithm demonstrates substantial performance improvement over non-cooperative CML.

ICRA Conference 2002 Conference Paper

Explore and Return: Experimental Validation of Real-Time Concurrent Mapping and Localization

• Paul Newman 0001
• John J. Leonard
• Juan D. Tardós
• José Neira

This paper describes a real-time implementation of feature-based concurrent mapping and localization (CML) running on a mobile robot in a dynamic indoor environment. Novel characteristics of this work include: (1) a hierarchical representation of uncertain geometric relationships that extends the SPMap framework, (2) use of robust statistics to perform extraction of line segments from laser data in real-time, and (3) the integration of CML with a "roadmap" path planning method for autonomous trajectory execution. These Innovations are combined to demonstrate the ability for a mobile robot to autonomously return back to its starting position within a few centimeters of precision, despite the presence of numerous people walking through the environment.

ICRA Conference 2002 Conference Paper

Stochastic Mapping Frameworks

• Richard J. Rikoski
• John J. Leonard
• Paul Newman 0001

Stochastic mapping is an approach to the concurrent mapping and localization problem. The approach is powerful because the feature and robot states are explicitly correlated. Improving the estimate of any state automatically improves the estimates of correlated states. This paper describes a number of extensions to the stochastic mapping framework, which are made possible by the incorporation of past vehicle states into the state vector to explicitly represent the robot's trajectory. Having access to past robot states simplifies the mapping, navigation, and cooperation. Experimental results using sonar data are presented.

IROS Conference 1999 Conference Paper

Mobile robot relocation using echolocation constraints

• Jong Hwan Lim
• John J. Leonard
• Seung Kyun Kang

Presents a technique for relocation of a mobile robot using sonar data. Relocation is the process of determining the position and orientation of a mobile robot with respect to the global reference frame in situations where no a priori estimate of the robot's location is available. A two dimensional model of the environment is employed consisting of planes, corners, edges, and cylinders. A physically based sonar sensor model is used to characterize the geometric constraints provided by echolocation measurements of different types of objects. The sensor model makes it possible to employ individual range returns as data features in a constraint-based search to determine the robot's position. To increase the efficiency of search, a hypothesize and verify technique is employed in which position of the robot are calculated from all possible combinations of two range returns that satisfy the measurement model. Accurate relocation is demonstrated from sparse sonar data in the presence of unmodeled objects.

IROS Conference 1998 Conference Paper

Adaptive concurrent mapping and localization using sonar

• Hans Jacob S. Feder
• John J. Leonard
• Christopher M. Smith

In order to create a truly autonomous vehicle, the task of concurrent mapping and localization (CML) in an a priori unknown environment is an important problem. Traditionally, the task of CML has been separated from the vehicle's motion and sensing strategies. We introduce a method for adaptive concurrent mapping and localization in unknown environments using a scanning sonar sensor. This method maximizes the information gained by the next action of the robot, given the space of available actions. The viability of the approach is shown in simulation and experiments. Results are shown for both adaptive control of motion and adaptive control of motion and sensing. Improved performance is demonstrated in comparison to straight-line motion and random motion.

ICRA Conference 1996 Conference Paper

Outlier rejection for autonomous acoustic navigation

• Jerome Vaganay
• John J. Leonard
• James G. Bellingham

Navigation is a critical requirement for the operation of autonomous underwater vehicles (AUVs). In this paper we present acoustic navigation results for the Odyssey II AUV obtained by using a Kalman filter that integrates dead-reckoning with acoustic range measurements made to an array of acoustic beacons pre-deployed in the operating environment. Because spurious acoustic measurements due to multipath propagation are common, initialization and outlier rejection techniques are addressed. The navigation algorithm has been extensively tested by post-processing of real data acquired by Odyssey II during field operations in a variety of environments. These include the Charles River Basin, the Atlantic Ocean (1. 5 km off the Florida coast), and the Pacific Ocean (375 km off the Oregon coast). Our results show improved performance over prior techniques based exclusively on fix computation and dead reckoning.

ICRA Conference 1995 Conference Paper

Underwater Sonar Data Fusion Using Efficient Multiple Hypothesis Algorithm

• John J. Leonard
• Bradley A. Moran
• Ingemar J. Cox
• Matthew L. Miller

This paper describes a geometric approach to underwater environmental modeling using sonar. We classify and localize geometric features of man-made objects by combining the boundary constraints of sonar returns obtained from multiple vantage points. The approach builds on our previous use of Reid's (1979) multiple hypothesis tracking (MHT) algorithm in order to resolve data association and motion correspondence ambiguities thereby to construct a model of the observed environment (Cox and Leonard, 1994). In particular we describe a new, computationally efficient implementation of the MHT algorithm originally reported in (Cox and Miller, 1995) and validate target models previously developed for air sonar. The technique fuses data by modeling the physics of underwater sonar and its interaction with different object features. We illustrate the approach in two dimensions with real acoustic data taken using a high-frequency (1. 25 MHz) pencil-beam profiling sonar, manually positioned along trajectories which circumnavigate prismatic objects.

AIJ Journal 1994 Journal Article

Modeling a dynamic environment using a Bayesian multiple hypothesis approach

• Ingemar J. Cox
• John J. Leonard

Dynamic world modeling requires the integration of multiple sensor observations obtained from multiple vehicle locations at different times. A crucial problem in this interpretation task is the presence of uncertainty in the origins of measurements (data association or correspondence uncertainty) as well as in the values of measurements (noise uncertainty). Almost all previous work in robotics has not distinguished between these two very different forms of uncertainty. In this paper we propose to model the uncertainty due to noise, e. g. the error in an object's position, by conventional covariance matrices. To represent the data association uncertainty, an hypothesis tree is constructed, the branches at any node representing different possible assignments of measurements to features. A rigorous Bayesian data association framework is then introduced that allows the probability of each hypothesis to be calculated. These probabilities can be used to guide an intelligent pruning strategy. The multiple hypothesis tree allows decisions concerning the assignment of measurements to be postponed. Instead, many different hypotheses are considered. Expected observations are predicted for each hypothesis and these are compared with actual measurements. Hypotheses that have their predictions supported by measurements increase in probability compared with hypotheses whose predictions are unsupported. By “looking ahead” two or three time steps and examining the probabilities at the leaves of the tree, very accurate assignment decisions can be made. For dynamic world modeling, the approach results in multiple world models at a given time step, each one representing a possible interpretation of all past and current measurements and each having an associated probability. In addition, each geometric feature has an associated covariance that models the uncertainty due to noise. This framework is independent of the sensing modality, being applicable to most temporal data association problems. It is therefore appropriate for the broad class of vision, acoustic and range sensors currently used on existing mobile robots. Preliminary results using ultrasonic range data demonstrate the feasibility of the approach.

IROS Conference 1991 Conference Paper

Simultaneous map building and localization for an autonomous mobile robot

• John J. Leonard
• Hugh F. Durrant-Whyte

Discusses a significant open problem in mobile robotics: simultaneous map building and localization, which the authors define as long-term globally referenced position estimation without a priori information. This problem is difficult because of the following paradox: to move precisely, a mobile robot must have an accurate environment map; however, to build an accurate map, the mobile robot's sensing locations must be known precisely. In this way, simultaneous map building and localization can be seen to present a question of 'which came first, the chicken or the egg? ' (The map or the motion?) When using ultrasonic sensing, to overcome this issue the authors equip the vehicle with multiple servo-mounted sonar sensors, to provide a means in which a subset of environment features can be precisely learned from the robot's initial location and subsequently tracked to provide precise positioning. >

IROS Conference 1990 Conference Paper

Dynamic map building for autonomous mobile robot

• John J. Leonard
• Hugh F. Durrant-Whyte
• Ingemar J. Cox

Presents an algorithm for autonomous map building and maintenance for a mobile robot. With each geometric target in the map the authors associate a validation measure to represent the belief in the validity of a target, in addition to the usual covariance matrix to represent spatial uncertainty. At each position update cycle, predicted features are generated for each target in the map and compared to features actually observed. Successful matches to targets with high validation measure are used for localization. Unpredicted observations are used to initialize target tracks for new environment features, while unobserved predictions result in a target's validation measure being decreased. They describe experimental results obtained with the algorithm that demonstrate successful map-building using real sonar data.

IROS Conference 1989 Conference Paper

Navigation by Correlating Geometric Sensor Data

• Hugh F. Durrant-Whyte
• John J. Leonard