Visual topic modeling (VTM) provides key insight into data sets based on learned semantic topic models. The Gaussian-Dirichlet Random Field (GDRF), a state-of-the-art VTM technique, models these semantic topics in continuous space as densities. However, ambiguity in learned topics is a disadvantage of such Dirichlet-based VTM algorithms. We propose the Guided Gaussian-Dirichlet Random Field (GGDRF). Our method applies Dirichlet Forest priors from natural language processing (NLP) to the vision domain as a way to embed visual scientific knowledge into the estimation process. This modification and addition to the GDRF provides a key shift from unsupervised machine learning to semi-supervised machine learning in the robotic VTM domain. We show through simulation and real-world underwater data that the proposed GGDRF outperforms the previous GDRF method both quantitatively and qualitatively by improving alignment between estimated topics and scientific interests.

With the goal of developing low-cost and innovative perception and localization techniques for autonomous vehicles, this work explores a system that solely relies on a LoRa receiver and a magnetometer for agent localization within urban environments. Using the received signal strength from LoRa beacons distributed across a test area of 16,000 square meters, a model of expected RSSI values per beacon is estimated using Gaussian Process (GP) regression. Motion is estimated using a probabilistic signal similarity classifier, and localization is obtained via a particle filter. Our experiments demonstrate that our proposed system is able to estimate our location to within three meters RMSE when the agent is within the convex hull of prior data. In real-world scenarios, characterized by signal interference and environmental complexities, our approach highlights the potential of leveraging affordable technology such as LoRa receivers and magnetometers for robust and accurate location estimation in complex urban environments. The integration of low-cost LoRa devices, Gaussian Process regression, particle filtering and our novel signal similarity motion estimator offers a promising avenue for achieving cost-effective localization solutions without compromising accuracy or reliability.

Fiducial markers are important tools for robotic navigation and imaging, enabling accurate localization and tracking of objects in challenging environments. In this paper, we present AcTag, a new fiducial marker design for use underwater with imaging sonar and cameras, as well as a method for the detection of AcTags within acoustic images. High amounts of noise and a nonlinear projection model make it difficult to use imaging sonar in autonomous localization and mapping. In order to expand the use of imaging sonar in autonomous underwater vehicles, our markerb design and detection algorithm for sonar images facilitate the identification of four unique landmarks per tag, and provide relative range and azimuth values to each landmark. We evaluate our marker and detection algorithm with simulated and real-world sonar data, reporting on the false positive and true positive rates, as well as the estimated error for the range and azimuth estimates per landmark. We also release an open-source library for generating tag families and detecting the tags.

Due to the difficulty and expense of underwater field trials, a high fidelity underwater simulator is a necessity for testing and developing algorithms. To fill this need, we present HoloOcean, an open source underwater simulator, built upon Unreal Engine 4 (UE4). HoloOcean comes equipped with multi-agent support, various sensor implementations of common underwater sensors, and simulated communications support. We also implement a novel sonar sensor model that leverages an octree representation of the environment for efficient and realistic sonar imagery generation. Due to being built upon UE4, new environments are straightforward to add, enabling easy extensions to be built. Finally, HoloOcean is controlled via a simple python interface, allowing simple installation via pip, and requiring few lines of code to execute simulations.

Knowledge of 3-D object shape is of great importance to robot manipulation tasks, but may not be readily available in unstructured environments. While vision is often occluded during robot-object interaction, high-resolution tactile sensors can give a dense local perspective of the object. However, tactile sensors have limited sensing area and the shape representation must faithfully approximate non-contact areas. In addition, a key challenge is efficiently incorporating these dense tactile measurements into a 3-D mapping framework. In this work, we propose an incremental shape mapping method using a GelSight tactile sensor and a depth camera. Local shape is recovered from tactile images via a learned model trained in simulation. Through efficient inference on a spatial factor graph informed by a Gaussian process, we build an implicit surface representation of the object. We demonstrate visuo-tactile mapping in both simulated and real-world experiments, to incrementally build 3-D reconstructions of household objects.

This paper presents a method for the robust selection of measurements in a simultaneous localization and mapping (SLAM) framework. Existing methods check consistency or compatibility on a pairwise basis, however many measurement types are not sufficiently constrained in a pairwise scenario to determine if either measurement is inconsistent with the other. This paper presents group-$k$consistency maximization ($\mathrm{G}k\text{CM}$) that estimates the largest set of measurements that is internally group-$k$consistent. Solving for the largest set of group-$k$consistent measurements can be formulated as an instance of the maximum clique problem on generalized graphs and can be solved by adapting current methods. This paper evaluates the performance of$\mathrm{G}k\text{CM}$using simulated data and compares it to pairwise consistency maximization (PCM) presented in previous work.

Sonar sensors play an integral part in underwater robotic perception by providing imagery at long distances where standard optical cameras cannot. They have proven to be an important part in various robotic algorithms including localization, mapping, and structure from motion. Unfortunately, generating realistic sonar imagery for algorithm development is difficult due to the high cost of field trials and lack of simulation methods. To remove these obstacles, we present various upgrades to the sonar simulation method in HoloOcean, our open-source marine robotics simulator. In particular, we improve the noise modeling using a novel cluster-based multipath ray-tracing algorithm, various probabilistic noise models, and material dependence. We also develop and integrate simulated models for side-scan, single-beam, and multibeam profiling sonars.

In this work, we investigate the problem of incre-mentally solving constrained non-linear optimization problems formulated as factor graphs. Prior incremental solvers were either restricted to the unconstrained case or required periodic batch relinearizations of the objective and constraints which are expensive and detract from the online nature of the algorithm. We present InCOpt, an Augmented Lagrangian-based incremental constrained optimizer that views matrix operations as message passing over the Bayes tree. We first show how the linear system, resulting from linearizing the constrained objective, can be represented as a Bayes tree. We then propose an algorithm that views forward and back substitutions, which naturally arise from solving the Lagrangian, as upward and downward passes on the tree. Using this formulation, In-COpt can exploit properties such as fluid/online relinearization leading to increased accuracy without a sacrifice in runtime. We evaluate our solver on different applications (navigation and manipulation) and provide an extensive evaluation against existing constrained and unconstrained solvers.

We address the problem of class agnostic, joint instance segmentation of scene data. While learning-based semantic instance segmentation methods have achieved impressive progress, their use is limited in robotics applications due to reliance on expensive training data annotations and assumptions of single sensor modality or known object classes. We propose a novel graph-based instance segmentation approach that combines information from a 2D image sequence and a 3D point cloud capturing the scene. Our approach propagates information with a general graph representation to produce a segmentation taking into account both geometric and photometric information. This allows us to leverage information from complementary sensor modalities without requiring training data. Our method shows improved object recall and boundary identification over state-of-the-art RGB-D segmentation methods. We demonstrate generality by evaluating on both RGB-D data and a LiDAR+image sensor data.

We address the problem of image registration to a compressed 3D map. While this is most often performed by comparing LiDAR scans to the point cloud based map, it depends on an expensive LiDAR sensor at run time and the large point cloud based map creates overhead in data storage and transmission. Recently, efforts have been underway to replace the expensive LiDAR sensor with cheaper cameras and perform 2D-3D localization. In contrast to the previous work that learns relative pose by comparing projected depth and camera images, we propose HyperMap, a paradigm shift from online depth map feature extraction to offline 3D map feature computation for the 2D-3D camera registration task through end-to-end training. In the proposed pipeline, we first perform offline 3D sparse convolution to extract and compress the voxelwise hypercolumn features for the whole map. Then at run-time, we project and decode the compressed map features to the rough initial camera pose to form a virtual feature image. A Convolutional Neural Network (CNN) is then used to predict the relative pose between the camera image and the virtual feature image. In addition, we propose an efficient occlusion handling layer, specifically designed for large point clouds, to remove occluded points in projection. Our experiments on synthetic and real datasets show that, by moving the feature computation load offline and compressing, we reduced map size by 87−94% while maintaining comparable or better accuracy.

Tactile perception is central to robot manipulation in unstructured environments. However, it requires contact, and a mature implementation must infer object models while also accounting for the motion induced by the interaction. In this work, we present a method to estimate both object shape and pose in real-time from a stream of tactile measurements. This is applied towards tactile exploration of an unknown object by planar pushing. We consider this as an online SLAM problem with a nonparametric shape representation. Our formulation of tactile inference alternates between Gaussian process implicit surface regression and pose estimation on a factor graph. Through a combination of local Gaussian processes and fixed-lag smoothing, we infer object shape and pose in real-time. We evaluate our system across different objects in both simulated and real-world planar pushing tasks.

We aim to assess the performance of LiDAR-to-map registration on compressive maps. Modern autonomous vehicles utilize pre-built HD (High-Definition) maps to perform sensor-to-map registration, which recovers pose estimation failures and reduces drift in a large-scale environment. However, sensor-to-map registration is usually realized by registering the sensor to a dense 3D model, which occupies massive storage space in the HD map and requires much data processing overhead. Although smaller 3D models are preferable, the optimal compressive map format for preservation of the best registration performance remains unclear.In this paper, we propose a novel and challenging benchmark to evaluate existing LiDAR-to-map registration methods from three perspectives: map compressibility, robustness, and precision. We compared various map formats, including raw points, hierarchical GMMs, and feature points, and show their performance trade-offs between compressibility and robustness on real-world LiDAR datasets: KITTI Odometry Dataset and Argoverse Tracking Dataset. Our benchmark reveals that state-of-the-art deep feature point based methods outperform traditional methods significantly when the map size budget is high. However, when map size budget is low, deep methods are outperformed by the methods using simpler models in Argoverse Tracking Dataset due to poor spatial coverage. In addition, we observe that the recently published TEASER++ significantly outperforms RANSAC for the feature point methods. Our analysis provides a valuable reference for the community to design budgeted real-world systems and find potential research opportunities. We will release the benchmark for public use.

A robot operating in the world constantly receives information about its environment in the form of new measurements at every time step. Smoothing-based estimation methods seek to optimize for the most likely robot state estimate using all measurements up till the current time step. Existing methods solve for this smoothing objective efficiently by framing the problem as that of incremental unconstrained optimization. However, in many cases observed measurements and knowledge of the environment is better modeled as hard constraints derived from real-world physics or dynamics. A key challenge is that the new optimality conditions introduced by the hard constraints break the matrix structure needed for incremental factorization in these incremental optimization methods. Our key insight is that if we leverage primal-dual methods, we can recover a matrix structure amenable to incremental factorization. We propose a framework ICS that combines a primal-dual method like the Augmented Lagrangian with an incremental Gauss Newton approach that reuses previously computed matrix factorizations. We evaluate ICS on a set of simulated and real-world problems involving equality constraints like object contact and inequality constraints like collision avoidance.

In this paper, we present an active SLAM framework for volumetric exploration of 3D underwater environments with multibeam sonar. Recent work in integrated SLAM and planning performs localization while maintaining volumetric free-space information. However, an absence of informative loop closures can lead to imperfect maps, and therefore unsafe behavior. To solve this, we propose a navigation policy that reduces vehicle pose uncertainty by balancing between volumetric exploration and revisitation. To identify locations to revisit, we build a 3D visual dictionary from real-world sonar data and compute a metric of submap saliency. Revisit actions are chosen based on propagated pose uncertainty and sensor information gain. Loop closures are integrated as constraints in our pose-graph SLAM formulation and these deform the global occupancy grid map. We evaluate our performance in simulation and real-world experiments, and highlight the advantages over an uncertainty-agnostic framework.

Fast, aerial navigation in cluttered environments requires a suitable map representation for path planning. In this paper, we propose the use of an efficient, structured multiresolution representation that expands the sensor range of dense local grids for memory-constrained platforms. While similar data structures have been proposed, we avoid processing redundant occupancy information and use the organization of the grid to improve efficiency. By layering 3D circular buffers that double in resolution at each level, obstacles near the robot are represented at finer resolutions while coarse spatial information is maintained at greater distances. We also introduce a novel method for efficiently calculating the Euclidean distance transform on the multiresolution grid by leveraging its structure. Lastly, we utilize our proposed framework to demonstrate improved stereo camera-based MAV obstacle avoidance with an optimization-based planner in simulation.

In this paper, we introduce an ambiguity-aware robust active SLAM (ARAS) framework that makes use of multi-hypothesis state and map estimations to achieve better robustness. Ambiguous measurements can result in multiple probable solutions in a multi-hypothesis SLAM (MH-SLAM) system if they are temporarily unsolvable (due to insufficient information), our ARAS aims at taking all these probable estimations into account explicitly for decision making and planning, which, to the best of our knowledge, has not yet been covered by any previous active SLAM approach (which mostly consider a single hypothesis at a time). This novel ARAS framework 1) adopts local contours for efficient multi-hypothesis exploration, 2) incorporates an active loop closing module that revisits mapped areas to acquire information for hypotheses pruning to maintain the overall computational efficiency, and 3) demonstrates how to use the output target pose for path planning under the multi-hypothesis estimations. Through extensive simulations and a real-world experiment, we demonstrate that the proposed ARAS algorithm can actively map general indoor environments more robustly than a similar single-hypothesis approach in the presence of ambiguities.

In this paper we present a novel multi-stereo visual-inertial odometry (VIO) framework which aims to improve the robustness of a robot's state estimate during aggressive motion and in visually challenging environments. Our system uses a fixed-lag smoother which jointly optimizes for poses and landmarks across all stereo pairs. We propose a 1-point RANdom SAmple Consensus (RANSAC) algorithm which is able to perform outlier rejection across features from all stereo pairs. To handle the problem of noisy extrinsics, we account for uncertainty in the calibration of each stereo pair and model it in both our front-end and back-end. The result is a VIO system which is able to maintain an accurate state estimate under conditions that have typically proven to be challenging for traditional state-of-the-art VIO systems. We demonstrate the benefits of our proposed multi-stereo algorithm by evaluating it with both simulated and real world data. We show that our proposed algorithm is able to maintain a state estimate in scenarios where traditional VIO algorithms fail.

An accurate characterization of pose uncertainty is essential for safe autonomous navigation. Early pose uncertainty characterization methods proposed by Smith, Self, and Cheeseman (SCC) used coordinate-based first-order methods to propagate uncertainty through nonlinear functions such as pose composition (head-to-tail), pose inversion, and relative pose extraction (tail-to-tail). Characterizing uncertainty in the Lie algebra of the special Euclidean group results in better uncertainty estimates. However, existing Lie-group-based uncertainty propagation techniques assume that individual poses are independent. After solving a pose graph, however, the entire trajectory is jointly distributed as factors induce correlation. Hence, the independence assumption does not capture reality. In addition, prior work has focused primarily on the pose composition operation. This article develops a framework for modeling the uncertainty of jointly distributed poses and describes how to perform the equivalent of the SSC pose operations while characterizing uncertainty in the Lie algebra. Evaluation on simulated and open-source datasets shows that the proposed methods result in more accurate uncertainty estimates and thus more accurate filtering of potential loop closures. An accompanying C++ library implementation is also released.

Autonomous navigation requires an accurate model or map of the environment. While dramatic progress in the prior two decades has enabled large-scale simultaneous localization and mapping (SLAM), the majority of existing methods rely on non-linear optimization techniques to find the maximum likelihood estimate (MLE) of the robot trajectory and surrounding environment. These methods are prone to local minima and are thus sensitive to initialization. Several recent papers have developed optimization algorithms for the Pose-Graph SLAM problem that can certify the optimality of a computed solution. Though this does not guarantee a priori that this approach generates an optimal solution, a recent extension has shown that when the noise lies within a critical threshold that the solution to the optimization algorithm is guaranteed to be optimal. To address the limitations of existing approaches, this paper illustrates that the Pose-Graph SLAM and Landmark SLAM can be formulated as polynomial optimization programs that are sum-of-squares (SOS) convex. This paper then describes how the Pose-Graph and Landmark SLAM problems can be solved to a global minimum without initialization regardless of noise level using the sparse bounded degree sum-of-squares (Sparse-BSOS) optimization method. Finally, the superior performance of the proposed approach when compared to existing SLAM methods is illustrated on graphs with several hundred nodes.

State-of-the-art robotic perception systems have achieved sufficiently good performance using Inertial Measurement Units (IMUs), cameras, and nonlinear optimization techniques, that they are now being deployed as technologies. However, many of these methods rely significantly on vision and often fail when visual tracking is lost due to lighting or scarcity of features. This paper presents a state-estimation technique for legged robots that takes into account the robot's kinematic model as well as its contact with the environment. We introduce forward kinematic factors and preintegrated contact factors into a factor graph framework that can be incrementally solved in real-time. The forward kinematic factor relates the robot's base pose to a contact frame through noisy encoder measurements. The preintegrated contact factor provides odometry measurements of this contact frame while accounting for possible foot slippage. Together, the two developed factors constrain the graph optimization problem allowing the robot's trajectory to be estimated. The paper evaluates the method using simulated and real sensory IMU and kinematic data from experiments with a Cassie-series robot designed by Agility Robotics. These preliminary experiments show that using the proposed method in addition to IMU decreases drift and improves localization accuracy, suggesting that its use can enable successful recovery from a loss of visual tracking.

This paper reports on a method for robust selection of inter-map loop closures in multi-robot simultaneous localization and mapping (SLAM). Existing robust SLAM methods assume a good initialization or an “odometry backbone” to classify inlier and outlier loop closures. In the multi-robot case, these assumptions do not always hold. This paper presents an algorithm called Pairwise Consistency Maximization (PCM) that estimates the largest pairwise internally consistent set of measurements. Finding the largest pairwise internally consistent set can be transformed into an instance of the maximum clique problem from graph theory, and by leveraging the associated literature it can be solved in realtime. This paper evaluates how well PCM approximates the combinatorial gold standard using simulated data. It also evaluates the performance of PCM on synthetic and real-world data sets in comparison with DCS, SCGP, and RANSAC, and shows that PCM significantly outperforms these methods.