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.

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.

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.

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.

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.

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.

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.

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.

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

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.

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.