We are motivated by the problem of autonomous vehicle performance validation. A key challenge is that an autonomous vehicle requires testing in every kind of driving scenario it could encounter, including rare events, to provide a strong case for safety and show there is no edge-case pathological behavior. Autonomous vehicle companies rely on potentially millions of miles driven in realistic simulation to expose the driving stack to enough miles to estimate rates and severity of collisions. To address scalability and coverage, we propose the use of a behavior foundation model, specifically a masked autoencoder (MAE), trained to reconstruct driving scenarios. We leverage the foundation model in two complementary ways: we (i) use the learned embedding space to group qualitatively similar scenarios together and (ii) fine-tune the model to label scenario difficulty based on the likelihood of a collision upon simulation. We use the difficulty scoring as importance weighting for the groups of scenarios. The result is an approach which can more rapidly estimate the rates and severity of collisions by prioritizing hard scenarios while ensuring exposure to every kind of driving scenario.

Autonomous navigation and exploration in unmapped environments remains a significant challenge in robotics due to the difficulty robots face in making commonsense inference of unobserved geometries. Recent advancements have demonstrated that generative modeling techniques, particularly diffusion models, can enable systems to infer these geometries from partial observation. In this work, we present implementation details and results for real-time, online occupancy prediction using a modified diffusion model. By removing attention-based visual conditioning and visual feature extraction components, we achieve a 73% reduction in runtime with minimal accuracy reduction. These modifications enable occupancy prediction across the entire map, rather than limiting it to the area around the robot where sensor data can be collected. We introduce a probabilistic update method for merging predicted occupancy data into running occupancy maps, resulting in a 71% improvement in predicting occupancy at map frontiers compared to previous methods. Finally, our code and a ROS node for on-robot operation can be found on our website: https://arpg.github.io/scenesense/.

Even though current vision language (V+L) models have achieved success in generating image captions, they often lack specificity and overlook various aspects of the image. Additionally, the attention learned through weak supervision operates opaquely and is difficult to control. To address these limitations, we propose the use of semantic roles as control signals in caption generation. Our hypothesis is that, by incorporating semantic roles as signals, the generated captions can be guided to follow specific predicate argument structures. To validate the effectiveness of our approach, we conducted experiments using data and compared the results with a baseline model VL-BART(CITATION). The experiments showed a significant improvement, with a gain of 45% in Smatch score (Standard NLP evaluation metric for semantic representations), demonstrating the efficacy of our approach. By focusing on specific objects and their associated semantic roles instead of providing a general description, our framework produces captions that exhibit enhanced quality, diversity, and controllability.

Millimeter Wave Radar is being adopted as a viable alternative to lidar and radar in adverse visually degraded conditions, such as in the presence of fog and dust. However, this sensor modality suffers from severe sparsity and noise under nominal conditions, which makes it difficult to use in precise applications such as mapping. This work presents a novel solution to generate accurate 3D maps from sparse radar point clouds. RMap uses a generative transformer architecture which upsamples, denoises, and fills the incomplete radar maps to resemble lidar maps. We test this method on the ColoRadar dataset to demonstrate its efficacy.

When exploring new areas, robotic systems generally exclusively plan and execute controls over geometry that has been directly measured. This planning paradigm can lead to unintuitive exploration or replanning latency when entering areas that were previous obstructed from view. To address this we present SceneSense, a real-time 3D diffusion model for synthesizing 3D occupancy information from partial observations that effectively predicts these occluded or out of view geometries for use in future planning and control frameworks. SceneSense uses a running occupancy map and a single RGB-D camera to generate predicted geometry around the platform at runtime, even when the geometry is occluded or out of view. Our architecture ensures that SceneSense never overwrites observed free or occupied space. By preserving the integrity of the observed map, SceneSense mitigates the risk of corrupting the observed space with generative predictions. While SceneSense is shown to operate well using a single RGB-D camera, the framework is flexible enough to extend to additional modalities. Unlike existing models that necessitate multiple views and offline scene synthesis, or are focused on filling gaps in observed data, our findings demonstrate that SceneSense is an effective approach to estimating unobserved local occupancy information at runtime. Local occupancy predictions from SceneSense are shown to better represent the ground truth occupancy distribution during the test exploration trajectories than the running occupancy map. The source code can be found on our website: https://arpg.github.io/scenesense/

This article surveys recent progress and discusses future opportunities for simultaneous localization and mapping (SLAM) in extreme underground environments. SLAM in subterranean environments, from tunnels, caves, and man-made underground structures on Earth, to lava tubes on Mars, is a key enabler for a range of applications, such as planetary exploration, search and rescue, disaster response, and automated mining, among others. SLAM in underground environments has recently received substantial attention, thanks to theDARPA Subterranean (SubT) Challenge, a global robotics competition aimed at assessing and pushing the state of the art in autonomous robotic exploration and mapping in complex underground environments. This article reports on the state of the art in underground SLAM by discussing different SLAM strategies and results across six teams that participated in the three-year-long SubT competition. In particular, the article has four main goals. First, we review the algorithms, architectures, and systems adopted by the teams; particular emphasis is put on light detection and ranging (LIDAR)-centric SLAM solutions (the go-to approach for virtually all teams in the competition), heterogeneous multirobot operation (including both aerial and ground robots), and real-world underground operation (from the presence of obscurants to the need to handle tight computational constraints). We do not shy away from discussing the “dirty details” behind the different SubT SLAM systems, which are often omitted from technical papers. Second, we discuss the maturity of the field by highlighting what is possible with the current SLAM systems and what we believe is within reach with some good systems engineering. Third, we outline what we believe are fundamental open problems, which are likely to require further research to break through. Finally, we provide a list of open-source SLAM implementations and datasets that have been produced during the SubT challenge and related efforts and constitute a useful resource for researchers and practitioners.

We survey the current state of millimeter-wave (mmWave) radar applications in robotics with a focus on unique capabilities, and discuss future opportunities based on the state of the art. Frequency modulated continuous wave mmWave radars operating in the 76–81 GHz range are an appealing alternative to lidars, cameras, and other sensors operating in the near-visual spectrum. Radar has been made more widely available in new packaging classes, more convenient for robotics and its longer wavelengths have the ability to bypass visual clutter, such as fog, dust, and smoke. We begin by covering radar principles as they relate to robotics. We then review the relevant new research across a broad spectrum of robotics applications beginning with motion estimation, localization, and mapping. We then cover object detection and classification, and then close with an analysis of current datasets and calibration techniques that provide entry points into radar research.

Typical algorithms for point cloud registration such as Iterative Closest Point (ICP) require a favorable initial transform estimate between two point clouds in order to perform a successful registration. State-of-the-art methods for choosing this starting condition rely on stochastic sampling or global optimization techniques such as branch and bound. In this work, we present a new method based on Bayesian optimization for finding the critical initial ICP transform. We provide three different configurations for our method which highlights the versatility of the algorithm to both find rapid results and refine them in situations where more runtime is available such as offline map building. Experiments are run on popular data sets and we show that our approach outperforms state-of-the-art methods when given similar computation time. Furthermore, it is compatible with other improvements to ICP, as it focuses solely on the selection of an initial transform, a starting point for all ICP-based methods.

As vision processing and natural language processing continue to advance, there is increasing interest in multimodal applications, such as image retrieval, caption generation, and human-robot interaction. These tasks require close alignment between the information in the images and text. In this paper, we present a new multimodal dataset that combines state of the art semantic annotation for language with the bounding boxes of corresponding images. This richer multimodal labeling supports cross-modal inference for applications in which such alignment is useful. Our semantic representations, developed in the natural language processing community, abstract away from the surface structure of the sentence, focusing on specific actions and the roles of their participants, a level that is equally relevant to images. We then utilize these representations in the form of semantic role labels in the captions and the images and demonstrate improvements in standard tasks such as image retrieval. The potential contributions of these additional labels is evaluated using a role-aware retrieval system based on graph convolutional and recurrent neural networks. The addition of semantic roles into this system provides a significant increase in capability and greater flexibility for these tasks, and could be extended to state-of-the-art techniques relying on transformers with larger amounts of annotated data.

Dense image alignment works by minimizing the photometric error of two images since it is assumed that the illumination changes between images close in time remain the same—this is what is called the brightness constancy assumption. However, this assumption does not hold with long-term maps since illumination changes continually from day to day (morning, afternoon, evening) and is dependent on certain external conditions like weather or even seasons. In this work, we present an image registration algorithm based on the Normalized Information Distance (NID) that is shown to be robust to extreme illumination changes comparing to the traditional direct methods. The pose is estimated by minimizing the NID function with the help of the nonlinear least square optimization library G2O. We share our source code1(CPU and GPU version) for the benefit of the community, which can be a strong basis for future tracking and mapping system based on NID.

Collaborative human-robot field operations rely on timely decision-making and coordination, which can be challenging for heterogeneous teams operating in large-scale deployments. In this work, we present the design of an immersive, mixed reality (MR) interface to support sense-making and situational awareness based on the data collection capabilities of both human and robotic team members. Our solution integrates state-of-the-art methods in environment mapping and MR so that users may gain rapid insights regarding the working environment, the current and previous locations of human and robot team members, and the environment data such team members have collected. We describe the implementation of our system, share lessons learned in collaborating with emergency responders throughout our design process, and offer a vision for the use of immersive displays for human-robot field team deployments in large-scale outdoor environments.

With significant progress being made toward improving endoscope technology such as capsule endoscopy and robotic endoscopy, the development of advanced strategies for manipulating, controlling, and more generally, easing the accessibility of these devices for physicians is an important next step. This article presents an autonomous navigation strategy for use in endoscopy, utilizing a state-dependent region estimation approach to allow for multimodal control design. This region estimator is evaluated for its accuracy in predicting yaw angle of the camera relative to the lumen center, and for estimating the location of the camera based on overall haustra morphology within the colon. To assess the utility of this region estimator, multimodal control is used to allow for autonomous navigation of the Endoculus, a robotic capsule endoscope, within a benchtop, to-scale, simulated colon. The estimation approach is presented and tested, demonstrating successful tracking of fixed velocity rotations at speeds up to 40°/s and allowing for curve anticipation approximately 10 cm before entering a curved section of the simulator. Finally, the multimodal control strategy utilizing this estimator is tested within the simulator over a variety of anatomic configurations. This strategy proves successful for navigation in both straight sections of this simulator and in tightly curved sections as small as 8 cm radius of curvature, with average velocities reaching 2.61 cm/s in straight sections and 0.99 cm/s in curved sections.

We present an approach for estimating the body-frame velocity of a mobile robot. We combine measurements from a millimeter-wave radar-on-a-chip sensor and an inertial measurement unit (IMU) in a batch optimization over a sliding window of recent measurements. The sensor suite employed is lightweight, low-power, and is invariant to ambient lighting conditions. This makes the proposed approach an attractive solution for platforms with limitations around payload and longevity, such as aerial vehicles conducting autonomous exploration in perceptually degraded operating conditions, including subterranean environments. We compare our radar-inertial velocity estimates to those from a visual-inertial (VI) approach. We show the accuracy of our method is comparable to VI in conditions favorable to VI, and far exceeds the accuracy of VI when conditions deteriorate.

Many modern simultaneous localization and mapping (SLAM) techniques rely on sparse landmark-based maps due to their real-time performance. However, these techniques frequently assert that these landmarks are fixed in position over time, known as the static-world assumption. This is rarely, if ever, the case in most real-world environments. Even worse, over long deployments, robots are bound to observe traditionally static landmarks change, for example when an autonomous vehicle encounters a construction zone. This work addresses this challenge, accounting for changes in complex three-dimensional environments with the creation of a probabilistic filter that operates on the features that give rise to landmarks. To accomplish this, landmarks are clustered into cliques and a filter is developed to estimate their persistence jointly among observations of the landmarks in a clique. This filter uses estimated spatial-temporal priors of geometric objects, allowing for dynamic and semi-static objects to be removed from a formally static map. The proposed algorithm is validated in a 3D simulated environment.

Multi-robot cooperation requires agents to make decisions that are consistent with the shared goal without disregarding action-specific preferences that might arise from asymmetry in capabilities and individual objectives. To accomplish this goal, we propose a method named SLiCC: Stackelberg Learning in Cooperative Control. SLiCC models the problem as a partially observable stochastic game composed of Stackelberg bimatrix games, and uses deep reinforcement learning to obtain the payoff matrices associated with these games. Appropriate cooperative actions are then selected with the derived Stackelberg equilibria. Using a bi-robot cooperative object transportation problem, we validate the performance of SLiCC against centralized multi-agent Q-learning and demonstrate that SLiCC achieves better combined utility.

We address the problem of light source estimation in environments containing multiple, in-scene illuminants. While there have been several recent advances in this domain, current approaches are either ineffectual in the absence of specific visual cues or assume full knowledge of surface reflectance. We develop a more general framework, which works directly on noisy, colored 3D reconstructions, as produced by RGB-D video sequences. To model multiple light sources, we employ a large set of potential lights. Using physically-based rendering techniques, we calculate each light's capacity to illuminate the scene. From this assessment, we formulate a non-linear, least squares optimization problem to determine which lights to activate. In contrast with traditional approaches in which photometric error is minimized, our objective function resembles that found in intrinsic image decomposition. This obviates estimating reflectance as an intermediate step. We evaluate our framework on both real-world and synthetic datasets, illuminated by up to three light sources, and show it is capable of modeling complex lighting scenarios with high-fidelity.

When registering 3-D point clouds it is expected that some points in one cloud do not have corresponding points in the other cloud. These non-correspondences are likely to occur near one another, as surface regions visible from one sensor pose are obscured or out of frame for another. In this work, a hidden Markov random field model is used to capture this prior within the framework of the iterative closest point algorithm. The EM algorithm is used to estimate the distribution parameters and learn the hidden component memberships. Experiments are presented demonstrating that this method outperforms several other outlier rejection methods when the point clouds have low or moderate overlap.

We present a dataset for evaluating the performance of visual-inertial odometry (VIO) systems employing an onboard light source. The dataset consists of 39 sequences, recorded in mines, tunnels, and other dark environments, totaling more than 160 minutes of stereo camera video and IMU data. In each sequence, the scene is illuminated by an onboard light of approximately 1300, 4500, or 9000 lumens. We accommodate both direct and indirect visual odometry methods by providing the geometric and photometric camera calibrations (i.e. response, attenuation, and exposure times). In contrast with existing datasets, we also calibrate the light source itself and publish data for inferring more complex light models. Ground-truth position data are available for a subset of sequences, as captured by a Leica total station. All remaining sequences start and end at the same position, permitting the use of total accumulated drift as a metric for evaluation. Using our proposed benchmark, we analyze the performance of several start-of-the-art VO and VIO frame-works. The full dataset, including sensor data, calibration sequences, and evaluation scripts, is publicly available online at http://arpg.colorado.edu/research/oivio.

Kalman filters are routinely used for many data fusion applications including navigation, tracking, and simultaneous localization and mapping problems. However, significant time and effort is frequently required to tune various Kalman filter model parameters, e.g. process noise covariance, pre-whitening filter models for non-white noise, etc. Conventional optimization techniques for tuning can get stuck in poor local minima and can be expensive to implement with real sensor data. To address these issues, a new “black box” Bayesian optimization strategy is developed for automatically tuning Kalman filters. In this approach, performance is characterized by one of two stochastic objective functions: normalized estimation error squared (NEES) when ground truth state models are available, or the normalized innovation error squared (NIS) when only sensor data is available. By intelligently sampling the parameter space to both learn and exploit a nonparametric Gaussian process surrogate function for the NEESINIS costs, Bayesian optimization can efficiently identify multiple local minima and provide uncertainty quantification on its results.

This paper is concerned with system identification and the calibration of parameters of dynamic models used in different robotic platforms. A constant time algorithm has been developed in order to automatically calibrate the parameters of a high-fidelity dynamical model for a robotic platform. The presented method is capable of choosing informative motion segments in order to calibrate model parameters in constant time while also calculating a confidence level on each estimated parameter. Simulations and experiments with a ⅛thscale four wheel drive vehicle are performed to calibrate two of the parameters of test vehicle which demonstrate the accuracy and efficiency of the approach.

Sparse feature-based maps provide a compact representation of the environment that admit efficient algorithms, for example simultaneous localization and mapping. These representations typically assume a static world and therefore contain static map features. However, since the world contains dynamic elements, determining when map features no longer correspond to the environment is essential for long-term utility. This work develops a feature-based model of the environment which evolves over time through feature persistence. Moreover, we augment the state-of-the-art sparse mapping model with a correlative structure that captures spatio-temporal properties, e.g. that nearby features frequently have similar persistence. We show that such relationships, typically addressed through an ad hoc formalism focusing only on feature repeatability, are crucial to evaluate through a probabilistically principled approach. The joint posterior over feature persistence can be computed efficiently and used to improve online data association decisions for localization. The proposed algorithms are validated in numerical simulation and using publicly available data sets.

Self-driving vehicles are being increasingly deployed in the wild. One of the most important next hurdles for autonomous driving is how such vehicles will optimally interact with one another and with their surroundings. In this paper, we consider the lane changing problem that is fundamental to road-bound multi-vehicle systems, and approach it through a combination of deep reinforcement learning (DRL) and game theory. We introduce a proactive-passive lane changing framework and formulate the lane changing problem as a Markov game between the proactive and passive vehicles. Based on different approaches to carry out DRL to solve the Markov game, we propose an asynchronous lane changing scheme as in a single-agent RL setting and a synchronous cooperative lane changing scheme that takes into consideration the adaptive behavior of the other vehicle in a vehicle's decision. Experimental results show that the synchronous scheme can effectively create and find proper merging moment after sufficient training. The framework and solution developed here demonstrate the potential of using reinforcement learning to solve multi-agent autonomous vehicle tasks such as the lane changing as they are formulated as Markov games.

Capsule endoscopes have gained popularity over the last decade as minimally invasive devices for diagnosing gastrointestinal abnormalities such as colorectal cancer. While this technology offers a less invasive and more convenient alternative to traditional scopes, these capsules are only able to provide observational capabilities due to their passive nature. With the addition of a reliable mobility system and a real-time navigation system, capsule endoscopes could transform from observational devices into active surgical tools, offering biopsy and therapeutic capabilities and even autonomous navigation in a single minimally invasive device. In this work, a vision system is developed to allow for autonomous lumen center tracking and haustral fold identification and tracking during colonoscopy. This system is tested for its ability to accurately identify and track multiple haustral folds across many frames in both simulated and in vivo video, and the lumen center tracking is tested onboard a robotic endoscope platform (REP) within an active simulator to demonstrate autonomous navigation. In addition, real-time localization is demonstrated using open source ORB-SLAM2. The vision system successfully identified 95.6% of Haustral folds in simulator frames and 70.6% in in vivo frames and false positives occurred in less than 1% of frames. The center tracking algorithm showed in vivo center estimates within a mean error of 6.6% of physician estimates and allowed for the REP to traverse 2 m of the active simulator in 6 minutes without intervention.

We present an approach to path following using so-called control funnel functions. Synthesizing controllers to “robustly” follow a reference trajectory is a fundamental problem for autonomous vehicles. Robustness, in this context, requires our controllers to handle a specified amount of deviation from the desired trajectory. Our approach considers a timing law that describes how fast to move along a given reference trajectory and a control feedback law for reducing deviations from the reference. We synthesize both feedback laws using “control funnel functions” that jointly encode the control law as well as its correctness argument over a mathematical model of the vehicle dynamics. We adapt a previously described demonstration-based learning algorithm to synthesize a control funnel function as well as the associated feedback law. We implement this law on top of a 1/8th scale autonomous vehicle called the Parkour car. We compare the performance of our path following approach against a trajectory tracking approach by specifying trajectories of varying lengths and curvatures. Our experiments demonstrate the improved robustness obtained from the use of control funnel functions.

We present a solution for online simultaneous localization and mapping (SLAM) self-calibration in the presence of drift in calibration parameters in order to support accurate long-term operation. Calibration parameters such as the camera focal length or camera-to-IMU extrinsics are frequently subject to drift over long periods of operation, inducing cumulative error in the reconstruction. The key contributions are modeling calibration parameters as a spatiotemporal quantity: sensor-to-sensor spatial calibration and sensor intrinsic parameters are continuously time-varying, with statistical tests for change detection and regression. An analysis of the long term effects of inappropriately modeling time-varying sensor calibration is also provided. Constant-time operation is achieved by selecting only a fixed number of informative segments of the trajectory for calibration parameter estimation, giving the added benefit of avoiding early linearization errors by not rolling past measurements into a prior distribution. Our approach is validated with simulated and real-world data.