# A Versatile Keyframe-Based Structureless Filter
for Visual Inertial Odometry

Jianzhu Huai1, Yukai Lin2, Charles Toth3, ,
Yuan Zhuang1†, , and Dong Chen1 1J. Huai, Y. Zhuang, D. Chen are with the
State Key Laboratory of Surveying, Mapping and Remote Sensing, Wuhan
University, 129 Luoyu Road, Wuhan, Hubei, 430079, China. Homepage of J. Huai:
https://www.jianzhuhuai.com/.2Y. Lin is with ETH Zurich, Switzerland.3Charles
Toth is with the Department of Civil, Environmental, and Geodetic Engineering,
The Ohio State University, Columbus, OH, USA.†Corresponding author,
<EMAIL_ADDRESS>
###### Abstract

Motion estimation by fusing data from at least a camera and an Inertial
Measurement Unit (IMU) enables many applications in robotics. However, among
the multitude of Visual Inertial Odometry (VIO) methods, few efficiently
estimate device motion with consistent covariance, and calibrate sensor
parameters online for handling data from consumer sensors. This paper
addresses the gap with a Keyframe-based Structureless Filter (KSF). For
efficiency, landmarks are not included in the filter’s state vector. For
robustness, KSF associates feature observations and manages state variables
using the concept of keyframes. For flexibility, KSF supports anytime
calibration of IMU systematic errors, as well as extrinsic, intrinsic, and
temporal parameters of each camera. Estimator consistency and observability of
sensor parameters were analyzed by simulation. Sensitivity to design options,
_e.g_., feature matching method and camera count was studied with the EuRoC
benchmark. Sensor parameter estimation was evaluated on raw TUM VI sequences
and smartphone data. Moreover, pose estimation accuracy was evaluated on EuRoC
and TUM VI sequences versus recent VIO methods. These tests confirm that KSF
reliably calibrates sensor parameters when the data contain adequate motion,
and consistently estimate motion with accuracy rivaling recent VIO methods.
Our implementation runs at 42 Hz with stereo camera images on a consumer
laptop.

###### Index Terms:

structureless filter, keyframe-based feature matching, keyframe-based state
management, online self-calibration, time delay, rolling shutter skew.

## Supplementary Material

A video of running the KSF method with real-time calibration and consistent
pose covariance on TUM VI raw room1 sequence is available at
https://youtu.be/ZJFTdtrP2HQ.

##  I Introduction

Visual Inertial Odometry (VIO) methods estimate the motion of a platform
outfitted with at least a camera and an Inertial Measurement Unit (IMU). They
are an enabling technique for augmented reality and robotics. For instance,
these methods have been widely used for state estimation of smartphones [1],
Unmanned Aerial Vehicles (UAVs) [2], and service robots [3], thanks to the low
weight and power consumption and the wide availability of the sensor assembly.
As the VIO technology matures, a plethora of VIO algorithms [4] have been
proposed over the years. They come in a variety of frontend feature
association schemes and backend estimation engines. The variants of feature
association schemes mainly revolve around whether feature descriptors are
extracted (_e.g_., [5] _vs_. [6]) and whether feature matching is done between
frames and selected frames _i.e_., keyframes (_e.g_., [5] _vs_. [7]). The
backend estimation engines are mainly based on filtering, optimization, neural
networks, or hybrids. To reduce computation time of jointly solving for ego-
motion and scene structure in an estimator, a structureless approach can be
taken where the estimator only solves for ego-motion and the scene structure
is solved separately.

Despite the rich literature on VIO, there remain some desired properties of a
VIO method that are not well addressed. Ideally, a VIO method should produce a
covariance estimate for the estimated pose (position and orientation). But for
some optimization-based methods, it is computationally intensive to obtain the
covariance [8], while some filtering-based methods report over-confident
covariance inconsistent to the actual estimation error [9]. Lack of consistent
covariance further prevents proper weighting of pose estimates in downstream
algorithms, _e.g_., pose graph optimization [10].

Figure 1: Flowchart of the keyframe-based structureless filter whose unique
components are shaded. \raisebox{-.9pt} {1}⃝-\raisebox{-.9pt} {2}⃝ are
described in III-E. \raisebox{-.9pt} {3}⃝-\raisebox{-.9pt} {6}⃝ are presented
in III-F. \raisebox{-.9pt} {7}⃝ is discussed in III-D.

As consumer sensor assemblies suitable for VIO proliferate, _e.g_., on
smartphones, it is desirable to calibrate the camera and the IMU in real time
and without turning to expensive equipment. Even in cases when such special
devices are available, a VIO method with self-calibration capability is
helpful, as it can indicate the need for a new calibration. Several methods
have been proposed to tackle pose estimation and sensor calibration at the
same time. Built upon the Multi-State Constraint Kalman Filter (MSCKF) [11],
Li and Mourikis [12] estimated a variety of parameters of a monocular camera-
IMU system, including IMU systematic errors, and camera intrinsic, extrinsic,
and temporal parameters. Also with MSCKF, the online sensor calibration method
in [13] estimates intrinsic and extrinsic parameters and time offsets of
multiple cameras. While the original MSCKF [11] has difficulty in handling a
stationary period, its improved variant [7] uses zero-velocity update which,
however, requires delicate thresholds.

Motivated by the need for consistent covariance estimation and real-time
sensor calibration, we propose a Keyframe-based Structureless Filter (KSF) for
VIO with on-the-fly calibration capability. A structureless estimator does not
include the scene structure variables in the state vector, thus is typically
more efficient than estimators that jointly estimate motion and scene
structure. In addition to being structureless, KSF integrates the concept of
keyframes and sensor self-calibration. Thanks to the use of keyframes, it can
cope with standstills without resorting to zero-velocity updates. The keyframe
concept has been used in the filtering backend of [14], but we use keyframes
in both the feature association frontend and the filtering backend. Besides
the keyframe concept, KSF differs from [12] in the concept of state variable
timestamps. Besides the two concepts, another difference to a close work [13]
is that KSF includes old velocity variables in the state vector so that
continuous-time camera poses can be predicted with IMU measurements instead of
linear fitting.

In summary, our contributions are:

  * •

For efficient and consistent motion estimation with data captured from a
platform of an IMU and at least one camera, the KSF is developed, employing
keyframes for image feature association in the frontend and for state variable
management in the backend. Its efficiency and accuracy were validated on
public benchmarks, its consistency by simulation.

  * •

To deal with data captured by consumer sensors, the proposed method supports
calibrating IMU random biases and systematic errors, and intrinsic, extrinsic,
and temporal parameters (time delay and rolling shutter effect) of every
camera in the platform. KSF’s ability to calibrate these parameters was
confirmed by simulation and on one public benchmark captured by low-cost
sensors.

Subsequently, Section II provides a non-exhaustive overview of recent VIO
methods, focused on feature matching and sensor calibration. Section III
presents the proposed KSF covering the state vector and error state, IMU and
camera measurement models, keyframe-based feature association frontend, and
the keyframe-based filtering backend. Next, the simulation study on estimator
consistency and sensor parameter observability is reported in Section IV.
Furthermore, Section V gives the sensitivity analysis of various design
options, estimation accuracy of sensor parameters and poses, and computation
time on real-world data. In the end, Section VI discusses the results and
lists caveats, and Section VII summarizes the work and points out future
directions.

##  II Related Work

This section briefly reviews traditional geometry-based visual inertial
odometry and calibration methods without claiming to be exhaustive. VIO
methods can be loosely grouped into geometry-based, learning-based, and hybrid
methods based on the extent to which geometric models of image information are
used in the inference engine [4]. We refer to [15] for a recent survey on
learning-based and hybrid VIO methods, and refer to [16] for a recent survey
on geometry-based VIO methods. At the time of writing this paper, the trending
learning-based methods are reported to be robust under challenging conditions,
_e.g_., [17], while geometry-based methods tend to be fast and accurate. The
following review focuses on the category of geometrical methods to which our
proposed method belongs.

A plethora of VIO methods based on traditional geometry models has been
reported, varying in the supported number of cameras and in the calibration
capability.

###  II-A Monocular Visual Inertial Odometry

Filtering In [18], measurements from a rigid camera-IMU platform were fused
with a global bundle adjustment or with an Extended Kalman Filter (EKF) to
estimate platform pose and velocity expressed in the camera frame at startup,
landmark positions, IMU biases, and the gravity vector. These methods were
initialized with the bundle adjustment algorithm from a special data sequence
with benign motion. With an EKF, Veth and Raquet [19] estimated pose and
velocity of the sensor assembly in the navigation frame, landmark positions,
IMU biases, and camera extrinsic parameters. Feature matches were found by
projecting landmarks to new images and searching in uncertainty ellipses.
Landmarks were initialized by using a terrain model or triangulation. The
MSCKF [11] keeps a sliding window of cloned poses for camera frames while not
using landmarks in the state vector. From every disappearing feature track
(all observations corresponding to the same landmark), a landmark is
triangulated, and then its linearized observations canceling out the landmark
parameters are used to constrain multiple cloned poses in the update step of
EKF. Also using an EKF, Jones and Soatto [20] estimated platform pose and
velocity in an inertial frame, landmark positions, IMU biases, the gravity
vector, and camera extrinsic parameters. A landmark point is represented by
its projection direction and the logarithm of its depth. Kelly and Sukhatme
[21] estimated pose and velocity of the sensor unit in a world frame (_e.g_.,
aligned to a calibration target), IMU biases, the gravity vector, camera
extrinsic parameters, and landmark positions with a UKF. Authors of [22, 23]
used an EKF to fuse IMU readings and up-to-scale motion estimates from a
visual odometry method. Li and Mourikis [9] improved MSCKF by using first
estimates of position and velocity in computing system transition matrix and
measurement Jacobians. Hesch _et al_. [24] improved MSCKF by including
distinguishable landmarks in the state vector and modifying the transition
matrix and measurement Jacobians to satisfy observability constraints.
Robocentric VIO [25] improved MSCKF by expressing the cloned poses relative to
the first one and including the gravity vector in the state vector.

Optimization-based Methods With iSAM2 [26], Forster _et al_. [27] estimated
the entire history of system states associated to sequential keyframes. Each
system state includes pose and velocity expressed in an inertial frame and IMU
biases. The measurements encoded as factors provided to iSAM2 include a prior,
preintegrated IMU factors, and structureless projection factors constructed by
eliminating the landmark parameters from linearized reprojection errors.

With a sliding window smoother, VINS-Mono [28] estimates a sliding window of
poses and velocities (expressed in a gravity-aligned world frame) and IMU
biases, inverse depth of landmarks, and extrinsic parameters and time delay of
the camera. For each selected frame, a least squares problem is constructed
from image features matched between frames by KLT [29], preintegrated IMU
factors, and a marginalization factor. To bound computation, either the second
latest frame or the oldest frame is marginalized from the problem. In [30],
VINS-Mono was extended for optimizing both points and lines with their
observations.

###  II-B Stereo Visual Inertial Odometry

For stereo cameras, Sun _et al_. [14] implemented MSCKF with observability
constraints to ensure consistency. They used KLT [29] to track features
between successive left camera images and between left and right camera
images. In every other update step, two cloned state variables were chosen
based on relative motion from those associated with recent frames and oldest
frames, and marginalized from the filter.

With an optimization backend, Huai _et al_. [31] estimated system state
variables associated with earlier keyframes in a spatial window and recent
frames in a temporal window. Each system state variable includes pose and
velocity in a gravity-aligned world frame and IMU biases. The system state
variables will fix their linearization points for computing Jacobians before
exiting the temporal window to ensure consistency. Kimera-VIO [10] finds
feature matches between consecutive stereo frames by KLT [29] and between left
and right rectified images by correlating along epipolar lines. By using
structureless vision factors, preintegrated IMU factors, and a prior of the
system state (pose, velocity, and IMU biases), a fixed-lag smoother optimizes
the system states associated with keyframes. The stereo VIO method in [32]
tracks features between consecutive stereo frames and between left and right
images by an inverse-compositional sparse optical flow method. For stereo VIO,
by using reprojection errors, preintegrated IMU factors, and a marginalization
factor, the method solves for a sliding window of system state variables
associated with keyframes and recent frames with the Gauss-Newton method. To
ensure consistency, the first-estimate Jacobian technique [33] is used,
_i.e_., Jacobians of system state variables involved in a marginalization
factor are evaluated at their estimates upon marginalization.

###  II-C $N$-Camera Visual Inertial Odometry

With the iterated EKF, ROVIO [34] uses the intensity errors of image patches
for landmarks to estimate robocentric pose, velocity, landmark positions, as
well as IMU biases and camera extrinsic parameters. The landmarks are
represented by a bearing vector and an inverse depth, allowing undelayed
initialization. With KLT [29] or descriptor-based matching, OpenVINS [7]
identifies feature matches between consecutive monocular or stereo frames.
These matches are used by a sliding window filter to update the state vector
including pose, velocity, cloned poses of earlier frames, and landmark
parameters. It is also possible to calibrate extrinsic and intrinsic
parameters of cameras and a time delay. For consistency, propagated values
(rather than updated values) of position, orientation, and velocity are used
in computing the transition matrix and observation Jacobians.

Optimization-based OKVIS [8] tracks BRISK descriptors [35] in a new frame of
$N$ images to previous keyframes and the last frame. With reprojection errors
constructed from these feature matches, preintegrated IMU measurements, a
marginalization factor, and a prior of position and yaw about gravity, OKVIS
solves for landmarks and system state variables (each system state includes
pose, velocity, and IMU biases) in a sliding window of keyframes and recent
frames. To limit the problem size, redundant state variables of the oldest
keyframe or the old recent frame are selected and marginalized. VINS-Fusion
[28] extends VINS-Mono to support stereo cameras and synchronous processing.

###  II-D Camera-IMU Online Calibration

As VIO algorithms flourish, methods to calibrate a camera-IMU system have made
great advances along the lines of both offline and online calibration. Here we
skim through the offline methods and briefly review online methods. The
interested reader is referred to [36] for recent developments in calibration
of visual inertial sensors.

Offline visual inertial calibration usually performs joint optimization of
structure, motion, and sensor parameters. Available offline approaches are
achieved with varying infrastructure support, _e.g_., using calibration
targets [37], revisiting a mapped area [38], or observing natural features
[36].

Online calibration of a camera-IMU system usually relies on filtering or
fixed-lag smoothing approaches while taking observations of existing features.
It has been common practice to estimate camera extrinsic parameters in real
time, _e.g_., [8, 20, 21, 34]. Additional parameters have also been
incorporated in real time estimators, _e.g_., [7]. The extended MSCKF [12]
estimates scale errors, misalignment, and g-sensitivity of the IMU and the
intrinsic, extrinsic, and temporal parameters of the camera including the
frame readout time. An improved VINS-Mono [39] estimates scale errors,
misalignment, and g-sensitivity of the IMU. To date, no formal observability
analysis for these additional parameters exists. But empirical evidence
indicates that a rich variety of motion is necessary for estimating these
parameters reliably [21, 36]. In response to the strict condition to render
all parameters observable, estimators capable of real-time calibration often
come with the option to lock sensor parameters when they are deemed
unobservable or well calibrated. For a filter, a parameter can be locked by
setting its covariance entry zero, _e.g_., [20].

##  III Keyframe-based Structureless Filter

The problem we consider is to track the pose (position and orientation) and
optionally calibrate the sensor parameters of a platform consisting of an IMU
and at least one camera using inertial readings and image observations of
opportunistic landmarks with unknown positions. Our proposed keyframe-based
VIO system has two major components: (i) the feature association frontend
which processes images and gives feature matches, and (ii) the estimation
backend which fuses feature matches and inertial measurements and estimates
motion and optional calibration parameters. Both components integrate the
concept of keyframes. In the following, we present first the notation
conventions, IMU and camera measurement models, then the state variables, and
finally the frontend, the backend filter and its consistency.

###  III-A Notation and Coordinate Frames

We use a $4\times 4$ transformation matrix $\mathbf{T}_{AB}$ composed of a
$3\times 3$ rotation matrix $\mathbf{R}_{AB}$ and a 3D translation vector
$\mathbf{t}_{AB}$, i.e.,
$\mathbf{T}_{AB}\triangleq\left[\begin{matrix}\mathbf{R}_{AB}&\mathbf{t}_{AB}\\\
\mathbf{0}^{\intercal}&1\end{matrix}\right]$, to relate homogeneous
coordinates of a point $\mathbf{p}$ in reference frame $\\{A\\}$,
$\leftidx{{}_{A}}{\mathbf{p}}=[\leftidx{{}_{A}}x,\leftidx{{}_{A}}y,\leftidx{{}_{A}}z,\leftidx{{}_{A}}w]$,
to those in $\\{B\\}$, as
$\leftidx{{}_{A}}{\mathbf{p}}=\mathbf{T}_{AB}\cdot\leftidx{{}_{B}}{\mathbf{p}}$.

This work uses several right-handed coordinate frames, including camera
frames, IMU sensor frame $\\{S\\}$, body frame $\\{B\\}$, and world frame
$\\{W\\}$. The conventional right-down-forward camera frame $\\{C_{k}\\}$ is
affixed to the imaging sensor of every camera $k\in[0,N-1]$. Without loss of
generality, we designate $\\{C_{0}\\}$ as the main camera, _e.g_., the left
camera in a stereo camera rig. The IMU sensor frame $\\{S\\}$ has the origin
at the intersection of the $x$\- and $y$-accelerometer. Its $x$-axis is
aligned to the $x$-accelerometer, and its $y$-axis perpendicular to the
$x$-axis and close to the $y$-accelerometer while in the plane spanned by
$x$\- and $y$-accelerometer. The estimated poses refer to a body frame
$\\{B\\}$ affixed to the platform. The body frame can be defined to coincide
$\\{S\\}$ in an IMU-centric manner. Alternatively, in a camera-centric manner,
the body frame is defined to have the same origin as $\\{S\\}$, and a constant
orientation $R_{SnC0}$ relative to the main camera, _e.g_., [12]. For easy
result comparison, $R_{SnC0}$ is set to the initial value of the relative
orientation between the IMU and the main camera. This work supports both
definitions of the body frame $\\{B\\}$ (see Table I). For coordinate frames
that evolve over time, we append a time parameter to such a frame to obtain a
quasi-inertial frame. For instance, $\\{B(t)\\}$ denotes the quasi-inertial
frame coinciding $\\{B\\}$ at time $t$.

A quasi-inertial world frame $\\{W\\}$ is introduced for expressing the
navigation state variables of the platform. It is instantiated at the start of
a VIO estimator with accelerometers’ measurements such that its $z$-axis is
aligned with the negative gravity direction.

###  III-B IMU Measurements

An IMU measurement model considers systematic and random errors in the
3-degree-of-freedom (3-DOF) gyroscope and the 3-DOF accelerometer. Many VIO
methods use a simplified IMU measurement model that only considers random
biases and noises. A generic IMU model also accounts for systematic errors
including scale factor errors, misalignment, relative orientations, and
g-sensitivity effect, _e.g_., [36, 37]. Next, we present the simplified model
first and then a generic IMU model, both being accommodated in our method.

Recall that the accelerometer triad measures the acceleration in body frame
${}_{B}\mathbf{a}_{s}$ caused by specific forces,

${}_{B}\mathbf{a}_{s}=\mathbf{R}_{WB}^{\intercal}\cdot(\dot{\mathbf{v}}_{WB}-\begin{bmatrix}0&0&-g\end{bmatrix}^{\intercal}),$
(1)

where $g$ is the local gravity. And the gyroscope measures angular velocity of
the platform in $\\{B\\}$ frame ${}_{B}\boldsymbol{\omega}_{WB}$.

Simple IMU model In a simplified IMU model, the IMU measurements
$\mathbf{a}_{m}$ and $\boldsymbol{\omega}_{m}$ are assumed to be affected by
accelerometer and gyroscope biases, $\mathbf{b}_{a}$ and $\mathbf{b}_{g}$, and
Gaussian white noise processes, $\boldsymbol{\nu}_{a}$ and
$\boldsymbol{\nu}_{g}$, _i.e_.,

$\displaystyle\mathbf{a}_{m}$
$\displaystyle=\leftidx_{B}\mathbf{a}_{s}+\mathbf{b}_{a}+\boldsymbol{\nu}_{a}$
(2) $\displaystyle\dot{\mathbf{b}}_{a}$ $\displaystyle=\boldsymbol{\nu}_{ba}$
(3) $\displaystyle\boldsymbol{\omega}_{m}$
$\displaystyle=\leftidx_{B}\boldsymbol{\omega}_{WB}+\mathbf{b}_{g}+\boldsymbol{\nu}_{g}$
(4) $\displaystyle\dot{\mathbf{b}}_{g}$ $\displaystyle=\boldsymbol{\nu}_{bg},$
(5)

where the biases are assumed to be driven by Gaussian white noise processes,
$\boldsymbol{\nu}_{ba}$ and $\boldsymbol{\nu}_{bg}$. The power spectral
densities of $\boldsymbol{\nu}_{a}$, $\boldsymbol{\nu}_{g}$,
$\boldsymbol{\nu}_{ba}$, and $\boldsymbol{\nu}_{bg}$, are usually assumed to
be $\sigma^{2}_{a}\mathbf{I}_{3}$, $\sigma^{2}_{g}\mathbf{I}_{3}$,
$\sigma^{2}_{ba}\mathbf{I}_{3}$, and $\sigma^{2}_{bg}\mathbf{I}_{3}$,
respectively, but they may have disparate diagonal values, and nonzero off-
diagonal entries, _e.g_., for a two DOF gyro.

In summary, the IMU parameters for the simple model are

$\mathbf{x}_{imu}=\left\\{\mbf b_{g}\enskip\mbf b_{a}\right\\}.$ (6)

Generic IMU model Our method incorporates the generic IMU model presented in
[12]. In this model, the accelerometer measurement $\mathbf{a}_{m}$ is
corrupted by systematic errors encoded in a $3\times 3$ matrix
$\mathbf{T}_{a}$, the accelerometer bias $\mathbf{b}_{a}$ and noise process
$\boldsymbol{\nu}_{a}$,

$\begin{split}\mathbf{a}_{m}&=\mathbf{T}_{a}{}_{B}\mathbf{a}_{s}+\mathbf{b}_{a}+\boldsymbol{\nu}_{a}\\\
\dot{\mathbf{b}}_{a}&=\boldsymbol{\nu}_{ba}.\end{split}$ (7)

The 9 entries of $\mathbf{T}_{a}$ encompasses 3-DOF scale factor error, 3-DOF
misalignment, and 3-DOF relative orientation between the accelerometer input
axes and $\\{B\\}$ frame.

The gyroscope measurement $\boldsymbol{\omega}_{m}$ is corrupted by systematic
errors encoded in a $3\times 3$ matrix $\mathbf{T}_{g}$ and g-sensitivity
effect encoded in a $3\times 3$ matrix $\mathbf{T}_{s}$, the gyroscope bias
$\mathbf{b}_{g}$ and noise process $\boldsymbol{\nu}_{g}$,

$\begin{split}\boldsymbol{\omega}_{m}&=\mathbf{T}_{g}{}_{B}\boldsymbol{\omega}_{WB}+\mathbf{T}_{s}{}_{B}\mathbf{a}_{s}+\mathbf{b}_{g}+\boldsymbol{\nu}_{g}\\\
\dot{\mathbf{b}}_{g}&=\boldsymbol{\nu}_{bg}.\end{split}$ (8)

The 9 entries of $\mathbf{T}_{\omega}$ encompasses 3-DOF scale factor error,
3-DOF misalignment, and 3-DOF relative orientation between the gyroscope input
axes and $\\{B\\}$ frame.

Alternatively, the IMU measurements can be modeled with separate coefficients
for scaling, misalignment, and bearing effects as done in [37], thus it will
be easier to interpret the resulting estimates. But to avoid dealing with
SO(3) relative orientations in implementation, we adopt the above formulation,
(7) and (8). In this case, the IMU parameters $\mbf x_{imu}$ are summarily,

$\mbf x_{imu}=\left\\{\mbf b_{g}\enskip\mbf
b_{a}\enskip\overset{\rightarrow}{\mbf{T}}_{g}\enskip\overset{\rightarrow}{\mbf{T}}_{s}\enskip\overset{\rightarrow}{\mbf{T}}_{a}\right\\}$
(9)

where $\overset{\rightarrow}{(\cdot)}$ stacks entries of a matrix into a
vector.

For a well-constrained system, the presented IMU models and compatible camera
extrinsic models are listed in Table I. For the generic IMU model (9), it is
necessary to use the body frame defined in a camera-centric manner (Section
III-A). Otherwise, if the IMU-centric $\\{B\\}$ frame is used,
$\mathbf{R}_{BC0}$ shall be unknown and thus variable, making the 3-DOF
relative orientation in $\mathbf{T}_{a}$ redundant. On the other hand, for the
simple IMU model (6), it is desirable to use the body frame defined in an IMU-
centric manner for the purpose of estimating the relative orientation
$\mathbf{R}_{BC0}$.

TABLE I: Compatible IMU and camera extrinsic parameters. | Body frame
---
definition
| IMU
---
parameters
| Main camera
---
extrinsics
| Other camera
---
extrinsics
| Camera-centric
---
body frame
| $\mathbf{b}_{g},\mathbf{b}_{a}$
---
$\mathbf{T}_{g},\mathbf{T}_{s},\mathbf{T}_{a}$
| $\mathbf{R}_{C0B}=\mathrm{const}$
---
$\mathbf{t}_{C0B}$
|  $\mathbf{T}_{BCk}$ or
---
$\mathbf{T}_{C0Ck}$
| IMU-centric
---
body frame
$\mathbf{b}_{g},\mathbf{b}_{a}$ | $\mathbf{T}_{BC}$ |  |  $\mathbf{T}_{BCk}$ or
---
$\mathbf{T}_{C0Ck}$

Propagation with IMU Data With a sequence of IMU readings
$\mathbf{z}_{imu,k}$, between $t_{k-1}$ and $t_{k}$, the navigation state
variable ${{\boldsymbol{\mathcal{\pi}}}}(t_{k})$, and the covariance of
${{\boldsymbol{\mathcal{\pi}}}}(t_{k})$ and IMU parameters
$\mathbf{x}_{imu}(t_{k})$,
$\boldsymbol{\Sigma}({{\boldsymbol{\mathcal{\pi}}}}(t_{k}),\mathbf{x}_{imu}(t_{k}))$,
can be propagated from given state variables
${{\boldsymbol{\mathcal{\pi}}}}(t_{k-1})$, $\mbf{x}_{imu}(t_{k-1})$, and
covariance
$\boldsymbol{\Sigma}({{\boldsymbol{\mathcal{\pi}}}}(t_{k-1}),\mbf{x}_{imu}(t_{k-1}))$
at $t_{k-1}$ as expressed by $\mathbf{f}(\cdot)$,

$\begin{split}\left\\{{{\boldsymbol{\mathcal{\pi}}}}(t_{k}),\boldsymbol{\Sigma}({{\boldsymbol{\mathcal{\pi}}}}(t_{k}),\mbf{x}_{imu}(t_{k})),\boldsymbol{\Phi}(t_{k},t_{k-1})\right\\}&=\\\
\mathbf{f}({{\boldsymbol{\mathcal{\pi}}}}(t_{k-1}),\mbf{x}_{imu}(t_{k-1}),\boldsymbol{\Sigma}({{\boldsymbol{\mathcal{\pi}}}}(t_{k-1}),\mbf{x}_{imu}(t_{k-1}))&,\\\
\mathbf{z}_{imu,k},\mbf n_{imu})&,\end{split}$ (10)

where the noises of IMU readings are stacked in $\mbf n_{imu}$, _i.e_.,

$\mbf
n_{imu}=[\boldsymbol{\nu}_{g}^{\intercal},\boldsymbol{\nu}_{a}^{\intercal},\boldsymbol{\nu}_{bg}^{\intercal},\boldsymbol{\nu}_{ba}^{\intercal}]^{\intercal}.$
(11)

The transition matrix $\boldsymbol{\Phi}(t_{k},t_{k-1})$ is the derivative of
the propagated error state, $\delta{{\boldsymbol{\mathcal{\pi}}}}(t_{k})$ and
$\delta\mbf{x}_{imu}(t_{k})$, relative to the initial error state,
$\delta{{\boldsymbol{\mathcal{\pi}}}}(t_{k-1})$ and
$\delta\mbf{x}_{imu}(t_{k-1})$, _i.e_.,

$\boldsymbol{\Phi}(t_{k},t_{k-1})=\frac{\partial(\delta{{\boldsymbol{\mathcal{\pi}}}}(t_{k}),\delta\mbf{x}_{imu}(t_{k}))}{\partial(\delta{{\boldsymbol{\mathcal{\pi}}}}(t_{k-1}),\delta\mbf{x}_{imu}(t_{k-1}))}.$
(12)

Detailed calculation of $\boldsymbol{\Phi}(t_{k},t_{k-1})$ and
$\boldsymbol{\Sigma}({{\boldsymbol{\mathcal{\pi}}}}(t_{k}),\mbf{x}_{imu}(t_{k}))$
in (10) can be found in [40, Appendix A], [41]. The propagation (10) should
support backward integration in time to deal with observations earlier than a
pose variable.

###  III-C Camera Measurements

The visual measurements are expressed by conventional reprojection errors. Our
adopted pinhole camera observation model relates a landmark $\mathbf{L}_{i}$’s
observation $\mathbf{z}_{i,j}^{k}$ in image $k$ of frame $j$ at time
$t_{i,j}^{k}$ to its parameters and the system state. For a landmark
$\mathbf{L}_{i}$ expressed in $\\{W\\}$,
$\leftidx{{}_{W}}{\mathbf{p}}_{i}=[x_{i},y_{i},z_{i},1]^{\intercal}$, the
observation model is given by

$\begin{split}\mathbf{z}_{i,j}^{k}&=\begin{bmatrix}u&v\end{bmatrix}^{\intercal}\\\
&=\mathbf{h}^{k}(\mathbf{T}_{BC_{k}}^{-1}\mathbf{T}_{WB(t_{i,j}^{k})}^{-1}\leftidx{{}_{W}}{\mathbf{p}}_{i})+\mathbf{w}_{c}\end{split}$
(13)

where the perspective projection function $\mathbf{h}^{k}(\cdot)$ factors in
camera intrinsic parameters, $\mathbf{x}_{c}^{k}$, and we assume that an image
observation is affected by Gaussian noise $\mathbf{w}_{c}\sim
N(\mathbf{0},\sigma_{c}^{2}\mathbf{I}_{2})$.

Alternatively, a landmark $\mathbf{L}_{i}$ may be anchored in camera $b$ of
frame $a$, $\\{C_{b}(t_{a})\\}$, where $t_{a}$ is frame $a$’s timestamp. Then,
$\mathbf{L}_{i}$ can be represented by a homogeneous point in terms of its
inverse depth $\rho_{i}$, and its observation direction
$[\alpha_{i},\beta_{i},1]$, _i.e_.,
$\leftidx{{}_{C_{b}(t_{a})}}{\mathbf{p}}_{i}=\frac{1}{\rho_{i}}[\alpha_{i},\beta_{i},1,\rho_{i}]^{\intercal}$,
and the observation model is given by

$\begin{split}\mathbf{z}_{i,j}^{k}&=\mathbf{h}^{k}(\mathbf{T}_{BCk}^{-1}\mathbf{T}_{WB(t_{i,j}^{k})}^{-1}\mathbf{T}_{WB(t_{a})}\mathbf{T}_{BC_{b}}\cdot\\\
&\qquad\quad\leftidx{{}_{C_{b}(t_{a})}}{\mathbf{p}}_{i})+\mathbf{w}_{c}\end{split}$
(14)

Notice that the enclosed term in (13) or (14) is obtained by transforming a
point (_i.e_., $\mathbf{Tp}$) or inversely transforming a point (_i.e_.,
$\mathbf{T^{-1}p}$), their Jacobians can be computed in a unified manner by
recursively using the chain rule as done in the reverse-mode automatic
differentiation [42]. Because in general the camera measurements in one image
are at different epochs due to the rolling shutter effect, the Jacobians of
the pose at feature observation epoch $t_{i,j}^{k}$,
$\mathbf{T}_{WB(t_{i,j}^{k})}$, relative to the pose and velocity at $t_{j}$,
$\mathbf{T}_{WB(t_{j})}$ and $\mathbf{v}_{WB(t_{j})}$, are computed with IMU
propagation matrix $\boldsymbol{\Phi}$ (12). Jacobians relative to time
parameters, _e.g_., camera time delay, are computed from the velocity of the
propagated pose $\mathbf{T}_{WB(t_{i,j}^{k})}$. The measurement Jacobian
relative to IMU parameters is ignored as in [43] because its entries are
usually small.

###  III-D State and Error State

This section presents variables used in formulating the nonlinear VIO problem.
We consider the true state $\mathbf{x}$ and the associated error state
$\delta\mathbf{x}$ to be unknown random variables, and denote the state
estimate (mean) by $\hat{\mathbf{x}}$.

State Vector The state of a VIO system $\mathbf{x}(t)$ consists of current
navigation state variable $\boldsymbol{\pi}(t)$, IMU parameters
$\mathbf{x}_{imu}$, parameters of $N$ cameras $\mathbf{x}_{c}$, and a sliding
window of past navigation state variables $\mathbf{x}_{w}$ at known epochs
$\\{t_{j}\\}$ where $j$ enumerates past navigation state variables for
$N_{kf}$ keyframes and $N_{tf}$ recent frames in the sliding window. That is,

$\displaystyle\mathbf{x}(t)$
$\displaystyle=\\{\boldsymbol{\pi}(t),\mathbf{x}_{imu},\mathbf{x}_{c},\mathbf{x}_{w}\\}$
(15) $\displaystyle\mathbf{x}_{c}$
$\displaystyle=\\{\mathbf{x}_{c}^{k}|k=0,1,\cdots,N-1\\}$ (16)
$\displaystyle\mathbf{x}_{w}$
$\displaystyle=\\{\boldsymbol{\pi}(t_{j})|j=0,1,\cdots N_{kf}+N_{tf}-1\\}.$
(17)

The navigation state $\boldsymbol{\pi}(t)$ expressed in $\\{W\\}$ includes
position $\mathbf{t}_{WB(t)}$, orientation $\mathbf{R}_{WB(t)}$, and velocity
$\mathbf{v}_{WB(t)}$ of the platform at time $t$, _i.e_.,

$\boldsymbol{\pi}(t):=\\{\mathbf{t}_{WB(t)},\mathbf{R}_{WB(t)},\mathbf{v}_{WB(t)}\\}.$
(18)

The IMU parameters $\mathbf{x}_{imu}$ are given in (6) or (9).

Camera Parameters The parameters for one camera $\mathbf{x}_{c}^{k}$ include
the camera extrinsic, intrinsic and temporal parameters. The extrinsic
parameters of the main camera in the $N$-camera system are represented by
$\mathbf{T}_{BC0}$. For other cameras, the extrinsic variable can be
parameterized by either $\mathbf{T}_{BCk}$ or
$\mathbf{T}_{BC0}\cdot\mathbf{T}_{C0Ck}$. The intrinsic parameters of one
camera include focal length, principal point, and distortion parameters when
assuming a pinhole projection model. The temporal parameters of camera $k$
include the relative time delay $t_{d}^{k}$ between camera $k$ and the IMU,
and frame readout time $t_{r}^{k}$. Without loss of generality, $t_{d}^{k}$ is
defined such that the raw timestamp of an image $k$ of frame $j$ by the camera
clock, $t_{c,j}^{k}$, plus $t_{d}^{k}$, is the timestamp by the IMU clock for
the central row or mid-exposure of the image, $t_{j}^{k}$, _i.e_.,
$t_{j}^{k}=t_{c,j}^{k}+t_{d}^{k}$.

Each navigation state in the sliding window, $\boldsymbol{\pi}(t_{j})$,
associated with frame $j$ is at a known epoch $t_{j}$ which is set to the
first estimate of the IMU timestamp for the central row of the main image of
frame $j$, _i.e_.,

$t_{j}=t_{c,j}^{0}+t_{d,j}^{0},$ (19)

where $t_{d,j}^{0}$ is the estimate of the main camera’s time delay
$t_{d}^{0}$ at processing frame $j$ and hence known. Tying navigation state
variables to known epochs is conceptually simpler than tying them to true
epochs of estimated values ([7, 43]).

With time delay $t_{d}^{k}$, and readout time $t_{r}^{k}$ for camera $k$, the
epoch to observe a landmark $i$ at $[u,v]$ in image $k$ of frame $j$ with raw
timestamp $t_{c,j}^{k}$ and $h$ rows is computed by

$\displaystyle t_{i,j}^{k}=t_{c,j}^{k}+t_{d}^{k}+(\frac{v-0.5h}{h})t_{r}^{k}$
(20)

Since landmarks are observed at varying epochs due to the rolling shutter
effect, velocity variables are included in the sliding window (17) so that the
camera pose for a landmark observation can be propagated from the relevant
navigation state variable with IMU measurements.

Random Constant State Variables Among all state variables, only the current
navigation state variable $\boldsymbol{\pi}(t)$ and the IMU biases are time-
variant. The historic navigation state variables $\mathbf{x}_{w}$ are unknown
random constants [41] because they are at known epochs. The IMU parameters
$\mathbf{x}_{imu}$ (6) or (9) excluding biases and the camera parameters
$\mathbf{x}_{c}$ are assumed to be random constants [41] because these
parameters change little within the timespan of the sliding window. This
practice was adopted in [12, 44] and OpenVINS [7].

If a sensor parameter is in fact not constant over time, noise can be added to
account for its additional uncertainty as is done for IMU biases. This
technique has been used for estimating time delay between a camera and an IMU
in [45, (32)].

The motivation for including these sensor parameters in an estimator is
threefold. First, the hardware is usually imperfect, especially for sensors on
mobile devices [46]. Secondly, estimating such random constant parameters
incurs marginal computation. Thirdly, these parameters can be locked up online
and thus excluded from estimation ad hoc. For a filter, this is done by
zeroing out their covariance blocks.

Error State Vector All state variables fit in the $\boxplus$-manifolds
axiomatized in [47]. Therefore, the error $\delta\mathbf{x}$ for a state
variable $\mathbf{x}$ on a $\boxplus$-manifold is defined with the
$\boxplus$-operator associated with the manifold,

$\mathbf{x}=\hat{\mathbf{x}}\boxplus\delta\mathbf{x}.$ (21)

In this sense, $\boxplus$ generalizes the conventional $+$. The error state
variables are mostly defined with the $+$-operator, _e.g_., for translation
$\mathbf{t}_{WB}$, velocity $\mathbf{v}_{WB}$, and gyroscope bias
$\mathbf{b}_{g}$,

$\displaystyle\mathbf{t}_{WB}$
$\displaystyle=\hat{\mathbf{t}}_{WB}+\delta\mathbf{t}_{WB}$ (22a)
$\displaystyle\mathbf{v}_{WB}$
$\displaystyle=\hat{\mathbf{v}}_{WB}+\delta\mathbf{v}_{WB}$ (22b)
$\displaystyle\mathbf{b}_{g}$
$\displaystyle=\hat{\mathbf{b}}_{g}+\delta\mathbf{b}_{g}.$ (22c)

The exceptions are rotations. For an element $\mathbf{R}_{WB}$ in the space of
orientation preserving rotations SO(3), its error state
$\delta\boldsymbol{\theta}_{WB}$ is defined such that right multiplying the
matrix exponential of $\delta\boldsymbol{\theta}_{WB}^{\times}$ by the
estimate $\hat{\mathbf{R}}_{WB}$ gives $\mathbf{R}_{WB}$, _i.e_.,

$\mathbf{R}_{WB}=\exp(\delta\boldsymbol{\theta}_{WB}^{\times})\hat{\mathbf{R}}_{WB}\approx(\mathbf{I}+\delta\boldsymbol{\theta}_{WB}^{\times})\cdot\hat{\mathbf{R}}_{WB},$
(23)

where $(\cdot)^{\times}$ obtains the skew-symmetric matrix for a 3D vector.
The small angle approximation in the last expression is useful in deriving
analytical Jacobians. Equivalently, in terms of Hamilton quaternions,

$\mathbf{q}_{WB}=\begin{bmatrix}\frac{\delta\boldsymbol{\theta}_{WB}}{\|\delta\boldsymbol{\theta}_{WB}\|}\sin\frac{\|\delta\boldsymbol{\theta}_{WB}\|}{2}\\\
\cos(\|\delta\boldsymbol{\theta}_{WB}\|/2)\end{bmatrix}\otimes\hat{\mathbf{q}}_{WB}\approx\begin{bmatrix}\frac{\delta\boldsymbol{\theta}_{WB}}{2}\\\
1\end{bmatrix}\otimes\hat{\mathbf{q}}_{WB},$ (24)

where $\mathbf{q}_{WB}$ and $\hat{\mathbf{q}}_{WB}$ are the quaternions for
$\mathbf{R}_{WB}$ and $\hat{\mathbf{R}}_{WB}$, respectively, and $\otimes$ is
the quaternion multiplication operator [48, (12)].

Camera Extrinsic Error Variables The error variable of the main camera’s
extrinsic parameters is defined considering the definition of the body frame
$\\{B\\}$ (Section III-A). And the error variables of other cameras’ extrinsic
parameters are defined considering specific parameterizations.

For the main camera, if the camera-centric body frame is used, then
$\mathbf{T}_{BC0}$ is fixed in orientation, and the extrinsic error is only
defined in the translation component which can be expressed by either
$\mathbf{t}_{BC0}$ or $\mathbf{t}_{C0B}$. Following the notation of [12], the
latter is used to define the extrinsic error, _i.e_.,

$\mathbf{t}_{C0B}=\hat{\mathbf{t}}_{C0B}+\delta\mathbf{t}_{C0B}.$ (25)

Otherwise, when the IMU-centric definition of $\\{B\\}$ is adopted, the main
camera’s extrinsic error consists of a rotation part
$\delta\boldsymbol{\theta}_{BC0}$ and a translation part
$\delta\mathbf{t}_{BC0}$, _i.e_.,

$\displaystyle\mathbf{t}_{BC0}$
$\displaystyle=\hat{\mathbf{t}}_{BC0}+\delta\mathbf{t}_{BC0}$ (26)
$\displaystyle\mathbf{R}_{BC0}$
$\displaystyle=\exp(\delta\boldsymbol{\theta}_{BC0}^{\times})\hat{\mathbf{R}}_{BC0}$
(27)

For other cameras, their extrinsic parameters, $\mathbf{T}_{BCk}$, can be
parameterized relative to the body frame, _i.e_., by themselves,

$\mathbf{T}_{BCk}=\mathbf{T}_{BCk}\enskip k\in[1,N-1],$ (28)

or to the main camera, _i.e_.,

$\mathbf{T}_{BCk}=\mathbf{T}_{BC0}\cdot\mathbf{T}_{C0Ck}\enskip k\in[1,N-1].$
(29)

The error variables of $\mathbf{T}_{BCk}$ and $\mathbf{T}_{C0Ck}$ are defined
for the translation and orientation components like (22a) and (23), _i.e_.,
for $k\in[1,N-1]$,

$\displaystyle\mathbf{t}_{BCk}$
$\displaystyle=\hat{\mathbf{t}}_{BCk}+\delta\mathbf{t}_{BCk}$ (30a)
$\displaystyle\mathbf{R}_{BCk}$
$\displaystyle=\exp(\delta\boldsymbol{\theta}_{BCk}^{\times})\hat{\mathbf{R}}_{BCk}$
(30b) $\displaystyle\mathbf{t}_{C0Ck}$
$\displaystyle=\hat{\mathbf{t}}_{C0Ck}+\delta\mathbf{t}_{C0Ck}$ (30c)
$\displaystyle\mathbf{R}_{C0Ck}$
$\displaystyle=\exp(\delta\boldsymbol{\theta}_{C0Ck}^{\times})\hat{\mathbf{R}}_{C0Ck}.$
(30d)

###  III-E Feature Association Frontend

The frontend extracts and matches image features by using BRISK descriptors
[35] and removes outliers by RANSAC methods provided in OpenGV [49], similarly
to [8].

Figure 2: The keyframe-based feature matching consists of matching the current
frame to the last frame, \raisebox{-.9pt} {1}⃝ and \raisebox{-.9pt} {4}⃝,
matching the current frame to two previous keyframes, \raisebox{-.9pt} {2}⃝,
\raisebox{-.9pt} {3}⃝, \raisebox{-.9pt} {5}⃝, and \raisebox{-.9pt} {6}⃝, and
matching between images of the current frame, \raisebox{-.9pt} {7}⃝. Showing
keyframes as shaded frames, this diagram assumes a platform with stereo
cameras. \raisebox{-.9pt} {1}⃝-\raisebox{-.9pt} {6}⃝ each includes a 3D-2D
matching phase and a 2D-2D matching phase (Section III-E). For a single camera
setup, only \raisebox{-.9pt} {1}⃝-\raisebox{-.9pt} {3}⃝ are needed.

Consider a frame captured by $N$ synchronized cameras. Each image in the frame
is taken by one camera, and these images are grouped into a frame because
their timestamps are close (but not necessarily identical). The matching
procedure associates features of the current frame to several previous frames
which include the last frame and two most recent keyframes, and associates
features between images of the current frame if applicable (Fig. 2).

For every previous frame, the matching operation consists of a 3D-2D matching
phase for features associated with 3D landmarks and a 2D-2D matching phase for
features not yet triangulated, both done by brute-force matching. Though a bit
counter intuitive, brute-force matching is quite efficient with up to 400
features per image (see Section V-D).

Given two frames, for every pair of images captured by the same camera, the
3D-2D phase matches descriptors of triangulated 3D landmarks observed in the
earlier image to descriptors in the latter image. Each match is then checked
by projecting the 3D landmark onto the latter image. Lastly, all valid matches
go through a three-point absolute pose RANSAC step [49] to further remove
outliers. Given two frames, for every pair of images captured by the same
camera, the 2D-2D phase matches descriptors not associated with landmarks in
the earlier image to those in the latter image. Then from these feature
matches, new landmarks will be triangulated. Notably, this triangulation step
is not required by the backend estimator. Its purpose is to remove feature
matches of large projection errors while keeping those of low rotation-
compensated disparity, and to provide depth estimates for 3D-2D matching.
After successful triangulation, these 2D-2D matches will go through a two-
point relative pose RANSAC step [50] to remove outliers. Each valid 2D-2D
match spawns a feature track which is all observations of the triangulated
landmark. These feature tracks are shared with the backend estimator. On the
other hand, they are to be extended in future 3D-2D and inter-camera matching
steps.

In the case of $N$ cameras indexed by $k\in[0,N-1]$, features are associated
between images captured by cameras of overlapping field-of-views, _e.g_.,
$\\{(0,1),(1,2),\cdots,(N-2,N-1),(N-1,0)\\}$. Unlike [8], matches that deviate
much from the epipolar lines will be removed to deal with outliers. Also, a
match of two features of two landmarks will cause the two landmarks and their
feature tracks to be fused.

The above matching procedure distinguishes ordinary frames and keyframes.
Keyframes are selected by using the criteria in [8]. For every image $k$ in
the current frame, we compute the overlap $o_{k}$ between the convex hull
$H_{k}$ of 2D image features associated with landmarks and the one of all 2D
image features, and the ratio $r_{k}$ between the number of 2D image features
associated with landmarks and the number of those fall into the convex hull
$H_{k}$. A keyframe is selected if the maximum of area overlaps
$\\{o_{k}|k\in[0,N-1]\\}$ is less than a threshold $T_{o}$ (typically 60%) or
if the maximum of match ratios $\\{r_{k}|k\in[0,N-1]\\}$ is less than $T_{r}$
(typically 20%).

###  III-F Filter Workflow

The backend, a keyframe-based structureless filter, estimates state variables
tied to frames and keyframes using completed feature tracks and marginalizes
old or redundant state variables (Fig. 1). The filter is structureless since
it does not include landmarks in the state vector, similarly to MSCKF [9]. The
novelty of the filter is that it manages motion-related state variables
depending on whether they are associated with ordinary frames or keyframes.
The keyframe-based state management scheme is facilitated by the selection of
keyframes in the frontend and is motivated by the need to make a structureless
filter work when the sensor setup is stationary.

At the beginning (Algorithm 1), the VIO system is initialized to zero
position, zero velocity, zero accelerometer bias, gyroscope bias averaged over
first few gyroscope readings, and an orientation such that the $z$-axis of the
world frame $\\{W\\}$ is aligned to the negative gravity direction by using a
few accelerometer readings. The other IMU parameters (9), the camera extrinsic
parameters $\mathbf{T}_{BCk}$, time delay $t_{d}^{k}$, and readout time
$t_{r}^{k}$, are initialized to their nominal values from datasheets or
experience.

The standard deviations of state variables (16)-(18) are usually initialized
to sensible values, see Table III for our simulation setup and Table IX for
real data tests.

Figure 3: Schematic drawing of a landmark $i$’s observation Jacobians relative
to the state vector whose nonzero entries are shaded. In accordance with Fig.
2, as a new frame $j$ arrives, this landmark completes its feature track at
frame $j-1$ which serves as its anchor frame (14). After the navigation state
variable for frame $j$ has been cloned as
$\boldsymbol{\pi}_{j}:=\boldsymbol{\pi}(t_{j})$, the computed Jacobians for
$i$ are stacked as shown and ready for canceling out the Jacobian for $i$’s
parameters (Section III-F).

As a frame of coarsely synchronized $N$ images arrives, the navigation state
(18) and the covariance matrix are propagated with inertial readings to the
state epoch ((19), Box \raisebox{-.9pt} {3}⃝ of Fig. 1) for the frame. Then,
the navigation state variable is cloned and augmented to the state vector and
the covariance matrix is also expanded for the cloned navigation state (Line
3, Box \raisebox{-.9pt} {4}⃝ of Fig. 1). Because the cloned navigation state
is assigned a known constant epoch (19), unlike [7] or [43, (30)], we need not
to account for uncertainty of the time delay estimate when expanding
covariance for the cloned state. But we need to compute the measurement
Jacobian relative to time delays of cameras in the update step (Line 12). In
parallel, feature descriptors are extracted from images in the frame and
matched to those in the last frame and two recent keyframes (Line 4, Boxes
\raisebox{-.9pt} {1}⃝ and \raisebox{-.9pt} {2}⃝ of Fig. 1, and Fig. 2) as in
the frontend of [8]. Feature tracks that disappear in the current frame will
be used as measurements for update.

A completed feature track with at least three observations is used to
triangulate a point landmark and all observations in the track provide
reprojection errors to the filter (Lines 6-8). A completed feature track of
two observations are discarded because they are not informative to the filter
[14]. For an example landmark $i$, the Jacobians of its observations relative
to the state vector are visualized in Fig. 3. Since the landmark is not
represented in the state vector, its Jacobian is canceled out by projecting
all measurement Jacobians to the left nullspace of the landmark’s Jacobian
[11]. The left nullspace is typically spanned by $2n-3$ column vectors for a
landmark well observed $n$ times [40, 4.51], but has $2n-2$ columns for a
landmark at infinity because of the rank-deficient landmark Jacobian. This
special case is considered in canceling out the landmark Jacobian and in
performing the subsequent Mahalanobis gating test. To remove outliers of large
measurement residuals, every reprojection measurement goes through a
Mahalanobis gating test (Line 9) [9]. All valid residuals and Jacobians for
these disappeared landmarks are stacked and used to update the state vector
and the covariance matrix (Line 12, Box \raisebox{-.9pt} {5}⃝ of Fig. 1) [11].

To bound computation, redundant frames are selected and marginalized from the
filter (Lines 13-15, Box \raisebox{-.9pt} {6}⃝ of Fig. 1) once the number of
navigation state variables in the sliding window exceeds the total number of
allowed old keyframes $N_{kf}$ and recent frames $N_{tf}$ successive in time,
$N_{kf}+N_{tf}$. In each marginalization operation, at least $X$ redundant
frames ($X$ is 3 for the monocular case, 2 for multiple camera setups) are
chosen (Line 13) because two reprojection measurements for a unknown landmark
is uninformative [14]. To meet this requirement, they are chosen first among
the recent non-keyframes while excluding the most recent $N_{tf}$ frames and
secondly among the oldest keyframes. For the case of Fig. 2, if $N_{kf}=2$ and
$N_{tf}=3$, then the redundant frames are keyframe $j_{1}$ and frame $j-3$ for
the latest frame $j$.

With these redundant frames, we update the filter with observations of
landmarks each observed more than twice in such frames (Line 14). For such a
landmark, if it can be triangulated with its entire observation history, its
observations in the redundant frames are used for Kalman update. All other
observations in the redundant frames are discarded. After the update,
corresponding state variables and entries in the covariance matrix for these
redundant frames are removed (Line 15).

Algorithm 1 Keyframe-based structureless filter workflow. Changes relative to
MSCKF [11] are shown in italics.

1:Initialize state and covariance

2:while a frame of $N$ images arrives do

3: Propagate and augment state vector and covariance

4: Extract descriptors and match to last frame and keyframes

5: for feature tracks disappearing in the frame do

6: Triangulate the point landmark

7: Compute residuals and Jacobians of reprojection errors

8: Cancel out Jacobians for the landmark

9: Remove outliers with Mahalanobis test

10: end for

11: Form the whole residual vector and Jacobian

12: Update state and covariance

13: Detect redundant frames

14: Update with reprojection errors in these frames

15: Remove associated state variables and covariance entries

16:end while

###  III-G Consistency

Despite easy interpretation and intuitive covariance tuning, the chosen
navigation error state is trajectory-dependent. According to [51], because the
evolution of the chosen position error (22a) and velocity error (22b) depend
on the trajectory of the navigation state [40, (A.27), (A.28)], naively using
the last estimate of the state vector to evaluate Jacobians for (10), (13),
and (14) will falsely eliminate the unobservable rotation about gravity ([9,
24]) and lead to optimistic covariance inconsistent to the actual estimator
error.

To ensure consistency, Jacobians relative to navigation state variables
appearing in propagation (12) and measurements (13) and (14) are evaluated at
propagated values of position and velocity, _i.e_., ”first estimates” [9], and
at last estimates of other variables. Unlike [7] and [6], we do not lock the
evaluation points of rotation in computing Jacobians because our chosen
rotation error (23) does not affect the nullspace corresponding to the
unobservable directions.

Jacobians of all other state variables are evaluated at the last estimate of
the state vector because they, _e.g_., IMU biases, time offsets, rolling
shutter effect, do not affect the nullspace [51].

##  IV Simulation Study

We evaluate the KSF with simulation to show that the designed method reports
covariance consistent to the actual estimation error and that the introduced
sensor parameters are observable. The simulation evaluates the proposed KSF in
terms of consistency and state estimation accuracy in comparison to a recent
VIO method, OKVIS [8].

Error Metrics The estimator consistency is measured by the Normalized
Estimation Error Squared (NEES) of components of the navigation state
variable. The expected value of NEES for a variable is its degrees of freedom,
hence 3 for position error $\delta\mathbf{t}_{WB}$, 3 for orientation error
$\delta\boldsymbol{\theta}_{WB}$, and 6 for pose error
$\delta\mathbf{T}_{WB}=(\delta\mathbf{t}_{WB},\delta\boldsymbol{\theta}_{WB})$.
An inconsistent estimator will optimistically estimate the covariance, thus
the computed NEES is much greater than its expected value [9]. Following [52,
(3.7.6-1)], with $n_{s}$ successful runs of an estimator, the NEES
$\boldsymbol{\epsilon}$ for position, orientation, and pose at epoch $t$ are

$\displaystyle\boldsymbol{\epsilon}_{X}(t)=\frac{1}{n_{s}}\Sigma_{i=1}^{n_{s}}\delta\mathbf{X}(t)^{\intercal}\boldsymbol{\Sigma}_{X}^{-1}(t)\delta\mathbf{X}(t)$
(31)

where $\mathbf{X}=\mathbf{t}_{WB},\boldsymbol{\theta}_{WB},\mathbf{T}_{WB}$
and $\boldsymbol{\Sigma}_{X}$ is its covariance.

The accuracy of the estimated state is measured by Root Mean Square Error
(RMSE) for components of the state vector. For a component $\mathbf{X}$, the
RMSE $r_{X}$ at epoch $t$ is computed by

$r_{X}(t)=\sqrt{\frac{1}{n_{s}}\Sigma_{i=1}^{n_{s}}\delta\mathbf{X}(t)^{\intercal}\delta\mathbf{X}(t)}$
(32)

Simulation Setup A scene with point landmarks distributed on four walls was
simulated. A camera-IMU platform traversed the scene for five minutes with two
motion types: a wavy circle (wave for short), and a yarn torus (Fig. 4). For
the two motion types, the platform moved at an average velocity 1.26 m/s and
2.30 m/s, respectively.

TABLE II: The covariance of the zero-mean Gaussian distributions from which discrete noise samples are drawn. $f$ is the IMU sampling frequency. $\Sigma$ | Gyroscope | Accelerometer
---|---|---
| Bias White
---
Noise
[53, (9) and (51)]
|  $\sigma_{bg}^{2}/f\mathbf{I}_{3}$ with
---
$\sigma_{bg}=2\cdot 10^{-5}$
$rad/s^{2}/\sqrt{Hz}$
|  $\sigma_{ba}^{2}/f\mathbf{I}_{3}$ with
---
$\sigma_{ba}=5.5\cdot 10^{-5}$
$m/s^{3}/\sqrt{Hz}$
| White Noise
---
[53, (50)]
|  $\sigma_{g}^{2}f\mathbf{I}_{3}$ with
---
$\sigma_{g}=1.2\cdot 10^{-3}$
$rad/s/\sqrt{Hz}$
|  $\sigma_{a}^{2}f\mathbf{I}_{3}$ with
---
$\sigma_{a}=8\cdot 10^{-3}$
$m/s^{2}/\sqrt{Hz}$

The simulated platform had a camera and an IMU. The camera captured images of
size $752\times 480$ at 10Hz. The rolling shutter effect was simulated by
solving for $t_{i,j}^{k}$ (20) as a root of the projection model (13) with the
Newton-Raphson method. For OKVIS, the rolling shutter effect was not added in
simulation. The image observations were corrupted by radial tangential
distortion and white Gaussian noise of 1 pixel standard deviation at each
dimension. The simulated inertial measurements were sampled at $f$=100 Hz,
corrupted by biases, white noise on biases and additive white noise. Discrete
noise samples were drawn from Gaussian distributions tabulated in Table II.
These noise parameters were chosen to be realistic for the IMU of a consumer
smartphone. The other parameters for generating the image and inertial data
were set to their mean values provided in Table III.

TABLE III: The IMU and camera parameters in KSF were initialized with samples drawn from Gaussian distributions with mean values and standard deviation $\sigma$ at each dimension. $f_{x}$, $f_{y}$, $c_{x}$, $c_{y}$ are the camera projection parameters, $k_{1}$, $k_{2}$, $p_{1}$, $p_{2}$ are the radial tangential distortion parameters, $t_{d}^{0}$ and $t_{r}^{0}$ are time offset and frame readout time of the camera. | $\mathbf{b}_{g}(^{\circ}/s)$ |  | $\mathbf{b}_{a}$
---
$(m/s^{2})$
$\mathbf{T}_{g}(1)$ | $\mathbf{T}_{s}(\frac{rad/s}{m/s^{2}})$ | $\mathbf{T}_{a}(1)$
Mean | $\mathbf{0}$ | $\mathbf{0}$ | $\mathbf{I}_{3}$ | $\mathbf{0}_{3\times 3}$ | $\mathbf{I}_{3}$
| $\sigma$
---
0.29 | 0.02 | 0.005 | 0.001 | 0.005
| $\mathbf{R}_{CB}$ |  | $\mathbf{t}_{C0B}$
---
$(cm)$
| $f_{x},f_{y}$
---
$c_{x},c_{y}$
$(pixel)$
| $k_{1},k_{2}$
---
$p_{1},p_{2}$
| $t_{d}^{0}\enskip t_{r}^{0}$
---
$(ms)$
Mean |  $\begin{bmatrix}0&-1&0\\\ 0&0&-1\\\ 1&0&0\end{bmatrix}$ | $\mathbf{0}$ |  | 350, 360
---
378, 238
| 0, 0
---
0, 0
| 500
---
20
| $\sigma$
---
0 | 2.0 |  | 5.0, 5.0
---
5.0, 5.0
| 0.05,0.01
---
0.001,0.001
| 5
---
5

Estimator Setup A simulation frontend was created to provide feature tracks to
an estimator, _i.e_., the backend of KSF or OKVIS. The frontend associated
observations of a landmark between consecutive frames and between current
frame and the last keyframe. For torus / wave motion, the average feature
track length was 5.8 / 6.8, and the average number of observed landmarks in an
image was 40.5 / 60.5.

An estimator were initialized with the true pose but a noisy velocity estimate
corrupted with noise sampled from Gaussian distribution
$N(\mathbf{0},0.05^{2}\mathbf{I}_{3}\hskip 1.99997ptm^{2}/s^{4})$. For KSF,
the initial values for camera and IMU parameters were drawn from Gaussian
distributions given in Table III. For OKVIS, the initial biases were drawn
from distributions given in Table III and the other parameters were locked to
mean values in Table III except for time delay and readout time of the camera
which were set zero.

(a)

(b)

Figure 4: The simulated scene with general wave motion (a) and torus motion
(b) for five minutes.

Each estimator ran 1000 times for each motion type. All runs were finished
successfully, _i.e_., with the error in position $\leq$ 100 m at the end.

Estimator Consistency We compared the NEES for KSF through wave motion with
Jacobians naively evaluated at last estimates, KSF through wave motion with
first estimate Jacobians (FEJ) by default, and KSF through torus motion with
FEJ by default. The NEES for OKVIS was not computed because of rank deficient
Jacobians in the estimator. The evolution of NEES is visualized in Fig. 5. The
NEES values averaged over the last 10 seconds to smooth out jitters are
tabulated in Table IV.

We had observed that the NEES could be reduced closer to its expected value by
decreasing the variance of noise in sampling IMU readings while keeping the
noise variance unchanged in the VIO estimator. Doing so is somewhat reasonable
in the sense that the simulation setup has unmodeled quantization errors.
However, for simplicity, the following results were obtained without weakening
noise added to IMU readings.

Figure 5: The history of NEES for poses estimated by KSF with naive Jacobians
under wave motion (KSF-Naive Wave), KSF with First Estimate Jacobians (FEJ)
under wave motion (KSF Wave), KSF with FEJ under torus motion (KSF Torus), and
OKVIS under torus motion (OKVIS Torus). Expected values of NEES for position,
orientation, and pose are 3, 3, and 6.

From curves in Fig. 5 and final values of NEES in Table IV, we see that the
proposed estimator, KSF with FEJ, is consistent in terms of NEES.
Surprisingly, OKVIS also achieves consistent NEES though nonlinear factors are
always linearized at the latest variable estimates. The results also indicate
that NEES is affected to some extent by motion type as the motion that excited
the IMU in diverse ways led NEESs closer to expected values.

TABLE IV: NEES and RMSE computed over 1000 runs for OKVIS with wave motion and no systematic error, KSF with naive Jacobians and wave motion, KSF with first estimate Jacobians (FEJ) and wave motion, and KSF with FEJ and torus motion. RMSE values are 0 at the start because poses were initialized with ground truth. |  | NEES averaged over
---
last 10 seconds
RMSE at end
|  | Position
---
(1)
| Orienta-
---
tion (1)
| Pose
---
(1)
| Position
---
(m)
| Orienta-
---
tion (∘)
| Theoretic
---
value
3 | 3 | 6 | N/A | N/A
| KSF-Naive
---
Wave
11.28 | 146.94 | 150.80 | 0.72 | 8.48
KSF Wave | 4.53 | 5.13 | 8.50 | 0.55 | 5.52
KSF Torus | 3.32 | 3.11 | 6.46 | 0.43 | 4.46
OKVIS Torus | 0.85 | 2.95 | 3.80 | 1.03 | 5.13

Estimation of Pose and Sensor Parameters From the final RMSE in Table IV, we
see that KSF outperformed OKVIS in pose accuracy, and the inconsistent KSF
with naively computed Jacobians led to worse pose estimates.

For KSF with wave and torus motion, the RMSEs of sensor parameters at
different epochs are tabulated in Table V. For both motion types, all sensor
parameter components decreased in uncertainty over time as observed in [12,
40], though at varying rates. The frame readout time $t_{r}$ converged in
general but became stagnant after its estimate was within a radius of the true
value.

The uncertainty of IMU parameters decreased faster with torus motion than with
the less diverse wave motion. Obviously, for wave motion (2nd row block of
Table V), accelerometer bias $\mathbf{b}_{a}$ and gyroscope bias
$\mathbf{b}_{g}$ converged very slowly. Besides the simple motion, we
suspected that inclusion of IMU parameters, _i.e_., $\mathbf{T}_{g}$,
$\mathbf{T}_{s}$, and $\mathbf{T}_{a}$, in the estimator also slowed the
convergence of IMU biases. To verify this, we ran KSF through wave motion
while locking additional IMU parameters to their nominal values. The resultant
RMSEs are given in the first row block of Table V. From the results, we see
that when the IMU systematic errors, $\mathbf{T}_{g}$, $\mathbf{T}_{s}$, and
$\mathbf{T}_{a}$, were removed, estimates for biases converged much faster,
and many camera parameters also sped up in convergence.

TABLE V: RMSE of sensor parameters at several epochs computed from 1000 simulation runs of KSF with first estimate Jacobians by default in three settings, KSF through wave motion with IMU parameters except biases locked at nominal values, KSF through wave motion, KSF through torus motion. | Test
---
setup
| Time
---
(s)
| $\mathbf{b}_{g}$
---
$(^{\circ}/s)$
| $\mathbf{b}_{a}$
---
$(m/s^{2})$
| $\mathbf{T}_{g}$
---
$(0.001)$
| $\mathbf{T}_{s}(0.001$
---
$\frac{rad/s}{m/s^{2}})$
| $\mathbf{T}_{a}$
---
$(0.001)$
| $\mathbf{t}_{C0B}$
---
$(cm)$
| $(f_{x},f_{y})$
---
$(pixel)$
| $(c_{x},c_{y})$
---
$(pixel)$
| $(k_{1},k_{2})$
---
$(0.001)$
| $(p_{1},p_{2})$
---
$(0.001)$
| $t_{d}^{0}$
---
$(ms)$
| $t_{r}^{0}$
---
$(ms)$
Wave with fixed $\mathbf{T}_{g}$ $\mathbf{T}_{s}$ $\mathbf{T}_{a}$ | 0.0 | 0.47 | 0.034 | 0.00 | 0.00 | 0.00 | 3.45 | 7.36 | 7.07 | 55.89 | 1.44 | 4.34 | 4.93
3.0 | 0.21 | 0.029 | 0.00 | 0.00 | 0.00 | 1.79 | 1.33 | 1.28 | 3.84 | 0.96 | 0.54 | 0.81
10.0 | 0.08 | 0.025 | 0.00 | 0.00 | 0.00 | 0.94 | 0.58 | 0.65 | 2.17 | 0.56 | 0.27 | 0.54
30.0 | 0.05 | 0.016 | 0.00 | 0.00 | 0.00 | 0.61 | 0.32 | 0.43 | 1.14 | 0.32 | 0.16 | 0.42
100.0 | 0.03 | 0.008 | 0.00 | 0.00 | 0.00 | 0.32 | 0.17 | 0.21 | 0.60 | 0.17 | 0.09 | 0.39
300.0 | 0.02 | 0.005 | 0.00 | 0.00 | 0.00 | 0.20 | 0.09 | 0.13 | 0.36 | 0.10 | 0.06 | 0.39
Wave | 0.0 | 0.50 | 0.035 | 14.92 | 3.04 | 14.83 | 3.45 | 7.26 | 7.02 | 49.82 | 1.40 | 4.96 | 4.89
3.0 | 0.43 | 0.033 | 10.59 | 1.89 | 12.77 | 2.16 | 1.90 | 1.70 | 4.24 | 1.12 | 1.08 | 0.93
10.0 | 0.42 | 0.032 | 6.81 | 1.13 | 9.67 | 1.48 | 1.04 | 1.47 | 2.17 | 1.00 | 0.45 | 0.56
30.0 | 0.40 | 0.032 | 5.11 | 0.92 | 8.09 | 1.20 | 0.63 | 1.35 | 1.23 | 0.89 | 0.27 | 0.43
100.0 | 0.35 | 0.031 | 3.44 | 0.74 | 6.28 | 0.83 | 0.40 | 0.96 | 0.67 | 0.64 | 0.15 | 0.40
300.0 | 0.29 | 0.031 | 2.21 | 0.60 | 5.10 | 0.56 | 0.26 | 0.58 | 0.40 | 0.39 | 0.09 | 0.39
Torus | 0.0 | 0.50 | 0.034 | 14.90 | 3.01 | 14.91 | 3.48 | 7.16 | 6.94 | 51.38 | 1.48 | 5.13 | 5.02
3.0 | 0.44 | 0.032 | 13.06 | 2.45 | 13.03 | 2.97 | 2.82 | 2.20 | 3.69 | 1.22 | 3.21 | 2.28
10.0 | 0.39 | 0.032 | 6.92 | 1.37 | 9.46 | 2.07 | 1.44 | 1.44 | 1.75 | 0.91 | 1.11 | 1.11
30.0 | 0.22 | 0.028 | 4.18 | 0.70 | 7.02 | 1.52 | 0.72 | 1.17 | 0.95 | 0.74 | 0.49 | 0.69
100.0 | 0.12 | 0.023 | 2.45 | 0.38 | 4.80 | 0.93 | 0.39 | 0.79 | 0.54 | 0.51 | 0.27 | 0.52
300.0 | 0.08 | 0.017 | 1.56 | 0.23 | 3.25 | 0.60 | 0.23 | 0.53 | 0.31 | 0.37 | 0.16 | 0.51

##  V Real-World Experiments

With real-world data, the proposed KSF was evaluated in four groups,
sensitivity of pose accuracy to several design options, $N$-camera-IMU
platform calibration in comparison to reference values, accuracy of motion
estimation relative to recent VIO methods, and computation cost.

The pose accuracy was measured with Root Mean Square (RMS) of the Absolute
Translation Error (ATE) [54, (24)], the Relative Rotation Error (RRE) [54,
(27)], and the Relative Translation Error (RTE) [54, (27)] as implemented by
[54]. ATE in units of meters measures the overall deviation of the estimated
pose sequence from ground truth in translation. RRE in units ${}^{\circ}/m$
and RTE in units % quantifies deviations from ground truth over trajectory
segments of fixed lengths, in rotation and translation, respectively. To
mitigate the random behavior of a VIO estimator, results from three runs were
used to compute the error metrics since empirically metrics computed from
three runs changed little, _e.g_., less than 0.05% in ATE. To compute relative
errors, RRE and RTE, distance intervals [40, 60, 80, 100, 120] were used as in
the UZH-FPV dataset [55].

RRE and RRE are invariant to a rigid transformation of the pose sequence.
However, ATE is dependent on the spatial alignment. As recommended in [54,
Table I], we aligned the trajectory estimated by a VIO method to the ground
truth with a transformation of a yaw-only rotation and a 3D translation. This
4-DOF alignment requires that poses in the estimated trajectory and ground
truth are expressed in world frames with z-axis aligned to gravity. The first
part is fulfilled since all VIO methods used for comparison estimate poses of
the IMU sensor frame in a gravity-aligned world frame. The second part is
fulfilled by benchmarks, EuRoC [56] and TUM VI [57].

For the following tests, we initialized a VIO estimator with the standard
deviation of position, orientation error, velocity, gyroscope bias, and
accelerometer bias, $0.01\mathbf{I}_{3}\hskip 1.99997ptm$,
$\mathrm{diag}(1^{\circ},1^{\circ},3^{\circ})$, $0.1\mathbf{I}_{3}\hskip
1.99997ptm/s$, $1.72\mathbf{I}_{3}\hskip 1.99997pt{}^{\circ}/s$, and
$0.1\mathbf{I}_{3}\hskip 1.99997ptm/s^{2}$, respectively.

###  V-A Sensitivity Study

By tuning KSF on the EuRoC dataset, we studied its dependence on the feature
tracking method, its sensitivity to key parameters including number of
keyframes $N_{kf}$ and number of temporally successive frames $N_{tf}$, and
the effect on pose estimation of landmark parameterization and sensor
calibration. The entire EuRoC dataset [56] without modification was used
because it is less demanding than other benchmarks, _e.g_., TUM VI [57], and
we feel that a challenging dataset may result in parameters not well suitable
to the vast average data.

We tested the parameter sensitivity of KSF in the monocular setting because
most findings are expected to carry over to $N$-camera settings. By default,
KSF processed data synchronously in the non-realtime fashion.

For each subsequent sensitivity test, we began with a manually selected
baseline combination of parameters which was expected to achieve good pose
estimation accuracy based on our experiences. Then we ran KSF with components
or parameters of interest differing from the baseline. In the end, the optimal
component or parameter combination was identified by looking at the pose
estimation accuracy of different combinations.

Feature Tracking Method To show the merit of keyframe-based feature matching,
we assessed KSF with alternative feature association methods including frame-
wise KLT feature tracking [29] (hence KSF-KLT), and sequential frame matching
by BRISK descriptors (hence KSF-seq). The relative errors are summarized in
VI, confirming the advantage of keyframe-based feature association. Visual
check of the superimposed trajectory plots indicated that both KSF-KLT and
KSF-seq often had jumps in trajectory when the Micro Aerial Vehicle (MAV) was
nearly stationary. This was expected because these two alternatives got mainly
uninformative feature observations of small disparities during standstill.

TABLE VI: Relative errors of KSF on EuRoC dataset with keyframe-based, KLT, and sequential feature association and of KSF with varying number of keyframes (first number after dash) and temporal frames (last number after dash). | RTE (%) | RRE (${}^{\circ}/m$)
---|---|---
KSF | 0.73 | 0.030
KSF-KLT | 3.76 | 0.053
KSF-seq | 0.87 | 0.034
KSF-3-3 | 3.42 | 0.047
KSF-5-5 | 0.81 | 0.032
KSF-7-5 | 0.72 | 0.030
KSF-7-7 | 0.73 | 0.031
KSF-9-5 | 0.71 | 0.030
KSF-9-9 | 0.69 | 0.031

Number of Keyframes and Temporal Frames The backend sliding window consists of
$N_{kf}$ keyframes and $N_{tf}$ recent temporal frames. A larger sliding
window is expected to give better pose estimates but incurs higher computation
as shown in VI. To balance accuracy and resource consumption, we selected 7
keyframes and 5 temporal frames for KSF in subsequent tests.

Lock Sensor Parameters As the proposed method involves quite a few sensor
parameters, we evaluated the effect of calibrating sensor parameters on pose
accuracy. Starting from an extreme baseline, unlocking (or locking) all sensor
parameters, we ran KSF with only one component of the sensor parameters locked
(or unlocked). Remarkably, the IMU biases were always estimated. Then the
relative errors for results of different calibration settings were computed
(Table VII). From the first half of Table VII, we see that locking IMU
parameters $\\{\mathbf{T}_{g},\mathbf{T}_{s},\mathbf{T}_{a}\\}$ or camera
intrinsic parameters can improve pose accuracy while locking camera extrinsic
parameters have a slight impact, and locking camera temporal parameters
worsens the accuracy. From the second half of Table VII, we see that
estimating IMU or camera intrinsic parameters adversely affects the results,
but estimating camera extrinsic or temporal parameters improves the accuracy.
Interestingly, Yang _et al_. [58, Table V] reported that online IMU
calibration marginally worsens RMS ATE values for many EuRoC sequences and
they reasoned that the MAV motion is not diverse enough. In summary, for the
EuRoC dataset, locking IMU and camera intrinsic parameters while calibrating
camera extrinsic and temporal parameters led to better pose accuracy. This
finding is likely due to good individual calibration of the IMU and cameras
used for capturing the EuRoC dataset.

TABLE VII: Relative errors of KSF on EuRoC dataset when estimating subsets of sensor parameters, including IMU parameters $\\{\mathbf{T}_{g},\mathbf{T}_{s},\mathbf{T}_{a}\\}$, camera extrinsic ($\mathbf{t}_{C0B}$), intrinsic (projection and distortion), and temporal (time delay $t_{d}^{0}$ and frame readout time) parameters. | RTE (%) | RRE (${}^{\circ}/m$)
---|---|---
KSF unlock all | 0.79 | 0.037
KSF lock $\mathbf{T}_{g}\,\mathbf{T}_{s}\,\mathbf{T}_{a}$ | 0.73 | 0.031
KSF lock extrinsic | 0.78 | 0.036
KSF lock intrinsic | 0.72 | 0.031
KSF lock temporal | 0.87 | 0.036
KSF lock all | 0.71 | 0.029
KSF unlock $\mathbf{T}_{g}\,\mathbf{T}_{s}\,\mathbf{T}_{a}$ | 0.75 | 0.032
KSF unlock extrinsic | 0.69 | 0.030
KSF unlock intrinsic | 0.73 | 0.029
KSF unlock $t_{d}^{0}$ | 0.70 | 0.029

Landmark Parameterization It was observed in [9, 59] that anchored inverse
depth (AID) parameterization (14) for landmarks outperformed the homogeneous
point (HP) parameterization (13). This was confirmed in our tests by relative
errors for KSF with AID by default versus with HP in Table VIII.

$N$-Camera Setup and Real-Time Processing We also evaluated KSF with input
from a stereo camera (KSF-n), KSF running in asynchronous mode (KSF-async),
_i.e_., real-time processing, and KSF-n in asynchronous mode (KSF-n-async).
The relative errors are given in Table VIII where we see the benefit of stereo
cameras in reducing the relative translation error. Surprisingly, asynchronous
processing led to slightly better result than synchronous processing. But this
trend was not observed on other benchmarks, _e.g_., TUM VI. Aside from the
random behavior of the algorithm, another cause could be that some of the rich
feature descriptors in the EuRoC dataset were redundant and discarding them
might be better regarding pose accuracy (see also [6]).

TABLE VIII: Pose accuracy of KSF on EuRoC dataset with an alternative landmark parameterization, Homogeneous Point (HP) in the world frame, with asynchronous real-time processing (async), with asynchronous processing of stereo images (n-async), and with non-realtime processing of stereo images (n). By default, KSF processes images from a single camera synchronously with anchored inverse depth parameterization. | RTE (%) | RRE (${}^{\circ}/m$)
---|---|---
KSF | 0.68 | 0.031
KSF-HP | 0.71 | 0.029
KSF-async | 0.67 | 0.030
KSF-n-async | 0.39 | 0.028
KSF-n | 0.40 | 0.029

###  V-B Sensor Calibration

To show the capability of KSF in estimating the extrinsic, intrinsic, and time
delay parameters of a $N$-camera-IMU platform, KSF was used to process several
raw sequences of TUM VI [57] and the estimated sensor parameters were compared
to provided reference values. Moreover, to show the possibility of calibrating
frame readout time, sequences captured by a few Android phones were processed
with KSF and the estimated readout times were compared to values obtained with
a LED panel.

Stereo-Camera-IMU Calibration The TUM VI benchmark provided the raw sequences
collected by the stereo-camera-IMU platform which was subject to a variety of
sensor systematic errors. On these raw sequences with diverse motion, KSF was
expected to give good estimates of these systematic errors. On the other hand,
the systematic errors of the platform had been estimated by the authors with
dedicated calibration sequences and reference values of relevant parameters
were provided. Thus, by comparing the reference values and the estimates from
KSF, we can assess KSF’s capability of sensor calibration.

To this end, we chose six room sessions of the raw TUM VI sequences (each
about two-minute long) because they contained rich motion and ground truth
covering the full trajectory. Images in these raw data were down sampled by
pyrDown [29] to 512$\times$512\. Both images and IMU data were timestamped
with the header.stamp field of raw messages. Otherwise these raw data were
intact. This contrasted with [39] where the initial IMU biases in the raw data
were corrected before online calibration.

KSF was initialized with nominal values and standard deviations for sensor
parameters as listed in Column 2, Table IX. Because the sensor pixel is
usually square, the same focal length $f^{k}$ was used for both $x$ and $y$
directions of camera $k$. The camera intrinsic parameters were initialized
assuming we know their rough values. For instance, $f^{k}$ was initialized
with a rounded number 190. The principal points were simply initialized at the
image centers, _i.e_., (256, 256). The IMU noise parameters were set as the
stereo KSF for processing the calibrated TUM VI sequences in Section V-C. Each
sequence was processed three times to mitigate the nondeterministic behavior
of the algorithm. For six raw sequences three runs each, we computed means and
standard deviations of estimated sensor parameters at the end of these 18
trials. The estimated parameters including IMU biases and systematic errors,
and camera extrinsic, intrinsic, and temporal parameters are given in Table
IX.

The calibration parameters coming with the TUM VI dataset were obtained by a
global bundle adjustment with special data captured while observing a
calibration target. These parameters followed the IMU and camera extrinsic
models defined by TUM VI. For comparison, they needed to be converted to
parameters of the IMU and camera extrinsic models used by KSF. The conversion
was derived by comparing TUM VI models with ours. Given TUM VI IMU model
coefficient matrices $\mathbf{M}_{g}$, $\mathbf{M}_{a}$, IMU biases,
$\mathbf{b}_{g,t}$ and $\mathbf{b}_{a,t}$, extrinsic parameters of the two
cameras in the TUM VI sensor frame $\\{St\\}$,
$[\mathbf{R}_{StC0},\mathbf{t}_{StC0}]$ and
$[\mathbf{R}_{StC1},\mathbf{t}_{StC1}]$, the parameters of KSF can be obtained
by

$\displaystyle\mathbf{b}_{g}$
$\displaystyle=\mathbf{M}_{g}^{-1}\mathbf{b}_{g,t}$ (33)
$\displaystyle\mathbf{b}_{a}$
$\displaystyle=\mathbf{M}_{a}^{-1}\mathbf{b}_{a,t}$ (34)
$\displaystyle\mathbf{T}_{g}$
$\displaystyle=\mathbf{M}_{g}^{-1}\mathbf{R}_{StC0}\mathbf{R}_{SnC}^{\intercal}$
(35) $\displaystyle\mathbf{T}_{s}$ $\displaystyle=\mathbf{0}$ (36)
$\displaystyle\mathbf{T}_{a}$
$\displaystyle=\mathbf{M}_{a}^{-1}\mathbf{R}_{StC0}\mathbf{R}_{SnC}^{\intercal}$
(37) $\displaystyle\mathbf{p}_{C0B}$
$\displaystyle=-\mathbf{R}_{StC0}^{\intercal}\mathbf{t}_{StC0}$ (38)
$\displaystyle\mathbf{p}_{BC1}$
$\displaystyle=\mathbf{R}_{SnC}\mathbf{R}_{StC0}^{\intercal}\mathbf{t}_{StC1}$
(39) $\displaystyle\mathbf{R}_{BC1}$
$\displaystyle=\mathbf{R}_{SnC}\mathbf{R}_{StC0}^{\intercal}\mathbf{R}_{StC1}$
(40)

where $\mathbf{R}_{SnC}$ is the orientation between the camera frame and the
nominal IMU frame $\\{Sn\\}$,

$\mathbf{R}_{SnC}=\begin{bmatrix}-1&0&0\\\ 0&0&-1\\\ 0&-1&0\end{bmatrix}.$
(41)

$\mathbf{R}_{SnC}$ is needed because KSF fixed orientation of the body frame
$\\{B\\}$ relative to the camera frame $\\{C\\}$ to $\mathbf{R}_{SnC}$ in
processing raw sequences of TUM VI, _i.e_., starting with a coarse
calibration.

TABLE IX: Mean and standard deviation of 18 estimated state vectors by running KSF in two configurations (calibrating all parameters except for frame readout time, calibrating all except for camera intrinsics and frame readout time), on six room sessions of TUM VI for three times. Each estimated state vector was the last one from a run. Note nominal values, 190 and (256, 256) had been subtracted from focal length and principal point. $\delta\boldsymbol{\theta}_{BC1}$ relates the nominal orientation and the estimated one such that $\mathbf{R}_{SnC}=\exp(\delta\boldsymbol{\theta}_{BC1}^{\times})\hat{\mathbf{R}}_{BC1}$. $\mathbf{1}:=[1,1,1]^{\intercal}$. Parameter |  Initial Value | Final Value |  TUM VI Ref.
---|---|---|---
calibrate all |  | fix camera
---
intrinsics
$\mathbf{b}_{g}{}(^{\circ}/s)$ |  | $\mathbf{0}\pm$
---
$1.72\mathbf{1}$
| 2.08 $\pm$ 0.31
---
0.47 $\pm$ 0.13
1.19 $\pm$ 0.36
| 1.97 $\pm$ 0.21
---
0.47 $\pm$ 0.14
1.08 $\pm$ 0.30
| 1.72
---
0.38
0.93
$\mathbf{b}_{a}(m/s^{2})$ | $\mathbf{0}\pm 0.1\mathbf{1}$ |  | -1.26 $\pm$ 0.04
---
-0.30 $\pm$ 0.05
0.44 $\pm$ 0.05
| -1.28 $\pm$ 0.04
---
-0.31 $\pm$ 0.04
0.41 $\pm$ 0.03
| -1.30
---
-0.39
0.38
| $\mathbf{T}_{g}-\mathbf{I}$
---
$(0.001)$
| $\mathbf{0}\pm$
---
$5\mathbf{1}_{3\times 3}$
| 58.27 $\pm$ 1.91
---
27.90 $\pm$ 3.67
-7.44 $\pm$ 1.10
-24.78 $\pm$ 3.25
-87.05 $\pm$ 0.86
30.46 $\pm$ 2.33
7.87 $\pm$ 0.99
-36.82 $\pm$ 3.31
-18.05 $\pm$ 1.34
| 58.47 $\pm$ 1.26
---
30.53 $\pm$ 1.47
-8.33 $\pm$ 0.58
-26.95 $\pm$ 1.15
-87.69 $\pm$ 0.69
35.48 $\pm$ 2.67
9.51 $\pm$ 1.05
-43.92 $\pm$ 3.29
-18.45 $\pm$ 0.83
| 59.30
---
30.09
-8.80
-27.24
-87.09
34.08
10.38
-41.27
-16.54
| $\mathbf{T}_{s}$
---
$(0.001$
$\frac{rad/s}{m/s^{2}})$
| $\mathbf{0}\pm$
---
$\mathbf{1}_{3\times 3}$
| 0.49 $\pm$ 0.38
---
0.05 $\pm$ 0.10
-0.67 $\pm$ 0.56
-0.05 $\pm$ 0.14
0.19 $\pm$ 0.16
-0.11 $\pm$ 0.25
0.43 $\pm$ 0.33
0.10 $\pm$ 0.22
-0.44 $\pm$ 0.71
| 0.29 $\pm$ 0.22
---
0.07 $\pm$ 0.14
-0.47 $\pm$ 0.39
-0.11 $\pm$ 0.13
0.05 $\pm$ 0.12
-0.12 $\pm$ 0.26
0.12 $\pm$ 0.26
0.03 $\pm$ 0.20
-0.26 $\pm$ 0.57
| 0.00
---
0.00
0.00
0.00
0.00
0.00
0.00
0.00
0.00
| $\mathbf{T}_{a}-\mathbf{I}$
---
$(0.001)$
| $\mathbf{0}\pm$
---
$5\mathbf{1}_{3\times 3}$
| -6.74 $\pm$ 1.69
---
21.72 $\pm$ 5.08
-10.89 $\pm$ 3.09
-25.13 $\pm$ 4.36
-4.35 $\pm$ 2.26
22.43 $\pm$ 6.61
20.23 $\pm$ 3.77
-30.40 $\pm$ 4.16
20.70 $\pm$ 5.04
| -7.76 $\pm$ 1.91
---
25.82 $\pm$ 3.08
-9.58 $\pm$ 3.33
-27.79 $\pm$ 3.46
-3.92 $\pm$ 2.20
29.00 $\pm$ 5.22
18.90 $\pm$ 2.94
-35.93 $\pm$ 3.76
23.66 $\pm$ 3.00
| -4.67
---
29.59
-7.41
-29.32
-2.39
34.55
18.71
-34.10
29.75
| $\mathbf{p}_{C0B}$
---
$(cm)$
$\mathbf{0}\pm 2\mathbf{1}$ |  | 3.82 $\pm$ 0.91
---
-5.40 $\pm$ 0.67
-4.57 $\pm$ 1.31
| 3.82 $\pm$ 0.32
---
-4.60 $\pm$ 0.55
-8.00 $\pm$ 0.46
| 4.71
---
-4.76
-6.90
| $f^{0},c_{x}^{0},c_{y}^{0}$
---
$(px)$
| $0\pm 5$
---
$0\pm 5$
$0\pm 5$
| 2.72 $\pm$ 0.87
---
-0.58 $\pm$ 1.08
1.53 $\pm$ 0.59
| 0.00 $\pm$ 0.00
---
0.00 $\pm$ 0.00
0.00 $\pm$ 0.00
| 0.98
---
-1.07
0.90
| $k_{1}^{0},k_{2}^{0}$
---
$k_{3}^{0},k_{4}^{0}$
$(0.001)$
| $0\pm 50$
---
$0\pm 10$
$0\pm 1$
$0\pm 1$
| 0.62 $\pm$ 1.89
---
-1.22 $\pm$ 1.01
-0.18 $\pm$ 0.13
-0.17 $\pm$ 0.06
| 0.00 $\pm$ 0.00
---
0.00 $\pm$ 0.00
0.00 $\pm$ 0.00
0.00 $\pm$ 0.00
| 3.48
---
0.72
-2.05
0.20
$t_{d}^{0}(ms)$ | $0\pm 5$ | 0.25 $\pm$ 0.08 | 0.21 $\pm$ 0.09 | 0.13
| $\mathbf{p}_{BC1}$
---
$(cm)$
$\mathbf{0}\pm 2\mathbf{1}$ |  | -4.48 $\pm$ 1.92
---
-4.01 $\pm$ 2.14
-5.24 $\pm$ 0.67
| -6.48 $\pm$ 0.32
---
-7.61 $\pm$ 0.42
-5.66 $\pm$ 0.30
| -5.40
---
-7.01
-4.95
$\delta\boldsymbol{\theta}_{BC1}({}^{\circ})$ |  | $\mathbf{0}\pm$
---
$0.57\mathbf{1}$
| -2.62 $\pm$ 0.17
---
0.03 $\pm$ 0.07
-0.26 $\pm$ 0.48
| -3.18 $\pm$ 0.10
---
-0.03 $\pm$ 0.02
0.74 $\pm$ 0.11
| -2.69
---
-0.05
0.04
| $f^{1},c_{x}^{1},c_{y}^{1}$
---
$(px)$
| $0\pm 5$
---
$0\pm 5$
| 2.47 $\pm$ 1.39
---
-5.02 $\pm$ 1.89
-0.56 $\pm$ 0.86
| 0.00 $\pm$ 0.00
---
0.00 $\pm$ 0.00
0.00 $\pm$ 0.00
| 0.44
---
-3.40
-1.08
| $k_{1}^{1},k_{2}^{1}$
---
$k_{3}^{1},k_{4}^{1}$
$(0.001)$
| $0\pm 50$
---
$0\pm 10$
$0\pm 1$
$0\pm 1$
| 2.59 $\pm$ 3.29
---
-2.13 $\pm$ 2.31
-0.27 $\pm$ 0.15
-0.06 $\pm$ 0.16
| 0.00 $\pm$ 0.00
---
0.00 $\pm$ 0.00
0.00 $\pm$ 0.00
0.00 $\pm$ 0.00
| 3.40
---
1.77
-2.66
0.33
$t_{d}^{1}(ms)$ | $0\pm 5$ | 0.24 $\pm$ 0.17 | 0.20 $\pm$ 0.11 | 0.13

By comparing the estimated sensor parameters by KSF and reference values in
Table IX (third column versus last one), we found that parameters
$\\{\mathbf{T}_{g},\mathbf{T}_{s}\\}$ could be estimated very close to their
reference values with small uncertainty. The small estimates for
$\mathbf{T}_{s}\\}$ entries mean that the g-sensitivity is negligible for the
IMU of TUM VI. Estimates for entries of $\\{\mathbf{T}_{a}\\}$ had more
uncertainty, likely because they are more challenging to calibrate. Some IMU
bias estimates were not very close to reference values mainly because biases
are typically random walk processes and reference values are obtained from
independent calibration data.

The camera extrinsic and intrinsic parameters, _e.g_., $\mathbf{p}_{C0B}$,
$f^{0},c_{x}^{0},c_{y}^{0}$ were mostly estimated with the uncertainty range
$[-\sigma,\sigma]$ covering their reference values. The distortion parameters,
_e.g_., $k_{1}^{0},k_{2}^{0},k_{3}^{0},k_{4}^{0}$, often had large uncertainty
and even obvious deviations from reference values at high order terms. This
likely indicated these parameters were not well observable.

When running KSF on these raw sequences, locking the IMU intrinsic parameters
$\\{\mathbf{T}_{g},\mathbf{T}_{s},\mathbf{T}_{a}\\}$ caused estimated poses of
most sequences to drift more than 1000 m. By contrast, KSF calibrating all
parameters except frame readout time achieved RRE 0.136 ${}^{\circ}/m$ and RTE
0.637% on the raw room sequences.

If the camera intrinsic parameters were known well close to their true values,
it might be tempting to lock these parameters during online calibration. With
this setting, calibration results for raw room sequences are given in Column
4, Table IX. Compared to calibrating all parameters (Column 3, Table IX), the
estimates for $\\{\mathbf{T}_{g},\mathbf{T}_{s},\mathbf{T}_{a}\\}$ and camera
extrinsic parameters generally came closer to their reference values. Also, in
this case, KSF achieved RRE 0.101 ${}^{\circ}/m$ and RTE 0.345% slightly
better than KSF calibrating all parameters. This indicated that calibrating
camera intrinsic parameters beforehand and locking them at good estimates can
help calibrating other parameters, _e.g_., IMU intrinsic parameters.

Temporal Parameter Estimation For calibrating frame readout time, we tested
KSF with three 10-minute sequences collected with rear color cameras and IMUs
on three Android smartphones, Honor V10, Lenovo Phab2 Pro, and Asus ZenFone
[46] which were moved randomly but smoothly in our workplace. We split a
sequence into four segments, each about two minutes, then ran KSF three times
on each segment, estimating all sensor parameters as in the Torus case of
Table V. For all runs, time offset and frame readout time of the camera were
initialized to 0 ms and 30 ms. The state vectors estimated at the end of these
12 runs for a smartphone were used to compute mean and standard deviation of
time delay and frame readout time of the camera. For comparison, we also
computed the frame readout time, _i.e_., rolling shutter skew, with a LED
panel which turns on columns of LEDs sequentially. By examining the tilted
line caused by the rolling shutter effect on an image, the frame readout time
can be easily computed [60]. For each smartphone, about 100 images captured at
varying exposure durations were used to compute the mean and standard
deviation of the frame readout time given in Table X. From the table, we see
that the time delay can be estimated with very small uncertainty. The frame
readout times by KSF, by the LED panel method, and from Android API are
reasonably close. Interestingly, estimates by KSF are consistently slightly
smaller than those obtained with the LED panel.

TABLE X: Temporal parameters estimated by KSF for the rear color camera of smartphones including Honor V10, Lenovo Phab2 Pro, and Asus ZenFone, and frame readout time returned by the Android camera API and estimated by manual drawing on images of a LED panel. |  | Time offset
---
(ms)
Frame readout time (ms)
Phone | KSF | KSF | API | LED panel
V10 | 12.946 $\pm$ 0.014 | 18.94 $\pm$ 0.15 | 20.73 | 20.68 $\pm$ 0.13
Phab2 Pro | -26.332 $\pm$ 0.038 | 29.34 $\pm$ 0.26 | 30.25 | 31.39 $\pm$ 0.30
ZenFone | -3.743 $\pm$ 0.066 | 6.45 $\pm$ 0.12 | 5.47 | 7.41 $\pm$ 0.31

###  V-C Pose Estimation

With two public benchmarks for VIO, EuRoC [56] captured by a MAV and TUM VI
[57] captured by a handheld device, we evaluated the performance of KSF in
regard to pose accuracy and compared to several recent open sourced VIO
methods, OKVIS [8], OpenVINS [7] and a stereo VIO method [32]. The evaluation
results of KSF versus other methods on the UZH-FPV dataset [55] is available
at its website111https://fpv.ifi.uzh.ch/uzh/2019-2020-uzh-fpv-temporary-
leader-board/osu-ethz-2/.

These methods were tested in two sensor setups, the left camera and the IMU,
and the stereo cameras and the IMU, except for the stereo VIO method [32]
which only supports the latter. To simplify tuning parameters, each method
with a specific sensor setup processed all data sequences of a benchmark with
the same set of parameters unless otherwise specified. Among parameters given
to these methods, we observed that the IMU noise parameters often required
manual tuning. If need be, the proper values for these parameters were found
by a simple search of a coefficient on white noise $c_{n}$ and a coefficient
on bias white noise $c_{bn}$ starting from baseline values of
$\\{\sigma_{g},\sigma_{a},\sigma_{bg},\sigma_{ba}\\}$. The coefficient pair
$(c_{n},c_{bn})$ scales the noise parameters as

$\begin{split}\sigma_{g}\leftarrow c_{n}\sigma_{g}&\quad\sigma_{a}\leftarrow
c_{n}\sigma_{a}\\\ \sigma_{bg}\leftarrow
c_{bn}\sigma_{bg}&\quad\sigma_{ba}\leftarrow c_{bn}\sigma_{ba}.\end{split}$
(42)

For KSF, we used the IMU-centric body frame (Table I), 5 old keyframes and 5
temporal frames, _i.e_., $N_{kf}=5$ and $N_{tf}=5$, in the sliding window, and
locked camera intrinsic parameters and frame readout times while estimating
camera extrinsic parameters and time offsets. These settings were sensible and
mostly supported by the sensitivity analysis in Section V-A. For OKVIS,
OpenVINS, and the stereo VIO method [32], we configured them with as many
default parameters provided by their authors as possible. Specifically,
OpenVINS was configured to run synchronously with landmark variables in the
state vector. When data from two cameras were available, the stereo mode was
chosen over the binocular mode. Method-specific parameters dependent on data
sequences are detailed in the following test results.

All methods were executed synchronously without enforcing real-time
processing. This was specially relevant for OKVIS which often ran nearly real-
time on our test computer. To reduce the random behavior, every method ran
three times on every sequence.

Benchmarking on EuRoC The EuRoC benchmark [56] was captured indoors by a MAV
mounted with a sensor platform of stereo cameras of global shutters and a
10-DOF IMU worth $\sim$$500. Ground truth of centimeter level position
accuracy by a motion capture system was provided covering all sequences. For
better comparison with other reported results on EuRoC, we used the ground
truth corrected by [7] in orientation for sequence V1_01 and otherwise the
ground truth provided by EuRoC. Additionally, sequence V2_03 was excluded
because its missing images caused some algorithm to break.

To save tuning efforts, the same IMU noise parameters were used for OKVIS with
stereo cameras, KSF with a monocular camera, and KSF with stereo cameras.
However, for OKVIS with a monocular camera, we had to inflate IMU noise
parameters by the coefficient pair (4, 4) relative to the stereo OKVIS setting
due to a grid search (42) for expected pose accuracy. For OpenVINS, stationary
segments at the start of several sequences were skipped to ensure good
initialization.

For these methods with a monocular or stereo setup, RMS of ATE on each
sequence and overall relative errors are tabulated in Table XI. OpenVINS had
good results in the monocular setup. The stereo VIO method [32] achieved best
results with stereo cameras. Overall, KSF achieved pose accuracy comparable to
recent VIO methods on EuRoC sequences.

TABLE XI: Mean RMS of ATE in meters and overall relative errors of VIO methods on EuRoC dataset over three runs. Best scores in either monocular or stereo mode are in $\mathbf{bold}$. RRE: Relative Rotation Error, RTE: Relative Translation Error. Method | MH_01 | MH_02 | MH_03 | MH_04 | MH_05 | V1_01 | V1_02 | V1_03 | V2_01 | V2_02 |  | RRE
---
(${}^{\circ}/m$)
| RTE
---
(%)
mono | OKVIS | 0.23 | 0.22 | 0.15 | 0.37 | 0.34 | 0.09 | 0.08 | 0.15 | 0.09 | 0.11 | 0.024 | 0.66
OpenVINS | 0.15 | 0.09 | 0.15 | 0.16 | 0.48 | 0.06 | 0.06 | 0.07 | 0.11 | 0.06 | 0.028 | 0.50
KSF | 0.13 | 0.29 | 0.16 | 0.29 | 0.42 | 0.09 | 0.11 | 0.19 | 0.12 | 0.15 | 0.027 | 0.69
stereo | OKVIS | 0.17 | 0.16 | 0.15 | 0.20 | 0.29 | 0.03 | 0.07 | 0.13 | 0.06 | 0.09 | 0.025 | 0.52
OpenVINS | 0.10 | 0.10 | 0.20 | 0.29 | 0.21 | 0.06 | 0.07 | 0.05 | 0.10 | 0.05 | 0.030 | 0.48
Usenko _et al_. [32] | 0.09 | 0.06 | 0.09 | 0.11 | 0.11 | 0.03 | 0.06 | 0.08 | 0.04 | 0.06 | 0.022 | 0.28
KSF | 0.05 | 0.08 | 0.10 | 0.25 | 0.27 | 0.04 | 0.05 | 0.09 | 0.04 | 0.06 | 0.027 | 0.37

Benchmarking on TUM VI The TUM VI benchmark [57] were captured indoors and
outdoors by a handheld device including stereo cameras of global shutters and
a MEMS IMU worth $\sim$$4. For each of eight outdoor sequences, the traveled
distance was greater than 900 m, the maximum being 2656 m. Ground truth of
centimeter level position accuracy by a motion capture system was provided for
all sequences at the start and at the end, and ground truth covering the whole
trajectory were available for sequences collected in the motion capture room.

For OKVIS in both monocular and stereo modes, the configuration file provided
by TUM VI was used. For OpenVINS, the acceleration variance threshold,
init_imu_thresh, was adjusted individually for each sequence so that the
algorithm could start without apparent drifting. Internally, the parameter is
used to detect if the device is moving and thus the algorithm is ready to be
initialized. For [32], parameters provided by the author were used.

For monocular or stereo KSF, the IMU noise parameters were tuned by the
aforementioned simple search starting from a baseline, _i.e_., the noise
parameters for OKVIS. For instance, with monocular KSF, we searched the list
of coefficient pairs with a roughly step size 2 on the logarithmic scale,
$\\{(0.05,0.1),(0.1,0.1),(0.1,0.25),(0.2,0.25)\\}$, which were selected based
on our observation that suitable IMU noise parameters for KSF tend to be
smaller than those for OKVIS. In the end, we found that monocular KSF worked
well with coefficient pair (0.2, 0.25) and stereo KSF with (0.1, 0.1).

All methods processed all sequences without significant drift >1000 m at the
end. The mean of RMS ATE over three runs of each sequence and overall relative
errors are tabulated in Table XII. From Table XII, we see that all methods
performed well with indoor sequences, but had difficulties with outdoor
sequences. Interestingly, all methods tackled well scenarios with few
features, _e.g_., slides1 - 3 which have segments captured while sliding
through a tube. From RMS ATE values, there is no clear winner, as the best
scores are distributed among these methods. From relative errors, KSF in both
monocular and stereo modes achieved overall best RTE.

TABLE XII: Mean RMS of ATE in meters and overall relative errors of VIO methods on TUM-VI benchmark over three runs. Best scores in either monocular or stereo mode are in $\mathbf{bold}$. RRE: Relative Rotation Error, RTE: Relative Translation Error, cor: corridor, mag:magistrale, out: outdoors. Session | Mono | Stereo
---|---|---
| OK-
---
VIS
| Open
---
VINS
KSF |  | OK-
---
VIS
| Open
---
VINS
| [32]
---
KSF
cor1 | 1.08 | 0.49 | 0.74 | 0.12 | 0.47 | 0.47 | 0.79
cor2 | 0.90 | 0.83 | 0.60 | 0.59 | 1.07 | 0.55 | 0.46
cor3 | 0.78 | 0.28 | 0.58 | 0.42 | 0.47 | 0.46 | 0.74
cor4 | 0.30 | 0.80 | 0.33 | 0.31 | 0.41 | 0.25 | 0.12
cor5 | 0.46 | 0.23 | 0.37 | 0.44 | 0.32 | 0.40 | 0.42
mag1 | 5.73 | 1.32 | 2.75 | 4.69 | 3.40 | 1.21 | 1.73
mag2 | 10.20 | 3.12 | 12.78 | 1.90 | 2.40 | 1.11 | 10.18
mag3 | 0.98 | 4.36 | 1.22 | 0.98 | 2.32 | 0.74 | 1.09
mag4 | 5.64 | 3.47 | 4.38 | 0.65 | 3.50 | 1.59 | 8.10
mag5 | 1.30 | 2.38 | 2.07 | 1.33 | 2.98 | 0.61 | 1.81
mag6 | 8.04 | 2.93 | 11.93 | 4.46 | 1.86 | 3.43 | 2.10
out1 | 104.3 | 108.8 | 69.63 | 93.14 | 118.6 | 255.1 | 80.68
out2 | 109.5 | 37.39 | 51.65 | 49.91 | 20.90 | 64.63 | 29.41
out3 | 17.40 | 40.44 | 45.15 | 22.14 | 54.56 | 38.26 | 25.56
out4 | 23.10 | 43.73 | 7.41 | 22.43 | 25.32 | 17.54 | 8.89
out5 | 17.81 | 78.06 | 9.45 | 7.67 | 12.81 | 7.89 | 13.09
out6 | 138.2 | 46.18 | 80.83 | 85.51 | 87.88 | 65.53 | 22.03
out7 | 20.41 | 22.95 | 31.16 | 4.61 | 14.37 | 4.16 | 30.86
out8 | 3.21 | 31.51 | 7.32 | 5.64 | 12.03 | 13.54 | 11.05
room1 | 0.09 | 0.06 | 0.08 | 0.06 | 0.06 | 0.09 | 0.05
room2 | 0.13 | 0.06 | 0.20 | 0.10 | 0.07 | 0.07 | 0.07
room3 | 0.07 | 0.10 | 0.09 | 0.06 | 0.08 | 0.13 | 0.07
room4 | 0.04 | 0.04 | 0.23 | 0.04 | 0.04 | 0.06 | 0.04
room5 | 0.08 | 0.08 | 0.11 | 0.06 | 0.08 | 0.14 | 0.10
room6 | 0.08 | 0.09 | 0.05 | 0.04 | 0.04 | 0.02 | 0.03
slides1 | 2.12 | 0.28 | 0.43 | 0.33 | 0.84 | 0.33 | 1.26
slides2 | 4.37 | 1.97 | 3.47 | 1.30 | 1.08 | 0.35 | 1.34
slides3 | 2.80 | 0.91 | 3.03 | 0.87 | 1.66 | 0.90 | 2.86
| RRE
---
(${}^{\circ}/m$)
0.156 | 0.120 | 0.118 | 0.087 | 0.105 | 0.118 | 0.135
| RTE
---
(%)
58.47 | 57.87 | 42.11 | 45.70 | 48.52 | 63.64 | 31.93

###  V-D Computation Timing

We measured the time consumption of major components of KSF on a consumer
laptop, Dell Insipiron 7590. The laptop had 16 GB RAM, a Intel Core i7-9750H
processor with 6 cores of base frequency 2.60 GHz. The program of KSF ran on
Ubuntu 18.04 without GPU acceleration. In implementation, the KSF program is
divided into three stages of a processing pipeline, feature extraction,
feature matching, and estimation including filter update and marginalization.
Each frame goes through these three ordered stages to complete processing.
Ideally these stages run in parallel while working on different frames such
that the throughput is only restrained by the most time-consuming stage.
However, the feature matching stage depends on results of the estimation stage
(Fig. 1). Thus, the average processing time for a frame is the total time for
feature matching and estimation.

To time these stages, monocular or stereo KSF was used to synchronously
process the whole EuRoC dataset once with timing service on. All sensor
parameters were unlocked, _i.e_., IMU parameters (9), camera extrinsic,
intrinsic and temporal parameters, $t_{d}^{k}$ and $t_{r}^{k}$, for $N$
cameras. In the sliding window, we kept $N_{kf}=7$ keyframes and $N_{tf}=5$
recent frames. From an image of a frame, up to 400 keypoints were extracted.

TABLE XIII: Time cost of major functions of KSF in processing EuRoC sequences In milliseconds. Values in bold identity routines that determine average time to process a frame. Function | Mono | Stereo
---|---|---
mean | $\sigma$ | mean | $\sigma$
Feature extraction 0 | 3.0 | 0.3 | 5.1 | 1.5
Feature extraction 1 | 0 | 0 | 5.1 | 1.6
Match to keyframes | 1.3 | 0.2 | 2.9 | 0.7
Match to last frame | 0.7 | 0.1 | 1.8 | 0.5
Match left to right | 0 | 0 | 0.5 | 0.1
Filter update | 4.5 | 1.4 | 13.4 | 3.6
Marginalization | 1.5 | 0.8 | 5.1 | 1.9
Time per frame | 8.0 | N/A | 23.7 | N/A

Means and standard deviations of computation costs for major components are
given in Table XIII. From the table, it is easy to see that based on the
pipelining discussion, the average time to process a frame was 8 ms (125 Hz)
in the monocular mode, and 23.7 ms (42.2 Hz) in the stereo mode. These numbers
show that KSF can process the EuRoC dataset in real time on a consumer laptop.
It is worth noting that feature matching functions of stereo KSF, _e.g_.,
match to last frame, took at least twice as long as the counterparts of
monocular KSF because of data from two cameras. And the filter update and
marginalization steps of stereo KSF were about three times as expensive as
those of monocular KSF. This was probably caused by increased number of
feature tracks due to stereo matching and parameters added to the state vector
for the extra camera. To put this in perspective, KSF runs at least three
times as fast as OKVIS, faster than OpenVINS because landmarks are not
included in the state vector, slower than the stereo VIO method [32] which
utilizes parallel implementation.

##  VI Discussion

The KSF method is versatile enough to calibrate a variety of sensor
parameters, including IMU scaling and misalignment errors and camera intrinsic
parameters. As expected, calibrating these parameters requires a rich variety
of motion that renders them observable. Our simulation in Section V showed
that estimating all sorts of IMU parameters caused slow convergence of biases
when the device went through monotonic motion. Additionally, tests on real-
world data indicated that estimating camera intrinsic parameters online led to
slightly worse pose estimation (Table VII, and Section V-B) when we had good
estimates for these parameters. In such cases, these less observable or well
calibrated parameters are better locked or alternative sensor models (Table I)
can be used.

KSF handles standstill periods at the start or in the middle of execution
without distinguishing motion types or using zero velocity update. The main
reason is the use of keyframes which typically do not change when the platform
is stationary. A minor reason is that KSF uses feature tracks of low
disparities to update the estimator as done in OKVIS [8]. Such feature tracks
appear more frequently when the camera platform is stationary or goes through
pure rotation. The corresponding landmarks are initialized with a large depth,
_e.g_., 1000 m, and used in the filter update step. Thus, KSF can initialize
even when the platform is stationary. Obviously doing so trades accuracy for
robustness and fast initialization.

Empirically, KSF tackled well scenes with few features in TUM VI [57], glass
elevators in ADVIO [61]. But it had a large jump in position, $\sim$70 m, for
an ADVIO sequence captured inside a steel elevator, and it often under
estimated the vertical motion for ADVIO sequences captured on escalators.
These cases are challenging because the apparent stationary visual features
are nearly or literally 100% outliers. Unsurprisingly, many proprietary
algorithms also broken down [61]. In the end, we recommend turning off VIO
methods in these special scenarios.

Furthermore, we observed that when the $N$-camera-IMU system was mounted on a
ground vehicle which traversed rough terrain, the accelerometer readings were
so disturbed by vibration that a VIO estimator did not give meaningful
results. In such cases, measures to reduce vibration, _e.g_., anti-vibration
mounts [62], are often necessary.

##  VII Conclusion And Future Work

This paper presents a versatile Keyframe-based Structureless Filter (KSF) for
visual inertial odometry with one or more cameras. The method associates
features relative to keyframes in the frontend and manages navigation state
variables in the filter’s sliding window according to whether they are
associated with keyframes, thus the method is able to deal with stationary
scenarios. This framework accommodates a couple of IMU models, several camera
extrinsic models, and a number of pinhole projection and lens distortion
models. The generic design of the filter allows calibrating or locking
scaling, misalignment, and relative orientation errors of the IMU, and
intrinsic, extrinsic, and temporal parameters of individual cameras.

The filter is able to estimate the sensor parameters accurately when the
motion is diverse enough for the sensor setup. This was confirmed with
simulation and by estimating the full spectrum of sensor parameters for raw
sequences of TUM VI benchmark [57]. From these tests, we observed that it
might benefit state estimation to lock some systematic errors of the IMU when
the motion is monotonic or to lock the camera intrinsic parameters when the
camera is well calibrated.

Also, KSF tracks device motion precisely with consistent covariance estimates
as shown by simulation. On EuRoC and TUM VI benchmarks, KSF with online
calibration of camera extrinsic parameters and time offsets, achieved
competitive pose accuracy relative to recent VIO methods, OKVIS [8], OpenVINS
[7], and a stereo VIO method [32]. In terms of speed, KSF processed EuRoC
stereo image streams at 42 Hz while performing full calibration.

In the near future, we will study alternative error state for filtering,
_e.g_., right invariant errors [63], which guarantees estimator consistency
without manipulating Jacobians.

Another interesting research topic is how to easily adapt a VIO method to data
collected by different sensors. In obtaining the accuracy report on public
benchmarks for the compared VIO methods, much effort was dedicated to
adjusting IMU noise parameters because of different sensors and IMU
integration implementations. As a result, we look forward to an approach that
alleviates the ”curse of manual tuning” [4].

## References

  * [1] Apple Inc. (2020) ARKit 3 for Augmented Reality. [Online]. Available: https://developer.apple.com/augmented-reality/
  * [2] J. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , Brisbane, Australia, May 2018, pp. 2502–2509.
  * [3] J. Huai, Y. Qin, F. Pang, and Z. Chen, “Segway DRIVE benchmark: place recognition and SLAM data collected by a fleet of delivery robots,” Segway Robotics, Beijing, China, Tech. Rep., Jul. 2019. [Online]. Available: http://arxiv.org/abs/1907.03424
  * [4] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” _IEEE Transactions on Robotics_ , vol. 32, no. 6, pp. 1309–1332, 2016.
  * [5] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A Versatile and Accurate Monocular SLAM System,” _IEEE Transactions on Robotics_ , vol. 31, no. 5, pp. 1147–1163, Oct. 2015. [Online]. Available: https://ieeexplore.ieee.org/document/7219438/
  * [6] J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 40, no. 3, pp. 611–625, Mar. 2018.
  * [7] P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in _Workshop on Visual-Inertial Navigation: Challenges and Applications at IROS_ , Macau, China, Nov. 2019.
  * [8] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimization,” _The International Journal of Robotics Research_ , vol. 34, no. 3, pp. 314–334, Mar. 2015. [Online]. Available: https://doi.org/10.1177/0278364914554813
  * [9] M. Li and A. I. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry,” _The International Journal of Robotics Research_ , vol. 32, no. 6, pp. 690–711, May 2013. [Online]. Available: https://doi.org/10.1177/0278364913481251
  * [10] A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: An open-source library for real-time metric-semantic localization and mapping,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , Paris, France, May 2020, pp. 1689–1696. [Online]. Available: https://github.com/MIT-SPARK/Kimera
  * [11] A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , Rome, Italy, Apr. 2007, pp. 3565–3572.
  * [12] M. Li, H. Yu, X. Zheng, and A. I. Mourikis, “High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , Hong Kong, China, May 2014, pp. 409–416.
  * [13] K. Eckenhoff, P. Geneva, and G. Huang, “MIMC-VINS: A versatile and resilient multi-IMU multi-camera visual-inertial navigation system,” Tech. Rep., Jun. 2020. [Online]. Available: http://arxiv.org/abs/2006.15699
  * [14] K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry for fast autonomous flight,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 2, pp. 965–972, Apr. 2018.
  * [15] C. Chen, B. Wang, C. X. Lu, N. Trigoni, and A. Markham, “A survey on deep learning for localization and mapping: Towards the age of spatial machine intelligence,” University of Oxford, Tech. Rep., Jun. 2020. [Online]. Available: http://arxiv.org/abs/2006.12567
  * [16] G. Huang, “Visual-inertial navigation: A concise review,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , Montreal, Canada, May 2019, pp. 9572–9582. [Online]. Available: http://arxiv.org/abs/1906.02650
  * [17] C. Chen, S. Rosa, Y. Miao, C. X. Lu, W. Wu, A. Markham, and N. Trigoni, “Selective sensor fusion for neural visual-inertial odometry,” in _IEEE Conf. on Computer Vision and Pattern Recognition (CVPR)_ , Long Beach, CA, USA, Jun. 2019, pp. 10 542–10 551.
  * [18] D. Strelow and S. Singh, “Motion estimation from image and inertial measurements,” _The International Journal of Robotics Research_ , vol. 23, no. 12, pp. 1157–1195, Dec. 2004.
  * [19] M. M. Veth and J. Raquet, “Fusing low-cost image and inertial sensors for passive navigation,” _Navigation_ , vol. 54, no. 1, pp. 11–20, 2007.
  * [20] E. S. Jones and S. Soatto, “Visual-inertial navigation, mapping and localization: A scalable real-time causal approach,” _The International Journal of Robotics Research_ , vol. 30, no. 4, pp. 407–430, 2011.
  * [21] J. Kelly and G. S. Sukhatme, “Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration,” _The International Journal of Robotics Research_ , vol. 30, no. 1, pp. 56–79, 2011.
  * [22] S. Weiss, M. W. Achtelik, S. Lynen, M. Chli, and R. Siegwart, “Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , St Paul, MN, USA, May 2012, pp. 957–964.
  * [23] E. T. Dill, “GPS/Optical/Inertial Integration for 3D Navigation and Mapping Using Multi-Copter Platforms,” Ph.D., Ohio University, Athens OH, May 2015.
  * [24] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Consistency analysis and improvement of vision-aided inertial navigation,” _IEEE Transactions on Robotics_ , vol. 30, no. 1, pp. 158–176, Feb. 2014.
  * [25] Z. Huai and G. Huang, “Robocentric visual-inertial odometry,” in _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_ , Madrid, Spain, Oct. 2018, pp. 6319–6326. [Online]. Available: https://doi.org/10.1177/0278364919853361
  * [26] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the Bayes tree,” _The International Journal of Robotics Research_ , vol. 31, no. 2, pp. 216–235, 2012.
  * [27] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” _IEEE Transactions on Robotics_ , vol. 33, no. 1, pp. 1–21, Feb. 2017. [Online]. Available: https://ieeexplore.ieee.org/document/7557075/
  * [28] T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 1004–1020, Aug. 2018.
  * [29] G. R. Bradski and A. Kaehler, _Learning OpenCV: Computer vision with the OpenCV library_ , 1st ed. Sebastopol, CA, USA: O’Reilly Media, Inc., 2008.
  * [30] Y. He, J. Zhao, Y. Guo, W. He, and K. Yuan, “PL-VIO: Tightly-coupled monocular visual–inertial odometry using point and line features,” _Sensors_ , vol. 18, no. 4, pp. 1159–1183, 2018.
  * [31] J. Huai, C. K. Toth, and D. A. Grejner-Brzezinska, “Stereo-inertial odometry using nonlinear optimization,” in _28th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2015)_ , Tampa, Florida, Sep. 2015, pp. 2087–2097.
  * [32] V. Usenko, N. Demmel, D. Schubert, J. Stückler, and D. Cremers, “Visual-inertial mapping with non-linear factor recovery,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 422–429, Apr. 2020.
  * [33] G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “Observability-based rules for designing consistent EKF SLAM estimators,” _The International Journal of Robotics Research_ , vol. 29, no. 5, pp. 502–528, 2010.
  * [34] M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback,” _The International Journal of Robotics Research_ , vol. 36, no. 10, pp. 1053–1072, Sep. 2017. [Online]. Available: https://doi.org/10.1177/0278364917728574
  * [35] S. Leutenegger, M. Chli, and R. Y. Siegwart, “BRISK: Binary robust invariant scalable keypoints,” in _Intl. Conf. on Computer Vision (ICCV)_ , Barcelona, Spain, Nov. 2011, pp. 2548–2555.
  * [36] T. Schneider, M. Li, C. Cadena, J. Nieto, and R. Siegwart, “Observability-aware self-calibration of visual and inertial sensors for ego-motion estimation,” _IEEE Sensors Journal_ , vol. 19, no. 10, pp. 3846–3860, 2019.
  * [37] J. Rehder, J. Nikolic, T. Schneider, T. Hinzmann, and R. Siegwart, “Extending kalibr: Calibrating the extrinsics of multiple IMUs and of individual axes,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , Stockholm, Sweden, May 2016, pp. 4304–4311.
  * [38] Y. Lin, V. Larsson, M. Geppert, Z. Kukelova, M. Pollefeys, and T. Sattler, “Infrastructure-based multi-camera calibration using radial projections,” in _Eur. Conf. on Computer Vision (ECCV)_ , Glasgow, UK, Aug. 2020.
  * [39] Y. Xiao, X. Ruan, J. Chai, X. Zhang, and X. Zhu, “Online IMU self-calibration for visual-inertial systems,” _Sensors_ , vol. 19, no. 7, pp. 1624–1649, 2019.
  * [40] J. Huai, “Collaborative SLAM with Crowdsourced Data,” Ph.D., The Ohio State University, Columbus OH, May 2017. [Online]. Available: https://etd.ohiolink.edu/pg_10?0::NO:10:P10_ACCESSION_NUM:osu1483669256597152
  * [41] C. Jekeli, _Inertial Navigation Systems with Geodetic Applications_. Berlin, Germany: de Gruyter, Jan. 2001.
  * [42] A. Géron, _Hands-on Machine Learning with Scikit-Learn & TensorFlow: Concepts, Tools, and Techniques to Build Intelligent Systems_, 1st ed. O’Reilly Media, Inc., 2017.
  * [43] M. Li and A. I. Mourikis, “Vision-aided inertial navigation with rolling-shutter cameras,” _The International Journal of Robotics Research_ , vol. 33, no. 11, pp. 1490–1507, 2014.
  * [44] C. Guo, D. Kottas, R. DuToit, A. Ahmed, R. Li, and S. Roumeliotis, “Efficient visual-inertial navigation using a rolling-shutter camera with inaccurate timestamps,” in _Robotics: Science and Systems (RSS)_ , Rome, Italy, Jul. 2014.
  * [45] M. Li and A. I. Mourikis, “Online temporal calibration for camera–IMU systems: Theory and algorithms,” _The International Journal of Robotics Research_ , vol. 33, no. 7, pp. 947–964, 2014.
  * [46] J. Huai, Y. Zhang, and A. Yilmaz, “The mobile AR sensor logger for Android and iOS devices,” in _IEEE SENSORS_ , Montreal, Canada, Oct. 2019, pp. 1–4. [Online]. Available: https://arxiv.org/abs/2001.00470
  * [47] C. Hertzberg, R. Wagner, U. Frese, and L. Schröder, “Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds,” _Information Fusion_ , vol. 14, no. 1, pp. 57–77, Jul. 2011. [Online]. Available: http://arxiv.org/abs/1107.1119
  * [48] J. Solà, “Quaternion kinematics for the error-state Kalman filter,” Institut de Robòtica i Informàtica Industrial, Barcelona, Tech. Rep., Nov. 2017. [Online]. Available: http://arxiv.org/abs/1711.02508
  * [49] L. Kneip and P. Furgale, “OpenGV: A unified and generalized approach to real-time calibrated geometric vision,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , Hong Kong, China, May 2014, pp. 1–8.
  * [50] L. Kneip, M. Chli, and R. Siegwart, “Robust real-time visual odometry with a single camera and an IMU,” in _British Machine Vision Conference 2011_ , Dundee, Scotland, UK, Aug. 2011, pp. 16.1–16.11.
  * [51] M. Brossard, A. Barrau, and S. Bonnabel, “Exploiting symmetries to design EKFs with consistency properties for navigation and SLAM,” _IEEE Sensors Journal_ , vol. 19, no. 4, pp. 1572–1579, 2018.
  * [52] Y. Bar-Shalom, X. Li, and T. Kirubarajan, _Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software_. John Wiley & Sons, 2004.
  * [53] O. J. Woodman, “An introduction to inertial navigation,” University of Cambridge, Computer Laboratory, Tech. Rep., 2007.
  * [54] Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory evaluation for visual(-inertial) odometry,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , Madrid, Spain, Oct. 2018, pp. 7244–7251.
  * [55] J. Delmerico, T. Cieslewski, H. Rebecq, M. Faessler, and D. Scaramuzza, “Are we ready for autonomous drone racing? The UZH-FPV drone racing dataset,” in _IEEE Intl. Conf. on Robotics and Automation (ICRA)_ , Montreal, Canada, May 2019, pp. 6713–6719.
  * [56] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart, “The EuRoC micro aerial vehicle datasets,” _The International Journal of Robotics Research_ , vol. 35, no. 10, pp. 1157–1163, 2016.
  * [57] D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, and D. Cremers, “The TUM VI benchmark for evaluating visual-inertial odometry,” in _IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)_ , Madrid, Spain, Oct. 2018. [Online]. Available: http://arxiv.org/abs/1804.06120
  * [58] Y. Yang, P. Geneva, X. Zuo, and G. Huang, “Online IMU intrinsic calibration: Is it necessary?” in _Robotics: Science and Systems (RSS)_ , Corvallis, Oregon, Jul. 2020, pp. 716–725.
  * [59] J. Solà, T. Vidal-Calleja, J. Civera, and J. M. M. Montiel, “Impact of landmark parametrization on monocular EKF-SLAM with points and lines,” _International Journal of Computer Vision_ , vol. 97, no. 3, pp. 339–368, May 2012.
  * [60] Image Engineering GmbH & Co. KG, “LED-panel v4 user manual,” Mar. 2020\. [Online]. Available: https://www.image-engineering.de/content/products/equipment/measurement_devices/led_panel/data/LED-Panel_manual.pdf
  * [61] S. Cortés, A. Solin, E. Rahtu, and J. Kannala, “ADVIO: An authentic dataset for visual-inertial odometry,” in _Eur. Conf. on Computer Vision (ECCV)_ , Munich, Germany, Sep. 2018, pp. 419–434.
  * [62] D. Titterton and J. Weston, _Strapdown Inertial Navigation Technology_ , 2nd ed. London, UK: The Institution of Engineering and Technology, 2004.
  * [63] T. Zhang, “The Invariance of State Estimation for Robot Navigation,” Thesis, University of Technology Sydney, Sydney, Australia, Feb. 2018. [Online]. Available: https://opus.lib.uts.edu.au/handle/10453/123279