To enable AI agents to interact seamlessly with both humans and 3D environments, they must not only perceive the 3D world accurately but also align human language with 3D spatial representations. While prior work has made significant progress by integrating language features into geometrically detailed 3D scene representations using 3D Gaussian Splatting (GS), these approaches rely on computationally intensive offline preprocessing of language features for each input image, limiting adaptability to new environments. In this work, we introduce Online Language Splatting, the first framework to achieve online, near real-time, open-vocabulary language mapping within a 3DGS-SLAM system without requiring pre-generated language features. The key challenge lies in efficiently fusing high-dimensional language features into 3D representations while balancing the computation speed, memory usage, rendering quality and open-vocabulary capability. To this end, we innovatively design: (1) a high-resolution CLIP embedding module capable of generating detailed language feature maps in 18ms per frame, (2) a two-stage online auto-encoder that compresses 768-dimensional CLIP features to 15 dimensions while preserving open-vocabulary capabilities, and (3) a color-language disentangled optimization approach to improve rendering quality. Experimental results show that our online method not only surpasses the state-of-the-art offline methods in accuracy but also achieves more than 40x efficiency boost, demonstrating the potential for dynamic and interactive AI applications.

Visual-inertial odometry (VIO), which fuses noisy inertial readings and camera measurements to provide 3D motion tracking, is a foundational component in many autonomous applications. With the increasing use of next-generation edge devices (e.g., AR/VR devices, nano drones, and mobile robotics) that are constrained by limited power, resources, and multitasking demands, balancing computational efficiency and accuracy in VIO estimators has become more critical than ever. Historically, state estimation algorithms have been developed using either optimization or filtering-based methods, with the key distinction being the ability to relinearize measurements and correct state estimates iteratively. It has been widely claimed that iterative methods improve accuracy by allowing for the reduction of error through relinearization at a higher computational demand. Conversely, filtering methods are more efficient but may suffer from significant linearization errors. However, these trade-offs have not been thoroughly examined in the context of visual-inertial motion tracking. In this paper, we conduct the first comprehensive study on the impact of iterative algorithms in sliding-window VIO. We analyze the relinearization of IMU and camera measurements separately, providing insights into how each affects system performance. By considering key factors such as system observability and measurement processes, we offer a deeper understanding of VIO estimator behavior. Our findings, backed by real-world tests, offer practical guidelines for balancing accuracy and efficiency, helping practitioners determine when to prioritize iterative methods or simpler filtering approaches while encouraging researchers and engineers to rethink VIO design for optimal resource allocation.

Energy-efficient visual-inertial motion tracking on SWAP-constrained edge devices (e.g., drones and AR glasses) is essential but challenging. Our previous work [1] introduced the first-of-its-kind quantized visual-inertial odometry (QVIO), utilizing either raw measurement quantization (zQVIO) or single-bit residual quantization (rQVIO). While QVIO has demonstrated significant data transfer reduction with competitive performance, it has limitations. Specifically, zQVIO directly quantizes raw measurements into multi-bit values, while requiring the ad-hoc inflation of measurement noise to account for quantization errors. On the other hand, rQVIO is limited to single-bit measurement with certain accuracy loss. This work introduces QVIO2 to address these issues. The proposed QVIO2 improves data quantization strategies and derives a Maximum A Posteriori (MAP) quantized estimator that rigorously handles both multi-bit and single-bit, raw and residual quantized measurements in a unified manner. These improvements lead to more communication-efficient and accurate systems. Additionally, we optimize the communication protocol to further reduce data transfer by eliminating unnecessary transmissions. Extensive numerical and experimental results demonstrate reduced communication requirements and improved accuracy. Compared to the previous QVIO system, zQVIO2 achieves the same accuracy with a 30 % reduction in data transfer, while rQVIO2 improves accuracy without increasing data communication. In real-world scenarios, our new zQVIO2 and rQVIO2 have demonstrated nearly no accuracy loss with only 4.6 bits and 3.5 bits of data communication, achieving compression rates of$7 \times$and$9.1 \times$.

Motion sensing and tracking with IMU data is essential for spatial intelligence, which however is challenging due to the presence of time-varying stochastic bias. IMU bias is affected by various factors such as temperature and vibration, making it highly complex and difficult to model analytically. Recent data-driven approaches using deep learning have shown promise in predicting bias from IMU readings. However, these methods often treat the task as a regression problem, overlooking the stochatic nature of bias. In contrast, we model bias, conditioned on IMU readings, as a probabilistic distribution and design a conditional diffusion model to approximate this distribution. Through this approach, we achieve improved performance and make predictions that align more closely with the known behavior of bias.

In the realm of robotics, the quest for achieving real-world autonomy, capable of executing large-scale and long-term operations, has positioned place recognition (PR) as a cornerstone technology. Despite the PR community's remarkable strides over the past two decades, garnering attention from fields like computer vision and robotics, the development of PR methods that sufficiently support real-world robotic systems remains a challenge. This article aims to bridge this gap by highlighting the crucial role of PR within the framework of simultaneous localization and mapping 2.0. This new phase in robotic navigation calls for scalable, adaptable, and efficient PR solutions by integrating advanced artificial intelligence technologies. For this goal, we provide a comprehensive review of the current state-of-the-art advancements in PR, alongside the remaining challenges, and underscore its broad applications in robotics. This article begins with an exploration of PR's formulation and key research challenges. We extensively review literature, focusing on related methods on place representation and solutions to various PR challenges. Applications showcasing PR's potential in robotics, key PR datasets, and open-source libraries are discussed.

The system observability analysis is of practical importance, for example, due to its ability to identify the unobservable directions of the estimated state which can influence estimation accuracy and help develop consistent and robust estimators. Recent studies focused on analyzing the observability of the state of various multisensor systems with a particular interest in unobservable directions induced by degenerate motions. However, those studies mostly stay in the specific sensor domain without aiding to extend the understanding to other heterogeneous systems. To this end, in this work, we provide degenerate motion analysis on general local and global sensor-paired systems, offering insights applicable to a wide range of existing navigation systems. Our analysis includes 9 degenerate motion identification including 5 already identified in literature and 4 new motions with both synchronous and asynchronous sensor-pair cases. Comprehensive numerical studies are conducted to verify those identified motions, show the effect of degenerate motion on state estimation, and demonstrate the generalizability of our analysis on various multisensor systems.

In this paper, we introduce a novel and efficient technique for consistent covariance recovery in nonlinear optimization-based Visual-Inertial Navigation Systems (VINS). Estimating uncertainty in real-time is crucial for evaluating system performance and enhancing downstream operations such as data association. However accessing the marginal covariance of the state variables of interest in optimization-based VINS presents a significant challenge – a computational bottleneck due to the need to invert the high-dimensional information (Hessian) matrix. In our recent work [1], the First-Estimates Jacobian (FEJ) methodology was used to properly fix state linearization points in the optimization-based VINS, which seems counter-intuitive but improves the estimation performance in both consistency and accuracy. Capitalizing on this unique aspect of the FEJ strategy, in this work we carefully design the covariance recovery algorithm to improve efficiency by avoiding redundant computation. Remarkably, our approach achieves a computational speed that is 4-10 times faster than the existing methods. Through comprehensive numerical evaluations across four state-of-the-art marginalization archetypes, we not only affirm the consistency of our covariance estimates but underscore its superior computational efficiency.

Achieving efficient and consistent localization with a prior map remains challenging in robotics. Conventional keyframe-based approaches often suffer from sub-optimal viewpoints due to limited field of view (FOV) and/or constrained motion, thus degrading the localization performance. To address this issue, we design a real-time tightly-coupled Neural Radiance Fields (NeRF)-aided visual-inertial navigation system (VINS). In particular, by effectively leveraging the NeRF’s potential to synthesize novel views, the proposed NeRF-VINS overcomes the limitations of traditional keyframe-based maps (with limited views) and optimally fuses IMU, monocular images, and synthetically rendered images within an efficient filter-based framework. This tightly-coupled fusion enables efficient 3D motion tracking with bounded errors. We extensively validate the proposed NeRF-VINS against the state-of-the-art methods that use prior map information, and demonstrate its ability to perform real-time localization, at 15 Hz, on a resource-constrained Jetson AGX Orin embedded platform.

In this paper, we strongly advocate square-root covariance (instead of information) filtering for Visual-Inertial Navigation Systems (VINS), in particular on resource-constrained edge devices, because of its superior efficiency and numerical stability. Although VINS have made tremendous progress in recent years, they still face resource stringency and numerical instability on embedded systems when imposing limited word length. To overcome these challenges, we develop an ultrafast and numerically-stable square-root filter (SRF)-based VINS algorithm (i.e., SR-VINS). The numerical stability of the proposed SR-VINS is inherited from the adoption of square-root covariance while the remarkable efficiency is largely enabled by the novel SRF update method that is based on our new permuted-QR (P-QR), which fully utilizes and properly maintains the upper triangular structure of the square-root covariance matrix. Furthermore, we choose a special ordering of the state variables which is amenable for (P-)QR operations in the SRF propagation and update and prevents unnecessary computation. The proposed SR-VINS is validated extensively through numerical studies, demonstrating that when the state-of-the-art (SOTA) filters have numerical difficulties, our SR-VINS has superior numerical stability, and remarkably, achieves efficient and robust performance on 32-bit single-precision float at a speed nearly twice as fast as the SOTA methods. We also conduct comprehensive real-world experiments to validate the efficiency, accuracy, and robustness of the proposed SR-VINS.

As edge devices equipped with cameras and inertial measurement units (IMUs) are emerging, it holds huge implications to endow these mobile devices with spatial computing capability. However, ultra-efficient visual-inertial estimation at the size, weight and power (SWAP)-constrained edge devices to provide accurate 3D motion tracking remains challenging. This is exacerbated by data transfer (between different processors and memory) that consumes significantly more energy than computing itself. To push the state of the art, this paper proposes the first-of-its-kind quantized visual-inertial odometry (QVIO) to offer energy-efficient 3D motion tracking. In particular, we first quantize raw visual measurements in an intuitive way with a given small number of bits and then perform an EKF update with these quantized measurements (termed zQVIO). To improve this ad-hoc quantizer (although it works well in practice), we systematically quantize each measurement residual into a single bit and perform maximum-a-posterior (MAP) estimation. measurements. Thanks to these quantizers, the proposed QVIO estimators significantly reduce the data transfer and thus improve energy efficiency. As shown in our extensive experiments, the proposed residual-quantized VIO (rQVIO) achieves remarkably competing performance even when using an average of only 3.7 bits per measurement, equivalent to a data reduction of 8.6 times compared to transmitting single-precision measurements.

Legged robots are emerging, and legged locomotion is in critical need, which requires precise leg-body kinematics to execute control commands or plan motion trajectories. This paper proposes online state estimation to determine legged kinematics of robots with an arbitrary number of legs, which includes the kinematic parameters of the leg-body transformation, time offset and the leg link lengths. In particular, we advocate an in-place dance gait for kinematic determination where the toes remain static on the ground and serve as static landmarks as in SLAM. As a visual-inertial sensor is typically available onboard robot and located at the floating base, we leverage efficient MSCKF-based visual-inertial navigation to estimate legged kinematics. To this end, we analytically derive the legged kinematic measurements and tightly fuse them along with visual-inertial measurements for MSCKF update of both the leg’s kinematics and body’s motion. The proposed method has been extensively validated in both simulations and experiments with different quadrupeds, showing its robustness and accuracy.

In many situations such as city delivery and wild inspection, quadrotors are often required to follow a predefined reference trajectory. However, these reference trajectories cannot be perfectly safe, resulting in conflicts between tracking the reference precisely, flying safely, and finishing the mission timely. This paper proposes to solve the above problem, by introducing a replanning framework that first generates a topological consistent collision-free initial path and then flexibly optimizes the rejoin point and trajectory duration to generate a smooth and safe local rejoining trajectory. To avoid local trajectory switching in different directions during high-frequency replanning, we propose a topology-preserving path search algorithm based on kinodynamic RRT*. To satisfy dynamic constraints, avoid delays, and achieve a smooth rejoin of the reference trajectory, we propose an optimization-based approach to refine the initial trajectory. The simulation results confirm that our proposed topological consistency and flexible optimization methods can reduce the risk of local trajectory and decrease obstacle avoidance delay for tracking reference trajectory. We also conduct real-world experiments in challenging environments and verify the effectiveness of our method.

To build commercial robots, skid-steering mechanical design is of increased popularity due to its manufacturing simplicity and unique mechanism. However, these also cause significant challenges on software and algorithm design, especially for the pose estimation (i.e., determining the robot’s rotation and position) of skid-steering robots, since they change their orientation with an inevitable skid. To tackle this problem, we propose a probabilistic sliding-window estimator dedicated to skid-steering robots, using measurements from a monocular camera, the wheel encoders, and optionally an inertial measurement unit (IMU). Specifically, we explicitly model the kinematics of skid-steering robots by both track instantaneous centers of rotation (ICRs) and correction factors, which are capable of compensating for the complexity of track-to-terrain interaction, the imperfectness of mechanical design, terrain conditions and smoothness, etc. To prevent performance reduction in robots’ long-term missions, the time- and location- varying kinematic parameters are estimated online along with pose estimation states in a tightly-coupled manner. More importantly, we conduct in-depth observability analysis for different sensors and design configurations in this paper, which provides us with theoretical tools in making the correct choice when building real commercial robots. In our experiments, we validate the proposed method by both simulation tests and real-world experiments, which demonstrate that our method outperforms competing methods by wide margins. Note to Practitioners—This paper was motivated by the problem of long-term pose estimation of the commonly commercial-used skid-steering robots with only low-cost sensors. Skid-steering robots change their orientation with a skid, which poses a significant challenge for pose estimation when using the wheel encoders. We propose to online estimate the robot’s kinematics, which succeeds in compensating for the complexity of track-to-terrain interaction, due to the slippage, the imperfectness of mechanical design, terrain conditions and smoothness. It is critical to estimate the kinematics and poses jointly to prevent performance reduction in robots’ long-term missions. We further theoretically analyze whether the kinematics parameters can be estimated under different sensor configurations, and find out the special degrade motions that make the parameters unobservable.

In this article, we revisit the optimal fusion of visual and inertial information from a monocular camera and an inertial measurement unit and propose a novel parallel visual-inertial simultaneous localization and mapping (SLAM) estimation framework in favor of the multithread computation on a single CPU. We start modeling the SLAM problem with a Bayesian batch estimator, and then split it into two submodules, localization and mapping, of different scales and processing rates, however, can thus run concurrently. The estimation consistency is taken into account in decoupling the two submodules so that when loop closure occurs the localization accuracy can seamlessly benefit from the mapping result via online global optimization, which distinguishes our solution from the others. To this end, we design the corresponding front-end and back-end to consistently solve localization and mapping in parallel, especially the hybrid robocentric and world-centric formulations are used for modeling the respective problems. We also demonstrate the effectiveness of the proposed method using both the synthetic data generated for Monte-Carlo simulations and diverse real datasets acquired in highly-dynamic, long-term, and large-scale SLAM scenarios. Simulation results validate the significantly improved consistency and accuracy by applying our method. Experimental results show the better (competitive at least) performance against a state-of-the-art method, while being capable of processing a huge amount of measurements in building large-scale mapswithoutblocking the high-accuracy real-time localization outputs.

State-of-the-art monocular visual-inertial odometry (VIO) approaches rely on sparse point features in part due to their efficiency, robustness, and prevalence, while ignoring high-level structural regularities such as planes that are common to man-made environments and can be exploited to further constrain motion. Generally, planes can be observed by a camera for significant periods of time due to their large spatial presence and thus, are amenable for long-term navigation. Therefore, in this paper, we design a novel real-time monocular VIO system that is fully regularized by planar features within a lightweight multi-state constraint Kalman filter (MSCKF). At the core of our method is an efficient robust monocular-based plane detection algorithm, which does not require additional sensing modalities such as a stereo or depth camera as commonly seen in the literature, while enabling real-time regularization of point features to environmental planes. Specifically, in the proposed MSCKF, long-lived planes are maintained in the state vector, while shorter ones are marginalized after use for efficiency. Planar regularities are applied to both in-state SLAM features and out-of-state MSCKF features, thus fully exploiting the environmental plane information to improve VIO performance. The proposed approach is evaluated with extensive Monte-Carlo simulations and different real-world experiments including an author-collected AR scenario, and shown to outperform the point-based VIO in structured environments. Video Demonstration https://youtu.be/bec7LbYaOS8AR Table Dataset https://github.com/rpng/ar_table_dataset

In this work, we present a comprehensive analysis of the application of the First-estimates Jacobian (FEJ) design methodology in nonlinear optimization-based Visual-Inertial Navigation Systems (VINS). The FEJ approach fixes system linearization points to preserve proper observability properties of VINS and has been shown to significantly improve the estimation performance of state-of-the-art filtering-based methods. However, its direct application to optimization-based estimators holds challenges and pitfalls, which we addressed in this paper. Specifically, we carefully examine the observability and its relation to inconsistency and FEJ, based on this, we explain how to properly apply and implement FEJ within four marginalization archetypes commonly used in non-linear optimizationbased frameworks. FEJ's effectiveness and applications to VINS are investigated and demonstrate significant performance improvements. Additionally, we offer a detailed discussion of results and guidelines on how to properly implement FEJ in optimization-based estimators.

As sensor calibration plays an important role in visual-inertial sensor fusion, this article performs an in-depth investigation of online self-calibration for robust and accurate visual-inertial state estimation. To this end, we first conduct complete observability analysis for visual-inertial navigation systems (VINS) with full calibration of sensing parameters, including inertial measurement unit (IMU)/camera intrinsics and IMU-camera spatial-temporal extrinsic calibration, along with readout time of rolling shutter (RS) cameras (if used). We study different inertial model variants containing intrinsic parameters that encompass most commonly used models for low-cost inertial sensors. With these models, the observability analysis of linearized VINS with full sensor calibration is performed. Our analysis theoretically proves the intuition commonly assumed in the literature—that is, VINS with full sensor calibration has four unobservable directions, corresponding to the system's global yaw and position, while all sensor calibration parameters are observable given fully excited motions. Moreover, we, for the first time, identify degenerate motion primitives for IMU and camera intrinsic calibration, which, when combined, may produce complex degenerate motions. We compare the proposedonlineself-calibration on commonly used IMUs against the state-of-artofflinecalibration toolbox Kalibr, showing that the proposed system achieves better consistency and repeatability. Based on our analysis and experimental evaluations, we also offer practical guidelines to effectively perform online IMU-camera self-calibration in practice.

We propose a keypoint-based object-level SLAM framework that can provide globally consistent 6DoF pose estimates for symmetric and asymmetric objects alike. To the best of our knowledge, our system is among the first to utilize the camera pose information from SLAM to provide prior knowledge for tracking keypoints on symmetric objects - ensuring that new measurements are consistent with the current 3D scene. Moreover, our semantic key-point network is trained to predict the Gaussian covariance for the keypoints that captures the true error of the prediction, and thus is not only useful as a weight for the residuals in the system's optimization problems, but also as a means to detect harmful statistical outliers without choosing a manual threshold. Experiments show that our method provides competitive performance to the state of the art in 6DoF object pose estimation, and at a real-time speed. Our code, pre-trained models, and keypoint labels are available https://github.com/rpng/suo_slam.

A navigation system which can output drift-free global trajectory estimation with local consistency holds great potential for autonomous vehicles and mobile devices. We propose a tightly-coupled GNSS-aided visual-inertial navigation system (GAINS) which is able to leverage the complementary sensing modality from a visual-inertial sensing pair, which provides high-frequency local information, and a Global Navigation Satellite System (GNSS) receiver with low-frequency global observations. Specifically, the raw GNSS measurements (including pseudorange, carrier phase changes, and Doppler frequency shift) are carefully leveraged and tightly fused within a visual-inertial framework. The proposed GAINS can accurately model the raw measurement uncertainties by canceling the atmospheric effects (e.g., ionospheric and tropospheric delays) which requires no prior model information. A robust state initialization procedure is presented to facilitate the fusion of global GNSS information with local visual-inertial odometry, and the spatiotemporal calibration between IMU-GNSS are also optimized in the estimator. The proposed GAINS is evaluated on extensive Monte-Carlo simulations on a trajectory generated from a large-scale urban driving dataset with specific verification for each component (i.e., online calibration and system initialization). GAINS also demonstrates competitive performance against existing state-of-the-art methods on a publicly available dataset with ground truth.

In this paper, we propose a novel consistent state estimator design for visual-inertial systems. Motivated by first-estimates Jacobian (FEJ) based estimators - which uses the first-ever estimates as linearization points to preserve proper observability properties of the linearized estimator thereby improving the consistency - we carefully model measurement linearization errors due to its Jacobian evaluation and propose a methodology which still leverages FEJ to ensure the estimator's observability properties, but additionally explicitly compensate for linearization errors caused by poor first estimates. We term this estimator FEJ2, which directly addresses the discrepancy between the best Jacobian evaluated at the latest state estimate and the first-estimates Jacobian evaluated at the first-time-ever state estimate. We show that this process explicitly models that the FEJ used is imperfect and thus contributes additional error which, as in FEJ2, should be modeled and consistently increase the state covariance during update. The proposed FEJ2 is evaluated against state-of-the-art visual-inertial estimators in both Monte-Carlo simulations and real-world experiments, which has been shown to outperform existing methods and to robustly handle poor first estimates and high measurement noises.

We revisit the problem of efficiently leveraging prior map information within a visual-inertial estimation framework. The use of traditional landmark-based maps with 2D-to-3D measurements along with the recently introduced keyframe-based maps with 2D-to-2D measurements are inves-tigated. The full joint estimation of the prior map is compared within a visual-inertial simulator to the Schmidt-Kalman filter (SKF) and measurement inflation methods in terms of their computational complexity, consistency, accuracy, and memory usage. This study shows that the SKF can enable efficient and consistent estimation for small workspace scenarios and the use of 2D-to-3D landmark maps have the highest levels of accuracy. Keyframe-based 2D-to-2D maps can reduce the required state size while still enabling accuracy gains. Finally, we show that measurement inflation methods, after tuning, can be accurate and efficient for large-scale environments if the guarantee of consistency is relaxed.

System modeling and parameter identification of micro aerial vehicles (MAV) are crucial for robust autonomy, especially under highly dynamic motions. Visual-inertial-aided online parameter identification has recently seen research attention due to the demanding of adaptation to platform configuration changes with minimal onboard sensor requirements. To this end, we design an online MAV system identification algorithm to tightly fuse visual, inertial and MAV aerodynamic information within a lightweight multi-state constraint Kalman filter (MSCKF) framework. In particular, while one could blindly fuse the MAV dynamic-induced relative motion constraints in EKF, we numerically show that due to the (quadrotor) MAV system modeling inaccuracy, they often become overconfident and negatively impact the state estimates. As such, we leverage the Schmidt-Kalman filter (SKF) for MAV system parameter identification to prevent corruption of state estimates. Through extensive simulations and real-world experiments, we validate the proposed SKF-based scheme and demonstrate its ability to perform robust system identification even in the presence of an inconsistent MAV dynamic model under different motions.

Accurate and reliable sensor calibration is essential to fuse LiDAR and inertial measurements, which are usually available in robotic applications. In this article, we propose a novel LiDAR-IMU calibration method within the continuous-time batch-optimization framework, where the intrinsics of both sensors and the spatial-temporal extrinsics between sensors are calibrated without using calibration infrastructure, such as fiducial tags. Compared to discrete-time approaches, the continuous-time formulation has natural advantages for fusing high-rate measurements from LiDAR and IMU sensors. To improve efficiency and address degenerate motions, the following two observability-aware modules are leveraged: first, The information-theoretic data selection policy selectsonlythe most informative segments for calibration during data collection, which significantly improves the calibration efficiency by processing only the selected informative segments. Second, the observability-aware state update mechanism in nonlinear least-squares optimization updatesonlythe identifiable directions in the state space with truncated singular value decomposition, which enables accurate calibration results even under degenerate cases where informative data segments are not available. The proposed LiDAR-IMU calibration approach has been validated extensively in both simulated and real-world experiments with different robot platforms, demonstrating its high accuracy and repeatability in commonly-seen human-made environments.

In this paper, we design a versatile multi-sensor aided inertial navigation system (MINS) that can efficiently fuse multi-modal measurements of IMU, camera, wheel encoder, GPS, and 3D LiDAR along with online spatiotemporal sensor calibration. Building upon our prior work [1] –[3], in this work we primarily focus on efficient LiDAR integration in a sliding-window filtering fashion. As each 3D LiDAR scan contains a large volume of 3D points which poses great challenges for real-time performance, we advocate using plane patches, which contain the environmental structural information, extracted from the sparse LiDAR point cloud to update/calibrate the system efficiently. The proposed LiDAR plane patch processing algorithm (including extraction, data association, and update) is shown to be efficient and consistent. Both Extensive Monte-Carlo simulations and real-world datasets with large-scale urban driving scenarios have been used to verify the accuracy and consistency of the proposed MINS algorithm.

Parallel tracking and mapping (PTAM) as a time-efficient framework for simultaneous localization and mapping (SLAM) has been becoming popular in recent years. However, in this paper, we vigilantly point out that the favorite parallel-pipeline design realized by recent proposed SLAM algorithms may lead to inaccurate state estimates which, as a consequence, cannot always guarantee the performance of the estimators in real application. This is mainly due to the imperfect design for processing loop-closure measurements which accidentally violates the Markov assumption for probabilistic SLAM problem. To address this issue, a novel estimator design is proposed that holds the advantage of parallel processing, while striving to be consistent with the Markov property of the batch probabilistic SLAM estimator, therefore, termed Markov parallel tracking and mapping (MPTAM). Especially, the experiments on challenging visual-inertial datasets are employed to further demonstrate the improvements of proposed estimator in terms of accuracy and efficiency, as compared with the state-of-the-art SLAM system.

In this work we augment our prior state-of-the-art visual-inertial odometry (VIO) system, OpenVINS [1], to produce accurate dense depth by filling in sparse depth estimates (depth completion) from VIO with image guidance – all while focusing on enabling real-time performance of the full VIO+depth system on embedded devices. We show that noisy depth values with varying sparsity produced from a VIO system can not only hurt the accuracy of predicted dense depth maps, but also make them considerably worse than those from an image-only depth network with the same underlying architecture. We investigate this sensitivity on both an outdoor simulated and indoor handheld RGB-D dataset, and present simple yet effective solutions to address these shortcomings of depth completion networks. The key changes to our state-of-the-art VIO system required to provide high quality sparse depths for the network while still enabling efficient state estimation on embedded devices are discussed. A comprehensive computational analysis is performed over different embedded devices to demonstrate the efficiency and accuracy of the proposed VIO depth completion system.

This paper studies the problem of multi-robot cooperative visual-inertial localization where each robot is equipped with only a single camera and IMU. We develop two cooperative visual-inertial odometry (C-VIO) algorithms within the multi-state constraint Kalman filter (MSCKF) framework, in which each robot utilizes not only its own measurements but constraints of common features co-observed with its neighbors within the current sliding window in order to improve the localization accuracy. The first centralized-equivalent algorithm tracks the robot-to-robot cross correlations and prioritizes the pose accuracy while requiring full capacity communication among all the robots during update. The second distributed algorithm ignores the robot-to-robot cross correlations to obtain a scalable, robust and efficient fully distributed structure where each robot only keeps its own states and communicates with its neighbors, while a covariance intersection (CI)-based update strategy is leveraged to guarantee consistency. The proposed algorithms are validated extensively in both Monte-Carlo simulations and real-world datasets, and shown to be able to achieve better accuracy with competitive efficiency.

In this work, we present a lightweight, tightly-coupled deep depth network and visual-inertial odometry (VIO) system, which can provide accurate state estimates and dense depth maps of the immediate surroundings. Leveraging the proposed lightweight Conditional Variational Autoencoder (CVAE) for depth inference and encoding, we provide the network with previously marginalized sparse features from VIO to increase the accuracy of initial depth prediction and generalization capability. The compact representation of dense depth, termed depth code, can be updated jointly with navigation states in a sliding window estimator in order to provide the dense local scene geometry. We additionally propose a novel method to obtain the CVAE’s Jacobian which is shown to be more than an order of magnitude faster than previous works, and we additionally leverage First-Estimate Jacobian (FEJ) to avoid recalculation. As opposed to previous works that rely on completely dense residuals, we propose to only provide sparse measurements to update the depth code and show through careful experimentation that our choice of sparse measurements and FEJs can still significantly improve the estimated depth maps. Our full system also exhibits state-of-the-art pose estimation accuracy, and we show that it can run in real-time with single-thread execution while utilizing GPU acceleration only for the network and code Jacobian.

In this paper we present a consistent and distributed state estimator for multi-robot cooperative localization (CL) which efficiently fuses environmental features and loop-closure constraints across time and robots. In particular, we leverage covariance intersection (CI) to allow each robot to only estimate its own state and autocovariance and compensate for the unknown correlations between robots. Two novel multi-robot methods for utilizing common environmental SLAM features are introduced and evaluated in terms of accuracy and efficiency. Moreover, we adapt CI to enable drift-free estimation through the use of loop-closure measurement constraints to other robots’ historical poses without a significant increase in computational cost. The proposed distributed CL estimator is validated against its non-realtime centralized counterpart extensively in both simulations and real-world experiments.

As cameras and inertial sensors are becoming ubiquitous in mobile devices and robots, it holds great potential to design visual-inertial navigation systems (VINS) for efficient versatile 3-D motion tracking, which utilize any (multiple) available cameras and inertial measurement units (IMUs) and are resilient to sensor failures or measurement depletion. To this end, rather than the standard VINS paradigm using a minimal sensing suite of a single camera and IMU, in this article, we design a real-time consistent multi-IMU multi-camera (MIMC) VINS estimator that is able to seamlessly fuse multimodal information from an arbitrary number of uncalibrated cameras and IMUs. Within an efficient multi-state constraint Kalman filter framework, the proposed MIMC-VINS algorithm optimally fuses asynchronous measurements from all sensors while providing smooth, uninterrupted, and accurate 3-D motion tracking even if some sensors fail. The key idea of the proposed MIMC-VINS is to perform high-order on-manifold state interpolation to efficiently process all available visual measurements without increasing the computational burden due to estimating additional sensors’ poses at asynchronous imaging times. In order to fuse the information from multiple IMUs, we propagate a joint system consisting of all IMU states while enforcing rigid-body constraints between the IMUs during the filter update stage. Finally, we estimate online both spatiotemporal extrinsic and visual intrinsic parameters to make our system robust to errors in prior sensor calibration. The proposed system is extensively validated in both Monte Carlo simulations and real-world experiments.

In this paper, we present an efficient and robust GPS-aided visual inertial odometry (GPS-VIO) system that fuses IMU-camera data with intermittent GPS measurements. To perform sensor fusion, spatiotemporal sensor calibration and initialization of the transform between the sensor reference frames are required. We propose an online calibration method for both the GPS-IMU extrinsics and time offset as well as a reference frame initialization procedure that is robust to GPS sensor noise. In addition, we prove the existence of four unobservable directions of the GPS-VIO system when estimating in the VIO reference frame, and advocate a state transformation to the GPS reference frame for full observability. We extensively evaluate the proposed approach in Monte-Carlo simulations where we investigate the system's robustness to different levels of GPS noise and loss of GPS signal, and additionally study the hyper-parameters used in the initialization procedure. Finally, the proposed system is validated in a large-scale real-world experiment.

In this paper we investigate the effect of tightly-coupled estimation on the performance of visual-inertial localization and dynamic object pose tracking. In particular, we show that while a joint estimation system outperforms its decoupled counterpart when given a "proper" model for the target's motion, inconsistent modeling, such as choosing improper levels for the target's propagation noises, can actually lead to a degradation in ego-motion accuracy. To address the realistic scenario where a good prior knowledge of the target's motion model is not available, we design a new system based on the Schmidt-Kalman Filter (SKF), in which target measurements do not update the navigation states, however all correlations are still properly tracked. This allows for both consistent modeling of the target errors and the ability to update target estimates whenever the tracking sensor receives non-target data such as bearing measurements to static, 3D environmental features. We show in extensive simulation that this system, along with a robot-centric representation of the target, leads to robust estimation performance even in the presence of an inconsistent target motion model. Finally, the system is validated in a real-world experiment, and is shown to offer accurate localization and object pose tracking performance.

In this paper, we present an open platform, termed OpenVINS, for visual-inertial estimation research for both the academic community and practitioners from industry. The open sourced codebase provides a foundation for researchers and engineers to quickly start developing new capabilities for their visual-inertial systems. This codebase has out of the box support for commonly desired visual-inertial estimation features, which include: (i) on-manifold sliding window Kalman filter, (ii) online camera intrinsic and extrinsic calibration, (iii) camera to inertial sensor time offset calibration, (iv) SLAM landmarks with different representations and consistent First-Estimates Jacobian (FEJ) treatments, (v) modular type system for state management, (vi) extendable visual-inertial system simulator, and (vii) extensive toolbox for algorithm evaluation. Moreover, we have also focused on detailed documentation and theoretical derivations to support rapid development and research, which are greatly lacked in the current open sourced algorithms. Finally, we perform comprehensive validation of the proposed OpenVINS against state-of-the-art open sourced algorithms, showing its competing estimation performance.

Batch optimization based inertial measurement unit (IMU) and visual sensor fusion enables high rate localization for many robotic tasks. However, it remains a challenge to ensure that the batch optimization is computationally efficient while being consistent for high rate IMU measurements without marginalization. In this paper, we derive inspiration from maximum likelihood estimation with partial-fixed estimates to provide a unified approach for handing both IMU preintegration and time-offset calibration. We present a modularized analytic combined IMU integrator (ACI2) with elegant derivations for IMU integrations, bias Jabcobians and related covariances. To simplify our derivation, we also prove that the right Jacobians for Hamilton quaterions and SO(3) are equivalent. Finally, we present a time offset calibrator that operates by fixing the linearization point for a given time offset. This reduces re-integration of the IMU measurements and thus improve efficiency. The proposed ACI2and time-offset calibration is verified by intensive Monte-Carlo simulations generated from real world datasets. A proof-of-concept real world experiment is also conducted to verify the proposed ACI2estimator.

Aiming for a lightweight and robust localization solution for low-cost, low-power autonomous robot platforms, such as educational or industrial ground vehicles, under challenging conditions (e.g., poor sensor calibration, low lighting and dynamic objects), we propose a two-stage localization system which incorporates both offline prior map building and online multi-modal localization. In particular, we develop an occupancy grid mapping system with probabilistic odometry fusion, accurate scan-to-submap covariance modeling, and accelerated loop-closure detection, which is further aided by 2D line features that exploit the environmental structural constraints. We then develop a versatile EKF-based online localization system which optimally (up to linearization) fuses multi-modal information provided by the pre-built occupancy grid map, IMU, odometry, and 2D LiDAR measurements with low computational requirements. Importantly, spatiotemporal calibration between these sensors are also estimated online to account for poor initial calibration and make the system more "plug-and-play", which improves both the accuracy and flexibility of the proposed multi-sensor fusion framework. In our experiments, our mapping system is shown to be more accurate than the state-of-the-art Google Cartographer. Then, extensive Monte-Carlo simulations are performed to verify both accuracy, consistency and efficiency of the proposed map-based localization system with full spatiotemporal calibration. We also validate the complete system (prior map building and online localization) with building-scale real-world datasets.

In this paper, we introduce a novel visual-inertial-wheel odometry (VIWO) system for ground vehicles, which efficiently fuses multi-modal visual, inertial and 2D wheel odometry measurements in a sliding-window filtering fashion. As multi-sensor fusion requires both intrinsic and extrinsic (spatiotemproal) calibration parameters which may vary over time during terrain navigation, we propose to perform VIWO along with online sensor calibration of wheel encoders' intrinsic and extrinsic parameters. To this end, we analytically derive the 2D wheel odometry measurement model from the raw wheel encoders' readings and optimally fuse this 2D relative motion information with 3D visual-inertial measurements. Additionally, an observability analysis is performed for the linearized VIWO system, which identifies five commonly-seen degenerate motions for wheel calibration parameters. The proposed system has been validated extensively in both Monte-Carlo simulations and real-world experiments in large-scale urban driving scenarios.

Multi-sensor fusion of multi-modal measurements from commodity inertial, visual and LiDAR sensors to provide robust and accurate 6DOF pose estimation holds great potential in robotics and beyond. In this paper, building upon our prior work (i.e., LIC-Fusion), we develop a sliding-window filter based LiDAR-Inertial-Camera odometry with online spatiotemporal calibration (i.e., LIC-Fusion 2.0), which introduces a novel sliding-window plane-feature tracking for efficiently processing 3D LiDAR point clouds. In particular, after motion compensation for LiDAR points by leveraging IMU data, low-curvature planar points are extracted and tracked across the sliding window. A novel outlier rejection criteria is proposed in the plane-feature tracking for high quality data association. Only the tracked planar points belonging to the same plane will be used for plane initialization, which makes the plane extraction efficient and robust. Moreover, we perform the observability analysis for the IMU-LiDAR subsystem under consideration and report the degenerate cases for spatiotemporal calibration using plane features. While the estimation consistency and identified degenerate motions are validated in Monte-Carlo simulations, different real-world experiments are also conducted to show that the proposed LIC-Fusion 2.0 outperforms its predecessor and other state-of-the-art methods.

It holds great implications for practical applications to enable centimeter-accuracy positioning for mobile and wearable sensor systems. In this paper, we propose a novel, high-precision, efficient visual-inertial (VI)-SLAM algorithm, termed Schmidt-EKF VI-SLAM (SEVIS), which optimally fuses IMU measurements and monocular images in a tightly-coupled manner to provide 3D motion tracking with bounded error. In particular, we adapt the Schmidt Kalman filter formulation to selectively include informative features in the state vector while treating them as nuisance parameters (or Schmidt states) once they become matured. This change in modeling allows for significant computational savings by no longer needing to constantly update the Schmidt states (or their covariance), while still allowing the EKF to correctly account for their cross-correlations with the active states. As a result, we achieve linear computational complexity in terms of map size, instead of quadratic as in the standard SLAM systems. In order to fully exploit the map information to bound navigation drifts, we advocate efficient keyframe-aided 2D-to-2D feature matching to find reliable correspondences between current 2D visual measurements and 3D map features. The proposed SEVIS is extensively validated in both simulations and experiments.

This paper presents a general multi-camera visual-inertial navigation system (mc-VINS) with online instrinsic and extrinsic calibration, which is able to utilize all the information from an arbitrary number of asynchronous cameras. In particular, within the standard multi-state constraint Kalman Filter (MSCKF) framework, we only clone the IMU poses related to a single “base camera” (rather than all cameras) in the state vector, while the IMU poses corresponding to all other camera images are represented via an interpolation of the poses bounding the measuring time. By doing so, we can fuse all observations from all cameras with inertial measurements while allowing for efficient, tightly-coupled state estimation through parallelization and asynchrony. Moreover, we perform online sensor calibration of each camera's intrinsics as well as the spatial (transformation) and temporal (time offset) extrinsic parameters between all involved sensors (cameras and IMU), thus enabling high-fidelity localization. We validate the proposed mc-VINS algorithm in various real-world experiments with different sensor configurations, showing the ability to offer real-time high-precision localization and calibration results.

In this paper, we present a real-time multi-IMU visual-inertial navigation system (mi-VINS) that utilizes the information from multiple inertial measurement units (IMUs) and thus is resilient to IMU sensor failures. In particular, in the proposed mi-VINS formulation, one of the IMUs serves as the “base” of the system, while the rest act as auxiliary sensors aiding in state estimation. A key advantage of this architecture is the ability to seamlessly “promote” an auxiliary IMU as a new base, for example, upon detection of the base IMU failure, thus being resilient to the single point of sensor failure as seen in conventional VINS. Moreover, in order to properly fuse the information of multiple IMUs, both the spatial (relative pose) and temporal (time offset) calibration parameters between each sensor and the base IMU are estimated online. The proposed miVINS with online spatial and temporal calibration is validated in both simulations and real-world experiments, and is shown to be able to provide accurate localization and calibration even in scenarios with IMU sensor failures.

Enabling real-time visual-inertial navigation in unknown environments while achieving bounded-error performance holds great potentials in robotic applications. To this end, in this paper, we propose a novel linear-complexity EKF for visual-inertial localization, which can efficiently utilize loop closure constraints, thus allowing for long-term persistent navigation. The key idea is to adapt the Schmidt-Kalman formulation within the multi-state constraint Kalman filter (MSCKF) framework, in which we selectively include keyframes as nuisance parameters in the state vector for loop closures but do not update their estimates and covariance in order to save computations while still tracking their cross-correlations with the current navigation states. As a result, the proposed Schmidt-MSCKF has only O(n) computational complexity while still incorporating loop closures into the system. The proposed approach is validated extensively on large-scale real-world experiments, showing significant performance improvements when compared to the standard MSCKF, while only incurring marginal computational overhead.

As inertial and visual sensors are becoming ubiquitous, visual-inertial navigation systems (VINS) have prevailed in a wide range of applications from mobile augmented reality to aerial navigation to autonomous driving, in part because of the complementary sensing capabilities and the decreasing costs and size of the sensors. In this paper, we survey thoroughly the research efforts taken in this field and strive to provide a concise but complete review of the related work - which is unfortunately missing in the literature while being greatly demanded by researchers and engineers - in the hope to accelerate the VINS research and beyond in our society as a whole.

This paper presents a tightly-coupled aided inertial navigation system (INS) with point and plane features, a general sensor fusion framework applicable to any visual and depth sensor (e.g., RGBD, LiDAR) configuration, in which the camera is used for point feature tracking and depth sensor for plane extraction. The proposed system exploits geometrical structures (planes) of the environments and adopts the closest point (CP) for plane parameterization. Moreover, we distinguish planar point features from non-planar point features in order to enforce point-on-plane constraints which are used in our state estimator, thus further exploiting structural information from the environment. We also introduce a simple but effective plane feature initialization algorithm for feature-based simultaneous localization and mapping (SLAM). In addition, we perform online spatial calibration between the IMU and the depth sensor as it is difficult to obtain this critical calibration parameter in high precision. Both Monte-Carlo simulations and real-world experiments are performed to validate the proposed approach.

Extending our recent work [1] that focuses on the observability analysis of aided inertial navigation systems (INS) using homogeneous geometric features including points, lines and planes, in this paper, we complete the analysis for the general aided INS using different combinations of geometric features (i.e., points, lines and planes). We analytically show that the linearized aided INS with different feature combinations generally possesses the same observability properties as those with same features, i.e., 4 unobservable directions, corresponding to the global yaw rotation and the global position of the sensor platform. During the analysis, we particularly propose a novel minimal representation of line features, i.e., the “closest point” parameterization, which uses a 4D Euclidean vector to describe a line and is proved to preserve the same observability properties. Based on that, for the first time, we provide two sets of unified representations for points, lines and planes, i.e., the quaternion form and the closest point (CP) form, and perform extensive observability analysis with analytically-computed Jacobians for these unified parameterizations. We validate the proposed CP representations and observability analysis with Monte-Carlo simulations, in which EKF-based vision-aided INS (VINS) with combinations of geometrical features in CP form are developed and compared.

Traditional attempts for loop closure detection typically use hand-crafted features, relying on geometric and visual information only, whereas more modern approaches tend to use semantic, appearance or geometric features extracted from deep convolutional neural networks (CNNs). While these approaches are successful in many applications, they do not utilize all of the information that a monocular image provides, and many of them, particularly the deep-learning based methods, require user-chosen thresholding to actually close loops - which may impact generality in practical applications. In this work, we address these issues by extracting all three modes of information from a custom deep CNN trained specifically for the task of place recognition. Our network is built upon a combination of a semantic segmentator, Variational Autoencoder (VAE) and triplet embedding network. The network is trained to construct a global feature space to describe both the visual appearance and semantic layout of an image. Then local keypoints are extracted from maximally-activated regions of low-level convolutional feature maps, and keypoint descriptors are extracted from these feature maps in a novel way that incorporates ideas from successful hand-crafted features. These keypoints are matched globally for loop closure candidates, and then used as a final geometric check to refute false positives. As a result, the proposed loop closure detection system requires no touchy thresholding, and is highly robust to false positives - achieving better precision-recall curves than the state-of-the-art NetVLAD, and with real-time speeds.

In this paper, we present a tightly-coupled monocular visual-inertial navigation system (VINS) using points and lines with degenerate motion analysis for 3D line triangulation. Based on line segment measurements from images, we propose two sliding window based 3D line triangulation algorithms and compare their performance. Analysis of the proposed algorithms reveals 3 degenerate camera motions that cause triangulation failures. Both geometrical interpretation and Monte-Carlo simulations are provided to verify these degenerate motions which prevent triangulation. In addition, commonly used line representations are compared through a monocular visual SLAM Monte-Carlo simulation. Finally, real-world experiments are conducted to validate the implementation of the proposed VINS system using the “closest point” line representation.

This paper presents a tightly-coupled multi-sensor fusion algorithm termed LiDAR-inertial-camera fusion (LIC-Fusion), which efficiently fuses IMU measurements, sparse visual features, and extracted LiDAR points. In particular, the proposed LIC-Fusion performs online spatial and temporal sensor calibration between all three asynchronous sensors, in order to compensate for possible calibration variations. The key contribution is the optimal (up to linearization errors) multi-modal sensor fusion of detected and tracked sparse edge/surf feature points from LiDAR scans within an efficient MSCKF-based framework, alongside sparse visual feature observations and IMU readings. We perform extensive experiments in both indoor and outdoor environments, showing that the proposed LIC-Fusion outperforms the state-of-the-art visual-inertial odometry (VIO) and LiDAR odometry methods in terms of estimation accuracy and robustness to aggressive motions.

In this article, we perform a thorough observability analysis for linearized inertial navigation systems (INS) aided by exteroceptive range and/or bearing sensors (such as cameras, LiDAR, and sonars) with different geometric features (points, lines, planes, or their combinations). In particular, by reviewing common representations of geometric features, we introduce two sets of unified feature representations, i.e., the quaternion and closest point (CP) parameterizations. While the observability of vision-aided INS (VINS) with point features has been extensively studied in the literature, we analytically show that the general aided INS with point features preserves the same observability property, i.e., four unobservable directions, corresponding to the global yaw and the global translation of the sensor platform. We further prove that there are at least five (or seven) unobservable directions for the linearized aided INS with a single line (plane) feature, and, for the first time, analytically derive the unobservable subspace for the case of multiple lines or planes. Building upon this analysis for homogeneous features, we examine the observability of the same system but with combinations of heterogeneous features, and show that, in general, the system preserves at least four unobservable directions, while if global measurements are available, as expected, the unobservable subspace will have lower dimensions. We validate our analysis in Monte-Carlo simulations using both EKF-based visual-inertial SLAM and visual-inertial odometry (VIO) with different geometric features.

In this paper, we address the problem of optimally fusing multiple heterogeneous and asynchronous sensors for use in 3D mapping and localization of autonomous vehicles. To this end, based on the factor graph-based optimization framework, we design a modular sensor-fusion system that allows for efficient and accurate incorporation of multiple navigation sensors operating at different sampling rates. In particular, we develop a general method of out-of-sequence (asynchronous) measurement alignment to incorporate heterogeneous sensors into a factor graph for mapping and localization in 3D, without requiring the addition of new graph nodes, thus allowing the graph to have an overall reduced complexity. The proposed sensor-fusion system is validated on a real-world experimental dataset, in which the asynchronous-measurement alignment is shown to have an improved performance when compared to a naive approach without alignment.

In this paper, we perform observability analysis for inertial navigation systems (INS) aided by generic exteroceptive range and/or bearing sensors with different geometric features including points, lines and planes. While the observability of vision-aided INS (VINS, which uses camera as a bearing sensor) with point features has been extensively studied in the literature, we analytically show that the same observability property remains if using generic range and/or bearing measurements, and if global measurements are also available, as expected, some unobservable directions dismiss. We study in-depth the effects of four degenerate motions on the system observability. In particular, building upon the observability analysis of the aided INS with point features, we perform observability analysis for the same system but with line and plane features, respectively, and show that there exist 5 (and 6) unobservable directions for a single line (and plane) feature. Moreover, we, for the first time, analytically derive the unobservable directions for the cases of multiple lines/planes. We validate our analysis through Monte Carlo simulations.

This paper presents the formalization of the closest point plane representation and an analysis of its incorporation in 3D indoor simultaneous localization and mapping (SLAM). We present a singularity free plane factor leveraging the closest point plane representation, and demonstrate its fusion with inertial preintegratation measurements in a graph-based optimization framework. The resulting LiDAR-inertial 3D plane SLAM (LIPS) system is validated both on a custom made LiDAR simulator and on a real-world experiment.

In this paper, we propose a novel robocentric formulation of visual-inertial navigation systems (VINS)within a multi-state constraint Kalman filter (MSCKF)framework and develop an efficient, lightweight, robocentric visual-inertial odometry (R-VIO)algorithm for consistent localization in challenging environments using only monocular vision. The key idea of the proposed approach is to deliberately reformulate the 3D VINS with respect to a moving local frame (i.e., robocentric), rather than a fixed global frame of reference as in the standard world-centric VINS, and instead utilize high-accuracy relative motion estimates for global pose update. As an immediate advantage of using this robocentric formulation, the proposed R-VIO can start from an arbitrary pose, without the need to align its orientation with the global gravity vector. More importantly, we analytically show that the proposed robocentric EKF-based VINS does not undergo the observability mismatch issue as in the standard world-centric frameworks which was identified as the main cause of inconsistency of estimation. The proposed R-VIO is extensively tested through both Monte Carlo simulations and real-world experiments using different sensor platforms in different environments and shown to achieve competitive performance with the state-of-the-art VINS algorithms in terms of consistency, accuracy and efficiency.

In this paper, we propose to use unit quaternions to represent point features in visual navigation. Contrary to the Cartesian 3D representation, the unit quaternion can well represent features at both large and small distances from the camera without suffering from convergence problems. Contrary to inverse-depth, homogeneous points, or anchored homogeneous points, the unit quaternion has error state of minimum dimension of three. In contrast to prior representations, the proposed method does not need to approximate an initial infinite depth uncertainty. In fact, the unit-quaternion error covariance can be initialized from the initial feature observations without prior information, and the initial error-states are not only bounded, but the bound is identical for all scene geometries. To the best of our knowledge, this is the first time bearing-only recursive estimation (in covariance form) of point features has been possible without using measurements to initialize error covariance. The proposed unit quaternion-based representation is validated on numerical examples.

Recent advancements in the performance and affordability of cameras and inertial measurement units (IMUs) have caused demand for efficient, accurate visual-inertial navigation solutions. In this paper, we present a system for the fusion of preintegrated inertial measurements with highly informative direct alignment of images. In particular, our preintegration theory is based on closed-form solutions of the continuous-time IMU kinematic model, instead of discrete time. This allows for more accurate computation of preintegrated measurements and their uncertainty as well as bias Jacobians. These measurements are fused via graph-based methods with relative pose constraints obtained from direct image alignment from a stereo platform. The proposed system is validated on publicly-available real-world datasets.

In this paper, we introduce a novel acoustic-inertial navigation system (AINS) for Autonomous Underwater Vehicles (AUVs). We are aiming to reduce the cost and latency of current underwater navigation systems that typically employ high-accuracy and thus high-cost inertial sensors. In particular, the proposed approach efficiently fuses the acoustic observations from a 2D imaging sonar and the inertial measurements from a MEMS inertial measurement unit (IMU) within a tightly-coupled EKF framework, while having no need to keep the acoustic features in the state vector. As a result, the computational complexity of the proposed AINS is independent from the scale of the operating environment. Moreover, we develop an acoustic feature linear triangulation to provide accurate initial estimates for iterative solvers, and perform an in-depth observability analysis to investigate the effects of sensor motion on the triangulation. Additionally, since it is challenging to perform a priori sensor extrinsic calibration underwater, we advocate to calibrate IMU-sonar online. The proposed AINS has been validated extensively in Monte-Carlo simulations.

In SLAM, the size of the state vector tends to grow when exploring unknown environments, causing the ever-increasing computational complexity. To reduce the computational cost, one needs to continuously marginalize part of previous states (features and/or poses). This can be achieved by using either the null-space operation or Schur complement based marginalization. The former was originally developed particularly for visual-inertial odometry (VIO), while the latter is the standard approach to reduce the state vector in bundle adjustment (BA). In this paper, for the first time ever, we prove that under mild assumptions (i.i.d. white Gaussian noise model and same linearization points) these two techniques retain the same amount of information about the state, which is validated with real-world experiments. Moreover, based on this key insight, we derive analytically the left null-space expressions for multi-state constraint Kalman filter (MSCKF)-based VIO, which is verified through Monte-Carlo simulations.

In this paper, we develop a robust efficient visual SLAM system that utilizes heterogeneous point and line features. By leveraging ORB-SLAM [1], the proposed system consists of stereo matching, frame tracking, local mapping, loop detection, and bundle adjustment of both point and line features. In particular, as the main theoretical contributions of this paper, we, for the first time, employ the orthonormal representation as the minimal parameterization to model line features along with point features in visual SLAM and analytically derive the Jacobians of the re-projection errors with respect to the line parameters, which significantly improves the SLAM solution. The proposed SLAM has been extensively tested in both synthetic and real-world experiments whose results demonstrate that the proposed system outperforms the state-of-the-art methods in various scenarios.

We present a method for compressing trajectories in an unsupervised manner. Given a set of trajectories sampled from a space we construct a basis for compression whose elements correspond to paths in the space which are topologically distinct. This is achieved by computing a canonical representative for each element in a generating set for the first homology group and decomposing these representatives into a set of distinct paths. Trajectory compression is subsequently accomplished through representation in terms of this basis. Robustness with respect to outliers is achieved by only considering those elements of the first homology group which exist in the super-level sets of the Kernel Density Estimation (KDE) above a threshold. Robustness with respect to small scale topological artifacts is achieved by only considering those elements of the first homology group which exist for a sufficient range in the super-level sets. We demonstrate this approach to trajectory compression in the context of a large set of crowd-sourced GPS trajectories captured in the city of Chicago. On this set, the compression method achieves a mean geometrical accuracy of 108 meters with a compression ratio of over 12.

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.

Graph-based SLAM approaches have had success recently despite suffering from ever-increasing computational costs due to the need of optimizing over the entire robot trajectory. To address this issue, in this paper, we advocate the decoupling of marginalization (node removal) and sparsification (edge reduction) to allow for short-term retention of dense factors induced by marginalization while enabling us to spread the computation of these two operations over time. In particular, we analytically show that during marginalization, the correct choice of linearization points in constructing dense marginal factors is to use the relative (local), instead of global, state estimates in the Markov blanket of the marginalized node, which has lacked a general consensus in the literature. Furthermore, during sparsification, we determine an online sparse topology through sparsity-regularized convex optimization, which guides us to construct consistent sparse factors to best approximate the original dense factors across the Markov blanket. The proposed approach is tested extensively on both 2D and 3D public datasets and shown to perform competitively to the state-of-the-art algorithms.

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.

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.

Nonlinear estimation problems, such as range-only and bearing-only target tracking, are often addressed using linearized estimators, e.g., the extended Kalman filter (EKF). These estimators generally suffer from linearization errors as well as the inability to track multimodal probability density functions. In this paper, we propose a bank of batch maximum a posteriori (MAP) estimators as a general estimation framework that provides relinearization of the entire state trajectory, multihypothesis tracking, and an efficient hypothesis generation scheme. Each estimator in the bank is initialized using a locally optimal state estimate for the current time step. Every time a new measurement becomes available, we relax the original batch-MAP problem and solve it incrementally. More specifically, we convert the relaxed one-step-ahead cost function into polynomial or rational form and compute all the local minima analytically. These local minima generate highly probable hypotheses for the target's trajectory and hence greatly improve the quality of the overall MAP estimate. Additionally, pruning of least probable hypotheses and marginalization of old states are employed to control the computational cost. Monte Carlo simulation and real-world experimental results show that the proposed approach significantly outperforms the standard EKF, the batch-MAP estimator, and the particle filter.

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.

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.

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

In this paper, we introduce an efficient maximum a posteriori (MAP) estimation algorithm, which effectively tracks multiple most probable hypotheses. In particular, due to multimodal distributions arising in most nonlinear problems, we employ a bank of MAP to track these modes (hypotheses). The key idea is that we analytically determine all the posterior modes for the current state at each time step, which are used to generate highly probable hypotheses for the entire trajectory. Moreover, since it is expensive to solve the MAP problem sequentially over time by an iterative method such as Gauss-Newton, in order to speed up its solution, we reuse the previous computations and incrementally update the square-root informationmatrix at every time step, while batch relinearization is performed only periodically or as needed.

Particle filtering (PF) is a popular nonlinear estimation technique and has been widely used in a variety of applications such as target tracking. Within the PF framework, one critical design choice that greatly affects the filter's performance is the selection of the proposal distribution from which particles are drawn. In this paper, we advocate the proposal distribution to be a Gaussian-mixture-based approximation of the posterior probability density function (pdf) after taking into account the most recent measurement. The novelty of our approach is that each Gaussian in the mixture is determined analytically to match the modes of the underlying unknown posterior pdf. As a result, particles are sampled along the most probable regions of the state space, hence reducing the probability of particle depletion. We adapt this proposal distribution into a new PF, termed Analytically-Guided-Sampling (AGS)-PF, and apply it to the particular problem of range-only target tracking. Both Monte-Carlo simulation and real-world experimental results validate the superior performance of the proposed AGS-PF over other state-of-the-art PF algorithms.

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic computational complexity in the number of states and the inconsistency of the state estimates. To address the first issue, we introduce a new sampling strategy for the UKF, which has constant computational complexity. As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF)-based SLAM, i.e., quadratic in the size of the state vector. Furthermore, we investigate the inconsistency issue by analyzing the observability properties of the linear-regression-based model employed by the UKF. Based on this analysis, we propose a new algorithm, termed observability-constrained (OC)-UKF, which ensures the unobservable subspace of the UKF’s linear-regression-based system model is of the same dimension as that of the nonlinear SLAM system. This results in substantial improvement in the accuracy and consistency of the state estimates. The superior performance of the OC-UKF over other state-of-the-art SLAM algorithms is validated by both Monte-Carlo simulations and real-world experiments.

Nonlinear estimation problems, such as bearingonly tracking, are often addressed using linearized estimators, e.g., the extended Kalman filter (EKF). These estimators generally suffer from linearization errors as well as the inability to track multimodal probability density functions (pdfs). In this paper, we propose a bank of batch maximum a posteriori (MAP) estimators as a general estimation framework that provides relinearization of the entire state history, multi-hypothesis tracking, and an efficient hypothesis generation scheme. Each estimator in the bank is initialized using a locally optimal state estimate for the current time step. Every time a new measurement becomes available, we convert the nonlinear cost function corresponding to this relaxed one-step subproblem into polynomial form, allowing to analytically and efficiently compute all stationary points. This local optimization generates highly probable hypotheses for the target trajectory and greatly improves the quality of the overall MAP estimate. Additionally, pruning and marginalization are employed to control the computational cost. Monte Carlo simulations and real-world experiments show that the proposed approach significantly outperforms the EKF, the standard batch MAP estimator, and the particle filter (PF), in terms of accuracy and consistency.

A sliding window filter (SWF) is an appealing smoothing algorithm for nonlinear estimation problems such as simultaneous localization and mapping (SLAM), since it is resource-adaptive by controlling the size of the sliding window, and can better address the nonlinearity of the problem by relinearizing available measurements. However, due to the marginalization employed to discard old states from the sliding window, the standard SWF has different parameter observability properties from the optimal batch maximum-a-posterior (MAP) estimator. Specifically, the nullspace of the Fisher information matrix (or Hessian) has lower dimension than that of the batch MAP estimator. This implies that the standard SWF acquires spurious information, which can lead to inconsistency. To address this problem, we propose an observability-constrained (OC)-SWF where the linearization points are selected so as to ensure the correct dimension of the nullspace of the Hessian, as well as minimize the linearization errors. We present both Monte Carlo simulations and real-world experimental results which show that the OC-SWF's performance is superior to the standard SWF, in terms of both accuracy and consistency.

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic, in the number of states, computational complexity, and the inconsistency of the state estimates. In particular, we introduce a new sampling strategy that minimizes the linearization error and whose computational complexity is constant (i.e., independent of the size of the state vector). As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF) when applied to SLAM. Furthermore, we investigate the observability properties of the linear-regression-based model employed by the UKF, and propose a new algorithm, termed the observability-constrained (OC)-UKF, that improves the consistency of the state estimates. The superior performance of the OC-UKF compared to the standard UKF and its robustness to large linearization errors are validated by extensive simulations.

In this work, we study the inconsistency of EKF-based SLAM from the perspective of observability. We analytically prove that when the Jacobians of the state and measurement models are evaluated at the latest state estimates during every time step, the linearized error-state system model of the EKF-based SLAM has observable subspace of dimension higher than that of the actual, nonlinear, SLAM system. As a result, the covariance estimates of the EKF undergo reduction in directions of the state space where no information is available, which is a primary cause of inconsistency. To address this issue, a new “First Estimates Jacobian” (FEJ) EKF is proposed, which is shown to perform better in terms of consistency. In the FEJ-EKF, the filter Jacobians are calculated using the first-ever available estimates for each state variable, which insures that the observable subspace of the error-state system model is of the same dimension as that of the underlying nonlinear SLAM system. The theoretical analysis is validated through extensive simulations.