Fig. 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.
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 sourced $^{1}$ to benefit the whole robotics community.
### 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.
### 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 (LeGOLOAM), 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.
---
### 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.
### 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
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
Fig. 2: The overview of our proposed method.
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
### A. The boxplus “⊕” and boxminus “⊖” operator
In this paper, we make use of the “⊓” and “□” 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, “⊓” and “□”, establishing a bijective map between the local neighborhood of $\mathcal{M}$ and its tangent space $\mathbb{R}^n$ [^23]:
$$ \boxplus:\mathcal{M}\times\mathbb{R}^{n}\to\mathcal{M},\quad\boxminus:\mathcal{M}\times\mathcal{M}\to\mathbb{R}^{n} $$
For the compound manifold $\mathcal{M} = SO(3) \times \mathbb{R}^n$, we have:
$$ \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\operatorname{Exp}(\mathbf{r})\\ \mathbf{a}_{1}+\mathbf{a}_{2}\end{bmatrix},\begin{bmatrix}\mathbf{R}_{1}\\ \mathbf{a}_{1}\end{bmatrix}\boxminus\begin{bmatrix}\mathbf{R}_{2}\\ \mathbf{a}_{2}\end{bmatrix}\triangleq\begin{bmatrix}\operatorname{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$, $\text{Exp}(\cdot)$ and $\text{Log}(\cdot)$ denote the Rodrigues' transformation between the rotation matrix and rotation vector $^2$.
### 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. Furthermore, 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 poses. 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.
$^{2}$
---
Fig. 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:
$$ \begin{aligned}^{G}\dot{\mathbf{p}}_{I}&=^{G}\mathbf{v}_{I},^{G}\dot{\mathbf{v}}_{I}=^{G}\mathbf{R}_{I}(\mathbf{a}_{m}-\mathbf{b}_{a}-\mathbf{n}_{a})+^{G}\mathbf{g},\\^{G}\dot{\mathbf{R}}_{I}&=^{G}\mathbf{R}_{I}\left[\boldsymbol{\omega}_{m}-\mathbf{b}_{\mathbf{g}}-\mathbf{n}_{\mathbf{g}}\right]_{\times},\dot{\mathbf{b}}_{\mathbf{g}}=\mathbf{n}_{\mathbf{b}\mathbf{g}},\dot{\mathbf{b}}=\mathbf{n}_{\mathbf{b}\mathbf{a}}\end{aligned} $$
where $^G(\cdot)$ denotes a vector represented in the global frame (i.e. the first gravitational aligned IMU frame [^22]), $^GR_I$ and $^G_P I$ are the attitude and position of the IMU relative to the global frame, $^G_g$ is the gravitational acceleration, $\omega_m$ and $\mathbf{a}_m$ are the raw gyroscope and accelerometer readings, $\mathbf{n}_a$ and $\mathbf{n}_g$ are the white noise of IMU measurement, $\mathbf{b}_a$ and $\mathbf{b}_g$ are the bias of gyroscope and accelerometer, which are modelled as random walk driven by Gaussian noise $\mathbf{n}_{bg}$ and $\mathbf{n}_{ba}$.
### 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:
$$ \mathbf{x}_{i}=\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}_{g_{i}}^{T}&\mathbf{b}_{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) $$
where
$$ \begin{aligned}\mathbf{u}_{i}&=\left[\boldsymbol{\omega}_{m_{i}}^{T}\quad\mathbf{a}_{m_{i}}^{T}\right]^{T},\quad\mathbf{w}_{i}=\left[\mathbf{n}_{\mathbf{g}_{i}}^{T}\quad\mathbf{n}_{\mathbf{a}_{i}}^{T}\quad\mathbf{n}_{\mathbf{b}\mathbf{g}_{i}}^{T}\quad\mathbf{n}_{\mathbf{b}\mathbf{a}_{i}}^{T}\right]^{T}\\\mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)&=\left[\begin{array}{c}\boldsymbol{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{g}_{i}}-\mathbf{n}_{\mathbf{g}_{i}}\\{}_{G}\mathbf{v}_{i}\\\mathbf{0}_{3\times1}\\\mathbf{0}_{3\times1}{}_{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{array}\right]\end{aligned} $$
### 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$:
$$\begin{aligned}&\delta\hat{\mathbf{x}}_{i}\triangleq\mathbf{x}_{i}\triangle\hat{\mathbf{x}}_{i}\\ &=\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}\\ &\sim\mathcal{N}(\mathbf{0}_{21\times1},\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{i}})\\ \end{aligned}$$
Fig. 4: The illustration of the update of our error-state iterated Kalman filter.
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 $\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}}=\mathbf{L o g}\big(^{G}\hat{\mathbf{R}}_{I_{i}}^{T}{^{G}\mathbf{R}}_{I_{i}}^{T}\big),\quad{^{I}}\delta\hat{\mathbf{r}}_{C_{i}}=\mathbf{L o g}\big({^{I}\hat{\mathbf{R}}}_{C_{i}}^{T}{^{I}\mathbf{R}}_{C_{i}}\big) $$
Once receiving a new IMU measurement, the state estimate is propagated by setting the process noise in (3) to zero:
$$ \begin{array}{r}{\hat{\mathbf{x}}_{i+1}=\hat{\mathbf{x}}_{i}\boxplus\left(\Delta t\cdot\mathbf{f}\left(\hat{\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{0}\right)\right).}\end{array} $$
The associated estimation error is propagated in the linearized error space as follows (see [^24] for more details):
$$ \begin{aligned}&\delta\hat{\mathbf{x}}_{i+1}=\mathbf{x}_{i+1}\boxminus\hat{\mathbf{x}}_{i+1}\tag{6}\\ &=\left(\mathbf{x}_{i}\boxplus\left(\Delta t\cdot\mathbf{f}(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i})\right)\right)\boxminus\left(\hat{\mathbf{x}}_{i}\boxplus\left(\Delta t\cdot\mathbf{f}(\hat{\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{0})\right)\right)\\ &\sim\mathcal{N}(\mathbf{0}_{21\times1},\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{i+1}})\\ \end{aligned} $$
where:
$$ \begin{array}{r}{\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{i+1}}=\mathbf{F}_{\delta\hat{\mathbf{x}}}\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{i}}\mathbf{F}_{\delta\hat{\mathbf{x}}}^{T}+\mathbf{F_{w}}\mathbf{Q}\mathbf{F_{w}^{T}}}\end{array} $$
$$ \mathbf{F}_{\delta\hat{\mathbf{x}}}=\left.\frac{\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}},\quad\mathbf{F}_{\mathbf{w}}=\left.\frac{\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.
### 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 $\boldsymbol{\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:
$$ \mathbf{x}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}\sim\mathcal{N}(\mathbf{0},{\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}}). $$
### 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 $\tilde{\mathbf{x}}_{k+1}$) of $\mathbf{x}_{k+1}$. The MAP estimate $\tilde{\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\tilde{\mathbf{x}}_{k+1}$ between the true state $\mathbf{x}_{k+1}$ and the current estimate $\tilde{\mathbf{x}}_{k+1}$, defined as
$$ \delta\breve{\mathbf{x}}_{k+1}\triangleq\mathbf{x}_{k+1}\triangleq\breve{\mathbf{x}}_{k+1}, $$
---
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\tilde{\mathbf{x}}_{k+1}$:
$$ \begin{aligned}\mathbf{x}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}&=(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1})\boxminus\hat{\mathbf{x}}_{k+1}\\&\approx\check{\mathbf{x}}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}+\mathcal{H}\delta\check{\mathbf{x}}_{k+1}\\&\sim\mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}),\end{aligned} $$
where
$$ \pmb{\mathscr{H}}=\frac{\left(\check{\mathbf{x}}_{k+1}\bigoplus\delta\check{\mathbf{x}}_{k+1}\right)\boxminus\hat{\mathbf{x}}_{k+1}}{\partial\delta\check{\mathbf{x}}_{k+1}}\big|_{\delta\check{\mathbf{x}}_{k+1}=\mathbf{0}} $$
is computed in detail in Appendix. B, (10) essentially imposes a prior distribution to $\delta\tilde{\mathbf{x}}_{k+1}$ as below:
$$ \delta\check{\mathbf{x}}_{k+1}\sim\mathcal{N}\left(-\mathcal{H}^{-1}\left(\check{\mathbf{x}}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}\right),\mathcal{H}^{-1}\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}\mathcal{H}^{-T}\right) $$
### 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 $\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 \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 [^5] [^9], 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:
$$ \mathbf{r}_{l}\left(\mathbf{\check{x}}_{k+1},^{L}\mathbf{p}_{j}\right)=\mathbf{u}_{j}^{T}\mathbf{\Gamma}\left(^{G}\mathbf{p}_{j}-\mathbf{q}_{j}\right) $$
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}_j^{\mathrm{gt}}$ by compensating the noise from $^L\mathbf{p}_j$:
$$ \mathbf{\Sigma}^{L}\mathbf{p}_{j}=\mathbf{\Sigma}^{L}\mathbf{p}_{j}^{\mathsf{g t}}+\mathbf{n}_{j},\mathbf{n}_{j}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{\mathbf{n}_{j}}). $$
This true point location together with the true state $\mathbf{x}_{k+1}$ should lead to zero residual in (12), i.e.,
$$ \mathbf{0}=\mathbf{r}_{l}\big(\mathbf{x}_{k+1},{}^{L}\mathbf{p}_{j}^{\mathsf{g t}}\big)=\mathbf{r}_{l}\big(\mathbf{\check{x}}_{k+1},{}^{L}\mathbf{p}_{j}\big)+\mathbf{H}_{j}^{l}\delta\mathbf{\check{x}}_{k+1}+\alpha_{j}, $$
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 $\boldsymbol{\alpha}_j \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\boldsymbol{\alpha}_j})$:
$$ \begin{aligned}\mathbf{H}_{j}^{l}=&\frac{\partial\mathbf{r}_{l}\big(\breve{\mathbf{x}}_{k+1}\bigoplus\delta\breve{\mathbf{x}}_{k+1},^{L}\mathbf{p}_{j}\big)}{\partial\delta\breve{\mathbf{x}}_{k+1}}\big|_{\delta\breve{\mathbf{x}}_{k+1}=\mathbf{0}}\\\mathbf{\Sigma}_{\alpha_{j}}=&\mathbf{F}_{\mathbf{p}_{j}}\mathbf{\Sigma}_{\mathbf{n}_{j}}\mathbf{F}_{\mathbf{p}_{j}}^{T}\\\mathbf{F}_{\mathbf{p}_{j}}=&\left(\frac{\partial\mathbf{r}_{l}\big(\breve{\mathbf{x}}_{k+1},^{L}\mathbf{p}_{j}\big)}{\partial^{L}\mathbf{p}_{j}}\right)={}^{G}\breve{\mathbf{R}}_{I_{k+1}}{}^{I}\mathbf{R}_{L}\end{aligned} $$
The detailed computation of $\mathbf{H}_j^l$ can be found in Appendix. C.
### H. Visual measurement
If the $(k+1)$-th frame is a camera image, we extract the FAST corner feature points $\pmb{C}_{k+1}$ from the undistorted image and use KLT optical flow to track feature points in $\pmb{C}_{k+1}$ seen by keyframes in the current sliding window (Section V). If a feature point in $\pmb{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 $\breve{\mathbf{x}}_{k+1}$. For an extracted corner point $^C\mathbf{p}_s = \left[ u_s \quad v_s \right]^T \in \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{aligned}^{C}\mathbf{P}_{s}=&\left({}^{G}\mathbf{\check{R}}_{I_{k+1}}{}^{I}\mathbf{\check{R}}_{C_{k+1}}\right)^{T}{}^{G}\mathbf{P}_{s}\\&-\left({}^{I}\mathbf{\check{R}}_{C_{k+1}}\right)^{T}{}^{G}\mathbf{\check{p}}_{I_{k+1}}-{}^{I}\mathbf{\check{p}}_{C_{k+1}}\\\mathbf{r}_{c}&\left(\mathbf{\check{x}}_{k+1},{}^{C}\mathbf{p}_{s},{}^{G}\mathbf{P}_{s}\right)={}^{C}\mathbf{p}_{s}-\boldsymbol{\pi}\left({}^{C}\mathbf{P}_{s}\right)\end{aligned} $$
$$ \pi(\cdot) $$
Now considering the measurement noise, we have:
$$ \begin{aligned}^{G}\mathbf{P}_{s}&=^{G}\mathbf{P}_{s}^{\mathrm{gt}}+\mathbf{n}_{\mathbf{P}_{s}},\mathbf{n}_{\mathbf{P}_{s}}\sim\mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{\mathbf{n}_{\mathbf{P}_{s}}})\\^{C}\mathbf{p}_{s}&=^{C}\mathbf{p}_{s}^{\mathrm{gt}}+\mathbf{n}_{\mathbf{p}_{s}},\mathbf{n}_{\mathbf{p}_{s}}\sim\mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{\mathbf{n}_{\mathbf{p}_{s}}})\end{aligned} $$
where $^G\mathbf{P}_s^{\mathrm{gt}}$ and $^C\mathbf{p}_s^{\mathrm{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}_s^{\mathrm{gt}})$ as:
$$ \begin{align*}\mathbf{0}&=\mathbf{r}_{c}(\mathbf{x}_{k+1},{}^{C}\mathbf{p}_{s}^{\mathrm{gt}},{}^{G}\mathbf{P}_{s}^{\mathrm{gt}})\\&\approx\mathbf{r}_{c}\left(\check{\mathbf{x}}_{k+1},{}^{C}\mathbf{p}_{s},{}^{G}\mathbf{P}_{s}\right)+\mathbf{H}_{s}^{c}\delta\check{\mathbf{x}}_{k+1}+\boldsymbol{\beta}_{s},\end{align*} $$
which constitutes another posteriori distribution for $\delta\tilde{\mathbf{x}}_{k+1}$. In (19), $\boldsymbol{\beta}_{s} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\boldsymbol{\beta}_{s}})$ and:
$$ \begin{aligned}\mathbf{H}_{s}^{c}&=\frac{\partial\mathbf{r}_{c}(\breve{\mathbf{x}}_{k+1}\boxplus\delta\breve{\mathbf{x}}_{k+1},C\mathbf{p}_{s},G\mathbf{P}_{s})}{\partial\delta\breve{\mathbf{x}}_{k+1}}\big|_{\delta\breve{\mathbf{x}}_{k+1}=\mathbf{0}}\\\boldsymbol{\Sigma}_{\beta_{s}}&=\boldsymbol{\Sigma}_{\mathbf{n}_{\mathbf{p}_{s}}}+\mathbf{F}_{\mathbf{P}_{s}}\boldsymbol{\Sigma}_{\mathbf{P}_{s}}\mathbf{F}_{\mathbf{P}_{s}}^{T}\\\mathbf{F}_{\mathbf{P}_{s}}&=\frac{\partial\mathbf{r}_{c}(\breve{\mathbf{x}}_{k+1},C\mathbf{p}_{s},G\mathbf{P}_{s})}{\partial^{G}\mathbf{P}_{s}}\end{aligned} $$
The detailed computation of $\mathbf{H}_{s}^{c}$ and $\mathbf{F}_{\mathbf{P}_{s}}$ is given in appendix. D
### 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\ddot{x}_{k+1}$:
$$ \begin{aligned}\min_{\delta\tilde{\mathbf{x}}_{k+1}}&\left(\left\|\tilde{\mathbf{x}}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}+\mathcal{H}\delta\tilde{\mathbf{x}}_{k+1}\right\|_{\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}}^{-1}\right.\\&\left.+\sum_{j=1}^{m_{l}}\left\|\mathbf{r}_{l}(\tilde{\mathbf{x}}_{k+1},^{L}\mathbf{p}_{j})+\mathbf{H}_{j}^{l}\delta\tilde{\mathbf{x}}_{k+1}\right\|_{\mathbf{\Sigma}_{\alpha_{j}}}^{-1}\right.\\&\left.+\sum_{s=1}^{m_{c}}\left\|\mathbf{r}_{c}(\tilde{\mathbf{x}}_{k+1},^{C}\mathbf{p}_{s},^{G}\mathbf{P}_{s})+\mathbf{H}_{s}^{c}\delta\tilde{\mathbf{x}}_{k+1}\right\|_{\mathbf{\Sigma}_{\beta_{s}}}^{-1}\right)\end{aligned} $$
where $\|\mathbf{x}\|_{\Sigma}^2 = \mathbf{x}\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{aligned}\mathbf{H}^{T}&=\left[\mathbf{H}_{1}^{l},\ldots,\mathbf{H}_{m_{l}}^{l},\mathbf{H}_{1}^{cT},\ldots,\mathbf{H}_{m_{c}}^{c}\right]^{T}\\\mathbf{R}&=diag(\boldsymbol{\Sigma}_{\alpha_{1}},\ldots,\boldsymbol{\Sigma}_{\alpha_{m_{l}}},\boldsymbol{\Sigma}_{\beta_{1}},\ldots,\boldsymbol{\Sigma}_{\beta_{m_{c}}})\\\tilde{\mathbf{z}}_{k+1}^{T}&=\left[\mathbf{r}_{l}(\tilde{\mathbf{x}}_{k+1},^{L}\mathbf{p}_{1}),\ldots,\mathbf{r}_{l}(\tilde{\mathbf{x}}_{k+1},^{L}\mathbf{p}_{m_{l}}),\right.\\&\left.\quad\mathbf{r}_{c}(\tilde{\mathbf{x}}_{k+1},^{C}\mathbf{p}_{1},^{G}\mathbf{p}_{1}),\ldots,\mathbf{r}_{c}(\tilde{\mathbf{x}}_{k+1},^{C}\mathbf{p}_{m_{c}},^{G}\mathbf{p}_{m_{c}})\right]\\\mathbf{P}&=(\mathcal{H})^{-1}\boldsymbol{\Sigma}_{\delta\tilde{\mathbf{x}}_{k+1}}(\mathcal{H})^{-T}\end{aligned} $$
Following [^5], we have the Kalman gain computed as:
$$ \mathbf{K}=\left(\mathbf{H}^{T}\mathbf{R}^{-1}\mathbf{H}+\mathbf{P}^{-1}\right)^{-1}\mathbf{H}^{T}\mathbf{R}^{-1} $$
Then we can update the state estimate as:
$$ \mathbf{\check{x}}_{k+1}=\mathbf{\check{x}}_{k+1}\boxplus\left(-\mathbf{K}\mathbf{\check{z}}_{k+1}-\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right)\left(\mathbf{\mathcal{H}}\right)^{-1}\left(\mathbf{\check{x}}_{k+1}\boxminus\mathbf{\hat{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 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:
$$ \begin{array}{r}{\hat{\mathbf{x}}_{k+1}=\check{\mathbf{x}}_{k+1},\quad\hat{\pmb{\Sigma}}_{\delta\bar{\mathbf{x}}_{k+1}}=\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right)\check{\pmb{\Sigma}}_{\delta\mathbf{x}_{k+1}}}\end{array} $$
### 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.
Fig. 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 lightweight, the LiDAR poses in the pose graph are fixed and the LiDAR raw point measurements are not engaged in the pose graph optimization.
Fig. 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.
### A. Our device for data sampling
### VI. EXPERIMENTS AND RESULTS
Our handheld device for data sampling is shown in Fig. 6 (a), which includes the power supply unit, the onboard DJI manifold-2c³ computation platform (equipped with an Intel i7-8550u CPU and 8 GB RAM), a global shutter camera, and a LiVOX AVIA⁴ LiDAR. The FoV of the camera is 82.9° × 66.5° while the FoV of LiDAR is 70.4° × 77.2°. 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) system⁵ on our device, shown in Fig. 6 (b).
Fig. 7: We evaluate the robustness of our algorithm under scenarios with the aggressive motion (sequence in a~d) and sensor failure by intentionally blocking the camera (e) and LiDAR sensor (f).
Fig. 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.
$^{3}$
$^{4}$
$^{5}$
---
Fig. 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 ♦ while the ending point of each trajectory is marked with ★. "VINS-Mono" stopped at middle way due to the failure of feature tracking.
### 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. 7 (a~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. 7(e)) and LiDAR (see Fig. 7(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 still 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.
### 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 station $^6$. 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 map $^7$, 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-Mono $^8$ and Fast-LIO $^9$ in Fig. 9 (c), where we can see that our method achieves the best overall performance in this experiment.
Fig. 10: The upper figure plots the different trajectories in experiments-4, while the lower figure shows the raw gyroscope reading in the experiment.
Fig. 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°,3.84%), (0.41°,0.34%) and (0.27°,0.21%) respectively.
$^{8}$
$^{9}$
---
Fig. 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.
### 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)).
### 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.
### F. Run time analysis
The average time consumption in experiment 1~4 are listed in TABLE. I, which demonstrates that $R^{2}$LIVE 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 TABLE I: The average running time of R2LIVE in Experiment-1~4 (Exp-1~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.
| 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 |
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.
## 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:
[^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:
[^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:
[^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. Engl. J. Med
[^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.
## APPENDIX
A. Computation of $\mathbf{F}_{\delta\hat{\mathbf{x}}}$ and $\mathbf{F}_{\mathbf{w}}$
$$ \begin{aligned}{}&{{}\mathbf{F}_{\hat{\mathbf{x}}_{\hat{k}}}=\left.\frac{\partial\left(\delta\hat{\mathbf{x}}_{i+1}\right)}{\partial\delta\hat{\mathbf{x}}_{i}}\right|_{\delta\hat{\mathbf{x}}_{i}=0,\mathbf{w}_{i}=0}}\\ {=}&{{}\left[\begin{array}{c c c c c}{\operatorname{E x p}(-\hat{\omega}_{i}\Delta t)}&{0}&{0}&{0}&{0}&{-\mathbf{J}_{r}(\hat{\omega}_{i}\Delta t)^{T}}&{0}\\ {0}&{\mathbf{I}}&{0}&{0}&{\mathbf{I}\Delta t}&{0}&{0}\\ {0}&{0}&{\mathbf{I}}&{0}&{0}&{0}&{0}\\ {0}&{0}&{0}&{\mathbf{I}}&{0}&{0}&{0}\\ {-^{G}\hat{\mathbf{R}}_{I_{i}}[\hat{\mathbf{a}}_{i}]_{\times}\Delta t}&{0}&{0}&{0}&{\mathbf{I}}&{0}&{-^{G}\hat{\mathbf{R}}_{I_{i}}\Delta t}\\ {0}&{0}&{0}&{0}&{0}&{\mathbf{I}}&{0}\\ {0}&{0}&{0}&{0}&{0}&{0}&{\mathbf{I}}\\ \end{array}\right]}\\ {}&{{}\mathbf{F}_{\mathbf{w}}=\left.\frac{\partial\left(\delta\hat{\mathbf{x}}_{i+1}\right)}{\partial\mathbf{w}_{i}}\right|_{\delta\hat{\mathbf{x}}_{i}=0,\mathbf{w}_{i}=0}}\\ {=}&{{}\left[\begin{array}{c c c c c}{-\mathbf{J}_{r}(\hat{\omega}_{i}\Delta t)^{T}}&{0}&{0}&{0}&{0}\\ {0}&{0}&{0}&{0}&{0}\\ {0}&{0}&{0}&{0}&{0}\\ {0}&{0}&{0}&{0}&{0}\\ {0}&{-^{G}\hat{\mathbf{R}}_{I_{i}}\Delta t}&{0}&{0}&{0}\\ {0}&{0}&{\mathbf{I}\Delta t}&{0}&{0}\\ {0}&{0}&{0}&{\mathbf{I}\Delta t}&{0}\\ \end{array}\right]}\\ \end{aligned} $$
where $\hat{\boldsymbol{\omega}}_i = \boldsymbol{\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)$:
$$ {\bf J}_{r}({\bf r})={\bf I}-\frac{1-\cos||{\bf r}||}{||{\bf r}||^{2}}\left[{\bf r}\right]_{\times}+\frac{||{\bf r}||-\sin(||{\bf r}||)}{||{\bf r}||^{3}}\left[{\bf r}\right]_{\times}^{2},{\bf r}\in\mathbb{R}^{3} $$
For the detailed derivation of $\mathbf{F}_{\delta\mathbf{x}}$ and $\mathbf{F}_{\mathbf{w}}$, please refer to the Section. B of our supplementary material.
B. The computation of H
$$ \begin{aligned}\mathcal{H}&=\frac{\left(\breve{\mathbf{X}}_{k+1}\bigoplus\delta\breve{\mathbf{X}}_{k+1}\right)\bigsqcup\hat{\mathbf{X}}_{k+1}}{\partial\delta\breve{\mathbf{X}}_{k+1}}\big|_{\delta\breve{\mathbf{X}}_{k+1}=\mathbf{0}}\\&=\begin{bmatrix}\mathbf{A}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{0}&\mathbf{B}&\mathbf{0}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}_{9\times9}\end{bmatrix}\end{aligned} $$
where the $3 \times 3$ matrix $\mathbf{A} = \mathbf{J}_r^{-1}(\text{Log}(^{G}\hat{\mathbf{R}}_{I_{k+1}}^T \mathbf{R}_{I_{k+1}}))$ and $\mathbf{B} = \mathbf{J}_r^{-1}(\text{Log}(^{I}\hat{\mathbf{R}}_{C_{k+1}}^T \mathbf{R}_{C_{k+1}}))$. $\mathbf{J}_r^{-1}(\cdot)$ are called the inverse right Jacobian matrix of $SO(3)$:
$$ \mathbf{J}_{r}^{-1}(\mathbf{r})=\mathbf{I}+\frac{1}{2}\left[\mathbf{r}\right]_{\times}+\left(\frac{1}{||\mathbf{r}||^{2}}-\frac{1+\cos(||\mathbf{r}||)}{2||\mathbf{r}||\sin(||\mathbf{r}||)}\right)\left[\mathbf{r}\right]_{\times}^{2} $$
C. The computation of $\mathbf{H}_{j}^{l}$
$$ \mathbf{H}_{j}^{l}=\mathbf{u}_{j}^{T}\begin{bmatrix}-\boldsymbol{^{G}\check{\mathbf{R}}}_{I_{k+1}}\left[\mathbf{P_{a}}\right]_{\times}&\mathbf{I}_{3\times3}&\mathbf{0}_{3\times15}\end{bmatrix} $$
where $\mathbf{P}_{\mathbf{a}} = {}^{I}\mathbf{R}_{L}{}^{L}\mathbf{p}_{j} + {}^{I}\mathbf{p}_{L}$
For the detailed derivation of $\mathbf{H}_j^l$, please refer to the Section.
D of our supplementary material.
D. The computation of $\mathbf{H}_{s}^{c}$ and $\mathbf{F}_{\mathbf{P}_{s}}$
$$ \begin{aligned}&\mathbf{H}_{s}^{c}=-\mathbf{F}_{\mathbf{A}}\cdot\mathbf{F}_{\mathbf{B}}\\&\mathbf{F}_{\mathbf{P}_{s}}=-\mathbf{F}_{\mathbf{A}}\cdot\mathbf{F}_{\mathbf{C}}\\ \end{aligned} $$
with:
$$ \begin{aligned}\mathbf{F}_{\mathbf{A}}&=\frac{1}{C P_{s z}}\begin{bmatrix}f_{x}&0&-f_{x}\frac{C P_{s x}}{C P_{s z}}\\ 0&f_{y}&-f_{y}\frac{C P_{s y}}{C P_{s z}}\end{bmatrix}\\\mathbf{F}_{\mathbf{B}}&=\left[\mathbf{M}_{\mathbf{A}}\quad\mathbf{M}_{\mathbf{B}}\quad\mathbf{M}_{\mathbf{C}}\quad-\mathbf{I}\quad\mathbf{0}_{3\times12}\right]\\\mathbf{F}_{\mathbf{C}}&=\left({}^{G}\check{\mathbf{R}}_{I_{k+1}}{}^{I}\check{\mathbf{R}}_{C}\right)^{T}\end{aligned} $$
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:
$$ \begin{aligned}\mathbf{M}_{\mathbf{A}}&=\left(^{I}\hat{\mathbf{R}}_{C}\right)^{T}\left[\left(^{G}\hat{\mathbf{R}}_{I_{k+1}}\right)^{T}{}^{G}\mathbf{P}_{s}\right]_{\times}\\\mathbf{M}_{\mathbf{B}}&=-\left(^{I}\hat{\mathbf{R}}_{C}\right)^{T}\\\mathbf{M}_{\mathbf{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_{k+1}}^{i}\right]_{\times}\end{aligned} $$
For the detailed derivation of $\mathbf{H}_{s}^{c}$ and $\mathbf{F}_{\mathbf{P}_{s}}$, please refer to Section. E of our supplementary material.
---
## Supplementary Material: R $^{2}$LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping
### A. Perturbation on SO(3)
In this appendix, we will use the following approximation of perturbation $\delta\mathbf{r} \to \mathbf{0}$ on $SO(3)$ [^25] [^26]:
$$ \mathrm{Exp}(\mathbf{r}+\delta\mathbf{r})\approx\mathrm{Exp}(\mathbf{r})\mathrm{Exp}(\mathbf{J}_{r}(\mathbf{r})\delta\mathbf{r}) $$
$$ \mathrm{E x p}(\mathbf{r})\mathrm{E x p}(\delta\mathbf{r})\approx\mathrm{E x p}(\mathbf{r}+\mathbf{J}_{r}^{-1}(\mathbf{r})\delta\mathbf{r}) $$
$$ \mathbf{R}\cdot\operatorname{E x p}(\delta\mathbf{r})\cdot\mathbf{u}\approx\mathbf{R}\left(\mathbf{I}+[\delta\mathbf{r}]_{\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 $[\cdot]_\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.
$$ {\bf J}_{r}({\bf r})={\bf I}-\frac{1-\cos||{\bf r}||}{||{\bf r}||^{2}}\left[{\bf r}\right]_{\times}+\frac{||{\bf r}||-\sin(||{\bf r}||)}{||{\bf r}||^{3}}\left[{\bf r}\right]_{\times}^{2} $$
$$ \mathbf{J}_{r}^{-1}(\mathbf{r})=\mathbf{I}+\frac{1}{2}\left[\mathbf{r}\right]_{\times}+\left(\frac{1}{||\mathbf{r}||^{2}}-\frac{1+\cos(||\mathbf{r}||)}{2||\mathbf{r}||\sin(||\mathbf{r}||)}\right)\left[\mathbf{r}\right]_{\times}^{2} $$
B. Computation of $\mathbf{F}_{\delta\mathbf{x}}$ and $\mathbf{F}_{\mathbf{w}}$
Combing (4) and (6), we have:
$$ \begin{aligned}&\delta\hat{\mathbf{x}}_{i+1}=\mathbf{x}_{i+1}\boxminus\hat{\mathbf{x}}_{i+1}\\ =&\left(\mathbf{x}_{i}\boxplus\left(\Delta t\cdot\mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)\right)\right)\boxminus\left(\hat{\mathbf{x}}_{i}\boxplus\left(\Delta t\cdot\mathbf{f}\left(\hat{\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{0}\right)\right)\right)\\ =&\begin{bmatrix}\mathbf{Log}\left(\left({}^{G}\hat{\mathbf{R}}_{I_{i}}\operatorname{Exp}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)\right)^{T}\cdot\left({}^{G}\hat{\mathbf{R}}_{I_{i}}\operatorname{Exp}\left({}^{G}\delta\mathbf{r}_{I_{i}}\right)\operatorname{Exp}\left(\boldsymbol{\omega}_{i}\Delta t\right)\right)\right)\\ {{}^{G}\delta\mathbf{p}_{I_{i}}+{}^{G}\delta\mathbf{v}_{i}\Delta t+\frac{1}{2}\mathbf{a}_{i}\Delta t^{2}-\frac{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}}\operatorname{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{b}\mathbf{g}_{i}}\\ {\delta\mathbf{a}_{g_{i}}+\mathbf{n}_{\mathbf{b}\mathbf{a}_{i}}}\end{bmatrix}\end{aligned} $$
with:
$$ \hat{\boldsymbol{\omega}}_{i}=\boldsymbol{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{g}_{i}},\boldsymbol{\omega}_{i}=\hat{\boldsymbol{\omega}}_{i}-\boldsymbol{\delta}\mathbf{b}_{\mathbf{g}_{i}}-\mathbf{n}_{\mathbf{g}_{i}} $$
$$ \hat{\mathbf{a}}_{i}=\mathbf{a}_{m_{i}}-\mathbf{b}_{\mathbf{a}_{i}},\mathbf{a}_{i}=\hat{\mathbf{a}}_{i}-\delta\mathbf{b}_{\mathbf{a}_{i}}-\mathbf{n}_{\mathbf{a}_{i}} $$
And we have the following simplification and approximation form Section. A.
$$ \begin{aligned}&\operatorname{Log}\left(\left(^{G}\hat{\mathbf{R}}_{I_{i}}\operatorname{Exp}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)\right)^{T}\cdot\left(^{G}\hat{\mathbf{R}}_{I_{i}}\operatorname{Exp}\left(^{G}\delta\mathbf{r}_{I_{i}}\right)\operatorname{Exp}\left(\boldsymbol{\omega}_{i}\Delta t\right)\right)\right)\\ =&\operatorname{Log}\left(\operatorname{Exp}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)^{T}\cdot\left(\operatorname{Exp}\left(^{G}\delta\mathbf{r}_{I_{i}}\right)\cdot\operatorname{Exp}\left(\boldsymbol{\omega}_{i}\Delta t\right)\right)\right)\\ \approx&\operatorname{Log}\left(\operatorname{Exp}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)^{T}\operatorname{Exp}\left(^{G}\delta\mathbf{r}_{I_{i}}\right)\operatorname{Exp}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)\cdot\right.\\ &\left.\quad\operatorname{Exp}\left(-\mathbf{J}_{r}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)\left(\delta\mathbf{b}_{g_{i}}+\mathbf{n}_{\mathbf{g}_{i}}\right)\right)\right)\\ \approx&\operatorname{Exp}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)\cdot^{G}\delta\mathbf{r}_{I_{i}}-\mathbf{J}_{r}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)^{T}\delta\mathbf{b}_{\mathbf{g}_{i}}-\mathbf{J}_{r}\left(\hat{\boldsymbol{\omega}}_{i}\Delta t\right)^{T}\mathbf{n}_{\mathbf{g}_{i}}\\ &\left(^{G}\mathbf{R}_{I_{i}}\operatorname{Exp}\left(^{G}\delta\mathbf{r}_{I_{i}}\right)\right)\mathbf{a}_{i}\Delta t\\ \approx&\left(^{G}\mathbf{R}_{I_{i}}\left(\mathbf{I}+\left[^{G}\delta\mathbf{r}_{I_{i}}\right]_{\times}\right)\right)\left(\hat{\mathbf{a}}_{i}-\delta\mathbf{b}_{\mathbf{a}_{i}}-\mathbf{n}_{\mathbf{a}_{i}}\right)\Delta t\\ \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}}\end{aligned} $$
To conclude, we have the computation of $\mathbf{F}_{\delta\mathbf{x}}$ and follow:
$$ \begin{aligned}&\mathbf{F}_{\delta\hat{\mathbf{x}}}=\left.\frac{\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}}\\ &=\left[\begin{array}{c c c c c}\operatorname{E x p}(-\hat{\boldsymbol{\omega}}_{i}\Delta t)&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&-\mathbf{J}_{r}(\hat{\boldsymbol{\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}\\ -\boldsymbol{^{G}\hat{\mathbf{R}}}_{I_{i}}[\hat{\mathbf{a}}_{i}]_{\times}\Delta t&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}&-\boldsymbol{^{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{array}\right]\end{aligned} $$
$$ \begin{aligned}&\mathbf{F}_{\mathbf{w}}=\left.\frac{\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}}\\ &=\begin{bmatrix}-\mathbf{J}_{r}(\hat{\boldsymbol{\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}\\ \end{aligned} $$
C. The computation of H
Recalling (15), we have:
$$ \begin{aligned}\mathcal{H}&=\frac{\left(\breve{\mathbf{X}}_{k+1}\bigoplus\delta\breve{\mathbf{X}}_{k+1}\right)\bigsqcup\hat{\mathbf{X}}_{k+1}}{\partial\delta\breve{\mathbf{X}}_{k+1}}\big|_{\delta\breve{\mathbf{X}}_{k+1}=\mathbf{0}}\\&=\begin{bmatrix}\mathbf{A}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{0}&\mathbf{B}&\mathbf{0}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}_{3\times9}\\\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}_{9\times9}\end{bmatrix}\end{aligned} $$
with the $3 \times 3$ matrix $\mathbf{A} = \mathbf{J}_r^{-1} (\text{Log}({}^G \hat{\mathbf{R}}_{I_{k+1}}^T G \mathbf{\check{R}}_{I_{k+1}}))$ and $\mathbf{B} = \mathbf{J}_r^{-1} (\text{Log}({}^I \hat{\mathbf{R}}_{C_{k+1}}^T I \mathbf{\check{R}}_{C_{k+1}}))$.
D. The computation of $\mathbf{H}_{j}^{l}$
Recalling (12) and (15), we have:
$$\begin{array}{r l}&{\mathbf{r}_{l}\big(\check{\mathbf{x}}_{k+1}\bigoplus\delta\check{\mathbf{x}}_{k+1},^{L}\mathbf{p}_{j}\big)=\mathbf{u}_{j}^{T}\left(^{G}\check{\mathbf{p}}_{I_{k+1}}+^{G}\delta\check{\mathbf{p}}_{I_{k+1}}-\right.}\\ &{\left.\mathbf{q}_{j}+^{G}\check{\mathbf{R}}_{I_{k+1}}\mathbb{E x p}\big(^{G}\check{\delta}\mathbf{r}_{I_{k+1}}\big)\left(^{I}\mathbf{R}_{L}{}^{L}\mathbf{p}_{j}+^{I}\mathbf{p}_{L}\right)\right)}\end{array}$$
And with the small perturbation approximation, we get:
$$ \begin{aligned}&\mathbf{\Phi}^{G}\tilde{\mathbf{R}}_{I_{k+1}}\mathbf{E}\mathbf{x}\mathbf{p}\big(^{G}\delta\tilde{\mathbf{r}}_{I_{k+1}}\big)\mathbf{P}_{\mathbf{a}}\\ \approx&\mathbf{\Phi}^{G}\tilde{\mathbf{R}}_{I_{k+1}}\left(\mathbf{I}+\left[^{G}\delta\check{\mathbf{r}}_{I_{k+1}}\right]_{\times}\right)\mathbf{P}_{\mathbf{a}}\\ =&\mathbf{\Phi}^{G}\check{\mathbf{R}}_{I_{k+1}}\mathbf{P}_{\mathbf{a}}-\mathbf{\Phi}^{G}\check{\mathbf{R}}_{I_{k+1}}\left[\mathbf{P}_{\mathbf{a}}\right]_{\times}\mathbf{\Phi}^{G}\delta\check{\mathbf{r}}_{I_{k+1}}\end{aligned} $$
where $\mathbf{P}_{\mathbf{a}} = {}^{I}\mathbf{R}_{L}{}^{L}\mathbf{p}_{j} + {}^{I}\mathbf{p}_{L}$. Combining (S3) and (S4) together we can obtain:
$$ \mathbf{H}_{j}^{l}=\mathbf{u}_{j}^{T}\begin{bmatrix}-\boldsymbol{^{G}}\check{\mathbf{R}}_{I_{k+1}}\left[\mathbf{P_{a}}\right]_{\times}&\mathbf{I}_{3\times3}&\mathbf{0}_{3\times15}\end{bmatrix} $$
E. The computation of $\mathbf{H}_{s}^{c}$ and $\mathbf{F}$
Recalling (16), we have:
$$ {}^{C}\mathbf{P}_{s}=\mathbf{P}_{\mathbf{C}}\big(\check{\mathbf{x}}_{k+1},{}^{G}\mathbf{P}_{s}\big)=\left[{}^{C}P_{s x}{}^{C}P_{s y}{}^{C}P_{s z}\right]^{T} $$
where the function $\mathbf{P}_{\mathbf{C}}(\tilde{\mathbf{x}}_{k+1}, {}^{G}\mathbf{P}_{s})$ is:
$$ \begin{aligned}\mathbf{P}_{\mathbf{C}}\big(\breve{\mathbf{x}}_{k+1},{}^{G}\mathbf{P}_{s}\big)&=\left({}^{G}\breve{\mathbf{R}}_{I_{k+1}}{}^{I}\breve{\mathbf{R}}_{C_{k+1}}\right)^{T}{}^{G}\mathbf{P}_{s}\\&\quad-\left({}^{I}\breve{\mathbf{R}}_{C_{k+1}}\right)^{T}{}^{G}\breve{\mathbf{p}}_{I_{k+1}}-{}^{I}\breve{\mathbf{p}}_{C_{k+1}}\end{aligned} $$
From (20), we have:
$$ \mathbf{r}_{c}\left(\check{\mathbf{x}}_{k+1},{{^{C}\mathbf{p}_{s}}},{^{G}\mathbf{P}_{s}}\right)={^{C}\mathbf{p}_{s}}-\mathbf{\pi}\big({^{C}\mathbf{P}_{s}}\big) $$
$$ \boldsymbol{\pi}(^{C}\mathbf{P}_{s})=\left[f_{x}\frac{^{C}P_{s x}}{^{C}P_{s z}}+c_{x}\quad f_{y}\frac{^{C}P_{s y}}{^{C}P_{s z}}+c_{y}\right]^{T} $$
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\tilde{\mathbf{x}}_{k+1}^{i}=0}$ in the following derivation, and we have:
$$ \mathbf{H}_{s}^{c}=-\frac{\partial\mathbf{\pi}\binom{C}{\mathbf{P}_{s}}}{\partial^{C}\mathbf{P}_{s}}\cdot\frac{\partial\mathbf{P}_{\mathbf{C}}\big(\check{\mathbf{x}}_{k+1}\boxplus\delta\check{\mathbf{x}}_{k+1},{}^{G}\mathbf{P}_{s}\big)}{\partial\delta\check{\mathbf{x}}_{k+1}} $$
$$ \mathbf{F}_{\mathbf{P}_{s}}=-\frac{\partial\mathbf{\pi}^{\left(C\mathbf{P}_{s}\right)}}{\partial^{C}\mathbf{P}_{s}}\cdot\frac{\partial\mathbf{P}_{\mathbf{C}}\left(\check{\mathbf{x}}_{k+1},{}^{G}\mathbf{P}_{s}\right)}{\partial^{G}\mathbf{P}_{s}} $$
where:
$$ \frac{\partial\mathbf{\pi}(^{C}\mathbf{P}_{s})}{\partial^{^{C}}\mathbf{P}_{s}}=\frac{1}{^{C}P_{sz}}\left[\begin{matrix}f_{x}&0&-f_{x}\frac{^{C}P_{sx}}{^{C}P_{sz}}\\ 0&f_{y}&-f_{y}\frac{^{C}P_{sy}}{^{C}P_{sz}}\end{matrix}\right] $$
$$ \frac{\partial\mathbf{P}_{\mathbf{b}}\left(\mathbf{\check{x}}_{k+1},{{}}^{G}\mathbf{P}_{s}\right)}{\partial{}^{G}\mathbf{P}_{s}}=\left({}^{G}\mathbf{\check{R}}_{I_{k+1}}{}^{I}\mathbf{\check{R}}_{C}\right)^{T} $$
According to Section. A, we have the following approximation of $\mathbf{P}_{\mathbf{C}}(\tilde{\mathbf{x}}_{k+1} \oplus \delta \tilde{\mathbf{x}}_{k+1}, ^G\mathbf{P}_s)$:
$$ \begin{aligned}&\mathbf{P}_{\mathbf{C}}\big(\bar{\mathbf{x}}_{k+1}\boxplus\delta\bar{\mathbf{x}}_{k+1},^{G}\mathbf{P}_{s}\big)\\ &=\left(^{G}\bar{\mathbf{R}}_{I_{k+1}}\mathbf{E}\mathbf{x}\mathbf{p}\left(^{G}\delta\bar{\mathbf{r}}_{I_{k+1}}\right)^{I}\bar{\mathbf{R}}_{C_{k+1}}\mathbf{E}\mathbf{x}\mathbf{p}\left(^{I}\delta\bar{\mathbf{r}}_{C_{k+1}}\right)\right)^{T}{}^{G}\mathbf{P}_{s}-^{I}\bar{\mathbf{p}}_{C}\\ &-^{I}\delta\bar{\mathbf{p}}_{C}-\left(^{I}\bar{\mathbf{R}}_{C}\mathbf{E}\mathbf{x}\mathbf{p}\left(^{I}\delta\bar{\mathbf{r}}_{C}\right)\right)^{T}\left({}^{G}\bar{\mathbf{p}}_{I_{k+1}}+{}^{G}\delta\bar{\mathbf{p}}_{I_{k+1}}\right)\\ &\approx\mathbf{P}_{\mathbf{b}}\big(\bar{\mathbf{x}}_{k+1}^{i},{}^{G}\mathbf{P}_{s}\big)+\left[\left({}^{G}\bar{\mathbf{R}}_{I_{k+1}}-{}^{I}\bar{\mathbf{R}}_{C}\right)^{T}{}^{G}\mathbf{P}_{s}\right]_{\times}{}^{I}\delta\bar{\mathbf{r}}_{C}\\ &+\left({}^{I}\bar{\mathbf{R}}_{C}\right)^{T}\left[\left({}^{G}\bar{\mathbf{R}}_{I_{k+1}}\right){}^{T}{}^{G}\mathbf{p}_{s}\right]_{\times}{}^{G}\delta\bar{\mathbf{r}}_{I_{k+1}}-\left({}^{I}\bar{\mathbf{R}}_{C}\right)^{T}{}^{G}\delta\bar{\mathbf{p}}_{I_{k+1}}\\ &-\left[\left({}^{I}\bar{\mathbf{R}}_{C}\right)^{T}{}^{G}\bar{\mathbf{p}}_{I_{k+1}}\right]_{\times}{}^{I}\delta\bar{\mathbf{r}}_{C}-{}^{I}\delta\bar{\mathbf{p}}_{C}\\ \end{aligned} $$
With this, we can derive:
$$ \begin{aligned}&\frac{\partial\mathbf{P}_{\mathbf{C}}\big(\tilde{\mathbf{x}}_{k+1}\big.\bigoplus\delta\tilde{\mathbf{x}}_{k+1},^{G}\mathbf{P}_{s}\big)}{\partial\delta\tilde{\mathbf{x}}_{k+1}}=\left[\mathbf{M}_{\mathbf{A}}\mathbf{\Lambda}\mathbf{M}_{\mathbf{B}}\mathbf{\Lambda}\mathbf{M}_{\mathbf{C}}\right.-\mathbf{I}\mathbf{\Lambda}\mathbf{0}_{3\times12}]\\&\mathbf{M}_{\mathbf{A}}=\left(^{I}\hat{\mathbf{R}}_{C}\right)^{T}\left[\left(^{G}\hat{\mathbf{R}}_{I_{k+1}}\right)^{T}{^{G}\mathbf{P}}_{s}\right]_{\times}\\&\mathbf{M}_{\mathbf{B}}=-\left(^{I}\hat{\mathbf{R}}_{C}\right)^{T}\\&\mathbf{M}_{\mathbf{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_{k+1}}^{i}\right]_{\times}\\ \end{aligned} $$
Substituting (S10), (S11) and (S12) into (S8) and (S9), we finish the computation of $\mathbf{H}_s^c$ and $\mathbf{F}_{\mathbf{P}_s}$.