To empower mobile robots with usable maps as well as highest state estimation accuracy and robustness, we present OKVIS2-X: a state-of-the-art multi-sensor Simultaneous Localization and Mapping (SLAM) system building dense volumetric occupancy maps, while scalable to large environments and operating in realtime. Our unified SLAM framework seamlessly integrates different sensor modalities: visual, inertial, measured or learned depth, LiDAR and Global Navigation Satellite System (GNSS) measurements. Unlike most state-of-the-art SLAM systems, we advocate using dense volumetric map representations when leveraging depth or range-sensing capabilities. We employ an efficient submapping strategy that allows our system to scale to large environments, showcased in sequences of up to 9 kilometers. OKVIS2-X enhances its accuracy and robustness by tightly-coupling the estimator and submaps through map alignment factors. Our system provides globally consistent maps, directly usable for autonomous navigation. To further improve the accuracy of OKVIS2-X, we also incorporate the option of performing online calibration of camera extrinsics. Our system achieves the highest trajectory accuracy in EuRoC against stateof-the-art alternatives, outperforms all competitors in the Hilti22 VI-only benchmark, while also proving competitive in the LiDAR version, and showcases state of the art accuracy in the diverse and large-scale sequences from the VBR dataset. Code available at: https://github.com/ethz-mrl/OKVIS2-X

In the presence of fast motion, point clouds obtained from mechanical spinning LiDAR can be easily distorted due to the slow scanning speed of the LiDAR. Existing LiDAR-only odometry algorithms generally ignore this distortion or compensate by linearly interpolating the estimated relative motion between scans. However, when there are abrupt and nonlinear motion changes, the linear interpolation method poorly compensates for the distortions, which can cause significant drift in motion estimates. In this work, we present a LiDAR-only odometry algorithm that estimates motion by slicing LiDAR scans into shorter times to compensate more agilely for point cloud distortions. Observations from only one small scan slice inevitably lack spatial uniqueness, so the multimodal problem needs to be addressed. For LiDAR-only odometry with small scan slices, we introduce the ensemble Kalman filter, a kind of Monte Carlo-based Bayesian filter. The proposed method makes it possible to perform odometry with only a very narrow field of view (FoV), and the robustness to point cloud distortion is improved. We demonstrate the effectiveness of the proposed method through Monte Carlo simulations and several tests with fast-moving scenarios. The experimental results prove the possibility of odometry with a very narrow FoV of down to 10 degrees and robustness against motion distortion.

In this paper, we propose a novel object-based visual-inertial navigation system fully embedded in a matrix Lie group and built upon the invariant Kalman filtering theory. Specifically, we focus on relative pose measurements of objects and derive an error equation at the associated tangent space. We prove that the observability property does not suffer from the filter inconsistency and nonlinear error terms are identically zero at the object initialization. A thorough Monte-Carlo simulation reveals that our approach yields consistent estimates and is very robust to a large initial state uncertainty. Further-more, we demonstrate a real-world application to the KITTI dataset with a deep neural network-based 3D object detector. Experimental results report that noises on pose measurements follow a Gaussian-like density matching our assumption. The proposed method improves the localization and object global mapping accuracy by probabilistically accounting for inertial readings and object pose uncertainties at multiple views.

In this article, we propose a new methodology to fuse visual-inertial measurements for land vehicles in a challenging urban environment in which a GNSS signal is not available nor reliable. Motivated by a degenerate case caused by a large bias of a MEMS IMU, we redesign a system model of visual-inertial odometry in a framework of extended Kalman filter. In particular, the system model is propagated through a reduced inertial sensor system composed of a 3-axis gyroscope, a 2-axis accelerometer, and a single-axis odometer. An analytical observability derivation reveals unobservable bases of our estimator, and these directions are resolved by using intermittent position measurements from a GNSS receiver. Furthermore, we inspect the uncertainties of the state vector in a Monte-Carlo simulation that agrees with our theoretical results. The proposed method is validated through the KITTI benchmark dataset and an extensive field testing showing a position drift as 1.25% in tunnels on average and a mean position error of 2.81m in the street canyon over a 6.7km driving.

In this article, we propose a visual-inertial navigation system that directly minimizes a photometric error without an explicit data-association. We focus on the photometric error parametrized by pose and structure parameters that is highly nonconvex due to the nonlinearity of image intensity. The key idea is to introduce anoptimal intensity gradientthat accounts for a projective uncertainty of a pixel. Ensembles sampled from the state uncertainty contribute to the proposed gradient and yield a correct update direction even in a bad initialization point. We present two sets of experiments to demonstrate the strengths of our framework. First, a thorough Monte Carlo simulation in a virtual trajectory is designed to reveal robustness to large initial uncertainty. Second, we show that the proposed framework can achieve superior estimation accuracy with efficient computation time over state-of-the-art visual-inertial fusion methods in a real-world UAV flight test, where most scenes are composed of a featureless floor.

In this paper, we propose a novel filtering-based method that fuses events from a dynamic vision sensor (DVS), images, and inertial measurements to estimate camera poses. A DVS is a bio-inspired sensor that generates events triggered by brightness changes. It can cover the drawbacks of a conventional camera by virtual of its independent pixels and high dynamic range. Specifically, we focus on optical flow obtained from both a stream of events and intensity images in which the former is much like a differential quantity, whereas the latter is a pixel difference in a much longer time interval than events. This nature characteristic motivates us to model optical flow estimated from events directly, but feature tracks for images in the filter design. An inequality constraint is considered in our method since the inverse scene-depth is larger than zero by its definition. Furthermore, we evaluate our proposed method in the benchmark DVS dataset and a dataset collected by the authors. The results reveal that the presented algorithm has reduced the position error by 49.9% on average and comparable accuracy only using events when compared to the state-of-the-art filtering-based estimator.