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.
惯性以及相机传感器的测量数据，从而实现实时性能。因子图优化则用于优化关键帧位姿和视觉路标位置的局部地图。
- 通过紧密融合不同类型的传感器，我们实现了高精度的状态估计。实验结果表明，我们的系统精度足以用于重建大规模室内外建筑结构的密集3D地图（见图1）。
我们的系统经过精心研发并已开源 $^{1}$，旨在造福整个机器人学界。
### 二、相关工作
在本节中，我们回顾了与本研究密切相关的现有工作，包括仅基于激光雷达的里程计与建图、激光雷达-惯性融合以及激光雷达-惯性-视觉方法。
### A. 激光雷达里程计与建图
Zhang等人[^10]首次提出了一种激光雷达里程计与建图框架——LOAM，该框架将ICP方法[^11]与点对平面及点对边缘距离计算相结合。通过让这两个模块以不同速率运行，它实现了良好的里程计与建图性能。为了让该算法能在计算能力受限的平台上实时运行，Shan等人[^12]提出了轻量级且针对地面优化的LOAM（LeGOLOAM），该方法在地面分割步骤中会剔除不可靠的特征。这些研究主要基于多线旋转式激光雷达。我们之前的工作[^9] [^13]考虑了视场角较小的固态激光雷达的低级物理特性，从而开发出一种精确且鲁棒的算法。然而，这些仅基于激光雷达测量的方法在特征缺失的环境或其他退化场景下非常脆弱。
---
### B. 激光雷达-惯性里程计
现有的激光雷达-惯性融合研究可分为两类：松耦合和紧耦合。松耦合方法分别处理两种传感器的数据以推导其运动约束，而紧耦合方法则通过联合优化直接融合激光雷达与惯性测量数据。与松耦合方法相比，紧耦合方法展现出更高的鲁棒性与精度，因此近年来受到了越来越多的研究关注。例如，[^14]中的作者提出了LIOM，该方法利用基于激光雷达-惯性里程计先验的图优化以及旋转约束细化方法。与前者相比，LIO-SAM[^15]通过因子图中关键帧位姿的滑动窗口优化来实现更高的精度。同样，Li等人针对传统激光雷达和固态激光雷达提出了基于滑动窗口优化的LiLi-OM[^16]。LINS[^17]是首个通过迭代卡尔曼滤波求解6自由度自我运动的紧耦合LIO。为了降低计算卡尔曼增益时的高计算量，我们之前的工作FAST-LIO[^5]提出了一种新的卡尔曼增益计算公式，其最终计算复杂度取决于状态维度而非测量维度。该成果在无人机机载嵌入式计算机上运行时，可实现高达50 Hz的里程计与建图速率。
### C. 激光雷达-惯性-视觉里程计
在激光雷达-惯性方法的基础上，融合了视觉传感器数据的激光雷达-惯性-视觉里程计展现出更高的鲁棒性与精度。在[^18]的研究中，激光雷达测量数据被用于为相机图像提供深度信息，从而形成类似RGB-D相机的系统，能够利用现有的视觉SLAM成果，如ORB-SLAM[^19]。这是一种松耦合方法，因为它忽略了激光雷达测量对状态的直接约束。Zuo等人[^20]提出了LIC-fusion框架，该框架结合了IMU测量数据、稀疏视觉特征以及激光雷达平面和边缘特征，并基于MSCKF框架进行了在线时空校准，据称其比现有最先进的方法更精确且更鲁棒。随后，他们进一步推出的LIC-Fusion 2.0[^21]改进了一种在滑动窗口内跨多次激光雷达扫描的平面特征跟踪算法，从而使激光雷达扫描匹配更具鲁棒性。
据我们所知，我们的工作是首个开源的紧耦合激光雷达-惯性-视觉融合系统。通过融合不同类型的传感器数据，我们实现了更高精度和鲁棒性的状态估计。大量实验结果表明，与现有的激光雷达-惯性融合估计器和视觉-惯性融合估计器（如FAST-LIO [^5]和VINS-mono [^22]）相比，我们的系统具有更高的精度和鲁棒性。
### III. 系统概述
图2展示了我们系统的概述，其中基于滤波的里程计利用来自激光雷达、摄像头和惯性传感器的数据来估计系统状态。
Fig. 2: The overview of our proposed method.
这一估计过程是在误差状态迭代卡尔曼滤波框架内进行的，具体细节见第IV节。为了进一步提升视觉测量的精度，我们利用因子图优化技术来优化局部滑动窗口内的视觉特征点，相关细节也在第V节中介绍。
### IV. 基于滤波的里程计
### A. boxplus “⊕”和boxminus “⊖”运算符
在本文中，我们利用定义在流形$\mathcal{M}$上的“⊓”和“□”运算来简化符号和推导过程。设$\mathcal{M}$为系统状态所在的流形。由于流形在局部上与$\mathbb{R}^n$同胚，其中n为$\mathcal{M}$的维度，因此我们可以使用“⊓”和“□”这两个运算符，在$\mathcal{M}$的局部邻域与其切空间$\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} $$
对于复合流形$\mathcal{M} = SO(3) \times \mathbb{R}^n$，我们有：
$$ \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} $$
其中$\mathbf{r} \in \mathbb{R}^3$、$\mathbf{a}_1, \mathbf{a}_2 \in \mathbb{R}^n$、$\text{Exp}(\cdot)$和$\text{Log}(\cdot)$表示旋转矩阵与旋转向量$^2$之间的罗德里格斯变换。
### B. 连续时间运动模型
在我们当前的研究中，假设激光雷达、摄像头和IMU这三种传感器之间的时间偏移已经预先校准。此外，由于激光雷达和IMU通常在出厂时就被集成并校准，因此我们假设它们之间的外部参数已知，但摄像头与IMU之间的外部参数则在线估计。另外，激光雷达通常是按顺序扫描点的，同一激光雷达帧中的点可能是在不同的机体姿态下测量的。这种运动通过IMU反向传播来补偿，如[^5]所示，因此假设激光雷达帧中的点是在同一时间测量的。基于这些前提，我们系统的输入数据序列可简化为图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.
我们假设IMU、激光雷达和摄像头传感器被刚性连接在一起，激光雷达与IMU之间的外部参数（激光雷达帧相对于IMU帧）为$^{I}\mathbf{T}_{L} = (^{I}\mathbf{R}_{L}, ^{I}\mathbf{p}_{L})$，摄像头与IMU之间的外部参数（摄像头帧相对于IMU帧）为$^{I}\mathbf{T}_{C} = (^{I}\mathbf{R}_{C}, ^{I}\mathbf{p}_{C})$。为方便起见，我们以IMU作为机体坐标系，由此得到以下连续运动模型：
$$ \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} $$
其中$^G(\cdot)$表示全局坐标系下的向量（即第一个与重力对齐的IMU帧[^22]），$^GR_I$和$^G_P I$分别为IMU相对于全局坐标系的姿态和位置，$^G_g$为重力加速度，$\omega_m$和$\mathbf{a}_m$为陀螺仪和加速度计的原始读数，$\mathbf{n}_a$和$\mathbf{n}_g$为IMU测量的白噪声，$\mathbf{b}_a$和$\mathbf{b}_g$为陀螺仪和加速度计的偏差，它们被建模为由高斯噪声$\mathbf{n}_{bg}$和$\mathbf{n}_{ba}$驱动的随机游走过程。
### C. 离散IMU模型
我们按照IMU采样率对连续模型（2）进行离散化处理。设 $\mathbf{x}_{i}$ 为第i次IMU测量时的状态向量：
$$ \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} $$
通过零阶保持器对模型（2）进行离散化（即一个采样周期内 $\Delta t$ 的IMU测量值保持不变），可得到：
$$ \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) $$
其中
$$ \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. 状态传播
在本研究中，我们采用流形上的迭代误差状态卡尔曼滤波器 [^24] 来估计状态向量 $\mathbf{x}_i$，其中状态估计误差 $\delta\hat{\mathbf{x}}_i$ 被定义在状态估计 $\hat{\mathbf{x}}_i$ 的切空间内：
$$\begin{aligned}&\delta\hat{\mathbf{x}}_{i}\triangleq\mathbf{x}_{i}\mathbin{\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.
需要注意的是，$\delta\hat{\mathbf{x}}_i \in \mathbb{R}^{21}$ 的维度最小（系统维度为21），且是一个协方差为 $\Sigma_{\delta\hat{\mathbf{x}}_i}$ 的随机向量。${}^{G}\delta\hat{\mathbf{r}}_{I_i}^T$ 和 ${}^{I}\delta\hat{\mathbf{r}}_{C_i}^T$ 分别为：
$$ ^{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) $$
一旦接收到新的IMU测量数据，只需将公式（3）中的过程噪声设为0，即可完成状态估计的传播：
$$ \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} $$
相应的估计误差会在线性化误差空间中按如下方式传播（更多细节见 [^24]）：
$$ \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} $$
其中：
$$ \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}} $$
其具体数值在附录A中给出。
公式（5）和（7）中的两次传播均从融合最新激光雷达/相机测量数据后的最优状态和协方差估计开始（例如第k次测量，详见第IV-I节），直到接收到下一次激光雷达/相机测量数据为止（例如第 $(k+1)$ 次测量）。时间索引i与k的关系如图3所示。
### E. 先验分布
令公式（5）和（7）中的两次传播在第 $(k+1)$ 次激光雷达/相机测量处停止（见图4），此时传播得到的状态估计和协方差分别为 $\hat{\mathbf{x}}_{k+1}$ 和 $\boldsymbol{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}$。它们本质上为融合第 $\mathbf{x}_{k+1}$ 次测量前的状态 $(k+1)$ 设定了先验分布，具体如下：
$$ \mathbf{x}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1}\sim\mathcal{N}(\mathbf{0},{\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{k+1}}}). $$
### F. 迭代更新的初始化
公式（8）中的先验分布会与激光雷达或相机测量数据融合，从而得到状态 $\tilde{\mathbf{x}}_{k+1}$ 的最大后验（MAP）估计（记为 $\mathbf{x}_{k+1}$）。该MAP估计 $\tilde{\mathbf{x}}_{k+1}$ 以先验估计 $\hat{\mathbf{x}}_{k+1}$ 作为初始值，由于问题的非线性特性，需通过迭代方式进行优化。在每次迭代中，真实状态 $\delta\tilde{\mathbf{x}}_{k+1}$ 与当前估计 $\mathbf{x}_{k+1}$ 之间的误差 $\tilde{\mathbf{x}}_{k+1}$ 定义为：
$$ \delta\breve{\mathbf{x}}_{k+1}\triangleq\mathbf{x}_{k+1}\triangleq\breve{\mathbf{x}}_{k+1}, $$
---
通过考虑式(8)中的先验以及激光雷达/视觉测量数据来最小化后验分布，从而解决这一问题。因此，由式(8)表示的以$\mathbf{x}_{k+1}$为变量的先验分布，应被转换为以$\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} $$
其中
$$ \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}} $$
其详细计算过程见附录B。（10）式本质上为 $\delta\tilde{\mathbf{x}}_{k+1}$ 设定了如下先验分布：
$$ \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. 激光雷达测量
如果第 $(k+1)$ 次测量是激光雷达帧，我们就按照 [^5] 中的方法从原始三维点中提取平面特征点，并按照第IV-B节的方法补偿帧内运动。记运动补偿后的特征点集合为 $\mathcal{L}_{k+1}$，我们计算每个特征点的残差 $^L\mathbf{p}_j \in \mathcal{L}_{k+1}$，其中 j 是特征点的索引，上标 L 表示该点是以激光雷达参考系表示的。
设 $\check{\mathbf{x}}_{k+1}$ 为 $\mathbf{x}_{k+1}$ 的当前估计值，我们可以将 $^L\mathbf{p}_j$ 从激光雷达坐标系转换到全局坐标系 $^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}}$。如同之前在 [^5] 和 [^9] 中采用的LOAM流程一样，我们在地图中搜索最近的平面特征点，并利用它们来拟合一个法向量为 $\mathbf{u}_j$、平面内一点为 $\mathbf{q}_j$ 的平面，测量残差为：
$$ \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) $$
设 $\mathbf{n}_j$ 为点 $^L\mathbf{p}_j$ 的测量噪声，通过补偿 $^L\mathbf{p}_j^{\mathrm{gt}}$ 中的噪声，我们可以得到该点的真实位置 $^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}}). $$
这个真实点位置与真实状态 $\mathbf{x}_{k+1}$ 结合后，应使式(12)中的残差为零，即：
$$ \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}, $$
这就构成了 $\delta\check{\mathbf{x}}_{k+1}$ 的后验分布。在式(14)中，$\mathbf{x}_{k+1}$ 由其误差 $\delta\check{\mathbf{x}}_{k+1}$（定义于式(9)）和 $\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} $$
关于 $\mathbf{H}_j^l$ 的详细计算过程可参见附录C。
### H. 视觉测量
如果第 $(k+1)$ 帧是相机图像，我们就从去畸变后的图像中提取FAST角点特征点 $\pmb{C}_{k+1}$，并利用KLT光流法在当前滑动窗口内的关键帧中跟踪 $\pmb{C}_{k+1}$ 中的特征点（见第V节）。如果 $\pmb{C}_{k+1}$ 中的某个特征点丢失或此前未被跟踪过，我们就利用最优估计的相机位姿在三维空间中三角测量出新的特征点（视觉地标）。
利用第 $(k+1)$ 帧中视觉地标与其跟踪特征点之间的重投影误差来更新当前的状态估计 $\breve{\mathbf{x}}_{k+1}$。对于提取出的角点 $^C\mathbf{p}_s = \left[ u_s \quad v_s \right]^T \in \mathcal{C}_{k+1}$（其中 s 为角点的索引），其在三维空间中的对应地标记为 $^G\mathbf{P}_s$，那么 $^C\mathbf{p}_s$ 的测量残差为：
$$ \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) $$
现在考虑测量噪声，我们有：
$$ \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} $$
其中 $^G\mathbf{P}_s^{\mathrm{gt}}$ 和 $^C\mathbf{p}_s^{\mathrm{gt}}$ 分别为 $^G\mathbf{P}_s$ 和 $^C\mathbf{p}_s$ 的真实值。据此，我们得到真实零残差 $\mathbf{r}_c(\mathbf{x}_{k+1},^C\mathbf{p}_s^{\mathrm{gt}})$ 的一阶泰勒展开式：
$$ \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*} $$
这就构成了 $\delta\tilde{\mathbf{x}}_{k+1}$ 的另一后验分布。在式(19)中，$\boldsymbol{\beta}_{s} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\boldsymbol{\beta}_{s}})$ 以及：
$$ \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} $$
关于 $\mathbf{H}_{s}^{c}$ 和 $\mathbf{F}_{\mathbf{P}_{s}}$ 的详细计算过程见附录D。
### I. 误差状态迭代卡尔曼滤波器的更新
结合先验分布(11)、基于激光雷达测量的后验分布(14)以及基于视觉测量的后验分布(19)，我们得到 $\delta\ddot{x}_{k+1}$ 的最大后验(MAP)估计：
$$ \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} $$
其中 $\|\mathbf{x}\|_{\Sigma}^2 = \mathbf{x}\Sigma\mathbf{x}^T$。注意激光雷达和相机的测量可能不会在同一时刻出现
---
（见图4），因此在上述优化中 $m_{l}$（或 $m_{c}$）可能为零。记
$$ \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} $$
根据[^5]，卡尔曼增益的计算方式如下：
$$ \mathbf{K}=\left(\mathbf{H}^{T}\mathbf{R}^{-1}\mathbf{H}+\mathbf{P}^{-1}\right)^{-1}\mathbf{H}^{T}\mathbf{R}^{-1} $$
随后我们可以更新状态估计值，公式如下：
$$ \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) $$
上述过程（从第四节G节到第四节I节）会反复迭代直至收敛（即更新量小于给定阈值）。收敛后的状态估计值会被用于：(1) 将新激光雷达坐标系下的点投影到世界坐标系，并添加到现有的点云地图中；(2) 若当前帧为关键帧，则对其中新的视觉地标进行三角测量；(3) 作为下一轮循环中第四节D节所涉及的传播过程的起始点。
$$ \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} $$
### 五、因子图优化
如第四节I节所述，新添加的关键帧中未被跟踪的视觉地标会被用于三角测量以生成新的视觉地标。由于关键帧位姿估计存在误差，这种三角测量的精度通常较低。为了进一步提升视觉地标的质量、优化关键帧位姿，同时校准相机与激光雷达-惯性测量单元子系统之间的时间偏移，我们采用因子图优化方法，在图像关键帧的滑动窗口内对相机位姿和视觉地标进行优化。
Fig. 5: Our factor graph optimization.
我们的因子图优化方法与VINS-Mono [^22]类似，但进一步纳入了由激光雷达测量产生的位姿约束，如图5所示。其中也包含了惯性测量单元预积分产生的约束，用以连接激光雷达因子与相机因子。为了保持后端优化的轻量化，位姿图中的激光雷达位姿被固定，且激光雷达的原始点测量结果不会参与位姿图优化。
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. 我们的数据采集设备
### 六、实验与结果
我们的手持式数据采集设备如图6(a)所示，包含电源单元、搭载英特尔i7-8550u CPU和8GB内存的DJI Manifold-2c³机载计算平台、全局快门相机以及LiVOX AVIA⁴激光雷达。相机的视场角为82.9°×66.5°，激光雷达的视场角为70.4°×77.2°。为了定量评估算法的精度（我们在第六节E节中的实验），我们在设备上安装了差分GPS（D-GPS）实时动态定位（RTK）系统⁵，如图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. 实验一：在剧烈运动及传感器故障场景下的鲁棒性评估
在本实验中，我们评估算法在剧烈运动场景下的鲁棒性（见图7(a~d)），该场景下最大角速度可达$300^{\circ}/s$（见图8中的陀螺仪读数）。此外，我们还通过故意遮挡相机（见图7(e)）和激光雷达（见图7(f)）来模拟严重的传感器故障情况。
我们估算的姿态和位置结果如图8所示，图中我们用不同颜色区分不同的测试阶段。由图8可见，即便在剧烈的旋转和平移运动中，我们估算的轨迹仍能紧密跟随实际运动轨迹。即使相机或激光雷达无法提供测量数据，估算出的轨迹依然十分平滑，且没有明显的性能下降。这些结果表明，我们的算法具备足够的鲁棒性，能够应对剧烈运动或传感器故障的情况。读者可观看XXX上附带的视频，了解实验的实际细节。
### C. 实验二：在狭窄隧道类环境及有移动物体的场景下的鲁棒性评估
在这项实验中，我们挑战了基于摄像头和激光雷达的SLAM领域中最困难的场景之一——港大站$^6$地铁站。这是一个典型的狭窄隧道式环境（见图9(a)左侧），其长度可达190米。此外，传感器视野中频繁出现许多移动行人。所有这些因素使得SLAM任务极具挑战性：1）长长的隧道结构大幅减少了激光雷达SLAM可利用的几何特征，尤其是在沿隧道行走或在隧道一端转动激光雷达时；2）高度重复的视觉特征（地面上的图案）会导致视觉SLAM中视觉特征的匹配与追踪出错；3）众多移动行人会给本就稀少的点特征和视觉特征带来异常值。
尽管存在这些挑战，我们的系统仍足够稳健，能够在该场景中正常运行。在图9(b)中，我们将点云数据与港大站的街道地图$^7$进行对齐，发现二者契合度极高。这证明了我们的定位技术具有极高的鲁棒性和准确性。此外，我们在图9(c)中将自己的估计轨迹与Vins-Mono$^8$以及Fast-LIO$^9$的轨迹进行了对比，可以看出在本实验中我们的方法整体表现最佳。
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. 实验三：大规模室内外城市环境中的高精度地图构建
在这项实验中，我们验证了所提出的方法具备足够的精度，能够重建出密集、高精度且大规模的城市环境室内外三维地图。我们采集了港大主楼外部和内部的数据。实时重建的三维地图如图12所示，其中清晰高质量的点云充分证明了所提方法的高精度特性。值得一提的是，无需任何额外处理（即闭环检测），我们的算法在遍历876米后仍能自行实现闭环（见图12(c)），这表明所提方法的漂移率极低。最后，我们将重建的地图与卫星图像进行融合，发现二者契合度极高（见图12(e)）。
### E. 实验四：利用D-GPS RTK对精度进行定量评估
在这项实验中，我们通过将Vins-Mono（惯性测量单元+摄像头）、Fast-LIO（惯性测量单元+激光雷达）以及我们算法的估计轨迹，与实时差分GPS（D-GPS）动态定位（RTK）技术获取的真实轨迹（见图10上方）进行对比，从而定量评估它们的精度。数据中的最大角速度达到了$130^{\circ}/s$（见图10中的陀螺仪读数）。我们计算了长度为50至300米的所有可能子序列的平移误差和旋转误差，各方法的相对位姿误差（RPE）如图11所示。
### F. 运行时间分析
实验1至4中的平均耗时列于表I中，结果表明$R^{2}$LIVE在台式电脑和嵌入式计算平台上均能实现实时运行。需要注意的是，因子图优化是在独立线程中运行的，因此允许以较低的频率运行。
### VII. 结论
在这封信中，我们提出了一种基于高速滤波里程计和因子图优化的鲁棒实时激光雷达-惯性-视觉融合框架。我们将表I中的数据进行了融合：R2LIVE在实验1~4（Exp-1~4）中的平均运行时间分别在台式电脑（配备Intel i7-9700K CPU和32GB RAM）和机载计算机（配备Intel i7-8550U CPU和8GB RAM）上测得。“LI-Odom”、“VI-Odom”和“FG-OPM”分别代表基于激光雷达-惯性传感器的滤波里程计、基于视觉-惯性传感器的滤波里程计以及因子图优化的平均耗时。
| 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 |
我们在误差状态迭代卡尔曼滤波器中融合了激光雷达、惯性传感器和摄像头的测量数据，并利用因子图优化来精细化滑动窗口内的图像关键帧和视觉地标所构成的局部地图。我们的系统已在大规模室内外环境以及狭窄隧道类环境中进行了测试，能够应对传感器故障和剧烈运动等挑战。在所有测试中，我们的方法在定位与建图方面均展现出了极高的精度和鲁棒性。
## 参考文献
[^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.
## 附录
A. $\mathbf{F}_{\delta\hat{\mathbf{x}}}$ 和 $\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} $$
其中 $\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}$ 和 $\mathbf{J}_r(\cdot)$ 被称为 $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} $$
关于 $\mathbf{F}_{\delta\mathbf{x}}$ 和 $\mathbf{F}_{\mathbf{w}}$ 的详细推导过程，请参阅补充材料中的B节。
B. 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} $$
其中 $3 \times 3$ 矩阵 $\mathbf{A} = \mathbf{J}_r^{-1}(\text{Log}(^{G}\hat{\mathbf{R}}_{I_{k+1}}^T \mathbf{R}_{I_{k+1}}))$ 和 $\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)$ 被称为 $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. $\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} $$
其中 $\mathbf{P}_{\mathbf{a}} = {}^{I}\mathbf{R}_{L}{}^{L}\mathbf{p}_{j} + {}^{I}\mathbf{p}_{L}$
关于 $\mathbf{H}_j^l$ 的详细推导过程，请参阅相关章节。
详见补充材料中的D节。
D. $\mathbf{H}_{s}^{c}$ 和 $\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} $$
其中：
$$ \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} $$
这里 $f_x$ 和 $f_y$ 为焦距，$c_x$ 和 $c_y$ 为图像平面上的主点偏移量，而 $3 \times 3$ 矩阵 $\mathbf{M_A}$、$\mathbf{M_B}$ 和 $\mathbf{M_C}$ 分别为：
$$ \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} $$
关于 $\mathbf{H}_{s}^{c}$ 和 $\mathbf{F}_{\mathbf{P}_{s}}$ 的详细推导过程，请参阅补充材料中的E节。
---
### 补充材料：R $^{2}$LIVE：一种鲁棒、实时的激光雷达-惯性-视觉紧耦合状态估计与建图算法
### A. 对SO(3)的扰动
在本附录中，我们将使用以下对 $\delta\mathbf{r} \to \mathbf{0}$ 的扰动近似 $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} $$
其中 $\mathbf{u} \in \mathbb{R}^3$，我们用 $[\cdot]_\times$ 表示向量 $(\cdot)$ 的斜对称矩阵；$\mathbf{J}_r(\mathbf{r})$ 和 $\mathbf{J}_r^{-1}(\mathbf{r})$ 分别被称为 $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} $$
$$ \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. $\mathbf{F}_{\delta\mathbf{x}}$ 和 $\mathbf{F}_{\mathbf{w}}$ 的计算
结合公式(4)和(6)，我们得到：
$$ \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} $$
其中：
$$ \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}} $$
根据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} $$
综上所述，我们得到了 $\mathbf{F}_{\delta\mathbf{x}}$ 的计算方法，具体如下：
$$ \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. H的计算
回顾公式(15)，我们有：
$$ \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} $$
其中涉及$3 \times 3$矩阵$\mathbf{A} = \mathbf{J}_r^{-1} (\text{Log}({}^G \hat{\mathbf{R}}_{I_{k+1}}^T G \mathbf{\check{R}}_{I_{k+1}}))$以及$\mathbf{B} = \mathbf{J}_r^{-1} (\text{Log}({}^I \hat{\mathbf{R}}_{C_{k+1}}^T I \mathbf{\check{R}}_{C_{k+1}}))$。
D. $\mathbf{H}_{j}^{l}$的计算
回顾公式(12)和(15)，我们有：
$$\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}$$
利用小扰动近似，我们得到：
$$ \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} $$
其中$\mathbf{P}_{\mathbf{a}} = {}^{I}\mathbf{R}_{L}{}^{L}\mathbf{p}_{j} + {}^{I}\mathbf{p}_{L}$。将(S3)和(S4)结合起来，我们可以得到：
$$ \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. $\mathbf{H}_{s}^{c}$和$\mathbf{F}$的计算
回顾公式(16)，我们有：
$$ {}^{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} $$
其中函数$\mathbf{P}_{\mathbf{C}}(\tilde{\mathbf{x}}_{k+1}, {}^{G}\mathbf{P}_{s})$定义为：
$$ \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} $$
由公式(20)可得：
$$ \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} $$
其中$f_{x}$和$f_{y}$为焦距，$c_{x}$和$c_{y}$为图像平面上的主点偏移量。
---
为方便起见，在后续推导中我们省略了$(\cdot)|_{\delta\tilde{\mathbf{x}}_{k+1}^{i}=0}$，于是有：
$$ \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}} $$
其中：
$$ \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} $$
根据第A节的内容，我们得到$\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} $$
据此，我们可以推导出：
$$ \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} $$
将(S10)、(S11)和(S12)代入(S8)和(S9)，即可完成$\mathbf{H}_s^c$和$\mathbf{F}_{\mathbf{P}_s}$的计算。