# R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state
Estimator and mapping

Jiarong Lin, Chunran Zheng, Wei Xu, and Fu Zhang J. Lin, C. Zheng, W. Xu and
F. Zhang are with the Department of Mechanical Engineering, Hong Kong
University, Hong Kong SAR., China. $\\{$jiarong.lin, zhengcr, wuweii,
<EMAIL_ADDRESS>
###### Abstract

In this letter, we propose a robust, real-time tightly-coupled multi-sensor
fusion framework, which fuses measurement from LiDAR, inertial sensor, and
visual camera to achieve robust and accurate state estimation. Our proposed
framework is composed of two parts: the filter-based odometry and factor graph
optimization. To guarantee real-time performance, we estimate the state within
the framework of error-state iterated Kalman-filter, and further improve the
overall precision with our factor graph optimization. Taking advantage of
measurement from all individual sensors, our algorithm is robust enough to
various visual failure, LiDAR-degenerated scenarios, and is able to run in
real-time on an on-board computation platform, as shown by extensive
experiments conducted in indoor, outdoor, and mixed environment of different
scale. Moreover, the results show that our proposed framework can improve the
accuracy of state-of-the-art LiDAR-inertial or visual-inertial odometry. To
share our findings and to make contributions to the community, we open source
our codes on our Github111https://github.com/hku-mars/r2live.

##  I Introduction

With the capacity of estimating ego-motion in six degrees of freedom (DOF) and
simultaneously building dense and high precision maps of surrounding
environments, LiDAR-based SLAM has been widely applied in the field of
autonomous driving vehicles [1], drones [2, 3], and etc. With the development
of LiDAR technologies, the emergence of low-cost LiDARs (e.g., Livox LiDAR
[4]) makes LiDAR more accessible. Following this trend, a number of related
works [5, 6, 7, 8] draw the attention of the community to this field of
research. However, LiDAR-based SLAM methods easily fail (i.e., degenerate) in
those scenarios with few available geometry features, which is more critical
for those LiDARs with small FoV [9]. In this work, to address the degeneration
problems of LiDAR-based odometry, we propose a LiDAR-inertial-visual fusion
framework to obtain the state estimation of higher robustness and accuracy.
The main contributions of our work are:

  * •

We take advantage of measurements from LiDAR, inertial and camera sensors and
fuse them in a tight-coupled way. Experiments show that our method is robust
enough in various challenging scenarios with aggressive motion, sensor
failure, and even in narrow tunnel-like environments with a large number of
moving objects and small LiDAR field of view.

  * •

We propose a framework with a high-rate filter-based odometry and a low-rarte
factor graph optimization. The filter-based odometry fuses the measurements of
LiDAR, inertial, and camera sensors within an error-state iterated Kalman
filter to achieve real-time performance. The factor graph optimization refines
a local map of keyframe poses and visual landmark positions.

  * •

By tightly fusing different types of sensors, we achieve high-accuracy state
estimation. Experiment results show that our system is accurate enough to be
used to reconstruct large-scale, indoor-outdoor dense 3D maps of building
structures (see Fig. 1).

Our system is carefully engineered and open sourced11footnotemark: 1 to
benefit the whole robotics community.

Figure 1: We use our proposed method to reconstruct a high precision, large
scale, indoor-outdoor, dense 3D maps of the main building of the University of
Hong Kong (HKU). The green path is the computed trajectory and the 3D points
are colored by height.

##  II Related work

In this section, we review existing works closely related to our work,
including LiDAR-only odometry and mapping, LiDAR-Inertial fusion and LiDAR-
Inertial-Visual methods.

###  II-A LiDAR Odometry and Mapping

Zhang et al [10] first proposed a LiDAR odometry and mapping framework, LOAM,
that combines ICP method [11] with point-to-plane and point-to-edge distance.
It achieves good odometry and mapping performance by running the two modules
at different rates. To make the algorithm run in real time at a computation
limited platform, Shan et al [12] propose a lightweight and ground-optimized
LOAM (LeGO-LOAM), which discards unreliable features in the step of ground
plane segmentation. These works are mainly based on multi-line spinning
LiDARs. Our previous work [9, 13] develop an accurate and robust algorithm by
considering the low-level physical properties of solid-state LiDARs with small
FOV. However, these methods solely based on LiDAR measurements and are very
vulnerable to featureless environments or other degenerated scenarios.

###  II-B LiDAR-Inertial Odometry

The existing works of LiDAR-Inertial fusion can be categorized into two
classes: loosely-coupled and the tightly-coupled. Loosely-coupled methods deal
with two sensors separately to infer their motion constraints while tightly-
coupled approaches directly fuse lidar and inertial measurements through
joint-optimization. Compared with loosely-coupled methods, tightly-coupled
methods show higher robustness and accuracy, therefore drawing increasing
research interests recently. For example, authors in [14] propose LIOM which
uses a graph optimization based on the priors from LiDAR-Inertial odometry and
a rotation-constrained refinement method. Compared with the former algorithm,
LIO-SAM [15] optimizes a sliding-window of keyframe poses in a factor graph to
achieve higher accuracy. Similarly, Li et al. propose LiLi-OM [16] for both
conventional and solid-state LiDARs based on sliding window optimization. LINS
[17] is the first tightly-coupled LIO that solves the 6 DOF ego-motion via
iterated Kalman filtering. To lower the high computation load in calculating
the Kalman gain, our previous work FAST-LIO [5] proposes a new formula of the
Kalman gain computation, the resultant computation complexity depends on the
state dimension instead of measurement dimension. The work achieves up to 50
Hz odometry and mapping rate while running on embedded computers onboard a
UAV.

###  II-C LiDAR-Inertial-Visual Odometry

On the basis of LiDAR-Inertial methods, LiDAR-Inertial-Visual odometry
incorporating measurements from visual sensors shows higher robustness and
accuracy. In the work of [18], the LiDAR measurements are used to provide
depth information for camera images, forming a system similar to RGB-D camera
that can leverage existing visual SLAM work such as ORB-SLAM [19]. This is a
loosely-coupled method as it ignores the direct constraints on state imposed
by LiDAR measurements. Zuo et al [20] propose a LIC-fusion framework combining
IMU measurements, sparse visual features, and LiDAR plane and edge features
with online spatial and temporal calibration based on the MSCKF framework,
which is claimed more accurate and robust than state-of-the-art methods. In
quick succession, their further work termed LIC-Fusion 2.0 [21] refines a
novel plane-feature tracking algorithm across multiple LiDAR scans within a
sliding-window to make LiDAR scan matching more robust.

To the best of our knowledge, our work is the first open sourced tightly-
coupled LiDAR-inertial-visual fusion system. By fusing different types of
sensor measurements, we achieve state estimation of higher accuracy and
robustness. Extensive results show that our system is more accurate and robust
than state-of-the-art LiDAR-inertial and Visual-inertial fusion estimator
(e.g., FAST-LIO [5], and VINS-mono[22]).

##  III The overview of our system

Figure 2: The overview of our proposed method.

The overview of our system is shown in Fig. 2, where the filter-based odometry
taking advantage of the measurements from LiDAR, camera and inertial sensor,
estimates the state within the framework of error-state iterated Kalman filter
as detailed in Section IV. To further improve the visual measurements, we
leverage the factor graph optimization to refine the visual landmarks within a
local sliding window as detailed in Section V.

##  IV Filter-based odometry

###  IV-A The boxplus “$\boxplus$” and boxminus “$\boxminus$” operator

In this paper, we make use of the “$\boxplus$” and “$\boxminus$” operations
encapsulated on a manifold $\mathcal{M}$ to simplify the notations and
derivations. Let $\mathcal{M}$ be the manifold on which the system state lies.
Since a manifold is locally homeomorphic to $\mathbb{R}^{n}$, where $n$ is the
dimension of $\mathcal{M}$, we can use two operators, “$\boxplus$” and
“$\boxminus$”, establishing a bijective map between the local neighborhood of
$\mathcal{M}$ and its tangent space $\mathbb{R}^{n}$ [23]:

$\displaystyle\boxplus:\mathcal{M}\times\mathbb{R}^{n}\rightarrow\mathcal{M},~{}~{}\boxminus:\mathcal{M}\times\mathcal{M}\rightarrow\mathbb{R}^{n}$
(1)

For the compound manifold $\mathcal{M}=SO(3)\times\mathbb{R}^{n}$, we have:

$\displaystyle\begin{bmatrix}\mathbf{R}\\\
\mathbf{a}_{1}\end{bmatrix}\boxplus\begin{bmatrix}\mathbf{r}\\\
\mathbf{a}_{2}\end{bmatrix}\triangleq\begin{bmatrix}\mathbf{R}\cdot\mathtt{Exp}(\mathbf{r})\\\
\mathbf{a}_{1}+\mathbf{a}_{2}\end{bmatrix},\hskip
2.84544pt\begin{bmatrix}\mathbf{R}_{1}\\\
\mathbf{a}_{1}\end{bmatrix}\boxminus\begin{bmatrix}\mathbf{R}_{2}\\\
\mathbf{a}_{2}\end{bmatrix}\triangleq\begin{bmatrix}\mathtt{Log}(\mathbf{R}_{2}^{T}\mathbf{R}_{1})\\\
\mathbf{a}_{1}-\mathbf{a}_{2}\end{bmatrix}$

where $\mathbf{r}\in\mathbb{R}^{3}$,
$\mathbf{a}_{1},\mathbf{a}_{2}\in\mathbb{R}^{n}$, $\mathtt{Exp}(\cdot)$ and
$\mathtt{Log}(\cdot)$ denote the Rodrigues’ transformation between the
rotation matrix and rotation
vector222https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula.

###  IV-B Continuous-time kinematic model

In our current work, we assume that the time offset among all the three
sensors, LiDAR, camera, and IMU, are pre-calibrated. Furthmore, we assume the
extrinsic between LiDAR and IMU are known as they are usually integrated and
calibrated in factory, but estimate the camera IMU extrinsic online. Moreover,
a LiDAR typically scans points sequentially, and points in a LiDAR frame could
be measured at different body pose. This motion is compensated by IMU back
propagation as shown in [5], hence points in a LiDAR frame are assumed to be
measured at the same time. With these, the input data sequences of our system
can be simplified into Fig. 3.

Figure 3: Illustration of the input data sequences, where the frame rate of
IMU, camera, and LiDAR is 200 Hz, 20 Hz and 10 Hz, respectively. The notation
$i$ denotes the index of IMU data while $k$ denotes the index of LiDAR or
camera measurements.

We assume that the IMU, LiDAR, and camera sensors are rigidly attached
together with the extrinsic between LiDAR and IMU (LiDAR frame w.r.t. IMU
frame) as ${}^{I}\mathbf{T}_{L}=(^{I}\mathbf{R}_{L},{{}^{I}\mathbf{p}_{L}})$,
and the extrinsic between camera and IMU (camera frame w.r.t. IMU frame) is
${}^{I}\mathbf{T}_{C}=(^{I}\mathbf{R}_{C},{{}^{I}\mathbf{p}_{C}})$. For the
sake of convenience, we take IMU as the body frame, which leads to the
following continuous kinematic model:

$\displaystyle{{}^{G}\dot{\mathbf{p}}_{I}}={{}^{G}{\mathbf{v}}_{I}},{{}^{G}\dot{\mathbf{v}}_{I}}={{}^{G}{\mathbf{R}}_{I}}(\mathbf{a}_{m}$
$\displaystyle-\mathbf{b}_{a}-\mathbf{n}_{a})+{{}^{G}{\mathbf{g}}},$
$\displaystyle{{}^{G}{\dot{\mathbf{R}}}_{I}}={{}^{G}{{\mathbf{R}}}_{I}}\left[\bm{\omega}_{m}-\mathbf{b}_{\mathbf{g}}-\mathbf{n}_{\mathbf{g}}\right]_{\times}$
$\displaystyle,~{}\dot{\mathbf{b}}_{\mathbf{g}}=\mathbf{n}_{\mathbf{bg}},\dot{\mathbf{b}}=\mathbf{n}_{\mathbf{ba}}$
(2)

where ${{}^{G}(\cdot)}$ denotes a vector represented in the global frame (i.e.
the first gravitational aligned IMU frame [22]), ${}^{G}\mathbf{R}_{I}$ and
${}^{G}\mathbf{p}_{I}$ are the attitude and position of the IMU relative to
the global frame, ${{}^{G}\mathbf{g}}$ is the gravitational acceleration,
$\bm{\omega}_{m}$ and $\mathbf{a}_{m}$ are the raw gyroscope and accelerometer
readings, $\mathbf{n}_{\mathbf{a}}$ and $\mathbf{n}_{\mathbf{g}}$ are the
white noise of IMU measurement, $\mathbf{b}_{\mathbf{a}}$ and
$\mathbf{b}_{\mathbf{g}}$ are the bias of gyroscope and accelerometer, which
are modelled as random walk driven by Gaussian noise
$\mathbf{n}_{\mathbf{bg}}$ and $\mathbf{n}_{\mathbf{ba}}$.

###  IV-C Discrete IMU model

We discretize the continuous model (2) at the IMU rate. Let $\mathbf{x}_{i}$
be the state vector at the $i$-th IMU measurement:

$\displaystyle\mathbf{x}_{i}$
$\displaystyle=\begin{bmatrix}{}^{G}\mathbf{R}_{I_{i}}^{T}&{}^{G}\mathbf{p}_{I_{i}}^{T}&{}^{I}\mathbf{R}_{C_{i}}^{T}&{}^{I}\mathbf{p}_{C_{i}}^{T}&{}^{G}\mathbf{v}_{i}^{T}&\mathbf{b}_{\mathbf{g}_{i}}^{T}&\mathbf{b}_{\mathbf{a}_{i}}^{T}\end{bmatrix}^{T}$

Discretizing (2) by zero-order holder (i.e., IMU measurements over one
sampling time period $\Delta t$ are constant), we obtain

$\mathbf{x}_{t+1}=\mathbf{x}_{i}\boxplus\left(\Delta
t\mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)\right)$
(3)

where

$\displaystyle\mathbf{u}_{i}$
$\displaystyle=\begin{bmatrix}\bm{\omega}_{m_{i}}^{T}&\mathbf{a}_{m_{i}}^{T}\end{bmatrix}^{T},\hskip
5.69046pt\mathbf{w}_{i}=\begin{bmatrix}\mathbf{n}_{\mathbf{g}_{i}}^{T}&\mathbf{n}_{\mathbf{a}_{i}}^{T}&\mathbf{n}_{\mathbf{b}\mathbf{g}_{i}}^{T}&\mathbf{n}_{\mathbf{b}\mathbf{a}_{i}}^{T}\end{bmatrix}^{T}\hskip
5.69046pt$ $\displaystyle\mathbf{f}$
$\displaystyle(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i})=\begin{bmatrix}\bm{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{g}_{i}}-\mathbf{n}_{\mathbf{g}_{i}}\\\
{}^{G}\mathbf{v}_{i}\\\ \mathbf{0}_{3\times 1}\\\ \mathbf{0}_{3\times 1}\\\
{}^{G}\mathbf{R}_{I_{i}}\left(\mathbf{a}_{m_{i}}-\mathbf{b}_{\mathbf{a}_{i}}-\mathbf{n}_{\mathbf{g}_{i}}\right)-{{}^{G}\mathbf{g}}\\\
\mathbf{b}_{\mathbf{g}_{i}}\\\ \mathbf{b}_{\mathbf{a}_{i}}\end{bmatrix}$

###  IV-D Propagation

In our work, we leverage an on-manifold iterated error state Kalman filter
[24] to estimate the state vector $\mathbf{x}_{i}$, in which the state
estimation error $\delta\hat{\mathbf{x}}_{i}$ is characterized in the tangent
space of the state estimate $\hat{\mathbf{x}}_{i}$:

$\displaystyle\delta\hat{\mathbf{x}}_{i}\triangleq\mathbf{x}_{i}\boxminus\hat{\mathbf{x}}_{i}$
$\displaystyle=\begin{bmatrix}{}^{G}\delta\hat{\mathbf{r}}_{I_{i}}^{T}&{}^{G}\delta\hat{\mathbf{p}}_{I_{i}}^{T}&{}^{{I}}\delta\hat{\mathbf{r}}_{C_{i}}^{T}&{}^{{I}}\delta\hat{\mathbf{p}}_{C_{i}}^{T}&{}^{G}\delta\hat{\mathbf{v}}_{i}^{T}&\delta\hat{\mathbf{b}}_{\mathbf{g}_{i}}^{T}&\delta\hat{\mathbf{b}}_{\mathbf{a}_{i}}^{T}\end{bmatrix}^{T}$
$\displaystyle\sim\mathcal{N}(\mathbf{0}_{21\times
1},\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{i}})$ (4)

Note that $\delta\hat{\mathbf{x}}_{i}\in\mathbb{R}^{21}$ is in minimum
dimension (the system dimension 21) and is a random vector with covariance
$\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{i}}$.
${}^{G}\delta\hat{\mathbf{r}}_{I_{i}}^{T}$ and
${{}^{{I}}\delta\hat{\mathbf{r}}_{C_{i}}^{T}}$ are:

${}^{G}\delta\hat{\mathbf{r}}_{I_{i}}=\mathtt{Log}({{}^{G}\hat{\mathbf{R}}_{I_{i}}^{T}}{{}^{G}{\mathbf{R}}_{I_{i}}^{T}}),~{}~{}{{{}^{{I}}\delta\hat{\mathbf{r}}_{C_{i}}}}=\mathtt{Log}({{}^{I}\hat{\mathbf{R}}_{C_{i}}^{T}}{{}^{I}{\mathbf{R}_{C_{i}}}})$

Once receiving a new IMU measurement, the state estimate is propagated by
setting the process noise in (3) to zero:

$\displaystyle\hat{\mathbf{x}}_{i+1}=\hat{\mathbf{x}}_{i}\boxplus\left(\Delta
t\cdot\mathbf{f}(\hat{\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{0})\right).$ (5)

The associated estimation error is propagated in the linearized error space as
follows (see [24] for more details):

$\displaystyle\delta{\hat{\mathbf{x}}}_{i+1}={\mathbf{x}}_{i+1}\boxminus\hat{\mathbf{x}}_{i+1}$
(6) $\displaystyle=\Large(\mathbf{x}_{i}\boxplus\left(\Delta
t\cdot\mathbf{f}({\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{w}_{i})\right)\Large)\boxminus\left(\hat{\mathbf{x}}_{i}\boxplus\left(\Delta
t\cdot\mathbf{f}(\hat{\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{0})\right)\right)$
$\displaystyle\sim\mathcal{N}(\mathbf{0}_{21\times
1},\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{i+1}})$

where:

$\displaystyle\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{i+1}}=\mathbf{F}_{\delta{\hat{\mathbf{x}}}}\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{i}}\mathbf{F}_{\delta{\hat{\mathbf{x}}}}^{T}+\mathbf{F}_{\mathbf{w}}\mathbf{Q}\mathbf{F}_{\mathbf{w}}^{T}$
(7)
$\displaystyle\mathbf{F}_{\delta{\hat{\mathbf{x}}}}=\left.\dfrac{\partial\left(\delta{\hat{\mathbf{x}}}_{i+1}\right)}{\partial\delta{\hat{\mathbf{x}}_{i}}}\right|_{\delta{\hat{\mathbf{x}}_{i}}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}},~{}~{}\mathbf{F}_{{\mathbf{w}}}=\left.\dfrac{\partial\left(\delta{\hat{\mathbf{x}}}_{i+1}\right)}{\partial{\mathbf{w}_{i}}}\right|_{\delta{\hat{\mathbf{x}}_{i}}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}}$

with their exact values computed in Appendix. -A.

The two propagation in (5) and (7) starts from the optimal state and
covariance estimate after fusing the most recent LiDAR/camera measurement
(e.g., the $k$-th measurement, see Section IV-I), and repeat until receiving
the next LiDAR/camera measurement (e.g., the $(k+1)$-th measurement). The
relation between time index $i$ and $k$ is shown in Fig. 3.

###  IV-E The prior distribution

Let the two propagation in (5) and (7) stop at the $(k+1)$-th LiDAR/camera
measurement (see Fig. 4), and the propagated state estimate and covariance are
$\hat{\mathbf{x}}_{k+1}$ and $\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}$,
respectively. They essentially impose a prior distribution for the state
$\mathbf{x}_{k+1}$ before fusing the $(k+1)$-th measurement as below:

Figure 4: The illustraction of the update of our error-state iterated Kalman
filter.

$\displaystyle\mathbf{x}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}$
$\displaystyle\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}).$
(8)

###  IV-F Initialization of iterated update

The prior distribution in (8) will be fused with the LiDAR or camera
measurements to produce a maximum a-posterior (MAP) estimate (denoted as
$\check{\mathbf{x}}_{k+1}$) of $\mathbf{x}_{k+1}$. The MAP estimate
$\check{\mathbf{x}}_{k+1}$ is initialized as the prior estimate
$\hat{\mathbf{x}}_{k+1}$ and is refined iteratively due to the nonlinear
nature of the problem. In each iteration, the error
$\delta{\check{\mathbf{x}}}_{k+1}$ between the true state $\mathbf{x}_{k+1}$
and the current estimate $\check{\mathbf{x}}_{k+1}$, defined as

$\delta{\check{\mathbf{x}}}_{k+1}\triangleq\mathbf{x}_{k+1}\boxminus\check{\mathbf{x}}_{k+1},$
(9)

will be solved by minimizing the posterior distribution considering the prior
in (8) and LiDAR/visual measurements. Therefore, the prior distribution in
terms of $\mathbf{x}_{k+1}$ represented by (8) should be transformed to an
equivalent prior distribution in terms of $\delta{\check{\mathbf{x}}}_{k+1}$:

$\begin{split}\mathbf{x}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}&=\left(\check{\mathbf{x}}_{k+1}\boxplus\delta{\check{\mathbf{x}}}_{k+1}\right)\boxminus\hat{\mathbf{x}}_{k+1}\\\
&\approx\check{\mathbf{x}}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}+\bm{\mathcal{H}}\delta{\check{\mathbf{x}}}_{k+1}\\\
&\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}),\end{split}$
(10)

where

$\bm{\mathcal{H}}=\dfrac{\left(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1}\right)\boxminus\hat{\mathbf{x}}_{k+1}}{\partial\delta\check{\mathbf{x}}_{k+1}}|_{\delta\check{\mathbf{x}}_{k+1}=\mathbf{0}}$

is computed in detail in Appendix. -B, (10) essentially imposes a prior
distribution to $\delta{\check{\mathbf{x}}}_{k+1}$ as below:

$\delta{\check{\mathbf{x}}}_{k+1}\sim\mathcal{N}(-\bm{\mathcal{H}}^{-1}\left(\check{\mathbf{x}}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}\right),\bm{\mathcal{H}}^{-1}\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}\bm{\mathcal{H}}^{-T})$
(11)

###  IV-G LiDAR measurement

If the $(k+1)$-th measurement is a LiDAR frame, we extract planar feature
points from the raw 3D points as in [5] and compensate the in-frame motion as
in Section IV-B. Denote ${\bm{\mathcal{L}}}_{k+1}$ the set of feature points
after motion compensation, we compute the residual of each feature point
${{}^{L}\mathbf{p}_{j}}\in{\bm{\mathcal{L}}}_{k+1}$ where $j$ is the index of
feature point and the superscript $L$ denotes that the point is represented in
the LiDAR-reference frame.

With $\check{\mathbf{x}}_{k+1}$ being the current estimate of
${\mathbf{x}}_{k+1}$, we can transform ${{}^{L}\mathbf{p}_{j}}$ from LiDAR
frame to the global frame
${{}^{G}\mathbf{p}_{j}}={{}^{G}{\check{\mathbf{R}}}_{I_{k+1}}}(^{I}\mathbf{R}_{L}{{}^{L}{\mathbf{p}}_{j}}+{{}^{I}\mathbf{p}_{L}})+{{}^{G}\check{\mathbf{p}}_{I_{k+1}}}.$
As the previous LOAM pipeline does in [9, 5], we search for the nearest planar
feature points in the map and use them to fit a plane with normal
$\mathbf{u}_{j}$ and an in-plane point ${{\mathbf{q}}_{j}}$, the measurement
residual is:

$\displaystyle\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1},{{}^{L}{\mathbf{p}}}_{j})=$
$\displaystyle\mathbf{u}_{j}^{T}\left({{}^{G}\mathbf{p}_{j}}-{{\mathbf{q}}_{j}}\right)$
(12)

Let $\mathbf{n}_{j}$ be the measurement noise of the point
${{}^{L}\mathbf{p}_{j}}$, we can obtain the true point location
${{}^{L}\mathbf{p}^{\mathtt{gt}}_{j}}$ by compensating the noise from
${{}^{L}\mathbf{p}_{j}}$:

$\displaystyle{{}^{L}\mathbf{p}_{j}}={{}^{L}\mathbf{p}^{\mathtt{gt}}_{j}}+{\mathbf{n}_{j}},{\mathbf{n}_{j}}\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\mathbf{n}_{j}}).$
(13)

This true point location together with the true state $\mathbf{x}_{k+1}$
should lead to zero residual in (12), i.e.,

$\displaystyle\mathbf{0}=\mathbf{r}_{l}(\mathbf{x}_{k+1},{{}^{L}\mathbf{p}}^{\mathtt{gt}}_{j})$
$\displaystyle=\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1},{{}^{L}{\mathbf{p}}}_{j})+\mathbf{H}^{{l}}_{j}\delta\check{\mathbf{x}}_{k+1}+\bm{\alpha}_{j},$
(14)

which constitutes a posteriori distribution for
$\delta\check{\mathbf{x}}_{k+1}$. In (14), $\mathbf{x}_{k+1}$ is parameterized
by its error $\delta\check{\mathbf{x}}_{k+1}$ defined in (9) and
$\bm{\alpha}_{j}\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\bm{\alpha}_{j}})$:

$\displaystyle\mathbf{H}^{{l}}_{j}=$
$\displaystyle\dfrac{\partial\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1}\boxplus\delta{\check{\mathbf{x}}}_{k+1},{{}^{L}{\mathbf{p}}}_{j})}{\partial\delta{\check{\mathbf{x}}}_{k+1}}|_{\delta{\check{\mathbf{x}}}_{k+1}=\mathbf{0}}$
$\displaystyle\bm{\Sigma}_{\bm{\alpha}_{j}}=$
$\displaystyle{\mathbf{F}_{{{\mathbf{p}}}_{j}}}\bm{\Sigma}_{\mathbf{n}_{j}}{\mathbf{F}_{{{\mathbf{p}}}_{j}}^{T}}$
(15) $\displaystyle{\mathbf{F}_{{{\mathbf{p}}}_{j}}}=$
$\displaystyle\left(\dfrac{\partial\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1},{{}^{L}{\mathbf{p}}}_{j})}{\partial{{}^{L}{\mathbf{p}}}_{j}}\right)={{}^{G}{\check{\mathbf{R}}}_{I_{k+1}}}^{I}\mathbf{R}_{L}$

The detailed computation of $\mathbf{H}^{{l}}_{j}$ can be found in Appendix.
-C.

###  IV-H Visual measurement

If the $(k+1)$-th frame is a camera image, we extract the FAST corner feature
points $\bm{\mathcal{C}}_{k+1}$ from the undistorted image and use KLT optical
flow to track feature points in $\bm{\mathcal{C}}_{k+1}$ seen by keyframes in
the current sliding window (Section V). If a feature point in
$\bm{\mathcal{C}}_{k+1}$ is lost or has not been yet tracked before, we
triangulate the new feature point in 3D space (visual landmarks) with the
optimal estimated camera poses.

The reprojection errors between visual landmarks and its tracked feature
points in the $(k+1)$-th frame are used for updating the current state
estimate $\check{\mathbf{x}}_{k+1}$. For an extracted corner point
${{}^{C}\mathbf{p}_{s}}=\begin{bmatrix}u_{s}&v_{s}\end{bmatrix}^{T}\in\bm{\mathcal{C}}_{k+1}$
where $s$ is the index of corner point, its correspondence landmark in 3D
space is denoted as ${}^{G}\mathbf{P}_{s}$, then the measurement residual of
${{}^{C}}\mathbf{p}_{s}$ is:

$\begin{split}&{}^{C}\mathbf{P}_{s}=\left({{}^{G}\check{\mathbf{R}}_{I_{k+1}}}{{}^{I}\check{\mathbf{R}}_{C_{k+1}}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}\\\
&\quad\quad\quad\quad\quad\quad\quad\quad-\left({{}^{I}\check{\mathbf{R}}_{C_{k+1}}}\right)^{T}{{{}^{G}}\check{\mathbf{p}}_{I_{k+1}}}-{{}^{I}\check{\mathbf{p}}_{C_{k+1}}}\\\
&\mathbf{r}_{c}\left({\check{\mathbf{x}}_{k+1},{{}^{C}{\mathbf{p}}}_{s}},{{}^{G}\mathbf{P}_{s}}\right)={{}^{C}}\mathbf{p}_{s}-\bm{\pi}({{}^{C}\mathbf{P}_{s}})\end{split}$
(16)

where $\bm{\pi}(\cdot)$ is the pin-hole projection model.

Now considering the measurement noise, we have:

$\displaystyle{{}^{G}\mathbf{P}_{s}}={{}^{G}\mathbf{P}_{s}^{\mathtt{gt}}}+\mathbf{n}_{\mathbf{P}_{s}},~{}$
$\displaystyle\mathbf{n}_{\mathbf{P}_{s}}\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\mathbf{n}_{\mathbf{P}_{s}}})$
(17)
$\displaystyle{{}^{C}\mathbf{p}_{s}}={{}^{C}\mathbf{p}_{s}^{\mathtt{gt}}}+\mathbf{n}_{\mathbf{p}_{s}},~{}$
$\displaystyle\mathbf{n}_{\mathbf{p}_{s}}\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\mathbf{n}_{\mathbf{p}_{s}}})$
(18)

where ${{}^{G}\mathbf{P}_{s}^{\mathtt{gt}}}$ and
${{}^{C}\mathbf{p}_{s}^{\mathtt{gt}}}$ are the true value of
${{}^{G}\mathbf{P}_{s}}$ and ${{}^{C}\mathbf{p}_{s}}$, respectively. With
these, we obtain the first order Taylor expansion of the true zero residual
$\mathbf{r}_{c}(\mathbf{x}_{k+1},{{}^{C}\mathbf{p}^{\mathtt{gt}}_{s}})$ as:

$\begin{split}\mathbf{0}&=\mathbf{r}_{c}(\mathbf{x}_{k+1},{{}^{C}\mathbf{p}}^{\mathtt{gt}}_{s},{{}^{G}\mathbf{P}^{\mathtt{gt}}_{s}})\\\
&\approx\mathbf{r}_{c}\left({\check{\mathbf{x}}_{k+1},{{}^{C}{\mathbf{p}}}_{s}},{{}^{G}\mathbf{P}_{s}}\right)+\mathbf{H}^{{c}}_{s}\delta\check{\mathbf{x}}_{k+1}+\bm{\beta}_{s},\end{split}$
(19)

which constitutes another posteriori distribution for
$\delta\check{\mathbf{x}}_{k+1}$. In (19),
$\bm{\beta}_{s}\sim\mathcal{N}(\mathbf{0},{\bm{\Sigma}_{\bm{\beta}_{s}}})$
and:

$\displaystyle\mathbf{H}^{{c}}_{s}$
$\displaystyle=\dfrac{\partial\mathbf{r}_{c}(\check{\mathbf{x}}_{k+1}\boxplus\delta{\check{\mathbf{x}}}_{k+1},{{}^{C}{\mathbf{p}}}_{s},{{}^{G}\mathbf{P}_{s}})}{\partial\delta{\check{\mathbf{x}}}_{k+1}}|_{\delta{\check{\mathbf{x}}}_{k+1}=\mathbf{0}}$
$\displaystyle{\bm{\Sigma}_{\bm{\beta}_{s}}}$
$\displaystyle=\bm{\Sigma}_{\mathbf{n}_{\mathbf{p}_{s}}}+{\mathbf{F}_{{{\mathbf{P}}}_{s}}}\bm{\Sigma}_{\mathbf{{P}}_{s}}{\mathbf{F}_{{{\mathbf{P}}}_{s}}}^{T}$
(20) $\displaystyle{\mathbf{F}_{{{\mathbf{P}}}_{s}}}$
$\displaystyle=\dfrac{\partial\mathbf{r}_{c}(\check{\mathbf{x}}_{k+1},{{}^{C}{\mathbf{p}}}_{s},{{}^{G}\mathbf{P}_{s}})}{\partial{{}^{G}{\mathbf{P}}}_{s}}$

The detailed computation of $\mathbf{H}^{{c}}_{s}$ and
${\mathbf{F}_{{{\mathbf{P}}}_{s}}}$ is given in appendix. -D

###  IV-I Update of error-state iterated Kalman filter

Combining the prior distribution (11), the posterior distribution due to LiDAR
measurement (14) and the posterior distribution due to visual measurement
(19), we obtain the maximum a posterior (MAP) estimation of
$\delta\check{\mathbf{x}}_{k+1}$:

$\begin{split}\mathop{\min}_{\delta\check{\mathbf{x}}_{k+1}}&\left(\left\|\check{\mathbf{x}}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}+\bm{\mathcal{H}}\delta{\check{\mathbf{x}}}_{k+1}\right\|_{\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}^{-1}}\right.\\\
+&\sum\nolimits_{j=1}^{m_{l}}{\left\|\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1},{{}^{L}{\mathbf{p}}}_{j})+\mathbf{H}^{l}_{j}\delta{\check{\mathbf{x}}}_{k+1}\right\|^{2}_{\bm{\Sigma}_{\bm{\alpha}_{j}}^{-1}}}\\\
+&\left.\sum\nolimits_{s=1}^{m_{c}}{\left\|\mathbf{r}_{c}(\check{\mathbf{x}}_{k+1},{{}^{C}\mathbf{p}_{s}},{{}^{G}\mathbf{P}}_{s})+\mathbf{H}^{{c}}_{s}\delta{\check{\mathbf{x}}}_{k+1}\right\|^{2}_{\bm{\Sigma}_{\bm{\beta}_{s}}^{-1}}}\right)\end{split}$

where
$\left\|\mathbf{x}\right\|_{\bm{\Sigma}}^{2}=\mathbf{x}\bm{\Sigma}\mathbf{x}^{T}$.
Notice that the measurements of LiDAR and camera may not appear at the same
time instant (see Fig. 4), therefore $m_{l}$ (or $m_{c}$) could be zero in the
above optimization. Denote

$\begin{split}\mathbf{H}^{T}&=\begin{bmatrix}{\mathbf{H}^{l}_{1}},\dots,{\mathbf{H}^{l}_{m_{l}}},{\mathbf{H}^{{c}}_{1}}^{T},\dots,{\mathbf{H}^{{c}}_{m_{c}}}\end{bmatrix}^{T}\\\
\mathbf{R}&=\text{diag}(\begin{matrix}\bm{\Sigma}_{\bm{\alpha}_{1}},\dots,\bm{\Sigma}_{\bm{\alpha}_{m_{l}}},\bm{\Sigma}_{\bm{\beta}_{1}},\dots,\bm{\Sigma}_{\bm{\beta}_{m_{c}}}\end{matrix})\\\
\check{\mathbf{z}}_{k+1}^{T}&=\left[\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1},{{}^{L}{\mathbf{p}}}_{1}),\dots,\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1},{{}^{L}{\mathbf{p}}}_{m_{l}}),\right.\\\
&\quad\left.\mathbf{r}_{c}(\check{\mathbf{x}}_{k+1},{{}^{C}\mathbf{p}_{1}},{{}^{G}\mathbf{P}}_{1}),\dots,\mathbf{r}_{c}(\check{\mathbf{x}}_{k+1},{{}^{C}\mathbf{p}_{m_{c}}},{{}^{G}\mathbf{P}}_{{m_{c}}})\right]\\\
\mathbf{P}&=\left(\bm{\mathcal{H}}\right)^{-1}\bm{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}{\left(\bm{\mathcal{H}}\right)}^{-T}\end{split}$
(21)

Following [5], we have the Kalman gain computed as:

$\displaystyle\mathbf{K}=\left(\mathbf{H}^{T}\mathbf{R}^{-1}\mathbf{H}+\mathbf{P}^{-1}\right)^{-1}\mathbf{H}^{T}\mathbf{R}^{-1}$
(22)

Then we can update the state estimate as:

$\displaystyle\check{\mathbf{x}}_{k+1}=$
$\displaystyle\check{\mathbf{x}}_{k+1}\boxplus\left(-\mathbf{K}\check{\mathbf{z}}_{k+1}-\left(\mathbf{I}-\mathbf{KH}\right)\left(\bm{\mathcal{H}}\right)^{-1}\left(\check{\mathbf{x}}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}\right)\right)$

The above process (Section IV-G to Section IV-I) is iterated until convergence
(i.e., the update is smaller than a given threshold). The converged state
estimate is then used to (1) project points in the the new LiDAR frame to the
world frame and append them to the existing point cloud map; (2) triangulate
new visual landmarks of the current frame if it is a keyframe; (3) serve as
the starting point of the propagation in Section IV-D for the next cycle:

$\hat{\mathbf{x}}_{k+1}=\check{\mathbf{x}}_{k+1},\hskip
5.69046pt\hat{\bm{\Sigma}}_{\delta\bar{\mathbf{x}}_{k+1}}=\left(\mathbf{I}-\mathbf{KH}\right)\check{\bm{\Sigma}}_{\delta\mathbf{x}_{k+1}}$

##  V Factor graph optimization

As mentioned in Section IV-I, untracked visual landmarks in the newly added
keyframe are triangulated to create new visual landmarks. This triangulation
is usually of low precision due to keyframe pose estimation error. To further
improve the quality of visual landmarks, keyframe poses, and simultaneously
calibrate the time offset between the camera and LiDAR-IMU subsystem, we
leverage a factor graph optimization for optimizing the camera-poses and the
visual landmarks within a sliding window of image keyframes.

Figure 5: Our factor graph optimization.

Our factor graph optimization is similar to VINS-Mono [22], but further
incorporates pose constraints due to LiDAR measurements as shown in Fig. 5.
Constraints due to IMU preintegration are also included to connect the LiDAR
factor with camera factor. To keep the back-end optimization light-weight, the
LiDAR poses in the pose graph are fixed and the LiDAR raw point measurements
are not engaged in the pose graph optimization.

##  VI Experiments and Results

###  VI-A Our device for data sampling

Our handheld device for data sampling is shown in Fig. 6 (a), which includes
the power supply unit, the onboard DJI
manifold-2c333https://www.dji.com/manifold-2 computation platform (equipped
with an Intel i7-8550u CPU and 8 GB RAM), a global shutter camera, and a LiVOX
AVIA444https://www.livoxtech.com/avia LiDAR. The FoV of the camera is
$82.9^{\circ}\times 66.5^{\circ}$ while the FoV of LiDAR is
$70.4^{\circ}\times 77.2^{\circ}$. For quantitatively evaluating the precision
of our algorithm (our experiment in Section. VI-E), we install a differential-
GPS (D-GPS) real-time kinematic (RTK) system555https://www.dji.com/d-rtk on
our device, shown in Fig. 6 (b).

Figure 6: Our handheld device for data sampling, (a) shows our minimum system,
with a total weight $2.09$ Kg; (b) a D-GPS RTK system is used to evaluate the
accuracy.

Figure 7: We evaluate the robustness of our algorithm under scenarios with the
aggressive motion (sequence in a$\sim$d) and sensor failure by intentionally
blocking the camera (e) and LiDAR sensor (f).

Figure 8: Our estimated pose and the raw gyroscope reading of Experiment-1.
The shaded area in blue, yellow and red represent different phases of
aggressive motion, camera-failure and LiDAR-failure, respectively. Figure 9:
We evaluate our algorithm in a Hong Kong MTR station consisting of cluttered
lobby and very long narrow tunnels, as shown in (a). The tunnel is up to $190$
meters long and is filled with moving pedestrians, making it extremely
challenging for both LiDAR-based and camera-based SLAM methods. (b): the map
built by our system is well aligned with the street map of MTR station. (c)
Trajectory comparison among our system “R2LIVE”, the LiDAR-inertial system
“Fast-LIO” , and visual-inertial system “VINS-Mono” and (our). The starting
point is marked with $\mathop{\raisebox{-1.18399pt}{$\leavevmode\hbox
to5.6pt{\vbox to8.4pt{\pgfpicture\makeatletter\hbox{\hskip
0.21527pt\lower-1.44235pt\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}{}{}{{}}\pgfsys@setlinewidth{0.43056pt}\pgfsys@invoke{
}\pgfsys@roundjoin\pgfsys@invoke{ }{}{{}}{} {}{} {}{} {}{} {}{}
{}\pgfsys@beginscope\pgfsys@invoke{
}\definecolor[named]{pgffillcolor}{rgb}{0,0,0}\pgfsys@color@gray@fill{0}\pgfsys@invoke{
}{}\pgfsys@moveto{0.0pt}{2.75554pt}\pgfsys@lineto{2.58333pt}{6.73817pt}\pgfsys@lineto{5.16666pt}{2.75554pt}\pgfsys@lineto{2.58333pt}{-1.22708pt}\pgfsys@lineto{0.0pt}{2.75554pt}\pgfsys@closepath\pgfsys@fillstroke\pgfsys@invoke{
} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}$}}$ while the ending
point of each trajectory is marked with $\bigstar$. “VINS-Mono” stopped at
middle way due to the failure of feature tracking.

###  VI-B Experiment-1: Robustness evaluation with aggressive motion and
sensor failure

In this experiment, we evaluate the robustness of our algorithm under the
scenario with aggressive motion (see Fig. 8 (a$\sim$d)), in which the maximum
angular speed reaches up to $300^{\circ}/s$ (see the gyroscope readings in
Fig. 8). In addition, we also simulate drastic sensor failures by
intentionally blocking the camera (see Fig. 8(e)) and LiDAR (see Fig. 8(f)).

The results of our estimated attitude and position are shown in Fig. 8, where
we use shade the different testing phases with different colors. As shown in
Fig. 8, we can see that our estimated trajectory can tightly track the actual
motion even in severe rotation and translation. Even when the camera or LiDAR
provides no measurements, the estimated trajectory is sill very smooth and
exhibits no noticeable degradation. These results show that our algorithm is
robust enough to endure with the case with aggressive motion or even with the
failure of sensors. We refer readers to the accompanying video on _XXX_
showing details of the experiment in practice.

###  VI-C Experiment-2: Robustness evaluation in a narrow tunnel-like
environments with moving objects

In this experiment, we challenge one of the most difficult scenarios in the
scope of camera-based and LiDAR-based SLAM, a MTR station, the HKU
station666https://en.wikipedia.org/wiki/HKU_station. It is a typical narrow
tunnel-like environment (see the left figure of Fig. 9(a)), with the maximum
reaches $190$ meters. Moreover, there are many moving pedestrians frequently
showing in the sensor views. All these factors make the SLAM extremely
challenging: 1) the long tunnel structure significantly reduces the geometric
features for LiDAR-based SLAM, especially when walking along the tunnel or
turning the LiDAR around at one end of the tunnel; 2) The highly repeated
visual feature (pattern on the floor) causes the error in matching and
tracking the visual features in visual SLAM; 3) The many moving pedestrians
could cause outliers to the already few point- and visual-features.

Despite these challenges, our system is robust enough to survive in this
scene. In Fig. 9 (b), we align our point cloud data with the HKU station
street map777https://www.mtr.com.hk/archive/en/services/maps/hku.pdf, and find
them match tightly. This demonstrates that our localization is of high
robustness and accuracy. Moreover, we plot our estimated trajectory together
with that of Vins-Mono888https://github.com/HKUST-Aerial-Robotics/VINS-Mono
and Fast-LIO999https://github.com/hku-mars/FAST_LIO/ in Fig. 9 (c), where we
can see that our method achieves the best overall performance in this
experiment.

Figure 10: The upper figure plots the different trajectories in experiments-4,
while the lower figure shows the raw gyroscope reading in the experiment.
Figure 11: The relative pose error in experiments-4, for the sequence of $300$
meters, the median of pose error of “Vins-Mono”, “Fast-LIO” and “R2LIVE” are
($0.35^{\circ}$,$3.84\%$), ($0.41^{\circ}$,$0.34\%$) and
($\mathbf{0.27}^{\circ}$,$\mathbf{0.21}\%$) respectively. Figure 12: The
reconstructed 3D maps in Experiment-3 are shown in (d), and the detail point
cloud with the correspondence panorama images are shown in (a) and (b). (c)
show that our algorithm can close the loop itself (returning the starting
point) without any additional processing (e.g. loop closure). In (e), we merge
our maps with the satellite image to further examine the accuracy of our
system.

###  VI-D Experiment-3: High precision maps building in large-scale indoor &
outdoor urban environment

In this experiment, we show that our proposed method is accurate enough to
reconstruct a dense 3D, high precision, large-scale indoor-outdoor map of
urban environments. We collect data of the HKU main building exterior and
interior. The real-time reconstructed 3D maps is shown in Fig. 12, in which
the clear and high-quality point cloud demonstrates that our proposed method
is of high accuracy. What worth to be mentioned is, without any additional
processing (i.e. loop closure), our algorithm can still close the loop itself
(see Fig. 12 (c)) after traversing $876$ meters, which demonstrates that our
proposed method is extremely low drift. Finally, we merge our maps together
with the satellite image and find them match tightly (see Fig. 12 (e)).

###  VI-E Experiment-4: Quantitative evaluation of precision using D-GPS RTK

In this experiment, we quantitatively evaluate the precision of Vins-Mono
(IMU+Camera), Fast-LIO (IMU+LiDAR) and our algorithm by comparing their
estimated trajectory with the ground truth trajectory (see the upper figure of
Fig. 10) obtained from a real-time differential-GPS (D-GPS) Kinematic (RTK).
The data has the maximum angular velocity reaching $130^{\circ}/s$ (see the
gyroscope reading in Fig. 10). We calculate translational and rotational
errors for all possible subsequences of length (50,…,300) meters, with the
relative pose error (RPE) among these methods shown in Fig. 11.

###  VI-F Run time analysis

The average time consumption in experiment 1$\sim$4 are listed in TABLE. I,
which demonstrates that R2LIVE can achieve real-time on both the desktop PC
and embedded computation platform. Noticed that the factor graph optimization
is run on a separate thread and therefore is allowed to run at a lower rate.

##  VII Conclusion

In this letter, we propose a robust, real-time LiDAR-inertial-visual fusion
framework based on a high-rate filter-based odometry and factor graph
optimization. We fuse the measurements of LiDAR, inertial, and camera sensors
within an error-state iterated Kalman filter and use the factor graph
optimization to refine the local map that in a sliding window of image
keyframes and visual landmarks. Our system was tested in large-scale, indoor-
outdoor, and narrow tunnel-like environments, challenging sensor failure and
aggressive motion. In all the tests, our method achieves a high level of
accuracy and robustness in localization and mapping.

PC/on-board | Exp-1 (ms) | Exp-2 (ms) | Exp-3 (ms) | Exp-4 (ms)
---|---|---|---|---
LI-Odom | 8.81 / 15.98 | 11.54 / 25.30 | 14.91 / 30.64 | 10.92 / 24.37
VI-Odom | 7.84 / 13.92 | 8.84 / 19.10 | 9.57 / 19.56 | 8.99 / 20.16
FG-OPM | 26.10 / 45.35 | 30.20 / 65.25 | 29.25 / 58.08 | 27.98 / 60.43
TABLE I: The average running time of R2LIVE in Experiment-1$\sim$4
(Exp-1$\sim$4) on desktop PC (with Intel i7-9700K CPU and 32GB RAM) and on-
board computed (with Intel i7-8550u CPU and 8GB RAM). The items ”LI-Odom”,
”VI-Odom” and ”FG-OPM” are the average time consumption of LiDAR-IMU filter-
based odometry, Visual-Inertial filter-based odometry, and factor graph
optimization, respectively.

## References

  * [1] J. Levinson, J. Askeland, J. Becker, J. Dolson, D. Held, S. Kammel, J. Z. Kolter, D. Langer, O. Pink, V. Pratt, _et al._ , “Towards fully autonomous driving: Systems and algorithms,” in _2011 IEEE Intelligent Vehicles Symposium (IV)_. IEEE, 2011, pp. 163–168.
  * [2] A. Bry, A. Bachrach, and N. Roy, “State estimation for aggressive flight in gps-denied environments using onboard sensing,” in _2012 IEEE International Conference on Robotics and Automation_. IEEE, 2012, pp. 1–8.
  * [3] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” _Journal of Field Robotics_ , vol. 36, no. 4, pp. 710–733, 2019.
  * [4] Z. Liu, F. Zhang, and X. Hong, “Low-cost retina-like robotic lidars based on incommensurable scanning,” _IEEE Transactions on Mechatronics_ , 2021, in press. [Online]. Available: https://arxiv.org/pdf/2006.11034.pdf
  * [5] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” _IEEE Robotics and Automation Letters_ , 2021, in press. [Online]. Available: https://arxiv.org/pdf/2010.08196.pdf
  * [6] J. Lin, X. Liu, and F. Zhang, “A decentralized framework for simultaneous calibration, localization and mapping with multiple lidars,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 4870–4877.
  * [7] Z. Liu and F. Zhang, “Balm: Bundle adjustment for lidar mapping,” _IEEE Robotics and Automation Letters_ , 2021, in press. [Online]. Available: https://arxiv.org/pdf/2010.08215.pdf
  * [8] X. Liu and F. Zhang, “Extrinsic calibration of multiple lidars of small fov in targetless environments,” _IEEE Robotics and Automation Letters_ , 2021, in press.
  * [9] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 3126–3131.
  * [10] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in _Robotics: Science and Systems_ , vol. 2, no. 9, 2014.
  * [11] K.-L. Low, “Linear least-squares optimization for point-to-plane icp surface registration,” _Chapel Hill, University of North Carolina_ , vol. 4, no. 10, pp. 1–3, 2004.
  * [12] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 4758–4765.
  * [13] J. Lin and F. Zhang, “A fast, complete, point cloud based loop closure for lidar odometry and mapping,” _arXiv preprint arXiv:1909.11811_ , 2019.
  * [14] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial odometry and mapping,” in _2019 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2019.
  * [15] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” _arXiv preprint arXiv:2007.00258_ , 2020.
  * [16] K. Li, M. Li, and U. D. Hanebeck, “Towards high-performance solid-state-lidar-inertial odometry and mapping,” _arXiv preprint arXiv:2010.13150_ , 2020.
  * [17] C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “Lins: A lidar-inertial state estimator for robust and efficient navigation,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 8899–8906.
  * [18] Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “Camvox: A low-cost and accurate lidar-assisted visual slam system,” _arXiv preprint arXiv:2011.11357_ , 2020.
  * [19] R. Mur-Artal and J. D. Tardós, “Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras,” _IEEE Transactions on Robotics_ , vol. 33, no. 5, pp. 1255–1262, 2017.
  * [20] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “Lic-fusion: Lidar-inertial-camera odometry,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2019, pp. 5848–5854.
  * [21] X. Zuo, Y. Yang, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, “Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking,” in _IROS 2020_ , 2020.
  * [22] 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, 2018.
  * [23] 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, 2013.
  * [24] D. He, W. Xu, and F. Zhang, “Embedding manifold structures into kalman filters,” _arXiv preprint arXiv:2102.03804_ , 2021.
  * [25] J. Sola, “Quaternion kinematics for the error-state kalman filter,” _arXiv preprint arXiv:1711.02508_ , 2017.
  * [26] T. D. Barfoot, _State estimation for robotics_. Cambridge University Press, 2017.

###  -A Computation of $\mathbf{F}_{\delta{\hat{\mathbf{x}}}}$ and
$\mathbf{F}_{\mathbf{w}}$

$\displaystyle\mathbf{F}_{\delta{\hat{\mathbf{x}}}}=\left.\dfrac{\partial\left(\delta{\hat{\mathbf{x}}}_{i+1}\right)}{\partial\delta{\hat{\mathbf{x}}_{i}}}\right|_{\delta{\hat{\mathbf{x}}_{i}}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}}$
$\displaystyle=$
$\displaystyle\begin{bmatrix}\mathtt{Exp}(-\hat{\bm{\omega}}_{i}\Delta
t)&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&-{\mathbf{J}_{r}(\hat{\bm{\omega}}_{i}\Delta
t)}^{T}&\mathbf{0}\\\
\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta
t&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
-^{G}\hat{\mathbf{R}}_{I_{i}}[\hat{\mathbf{a}}_{i}]_{\times}\Delta
t&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&-^{G}\hat{\mathbf{R}}_{I_{i}}\Delta
t\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\end{bmatrix}$
$\displaystyle\mathbf{F}_{{\mathbf{w}}}=\left.\dfrac{\partial\left(\delta{\hat{\mathbf{x}}}_{i+1}\right)}{\partial{\mathbf{w}_{i}}}\right|_{\delta{\hat{\mathbf{x}}_{i}}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}}$
$\displaystyle=$
$\displaystyle\begin{bmatrix}-{\mathbf{J}_{r}(\hat{\bm{\omega}}_{i}\Delta
t)}^{T}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&-^{G}\hat{\mathbf{R}}_{I_{i}}\Delta t&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta t&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta t\\\ \end{bmatrix}$

where $\hat{\bm{\omega}}_{i}=\bm{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{g}_{i}}$
, $\hat{\mathbf{a}}_{i}=\mathbf{a}_{m_{i}}-\mathbf{b}_{\mathbf{a}_{i}}$, and
$\mathbf{J}_{r}(\cdot)$ are called the right Jacobian matrix of $SO(3)$:

$\mathbf{J}_{r}(\mathbf{r})=\mathbf{I}-\dfrac{1-\cos||\mathbf{r}||}{||\mathbf{r}||^{2}}\left[\mathbf{r}\right]_{\times}+\dfrac{||\mathbf{r}||-\sin(||\mathbf{r}||)}{||\mathbf{r}||^{3}}\left[\mathbf{r}\right]_{\times}^{2},\mathbf{r}\in\mathbb{R}^{3}$

For the detailed derivation of $\mathbf{F}_{\check{\delta{\mathbf{x}}}}$ and
$\mathbf{F}_{\mathbf{w}}$, please refer to the Section. B of our supplementary
material.

###  -B The computation of $\bm{\mathcal{H}}$

$\displaystyle\bm{\mathcal{H}}$
$\displaystyle=\dfrac{\left(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1}\right)\boxminus\hat{\mathbf{x}}_{k+1}}{\partial\delta\check{\mathbf{x}}_{k+1}}|_{\delta\check{\mathbf{x}}_{k+1}=\mathbf{0}}$
$\displaystyle=\begin{bmatrix}\mathbf{A}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times
9}\\\ \mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times 9}\\\
\mathbf{0}&\mathbf{0}&\mathbf{B}&\mathbf{0}&\mathbf{0}_{3\times 9}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}_{3\times 9}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}_{9\times 9}\\\
\end{bmatrix}$

where the $3\times 3$ matrix
$\mathbf{A}=\mathbf{J}_{r}^{-1}(\mathtt{Log}({{{}^{G}\hat{\mathbf{R}}_{I_{k+1}}}^{T}}{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}))$
and
$\mathbf{B}=\mathbf{J}_{r}^{-1}(\mathtt{Log}({{{}^{I}\hat{\mathbf{R}}_{C_{k+1}}}^{T}}{{}^{I}\check{\mathbf{R}}_{C_{k+1}}}))$.
$\mathbf{J}_{r}^{-1}(\cdot)$ are called the inverse right Jacobian matrix of
$SO(3)$:

$\displaystyle\mathbf{J}_{r}^{-1}(\mathbf{r})=$
$\displaystyle\mathbf{I}+\dfrac{1}{2}\left[\mathbf{r}\right]_{\times}+\left(\dfrac{1}{||\mathbf{r}||^{2}}-\dfrac{1+\cos(||\mathbf{r}||)}{2||\mathbf{r}||\sin(||\mathbf{r}||)}\right)\left[\mathbf{r}\right]_{\times}^{2}$
(23)

###  -C The computation of $\mathbf{H}^{{l}}_{j}$

$\displaystyle\mathbf{H}^{{l}}_{j}=\mathbf{u}_{j}^{T}\begin{bmatrix}-{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\left[\mathbf{P_{a}}\right]_{\times}&\mathbf{I}_{3\times
3}&\mathbf{0}_{3\times 15}\end{bmatrix}$

where
$\mathbf{P_{a}}={{}^{I}\mathbf{R}}_{L}{{}^{L}{\mathbf{p}}_{j}}+{{}^{I}\mathbf{p}_{L}}$.

For the detailed derivation of $\mathbf{H}^{{l}}_{j}$, please refer to the
Section. D of our supplementary material.

###  -D The computation of $\mathbf{H}^{{c}}_{s}$ and
${\mathbf{F}_{{{\mathbf{P}}}_{s}}}$

$\displaystyle\mathbf{H}^{{c}}_{s}=-\mathbf{F_{A}}\cdot\mathbf{F_{B}}$
$\displaystyle\mathbf{F}_{{{\mathbf{P}}}_{s}}=-\mathbf{F_{A}}\cdot\mathbf{F_{C}}$

with:

$\displaystyle\mathbf{F_{A}}$
$\displaystyle=\dfrac{1}{{{}^{C}{P}_{s}}_{z}}\begin{bmatrix}f_{x}&0&-f_{x}\dfrac{{{}^{C}{P}_{s}}_{x}}{{{}^{C}{P}_{s}}_{z}}\\\
0&f_{y}&-f_{y}\dfrac{{{}^{C}{P}_{s}}_{y}}{{{}^{C}{P}_{s}}_{z}}\end{bmatrix}$
(24) $\displaystyle\mathbf{F_{B}}$
$\displaystyle=\begin{bmatrix}\mathbf{M_{A}}&\mathbf{M_{B}}&\mathbf{M_{C}}&-\mathbf{I}&\mathbf{0}_{3\times
12}\end{bmatrix}$ $\displaystyle\mathbf{F_{C}}$
$\displaystyle=\left({{}^{G}\check{\mathbf{R}}_{I_{k+1}}}{{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}$
(25)

where $f_{x}$ and $f_{y}$ are the focal length, $c_{x}$ and $c_{y}$ are the
principal point offsets in image plane, and the $3\times 3$ matrix
$\mathbf{M_{A}}$, $\mathbf{M_{B}}$ and $\mathbf{M_{C}}$ are:

$\displaystyle\mathbf{M_{A}}=\left({{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}\left[\left({{}^{G}\hat{\mathbf{R}}_{I_{k+1}}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}\right]_{\times}$
$\displaystyle\mathbf{M_{B}}=-\left({{}^{I}\hat{\mathbf{R}}_{C}}\right)^{T}$
$\displaystyle\mathbf{M_{C}}=\left[\left({{}^{G}\hat{\mathbf{R}}_{I_{k+1}}}{{}^{I}\hat{\mathbf{R}}_{C}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}\right]_{\times}-\left[\left({{}^{I}\hat{\mathbf{R}}_{C}}\right)^{T}{{{}^{G}}\hat{\mathbf{p}}^{i}_{I_{k+1}}}\right]_{\times}$

For the detailed derivation of $\mathbf{H}^{{c}}_{s}$ and
${\mathbf{F}_{{{\mathbf{P}}}_{s}}}$, please refer to Section. E of our
supplementary material.

Supplementary Material: R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual
tightly-coupled state Estimator and mapping

###  S0-A Perturbation on $SO(3)$

In this appendix, we will use the following approximation of perturbation
$\delta\mathbf{r}\rightarrow\mathbf{0}$ on $SO(3)$ [25, 26]:

$\displaystyle\mathtt{Exp}(\mathbf{r}+\delta\mathbf{r})$
$\displaystyle\approx\mathtt{Exp}(\mathbf{r})\mathtt{Exp}(\mathbf{J}_{r}(\mathbf{r})\delta\mathbf{r})$
$\displaystyle\mathtt{Exp}(\mathbf{r})\mathtt{Exp}(\delta\mathbf{r})$
$\displaystyle\approx\mathtt{Exp}(\mathbf{r}+\mathbf{J}_{r}^{-1}(\mathbf{r})\delta\mathbf{r})$
$\displaystyle\mathbf{R}\cdot\mathtt{Exp}(\delta\mathbf{r})\cdot\mathbf{u}$
$\displaystyle\approx\mathbf{R}\left(\mathbf{I}+\left[\delta\mathbf{r}\right]_{\times}\right)\mathbf{u}=\mathbf{R}\mathbf{u}-\mathbf{R}\left[\mathbf{u}\right]_{\times}\delta\mathbf{r}$

where $\mathbf{u}\in\mathbb{R}^{3}$ and we use $\left[\cdot\right]_{\times}$
denote the skew-symmetric matrix of vector $(\cdot)$;
$\mathbf{J}_{r}(\mathbf{r})$ and $\mathbf{J}_{r}^{-1}(\mathbf{r})$ are called
the right Jacobian and the inverse right Jacobian of $SO(3)$, respectively.

$\displaystyle\mathbf{J}_{r}(\mathbf{r})=$
$\displaystyle\mathbf{I}-\dfrac{1-\cos||\mathbf{r}||}{||\mathbf{r}||^{2}}\left[\mathbf{r}\right]_{\times}+\dfrac{||\mathbf{r}||-\sin(||\mathbf{r}||)}{||\mathbf{r}||^{3}}\left[\mathbf{r}\right]_{\times}^{2}$
$\displaystyle\mathbf{J}_{r}^{-1}(\mathbf{r})=$
$\displaystyle\mathbf{I}+\dfrac{1}{2}\left[\mathbf{r}\right]_{\times}+\left(\dfrac{1}{||\mathbf{r}||^{2}}-\dfrac{1+\cos(||\mathbf{r}||)}{2||\mathbf{r}||\sin(||\mathbf{r}||)}\right)\left[\mathbf{r}\right]_{\times}^{2}$

###  S0-B Computation of $\mathbf{F}_{\delta{\mathbf{x}}}$ and
$\mathbf{F}_{\mathbf{w}}$

Combing (4) and (6) , we have:

$\displaystyle\quad\quad\delta{\hat{\mathbf{x}}}_{i+1}={\mathbf{x}}_{i+1}\boxminus\hat{\mathbf{x}}_{i+1}$
$\displaystyle=\Large(\mathbf{x}_{i}\boxplus\left(\Delta
t\cdot\mathbf{f}({\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{w}_{i})\right)\Large)\boxminus\left(\hat{\mathbf{x}}_{i}\boxplus\left(\Delta
t\cdot\mathbf{f}(\hat{\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{0})\right)\right)$
$=\begin{bmatrix}\mathbf{Log}\left(\left({}^{G}\hat{\mathbf{R}}_{I_{i}}\mathtt{Exp}\left({\hat{\bm{\omega}}}_{i}\Delta
t\right)\right)^{T}\cdot\left({}^{G}\hat{\mathbf{R}}_{I_{i}}\mathtt{Exp}\left({{}^{G}\delta\mathbf{r}_{I_{i}}}\right)\mathtt{Exp}\left({\bm{\omega}}_{i}\Delta
t\right)\right)\right)\\\
{{}^{G}\delta\mathbf{p}_{I_{i}}}+{{}^{G}\delta\mathbf{v}_{i}}\Delta
t+\dfrac{1}{2}{\mathbf{a}}_{i}\Delta
t^{2}-\dfrac{1}{2}{\hat{\mathbf{a}}}_{i}\Delta t^{2}\\\
{{}^{I}\delta\mathbf{r}_{C_{i}}}\\\ {{}^{{I}}\delta\mathbf{p}_{C_{i}}}\\\
{{}^{G}\delta\mathbf{v}_{i}}+\left({}^{G}\hat{\mathbf{R}}_{I_{i}}\mathtt{Exp}\left({{}^{G}\delta\mathbf{r}_{I_{i}}}\right)\right){\mathbf{a}}_{i}\Delta
t-{{}^{G}\hat{\mathbf{R}}_{I_{i}}}\hat{\mathbf{a}}_{i}\Delta t\\\
\delta\mathbf{b}_{g_{i}}+\mathbf{n}_{\mathbf{bg}_{i}}\\\
\delta\mathbf{a}_{g_{i}}+\mathbf{n}_{\mathbf{ba}_{i}}\\\ \end{bmatrix}$

with:

$\displaystyle\hat{\bm{\omega}}_{i}=\bm{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{g}_{i}},$
$\displaystyle\hskip
5.69046pt{\bm{\omega}}_{i}=\hat{\bm{\omega}}_{i}-\delta\mathbf{b}_{\mathbf{g}_{i}}-\mathbf{n}_{\mathbf{g}_{i}}$
(S1)
$\displaystyle~{}\hat{\mathbf{a}}_{i}=\mathbf{a}_{m_{i}}-\mathbf{b}_{\mathbf{a}_{i}},$
$\displaystyle\hskip
5.69046pt{\mathbf{a}}_{i}=\hat{\mathbf{a}}_{i}-\delta\mathbf{b}_{\mathbf{a}_{i}}-\mathbf{n}_{\mathbf{a}_{i}}$
(S2)

And we have the following simplification and approximation form Section. A.

$\displaystyle\mathtt{Log}\left(\left({}^{G}\hat{\mathbf{R}}_{I_{i}}\mathtt{Exp}\left({\hat{\bm{\omega}}}_{i}\Delta
t\right)\right)^{T}\cdot\left({}^{G}\hat{\mathbf{R}}_{I_{i}}\mathtt{Exp}\left({{}^{G}\delta\mathbf{r}_{I_{i}}}\right)\mathtt{Exp}\left({\bm{\omega}}_{i}\Delta
t\right)\right)\right)$ $\displaystyle=$
$\displaystyle\mathtt{Log}\left(\mathtt{Exp}\left(\hat{\bm{\omega}}_{i}\Delta
t\right)^{T}\cdot\left(\mathtt{Exp}\left({{}^{G}\delta\mathbf{r}_{I_{i}}}\right)\cdot\mathtt{Exp}\left({\bm{\omega}}_{i}\Delta
t\right)\right)\right)$ $\displaystyle\approx$
$\displaystyle\mathtt{Log}\left(\mathtt{Exp}\left(\hat{\bm{\omega}}_{i}\Delta
t\right)^{T}\mathtt{Exp}\left({{}^{G}\delta\mathbf{r}_{I_{i}}}\right)\mathtt{Exp}\left(\hat{\bm{\omega}}_{i}\Delta
t\right)\cdot\right.$ $\displaystyle\hskip
22.76228pt\left.\mathtt{Exp}\left(-\mathbf{J}_{r}(\hat{\bm{\omega}}_{i}\Delta
t)\left(\delta\mathbf{b}_{g_{i}}+\mathbf{n}_{\mathbf{g}_{i}}\right)\right)\right)$
$\displaystyle\approx$
$\displaystyle\mathtt{Exp}\left(\hat{\bm{\omega}}_{i}\Delta
t\right)\cdot{{}^{G}\delta\mathbf{r}_{I_{i}}}-{\mathbf{J}_{r}(\hat{\bm{\omega}}_{i}\Delta
t)}^{T}\delta\mathbf{b}_{\mathbf{g}_{i}}-{\mathbf{J}_{r}(\hat{\bm{\omega}}_{i}\Delta
t)}^{T}\mathbf{n}_{\mathbf{g}_{i}}$
$\displaystyle\left({}^{G}\mathbf{R}_{I_{i}}\mathtt{Exp}\left({{}^{G}\delta\mathbf{r}_{I_{i}}}\right)\right){\mathbf{a}}_{i}\Delta
t$ $\displaystyle\approx$
$\displaystyle\left({}^{G}\mathbf{R}_{I_{i}}\left(\mathbf{I}+[^{G}\delta\mathbf{r}_{I_{i}}]_{\times}\right)\right)\left(\hat{\mathbf{a}}_{i}-\delta\mathbf{b}_{\mathbf{a}_{i}}-\mathbf{n}_{\mathbf{a}_{i}}\right)\Delta
t$ $\displaystyle\approx$ ${}^{G}\mathbf{R}_{I_{i}}\hat{\mathbf{a}}_{i}\Delta
t-^{G}\mathbf{R}_{I_{i}}\delta\mathbf{b}_{\mathbf{a}_{i}}\Delta
t-^{G}\mathbf{R}_{I_{i}}\mathbf{n}_{\mathbf{a}_{i}}\Delta
t-{{}^{G}\mathbf{R}_{I_{i}}}\left[\hat{\mathbf{a}}_{i}\right]_{\times}{{}^{G}\delta\mathbf{r}_{I_{i}}}$

To conclude, we have the computation of $\mathbf{F}_{\delta{\mathbf{x}}}$ and
$\mathbf{F}_{\mathbf{w}}$ as follow:

$\displaystyle\mathbf{F}_{\delta{\hat{\mathbf{x}}}}=\left.\dfrac{\partial\left(\delta{\hat{\mathbf{x}}}_{i+1}\right)}{\partial\delta{\hat{\mathbf{x}}_{i}}}\right|_{\delta{\hat{\mathbf{x}}_{i}}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}}$
$\displaystyle=$
$\displaystyle\begin{bmatrix}\mathtt{Exp}(-\hat{\bm{\omega}}_{i}\Delta
t)&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&-{\mathbf{J}_{r}(\hat{\bm{\omega}}_{i}\Delta
t)}^{T}&\mathbf{0}\\\
\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta
t&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
-^{G}\hat{\mathbf{R}}_{I_{i}}[\hat{\mathbf{a}}_{i}]_{\times}\Delta
t&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&-^{G}\hat{\mathbf{R}}_{I_{i}}\Delta
t\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\end{bmatrix}$
$\displaystyle\mathbf{F}_{{\mathbf{w}}}=\left.\dfrac{\partial\left(\delta{\hat{\mathbf{x}}}_{i+1}\right)}{\partial{\mathbf{w}_{i}}}\right|_{\delta{\hat{\mathbf{x}}_{i}}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}}$
$\displaystyle=$
$\displaystyle\begin{bmatrix}-{\mathbf{J}_{r}(\hat{\bm{\omega}}_{i}\Delta
t)}^{T}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&-^{G}\hat{\mathbf{R}}_{I_{i}}\Delta t&\mathbf{0}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta t&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\Delta t\\\ \end{bmatrix}$

###  S0-C The computation of $\bm{\mathcal{H}}$

Recalling (15), we have:

$\displaystyle\bm{\mathcal{H}}$
$\displaystyle=\dfrac{\left(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1}\right)\boxminus\hat{\mathbf{x}}_{k+1}}{\partial\delta\check{\mathbf{x}}_{k+1}}|_{\delta\check{\mathbf{x}}_{k+1}=\mathbf{0}}$
$\displaystyle=\begin{bmatrix}\mathbf{A}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times
9}\\\ \mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times 9}\\\
\mathbf{0}&\mathbf{0}&\mathbf{B}&\mathbf{0}&\mathbf{0}_{3\times 9}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}_{3\times 9}\\\
\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}_{9\times 9}\\\
\end{bmatrix}$

with the $3\times 3$ matrix
$\mathbf{A}=\mathbf{J}_{r}^{-1}(\mathtt{Log}({{{}^{G}\hat{\mathbf{R}}_{I_{k+1}}}^{T}}{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}))$
and
$\mathbf{B}=\mathbf{J}_{r}^{-1}(\mathtt{Log}({{{}^{I}\hat{\mathbf{R}}_{C_{k+1}}}^{T}}{{}^{I}\check{\mathbf{R}}_{C_{k+1}}}))$.

###  S0-D The computation of $\mathbf{H}^{{l}}_{j}$

Recalling (12) and (15), we have:

$\displaystyle\mathbf{r}_{l}(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1},{{}^{L}{\mathbf{p}}}_{j})=\mathbf{u}_{j}^{T}\left({{}^{G}\check{\mathbf{p}}_{I_{k+1}}}+{{}^{G}\delta\check{\mathbf{p}}_{I_{k+1}}}-\right.$
$\displaystyle{{\mathbf{q}}_{j}}+{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\mathtt{Exp}({{}^{G}\check{\delta\mathbf{r}}_{I_{k+1}}})\left.(^{I}\mathbf{R}_{L}{{}^{L}{\mathbf{p}}_{j}}+{{}^{I}\mathbf{p}_{L}})\right)$
(S3)

And with the small perturbation approximation, we get:

$\displaystyle{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\mathtt{Exp}({{}^{G}\delta\check{\mathbf{r}}_{I_{k+1}}})\mathbf{P_{a}}$
$\displaystyle\approx$
$\displaystyle{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\left(\mathbf{I}+\left[{{}^{G}\delta\check{\mathbf{r}}_{I_{k+1}}}\right]_{\times}\right)\mathbf{P_{a}}$
$\displaystyle=$
$\displaystyle{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\mathbf{P_{a}}-{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\left[\mathbf{P_{a}}\right]_{\times}{{}^{G}\delta\check{\mathbf{r}}_{I_{k+1}}}$
(S4)

where
$\mathbf{P_{a}}={{}^{I}\mathbf{R}}_{L}{{}^{L}{\mathbf{p}}_{j}}+{{}^{I}\mathbf{p}_{L}}$.
Combining (S3) and (S4) together we can obtain:

$\displaystyle\mathbf{H}^{{l}}_{j}=\mathbf{u}_{j}^{T}\begin{bmatrix}-{{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\left[\mathbf{P_{a}}\right]_{\times}&\mathbf{I}_{3\times
3}&\mathbf{0}_{3\times 15}\end{bmatrix}$

###  S0-E The computation of $\mathbf{H}^{{c}}_{s}$ and
${\mathbf{F}_{{{\mathbf{P}}}_{s}}}$

Recalling (16), we have:

${{}^{C}\mathbf{P}_{s}}=\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})=\begin{bmatrix}{{}^{C}{P}_{s}}_{x}~{}{{}^{C}{P}_{s}}_{y}~{}{{}^{C}{P}_{s}}_{z}\end{bmatrix}^{T}$

where the function
$\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})$ is:

$\displaystyle\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})=\left({{}^{G}\check{\mathbf{R}}_{I_{k+1}}}{{}^{I}\check{\mathbf{R}}_{C_{k+1}}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}$
(S5) $\displaystyle\hskip
71.13188pt-\left({{}^{I}\check{\mathbf{R}}_{C_{k+1}}}\right)^{T}{{{}^{G}}\check{\mathbf{p}}_{I_{k+1}}}-{{}^{I}\check{\mathbf{p}}_{C_{k+1}}}$
(S6)

From (20), we have:

$\displaystyle\mathbf{r}_{c}\left({\check{\mathbf{x}}_{k+1},{{}^{C}{\mathbf{p}}}_{s}},{{}^{G}\mathbf{P}_{s}}\right)$
$\displaystyle={{}^{C}}\mathbf{p}_{s}-\bm{\pi}({{}^{C}\mathbf{P}_{s}})$
$\displaystyle\bm{\pi}({{}^{C}\mathbf{P}_{s}})$
$\displaystyle=\begin{bmatrix}f_{x}\dfrac{{{}^{C}{P}_{s}}_{x}}{{{}^{C}{P}_{s}}_{z}}+c_{x}~{}~{}f_{y}\dfrac{{{}^{C}{P}_{s}}_{y}}{{{}^{C}{P}_{s}}_{z}}+c_{y}\end{bmatrix}^{T}$
(S7)

where $f_{x}$ and $f_{y}$ are the focal length, $c_{x}$ and $c_{y}$ are the
principal point offsets in image plane.

For conveniently, we omit the
$(\cdot)|_{\delta{\check{\mathbf{x}}}^{i}_{k+1}=\mathbf{0}}$ in the following
derivation, and we have:

$\displaystyle\mathbf{H}^{{c}}_{s}=-\dfrac{\partial\bm{\pi}({{}^{C}\mathbf{P}_{s}})}{\partial{{}^{C}\mathbf{P}_{s}}}\cdot\dfrac{\partial\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})}{\partial\delta{\check{\mathbf{x}}}_{k+1}}$
(S8)
$\displaystyle\mathbf{F}_{{{\mathbf{P}}}_{s}}=-\dfrac{\partial\bm{\pi}({{}^{C}\mathbf{P}_{s}})}{\partial{{}^{C}\mathbf{P}_{s}}}\cdot\dfrac{\partial\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})}{\partial{{}^{G}{\mathbf{P}}}_{s}}$
(S9)

where:

$\displaystyle\dfrac{\partial\bm{\pi}({{}^{C}\mathbf{P}_{s}})}{\partial{{}^{C}\mathbf{P}_{s}}}$
$\displaystyle=\dfrac{1}{{{}^{C}{P}_{s}}_{z}}\begin{bmatrix}f_{x}&0&-f_{x}\dfrac{{{}^{C}{P}_{s}}_{x}}{{{}^{C}{P}_{s}}_{z}}\\\
0&f_{y}&-f_{y}\dfrac{{{}^{C}{P}_{s}}_{y}}{{{}^{C}{P}_{s}}_{z}}\end{bmatrix}$
(S10)
$\displaystyle\dfrac{\partial\mathbf{P_{b}}(\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})}{\partial{{}^{G}{\mathbf{P}}}_{s}}$
$\displaystyle=\left({{}^{G}\check{\mathbf{R}}_{I_{k+1}}}{{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}$
(S11)

According to Section. A, we have the following approximation of
$\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})$:

$\displaystyle\hskip
5.69046pt\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})$
$\displaystyle=\left({{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\mathtt{Exp}\left({{}^{G}{\delta\check{\mathbf{r}}_{I_{k+1}}}}\right){{}^{I}\check{\mathbf{R}}_{C_{k+1}}}\mathtt{Exp}\left({{}^{I}{\delta\check{\mathbf{r}}_{C_{k+1}}}}\right)\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}-{{}^{I}\check{\mathbf{p}}_{C}}$
$\displaystyle-{{}^{I}{\delta\check{\mathbf{p}}}_{C}}-\left({{}^{I}\check{\mathbf{R}}_{C}}\mathtt{Exp}\left({{}^{I}{\delta\check{\mathbf{r}}_{C}}}\right)\right)^{T}\left({{{}^{G}}\check{\mathbf{p}}_{I_{k+1}}}+{{}^{G}\delta\check{\mathbf{p}}}_{I_{k+1}}\right)$
$\displaystyle\approx\mathbf{P_{b}}(\check{\mathbf{x}}^{i}_{k+1},{{}^{G}\mathbf{P}_{s}})+\left[\left({{}^{G}\check{\mathbf{R}}_{I_{k+1}}}{{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}\right]_{\times}{{}^{I}{\delta\check{\mathbf{r}}_{C}}}$
$\displaystyle+\left({{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}\left[\left({{}^{G}\check{\mathbf{R}}_{I_{k+1}}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}\right]_{\times}{{}^{G}{\delta\check{\mathbf{r}}_{I_{k+1}}}}-\left({{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}{{}^{G}\delta\check{\mathbf{p}}}_{I_{k+1}}$
$\displaystyle-\left[\left({{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}{{{}^{G}}\check{\mathbf{p}}_{I_{k+1}}}\right]_{\times}{{}^{I}{\delta\check{\mathbf{r}}}_{C}}-{{}^{I}{\delta\check{\mathbf{p}}}_{C}}$

With this, we can derive:

$\displaystyle\dfrac{\partial\mathbf{P}_{\mathbf{C}}(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1},{{}^{G}\mathbf{P}_{s}})}{\partial\delta{\check{\mathbf{x}}}_{k+1}}=\begin{bmatrix}\mathbf{M_{A}}~{}~{}\mathbf{M_{B}}~{}~{}\mathbf{M_{C}}~{}~{}-\mathbf{I}~{}~{}\mathbf{0}_{3\times
12}\end{bmatrix}$ (S12)
$\displaystyle\mathbf{M_{A}}=\left({{}^{I}\check{\mathbf{R}}_{C}}\right)^{T}\left[\left({{}^{G}\hat{\mathbf{R}}_{I_{k+1}}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}\right]_{\times}$
$\displaystyle\mathbf{M_{B}}=-\left({{}^{I}\hat{\mathbf{R}}_{C}}\right)^{T}$
$\displaystyle\mathbf{M_{C}}=\left[\left({{}^{G}\hat{\mathbf{R}}_{I_{k+1}}}{{}^{I}\hat{\mathbf{R}}_{C}}\right)^{T}{{{}^{G}}{\mathbf{P}}_{s}}\right]_{\times}-\left[\left({{}^{I}\hat{\mathbf{R}}_{C}}\right)^{T}{{{}^{G}}\hat{\mathbf{p}}^{i}_{I_{k+1}}}\right]_{\times}$

Substituting (S10), (S11) and (S12) into (S8) and (S9), we finish the
computation of $\mathbf{H}^{{c}}_{s}$ and ${\mathbf{F}_{{{\mathbf{P}}}_{s}}}$.


* Berkowitz (2017) Evan Berkowitz, “METAQ: Bundle Supercomputing Tasks,” (2017), https://github.com/evanberkowitz/metaq, arXiv:1702.06122 [physics.comp-ph] .
  * Berkowitz  _et al._ (2018b) Evan Berkowitz, Gustav R. Jansen, Kenneth McElvain, and André Walker-Loud, “Job Management and Task Bundling,” EPJ Web Conf. 175, 09007 (2018b), arXiv:1710.01986 [hep-lat] .
  * Chang  _et al._ (2020) Chia Cheng Chang, Christopher Körber, and André Walker-Loud, “EspressoDB: A Scientific Database for Managing High-Performance Computing Workflow,” J. Open Source Softw. 5, 2007 (2020), arXiv:1912.03580 [hep-lat] .
  * Lepage (2020a) Peter Lepage, “gplepage/gvar: gvar version 11.2,” (2020a), https://github.com/gplepage/gvar.
  * Lepage (2020b) Peter Lepage, “gplepage/lsqfit: lsqfit version 11.5.1,” (2020b), https://github.com/gplepage/lsqfit.
  * Aoki  _et al._ (2010) S. Aoki _et al._ (PACS-CS), “Physical Point Simulation in 2+1 Flavor Lattice QCD,” Phys. Rev. D 81, 074503 (2010), arXiv:0911.2561 [hep-lat] .
  * Luscher and Schaefer (2013) Martin Luscher and Stefan Schaefer, “Lattice QCD with open boundary conditions and twisted-mass reweighting,” Comput. Phys. Commun. 184, 519–528 (2013), arXiv:1206.2809 [hep-lat] .
  * Finkenrath  _et al._ (2013) Jacob Finkenrath, Francesco Knechtli, and Björn Leder, “One flavor mass reweighting in lattice QCD,” Nucl. Phys. B 877, 441–456 (2013), [Erratum: Nucl.Phys.B 880, 574–575 (2014)], arXiv:1306.3962 [hep-lat] .
  * Leder  _et al._ (2014) Björn Leder, Jacob Finkenrath, and Francesco Knechtli, “One flavor mass reweighting: foundations,” PoS LATTICE2013, 035 (2014), arXiv:1401.1079 [hep-lat] .
  * Hasenbusch (2001) Martin Hasenbusch, “Speeding up the hybrid Monte Carlo algorithm for dynamical fermions,” Phys. Lett. B 519, 177–182 (2001), arXiv:hep-lat/0107019 .
  * Hasenfratz  _et al._ (2008) Anna Hasenfratz, Roland Hoffmann, and Stefan Schaefer, “Reweighting towards the chiral limit,” Phys. Rev. D 78, 014515 (2008), arXiv:0805.2369 [hep-lat] .
  * Liu  _et al._ (2013) Qi Liu, Norman H. Christ, and Chulwoo Jung, “Light Quark Mass Reweighting,” Phys. Rev. D 87, 054503 (2013), arXiv:1206.0080 [hep-lat] .

##  Appendix C STABILITY PLOTS OF THE OMEGA GROUND STATE MASS

Here we present the stability plots for the remaining Omega correlator fits
used in our analysis, which are presented in Fig. 12–25.

Figure 12:  Same as Fig. 1 for the a15m400 ensemble.

Figure 13:  Same as Fig. 1 for the a15m350 ensemble.

Figure 14:  Same as Fig. 1 for the a15m310 and a15m310L ensembles.

Figure 15:  Same as Fig. 1 for the a15m220 ensemble.

Figure 16:  Same as Fig. 1 for the a12m400 ensemble.

Figure 17:  Same as Fig. 1 for the a12m350 ensemble.

Figure 18:  Same as Fig. 1 for the a12m310 and a12m310XL ensembles.

Figure 19:  Same as Fig. 1 for the a12m220 ensembles.

Figure 20:  Same as Fig. 1 for the a12m220ms ensembles.

Figure 21:  Same as Fig. 1 for the a12m180L ensembles.

Figure 22:  Same as Fig. 1 for the a09m400 ensemble.

Figure 23:  Same as Fig. 1 for the a09m350 ensemble.

Figure 24:  Same as Fig. 1 for the a09m310 ensemble.

Figure 25:  Same as Fig. 1 for the a09m220 ensemble.