退化环境定位（四）：基于强度的感知定位方法

系列文章目录

相关文章：退化环境定位（一）
相关文章：退化环境定位（二）
相关文章：退化环境定位（三）：基于强度的感知定位方法

---

---

3.7. InLIOM：紧密耦合强度 LiDAR 惯性里程计和映射

原文：链接
摘要：状态估计和地图绘制是自动驾驶汽车智能导航的重要前提。然而，在城市环境中保持高精度仍然具有挑战性，尤其是在卫星信号不可用的情况下。本文提出了一种新的框架 InLIOM，它将 LiDAR 强度测量紧密耦合到系统中，以提高各种具有挑战性的环境中的测绘性能。所提出的框架引入了一种基于扫描到扫描优化的稳定强度 LiDAR 里程计。该方法通过从连续帧的强度信息中成对提取特征，解决了 LiDAR 强度的不稳定性问题。为了确保里程计的稳健性，基于残差的免训练动态对象过滤模块进一步集成到扫描到扫描配准过程中。获得的强度 LiDAR 里程计解决方案与其他多传感器相对和绝对测量值一起纳入因子图，获得全局优化估计。在室内和室外城市环境中的实验表明，所提出的框架实现了优于最先进方法的精度。我们的方法可以稳健地适应高动态道路、隧道、地下停车场和大规模城市场景。

主要贡献

• 提出高效里程计方法：提出一种高效的强度激光雷达里程计方法，借助基于残差的动态物体滤波器模块，实现可靠的扫描 - 扫描位姿估计，提升了在复杂场景下的位姿计算稳定性。
• 优化框架：提出改进的基于因子图的紧耦合优化框架，将激光雷达强度和几何特征融入其中，确保全局优化的稳定性，有助于构建更精准的地图。
• 性能优势：与当前先进框架相比，该方法在室内外环境中，都能实现与之相当的状态估计和建图精度，适用性广泛。
  图2是动态物体过滤算法的框架图。在图(f)中，两帧之间移动汽车的像素可以用读取的像素表示。动态物体过滤结果图(h) 可通过从图(d) 中减去图(g) 来计算得出。

图3是不同观测位置下交通标志的强度示例。两个固定位置的交通标志如图(a) 放置。在实验过程中，自动驾驶车辆持续从右向左行驶，交通标志的强度点云按顺序如图(b)-(k) 所示。连续两次扫描的强度变化是连续的，而两次间隔较远的扫描之间有显著差异。

图5是使用强度图像进行的特征提取测试。三种不同环境验证了所提出的特征提取方法的有效性。每条线连接了第 $t-1$ 帧和第 $t$ 帧中的成对特征点，相同颜色表示一对对应的特征点。为清晰展示测试结果，每个实验仅展示了几组成对的特征点。

B. 动态物体滤波器

1) 距离图像

为确保系统的实时性，我们的方法将三维点云投影到二维距离图像中以便后续处理。考虑点云 $p_i(x_i,y_i,z_i)$，其中 $d_i$ 是点到车辆坐标系原点的距离。点 $p_i$ 在距离图像中的行坐标 $r_i$ 和列坐标 $c_i$ 可描述为：
$$r_i=\lceil \frac{(\arcsin (\frac{z_i}{d_i})-an{g}_b\frac{\pi }{180})(C_{scan}-1)}{(an{g}_t-an{g}_b)\frac{\pi }{180}}\rceil \text{\hspace{1em}(1)}$$
$$c_i=-\lceil (\arctan (\frac{x_i}{y_i})\frac{180}{\pi }-90)\frac{H_{scan}}{360}\rceil +\frac{H_{scan}}{2}$$
其中 $an{g}_t$ 和 $an{g}_b$ 分别表示垂直视场的顶部和底部角度，$C_{scan}$ 是激光雷达通道数，$H_{scan}$ 是每个通道的水平点数。

通过计算每个点云的索引 $(r_i,c_i)$，相应的像素可用于构建距离图像。我们的方法不存储 $d_i$，而是存储 $p_i$ 的球坐标，这样可以展示物体运动趋势的更多细节。存储值 $s_i$ 定义为：
$$s_i=min(atan2(y_i,x_i),atan2(z_i,\sqrt{x_i^2+y_i^2}))\text{\hspace{1em}(3)}$$

因此，每次扫描的点云可以投影到一个 $C_{scan}\times H_{scan}$ 像素的距离图像中，该图像可以直接输入到图像处理算法中。

2) 残差图像

为了实时分割动态物体，我们的方法仅利用当前第 $t$ 帧的距离图像 $rang{e}_t$ 和上一帧（第 $t-1$ 帧）的距离图像 $rang{e}_{t-1}$ 的激光雷达扫描数据。由于车辆的自身运动不可忽视，尤其是在高动态环境中。如图2所示，我们的方法首先使用惯性测量单元（IMU）将 $rang{e}_{t-1}$ 变换到 $rang{e}_t$ 的坐标系下，从而生成 $rang{e}_{t-1,t}$。在此之后，在连续的激光雷达扫描序列中，可以假设只有动态物体的位置会发生变化。考虑 $rang{e}_t$ 中的一个像素 $(r,c)$，其像素值为 $s_t$。距离图像 $rang{e}_{t-1,t}$ 中相同索引 $(r,c)$ 的像素值为 $s_{t-1,t}$。那么残差 $residua{l}_{r,c}$ 可以描述为：
$$residua{l}_{r,c}=\frac{|s_t-s_{t-1,t}|}{s_t}\text{\hspace{1em}(4)}$$

3) 动态物体过滤结果

如果 $residua{l}_{r,c}>\tau$（$\tau$ 为设定的阈值），那么像素 $(r,c)$ 可以被标记为动态像素。通过计算所有像素的残差值，就可以得到第 $t$ 帧的残差图像 $residua{l}_t$，其中剩余的点表示动态障碍物。然后，从距离图像 $rang{e}_t$ 中减去残差图像 $residua{l}_t$，就可以得到最终的动态物体过滤结果 $filtere{d}_t$。

C. 强度激光雷达里程计

强度特征在激光雷达SLAM中是近期才受到关注的，对于一些室内退化环境（如走廊），它能为建图获取更多信息。为提高室外环境中里程计的稳定性，该方法也考虑将强度特征加入框架。如图3所示，发现强度特征不稳定，可能受室外环境光线和物体观测距离影响。因此，与在特征提取过程中混合强度和几何特征的研究不同，本文方法设计了一个独立模块来构建强度激光雷达里程计，如图4所示。这种不稳定性也是该方法不将其作为长期特征存储在地图中用于回环检测的原因，不稳定特征可能增加系统误差。强度激光雷达里程计的细节如下：

1) 强度图像

考虑到上述情况和算法的复杂性，该方法首先将经过滤波的点云投影到强度图像中，以构建强度激光雷达里程计。与上述构建距离图像类似，投影索引的计算与公式1和公式2相同。在强度图像中**，该方法使用灰度图像存储每个像素的强度值**，这有助于降低空间复杂度。

2) 特征提取与一致性判断

该方法直接从强度图像（灰度图像）中提取特征。在此过程中使用了ORB（Oriented FAST and Rotated BRIEF）特征提取算法，因其计算速度快[31, 32]。由于OpenCV版本中特征分布不均匀，该方法设计了一个一致性判断模块来过滤ORB算法的特征提取结果。一致性判断规则如下：

• 二维距离一致性：距离图像中每对特征点的二维距离需要保持一致。
• 三维运动一致性：找到每对特征点对应的原始点云，两点之间的三维距离应与IMU预积分估计的运动一致。
• 邻域一致性：如果选择一个特征点，其八个邻域特征点需要被舍弃，以避免所有点集中在同一区域。

遵循这些一致性规则，可以选择强度图像中的相应特征点。如图5所示，进行了一个简单测试来验证从强度图像中提取特征的效果。

3) 扫描间优化

为得到强度激光雷达里程计的结果，需要对获得的匹配特征点进行进一步处理。首先，通过找到特征点的深度数据，将两帧特征点重建为源点云和目标点云。在此过程中，在点云中重建特征点之间的对应关系。接下来，迭代最近点（ICP）算法可以有效地对齐两个有序点云，同时实现高精度。然后，从强度激光雷达里程计中可以得到位姿估计矩阵。

位姿图优化引入了五种类型的因子来构建因子图：(a) 惯性测量单元（IMU）预积分因子，(b) 激光雷达（LiDAR）几何因子，© 激光雷达强度因子，(d) 回环检测因子，以及(e) 全球定位系统（GPS）因子。特别地，(g) 激光雷达原始数据被分离为两种类型的里程计：(i) 几何扫描到窗口里程计，和(j) 强度扫描到扫描里程计，以分别建立添加到图中的因子。为了提高建图系统的效率，仅将(h) 激光雷达关键帧设置为(k) 状态节点进行优化。独立使用激光雷达的两种特征（几何和强度）增强了建图系统的稳定性。

3.8. RI-LIO：反射率图像辅助的紧耦合激光雷达 - 惯性里程计

源代码：github
原文：链接

摘要： 在本文中，我们提出了 RI-LIO，这是一种全新的反射率图像辅助的紧耦合激光雷达 - 惯性里程计（LIO）框架，该框架引入了额外的反射率纹理信息，以有效减少仅基于几何方法的漂移问题。为实现这一点，我们通过融合点到平面的几何测量和反射率图像测量，构建了一个迭代扩展卡尔曼滤波框架。具体而言，几何测量被定义为新扫描的原始点到全局增量 kd 树地图中其最近邻平面的距离。在激光雷达运动畸变信息由其对应的原始点提供后，所搜索到的最近邻点被用于渲染稀疏反射率图像。然后，通过直接最小化光度误差，构建反射率测量，以使稀疏反射率图像与当前扫描的密集反射率图像对齐。此外，基于高分辨率激光雷达的原理，我们提出了一种校正后的球投影模型，用于将空间点投影到图像帧中。最后，我们在结构化、非结构化以及具有挑战性的野外场景中进行了广泛的实验。结果表明，所提出的方法在鲁棒性和准确性方面优于现有的仅基于几何的方法，尤其是在旋转方向上。

图1. RI-LIO系统概述
该系统主要包括反射率残差计算（绿色箭头和标记，见第三节C部分）、几何残差计算（紫色箭头和标记，见第三节D部分）以及状态估计（见第三节E部分）。惯性测量单元（IMU）提供激光雷达的运动畸变信息以及状态估计的先验信息。不同颜色的点表示它们是在不同时刻$t_{{\rho }_i}$、不同的激光雷达帧 $L_{{\rho }_i}$中采样得到的。

A. 符号说明

我们将点集表示为 $\mathcal{P}$。令 $\mathcal{I}[\cdot ]$ 和 $\mathcal{M}[\cdot ]$ 分别表示图像中的反射率值和全局地图中的反射率值。${}^G(\cdot )$、${}^I(\cdot )$、${}^L(\cdot )$ 和 ${}^C(\cdot )$ 分别表示在全局坐标系、惯性测量单元（IMU）坐标系、激光雷达（LiDAR）坐标系和图像坐标系下 $(\cdot )$ 的值。流形 ${}^b{\mathbf{R}}_a\in SO(3)$ 和 ${}^b{\mathbf{t}}_a\in {\mathbb{R}}^3$ 表示 $a$ 坐标系相对于 $b$ 坐标系的姿态和位置。流形上的运算符为“$⊞$” 和 “$\boxminus$”。在时刻 $t_k$ 的每次激光雷达扫描是由在其各自采样时刻 ${\rho }_i$（${\rho }_i$ 在 $t_k$ 之前）的点组成的，因此我们将 $L_k$ 和 $L_{{\rho }_i}$ 分别表示为在时刻 $t_k$ 和 ${\rho }_i$ 的激光雷达坐标系。对于状态估计，我们用 $\mathbf{x}$、$\hat{\mathbf{x}}$ 和 $\tilde{\mathbf{x}}$ 分别表示真实值、预测值和误差状态值。

B. 反射率图像生成

激光雷达（LiDAR）在检测每个激光点的距离时，也会估计被照射表面的反射率值，该值反映了表面的反射强度。与三维点云相比，二维图像视角是对反射率纹理更紧凑的表示。在本节中，我们专注于通过球投影来生成反射率图像。具体而言，当接收到三维点云$\mathcal{P}$时，我们通过线性变换将每个点的球坐标$[R,\theta ,\phi ]$转换为图像坐标$[u,v]$，其表达式为：
$$u=\frac{w}{2\pi }(\pi -\phi ),v=\frac{h}{{\theta }^{fov}}({\theta }^{up}-\theta )\text{\hspace{1em}(1)}$$
其中，$w$、$h$分别是图像的宽度和高度，${\theta }^{fov}={\theta }^{up}-{\theta }^{down}$是激光雷达的垂直视场角，${\theta }^{up}$和${\theta }^{down}$分别是最大和最小仰角。然而，在实际生成反射率图像时，有两种不同的获取图像坐标的方法：

图2. 由不同投影方法生成的反射率图像
红色像素表示在将一次扫描的相同激光雷达点投影到图像帧中时，不同方法所产生的空洞。

1) 基于激光编号的投影（PBID）[21]

$u$和$v$直接由编码器值$e=[1,w]$和激光头编号$r=[1,h]$确定，即$u=e$且$v=r$。这意味着方位角和仰角根据公式(1)进行理想的均匀划分，即：
$${\phi }_r^{ideal}=\pi -\frac{2\pi e}{w},{\theta }_r^{ideal}={\theta }^{up}-\frac{r{\theta }^{fov}}{h}\text{\hspace{1em}(2)}$$
其中，下标$r$表示该点是由第$r$个激光头捕获的。这种方法忽略了实际的激光雷达点坐标，生成的图像没有空洞，如图2所示。因此，当新的扫描数据到达时，我们直接通过PBID生成密集的反射率图像，如图1所示。

2) 基于实际角度的投影（PBRA）

$u$和$v$由实际的激光雷达点坐标${}^{L_{{\rho }_i}}\mathbf{p}=(x,y,z{)}^T$计算得出。根据从笛卡尔坐标到球坐标的转换，我们可以得到：
$${\phi }_r^{real}=\arctan (y,x),{\theta }_r^{real}=\arcsin \left(\frac{z}{R-n}\right)\text{\hspace{1em}(3)}$$
其中，$R={\Vert }^{L_{{\rho }_i}}\mathbf{p}{\Vert }_2$是距离，$n$是激光束原点到其旋转中心的长度，如图3所示。然后将公式(3)代入公式(1)得到图像坐标。这种方法反映了实际的激光头分布，并从数学上把激光雷达点投影到图像帧中。然而，由于实际激光头的不均匀分布，通过PBRA生成的图像中有许多空洞，如图2所示。

为了统一图像帧并减少空洞，由PBRA生成的像素坐标应校正为PBID坐标。图3(a)和(b)分别显示了第$r$个激光头在PBID和PBRA之间在垂直和水平方向上的投影偏差$b{v}_r$和$b{u}_r$，其表达式为：
$$b{u}_r=\frac{w}{2\pi }({\phi }_r^{ideal}-{\phi }_r^{real}),b{v}_r=\frac{h}{{\theta }^{fov}}({\theta }_r^{ideal}-{\theta }_r^{real})\text{\hspace{1em}(4)}$$

结合公式(1) - (4)，我们提出了校正后的基于实际角度的投影（CPBRA）方法。在这种方法中，激光雷达坐标系中的每个点${}^{L_{{\rho }_i}}\mathbf{p}=(x,y,z{)}^T$通过函数$\Pi :{\mathbb{R}}^3\mapsto {\mathbb{R}}^2$转换到PBID坐标系中的${}^C\mathbf{p}=(u,v{)}^T$，即：
$${}^C\mathbf{p}=\Pi {(}^{L_{{\rho }_i}}\mathbf{p})=\left[\begin{array}{c}\frac{w}{2\pi }[\pi -\arctan (y,x)]+b{u}_r\\ \frac{h}{{\theta }^{fov}}[{\theta }^{up}-\arcsin (\frac{z}{R-n})]+b{v}_r\end{array}\right]\text{\hspace{1em}(5)}$$

然后，来自全局地图中没有激光编号的点可以通过CPBRA进行投影，以生成稀疏的反射率图像。为了最小化投影误差，定义以下损失函数来获取每个激光头的$b{v}_r$和$b{u}_r$：
$${\mathbf{E}}_{uv}=\sum _{r=1}^N\sum _{{}^{L_{{\rho }_i}}\mathbf{p}\in {\mathcal{P}}_r}^C{\mathbf{p}}^{gt}-\Pi {(}^{L_{{\rho }_i}}\mathbf{p})\text{\hspace{1em}(6)}$$
其中，${}^C{\mathbf{p}}^{gt}$是通过PBID获得的真实值，${\mathcal{P}}_r$表示由第$r$个激光头捕获的点集。

通过梯度下降算法，CPBRA的投影误差在垂直和水平方向上都被降低到0.02像素以内。通过分析如图3©所示的投影误差分布，在III - E2节中我们近似将其视为均值为零的高斯分布。此外，由CPBRA生成的反射率图像与PBID一样没有空洞，如图2所示。

图(a)侧视图展示了基于激光编号投影（PBID，理想情况）和基于实际角度投影（PBRA，实际情况）在垂直方向上的投影偏差，图(b)俯视图展示了水平方向上的投影偏差。图©展示了校正后的基于实际角度投影（CPBRA）的投影误差分布。

在全局增量kd树（ikd - tree）地图中搜索最近邻点，以模拟被真实激光束击中，然后进行投影来渲染稀疏像素。

C. 反射率图像测量模型

在本节中，反射率图像测量被定义为密集反射率图像和稀疏反射率图像之间的光度误差（见图1中的绿色虚线框）。密集反射率图像是通过基于激光编号的投影（PBID）方法，直接从新的扫描数据生成的。由于每个输入点对应一个像素，所以该图像是密集的。而稀疏反射率图像则是由全局地图点渲染得到的，其生成过程如下。

如图4所示，与光线投射法[22]类似，激光束可视为光线，并且在遇到遮挡时会返回。因此，假设全局增量kd树（ikd - tree）地图中的最近邻点（NN点，浅蓝色点）与当前扫描点（红色点）是由同一激光束捕获的，也就是说，可以利用NN点的反射率来渲染图像。由于NN点与当前扫描点之间仍存在不可忽略的误差，我们通过在第三节B部分中介绍的校正后的基于实际角度的投影（CPBRA）方法，将NN点重新投影到图像帧中，以获取更精确的像素位置。由于当前扫描可能已进行下采样，或者在一定范围内可能不存在NN点，所以渲染出的图像是稀疏的。

具体而言，当新的扫描数据到达时，按照图1中绿色标记的序号，执行以下步骤来计算反射率残差：

步骤1：运动补偿
首先，我们使用惯性测量单元（IMU）的传播模型来估计在(t_k)时刻激光雷达的运动，具体内容见第三节E1部分。然后，根据文献[3]进行反向传播，以得到在(t_{\rho_i})时刻的运动补偿矩阵${[}^{L_k}{\mathbf{R}}_{L_{{\rho }_i}}{,}^{L_k}{\mathbf{t}}_{L_{{\rho }_i}}]\in SE(3)$。每个点${}^{L_{{\rho }_i}}\mathbf{p}$会累积到当前扫描$L_k$中，即：
$${}^{L_k}\mathbf{p}{=}^{L_k}{\mathbf{R}}_{L_{{\rho }_i}}^{L_{{\rho }_i}}\mathbf{p}{+}^{L_k}{\mathbf{t}}_{L_{{\rho }_i}}$$

步骤2：最近邻搜索
扫描点(^{L_k}\mathbf{p})将通过估计得到的位姿${(}^G{\hat{\mathbf{R}}}_{L_k}{,}^G{\hat{\mathbf{t}}}_{L_k})\in SE(3)$进一步转换到世界坐标系中，即${}^G\mathbf{p}{=}^G{\hat{\mathbf{R}}}_{L_k}^{L_k}\mathbf{p}{+}^G{\hat{\mathbf{t}}}_{L_k}$。然后，可以在全局ikd - tree地图中搜索其最近邻点${}^G{\mathbf{p}}^N$，相关内容见文献[23]。

步骤3：虚拟运动
为了进行投影，最近邻点${}^G{\mathbf{p}}^N$需要转换到每个采样时刻$t_{{\rho }_i}$的激光雷达坐标系中，我们将此定义为虚拟运动，即：
$${}^{L_{{\rho }_i}}{\mathbf{p}}^N{=}^{L_k}{\mathbf{R}}_{L_{{\rho }_i}}^T{(}^G{\hat{\mathbf{R}}}_{L_k}^T{(}^G{\mathbf{p}}^N{-}^G{\hat{\mathbf{t}}}_{L_k}){-}^{L_k}{\mathbf{t}}_{L_{{\rho }_i}})$$

步骤4：校正后的基于实际角度的投影
然后，点${}^{L_{{\rho }_i}}{\mathbf{p}}^N$可以通过第三节B部分中介绍的CPBRA方法进行投影，从而生成稀疏反射率图像${\mathcal{I}}_s$，即${}^C\mathbf{p}=\Pi {(}^{L_{{\rho }_i}}{\mathbf{p}}^N)$。相应的像素值由全局地图点的反射率值确定，即${\mathcal{I}}_s{[}^C\mathbf{p}]=\mathcal{M}{[}^G{\mathbf{p}}^N]$。

步骤5：反射率残差计算
如果位姿估计结果是准确的，那么密集反射率图像与稀疏反射率图像之间的光度误差应该为零，因此反射率残差定义为：
$${\mathbf{z}}_C={\mathcal{I}}_d[\Pi {(}^C\mathbf{p})]-\mathcal{M}{[}^G{\mathbf{p}}^N]\text{\hspace{1em}(7)}$$
其中，${\mathcal{I}}_d[\cdot ]$表示密集反射率图像中的像素值，在后续部分中简记为$\mathcal{I}[\cdot ]$ 。

D. 点到平面的几何测量模型

我们采用与FAST - LIO2[3]相同的扫描到地图的方法，该方法直接在全局地图中提取平面元素，然后将新扫描的每个点与其对应的平面之间的距离作为几何残差（见图1中的紫色虚线框）。

具体来说，当新的扫描数据到达时，执行以下步骤来计算几何残差，步骤序号与图1中的紫色标记相对应：

• 步骤1：运动补偿：见第三节C部分的步骤1。
• 步骤2：K近邻搜索：见第三节C部分的步骤2。不过，在这里，需要在全局增量kd树（ikd - tree）地图中搜索 $k$ 近邻（$k$NN）点 ${}^G{\mathbf{p}}_{\kappa }^N\in {\mathcal{P}}^N$，其中 $\kappa =[1,k]$。
• 步骤3：几何残差计算：由 ${\mathcal{P}}^N$ 组成的矩阵通过奇异值分解（SVD）进行分解，其中对应最小特征值的归一化特征向量就是平面法向量 ${}^G\mathbf{u}$。几何残差定义为：
  $${\mathbf{z}}_S{=}^G{\mathbf{u}}^T{(}^G\mathbf{p}{-}^G{\mathbf{p}}_{\kappa }^N)\text{\hspace{1em}(8)}$$

E. 状态估计

1) 状态定义和传播

我们通过迭代扩展卡尔曼滤波器（IEKF）将几何残差和反射率残差紧密耦合，这与FAST - LIO2 [3] 类似。假设激光雷达（LiDAR）和惯性测量单元（IMU）刚性连接在一起，并且外参${(}^I{\mathbf{R}}_L{,}^I{\mathbf{t}}_L)\in SE(3)$已经过校准，状态变量定义为：
$$\mathbf{x}\triangleq {[}^G{\mathbf{R}}_I^T{}^G{\mathbf{t}}_I^T{}^G{\mathbf{v}}^T{\mathbf{b}}_g^T{\mathbf{b}}_a^T{}^G{\mathbf{g}}^T]\in SO(3)\times {\mathbb{R}}^{15}$$
其中，${}^G{\mathbf{R}}_I$ 和 ${}^G{\mathbf{t}}_I$ 分别表示IMU坐标系相对于全局坐标系的姿态和位置，${}^G\mathbf{v}$ 和 ${}^G\mathbf{g}$ 分别是全局坐标系中的速度和重力向量，${\mathbf{b}}_g$ 和 ${\mathbf{b}}_a$ 是IMU的偏差。

在使用测量模型更新状态之前，状态可以利用IMU的线性加速度和角速度，随时间向前传播。基于[3]中提出的标准惯性运动学模型，可以得到带有协方差${\hat{\mathbf{P}}}_k$ 的状态估计先验值 ${\hat{\mathbf{x}}}_k$。

2) 含噪声的反射率残差

反射率残差的测量噪声有两个来源：激光雷达在 $\mathcal{I}{(}^C{\mathbf{p}}_j)$ 中的反射率像素噪声，以及投影模型误差 ${}^C{\mathbf{p}}_j$：
$$\mathcal{I}{(}^C{\mathbf{p}}_j)={\mathcal{I}}^{gt}{(}^C{\mathbf{p}}_j)+n_{r_j},n_{r_j}\sim \mathcal{N}(0,{\Sigma }_{n_{r_j}})\text{\hspace{1em}(9)}$$
$${}^C{\mathbf{p}}_j{=}^C{\mathbf{p}}_j^{gt}+{\mathbf{n}}_{{\Pi }_j},{\mathbf{n}}_{{\Pi }_j}\sim \mathcal{N}(0,{\Sigma }_{{\mathbf{n}}_{{\Pi }_j}})\text{\hspace{1em}(10)}$$
其中，${\mathcal{I}}^{gt}$ 和 ${}^C{\mathbf{p}}_j^{gt}$ 分别是 $\mathcal{I}$ 和 ${}^C{\mathbf{p}}_j$ 的真实值，${}^C{\mathbf{p}}_j$ 是从 G p j N ∈ P C = { p 1 , … , p m C } ^G\mathbf{p}_j^N \in \mathcal{P}_C = \{\mathbf{p}_1, \ldots, \mathbf{p}_{m_C}\} GpjN​∈PC​={ p1​,…,pmC​​} 投影得到的。

为了推导，反射率残差 ${\mathbf{z}}_C$ 被重新定义为函数形式 $h_C({\hat{\mathbf{x}}}_k,\mathcal{I}{,}^C{\mathbf{p}}_j)$。然后，我们得到一阶泰勒展开（假设真实残差为零）：
$$\begin{array}{rl}0& =h_C({\mathbf{x}}_k,{\mathcal{I}}^{gt}{,}^C{\mathbf{p}}_j^{gt})=h_C({\hat{\mathbf{x}}}_k⊞{\tilde{\mathbf{x}}}_k,\mathcal{I}{,}^C{\mathbf{p}}_j)+{\mathbf{\eta }}_j^C\approx h_C({\hat{\mathbf{x}}}_k,\mathcal{I}{,}^C{\mathbf{p}}_j)+{\mathbf{H}}_j^C{\tilde{\mathbf{x}}}_k+{\mathbf{\eta }}_j^C,{\mathbf{\eta }}_j^C\sim \mathcal{N}(0,{\mathbf{\Sigma }}_{{\mathbf{\eta }}_j^C})\end{array}\text{\hspace{1em}(11)}$$
其中
$$\begin{array}{rl}{\mathbf{H}}_j^C& ={\frac{\partial h_C({\hat{\mathbf{x}}}_k⊞{\tilde{\mathbf{x}}}_k,\mathcal{I}{,}^C{\mathbf{p}}_j)}{\partial {\tilde{\mathbf{x}}}_k}|}_{{\tilde{\mathbf{x}}}_k=0}={\frac{\partial \mathcal{I}{[}^C{\mathbf{p}}_j]}{{\partial }^C{\mathbf{p}}_j}\cdot \frac{\partial \Pi {(}^{L_{{\rho }_j}}{\mathbf{p}}_j^N)}{{\partial }^{L_{{\rho }_j}}{\mathbf{p}}_j^N}\cdot \frac{{\partial }^{L_{{\rho }_j}}{\mathbf{p}}_j^N}{\partial {\tilde{\mathbf{x}}}_k}|}_{{\tilde{\mathbf{x}}}_k=0}\end{array}\text{\hspace{1em}(12)}$$
$$\frac{\partial \Pi {(}^{L_{{\rho }_j}}{\mathbf{p}}_j^N)}{{\partial }^{L_{{\rho }_j}}{\mathbf{p}}_j^N}=\left[\begin{array}{cc}\frac{w}{2\pi }& 0\\ 0& \frac{h}{f}\end{array}\right]\left[\begin{array}{ccc}\frac{y}{L^2}& \frac{x}{L^2}& 0\\ \frac{xz}{L^2{R}^2}& \frac{yz}{L^2{R}^2}& -\frac{L}{R^2}\end{array}\right]\text{\hspace{1em}(13)}$$
$$\frac{{\partial }^{L_{{\rho }_i}}{\mathbf{p}}_j^N}{\partial {\tilde{\mathbf{x}}}_k}={(}^I{\mathbf{R}}_L{}^{L_k}{\mathbf{R}}_{L_{{\rho }_i}}{)}^T\left[\begin{array}{cc}{-}^G{\hat{\mathbf{R}}}_{I_k}^T& \\ & {}^G{\hat{\mathbf{R}}}_{I_k}^T{}^G{\mathbf{p}}_j^N\end{array}\right]\times \left[\begin{array}{c}{0}_{3\times 12}\end{array}\right]\text{\hspace{1em}(14)}$$
$${\mathbf{\Sigma }}_{{\mathbf{\eta }}_j^C}={\mathbf{\Sigma }}_{n_{r_j}}+{\mathbf{F}}_{{\mathbf{n}}_{{\Pi }_j}}{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\Pi }_j}}{\mathbf{F}}_{{\mathbf{n}}_{{\Pi }_j}}^T,{\mathbf{F}}_{{\mathbf{n}}_{{\Pi }_j}}=\frac{\partial h_C({\hat{\mathbf{x}}}_k,\mathcal{I}{,}^C{\mathbf{p}}_j)}{{\partial }^C{\mathbf{p}}_j}\text{\hspace{1em}(15)}$$
其中 ${\tilde{\mathbf{x}}}_k={\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k$，斜对称矩阵表示为 $[\cdot {]}_{\times }$，$L=\sqrt{x^2+y^2}$，$\frac{\partial \mathcal{I}{[}^C{\mathbf{p}}_j]}{{\partial }^C{\mathbf{p}}_j}$ 是可以直接从相邻像素计算得到的图像梯度。

3) 含噪声的几何残差

点到平面几何残差的测量噪声主要来自激光雷达测距和光束方向噪声 ${\mathbf{n}}_{{\rho }_i}$，当测量点 L ρ i p i ∈ P S = { p 1 , … , p m S } ^{L_{\rho_i}}\mathbf{p}_i \in \mathcal{P}_S = \{\mathbf{p}_1, \ldots, \mathbf{p}_{m_S}\} Lρi​​pi​∈PS​={ p1​,…,pmS​​} 时，即：
$${}^{L_{{\rho }_i}}{\mathbf{p}}_i{=}^{L_{{\rho }_i}}{\mathbf{p}}_i^{gt}+{\mathbf{n}}_{{\rho }_i},{\mathbf{n}}_{{\rho }_i}\sim \mathcal{N}(0,{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\rho }_i}})\text{\hspace{1em}(16)}$$
其中，${}^{L_{{\rho }_i}}{\mathbf{p}}_i^{gt}$ 是 ${}^{L_{{\rho }_i}}{\mathbf{p}}_i$ 的真实值。为了推导，几何残差 ${\mathbf{z}}_S$ 被重新定义为函数形式 $h_S({\hat{\mathbf{x}}}_k{,}^{L_{{\rho }_i}}{\mathbf{p}}_i)$。真实值 ${}^{L_{{\rho }_i}}{\mathbf{p}}_i^{gt}$ 和 ${\mathbf{x}}_k$ 会导致真实残差为零，其一阶泰勒展开为：
$$\begin{array}{rl}0& =h_S({\mathbf{x}}_k{,}^{L_{{\rho }_i}}{\mathbf{p}}_i^{gt})=h_S({\hat{\mathbf{x}}}_k⊞{\tilde{\mathbf{x}}}_k{,}^{L_{{\rho }_i}}{\mathbf{p}}_i)+{\mathbf{\eta }}_i^S\\ & \approx h_S({\hat{\mathbf{x}}}_k{,}^{L_{{\rho }_i}}{\mathbf{p}}_i)+{\mathbf{H}}_i^S{\tilde{\mathbf{x}}}_k+{\mathbf{\eta }}_i^S,{\mathbf{\eta }}_i^S\sim \mathcal{N}(0,{\mathbf{\Sigma }}_{{\mathbf{\eta }}_i^S})\end{array}\text{\hspace{1em}(17)}$$
其中
$$\begin{array}{rl}{\mathbf{H}}_i^S& ={\frac{\partial h_S({\hat{\mathbf{x}}}_k⊞{\tilde{\mathbf{x}}}_k{,}^{L_{{\rho }_i}}{\mathbf{p}}_i)}{\partial {\tilde{\mathbf{x}}}_k}|}_{{\tilde{\mathbf{x}}}_k=0}{=}^G{\mathbf{u}}^T\left[\begin{array}{ccc}{-}^G{\hat{\mathbf{R}}}_{I_k}^T{[}^I{\mathbf{R}}_L{}^{L_k}{\mathbf{p}}_i{+}^I{\mathbf{t}}_L]\times & & \\ & {\mathbf{I}}_{3\times 3}& 0_{3\times 12}\end{array}\right]\end{array}\text{\hspace{1em}(18)}$$
$${\mathbf{\Sigma }}_{{\mathbf{\eta }}_i^S}={\mathbf{F}}_{{\mathbf{n}}_{{\rho }_i}}{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\rho }_i}}{\mathbf{F}}_{{\mathbf{n}}_{{\rho }_i}}^T,{\mathbf{F}}_{{\mathbf{n}}_{{\rho }_i}}=\frac{\partial h_S({\hat{\mathbf{x}}}_k{,}^{L_{{\rho }_i}}{\mathbf{p}}_i)}{{\partial }^{L_{{\rho }_i}}{\mathbf{p}}_i}\text{\hspace{1em}(19)}$$

然后，我们将 ${\mathbf{x}}_k$ 的先验分布与几何和反射率残差分布相结合，以获得 ${\tilde{\mathbf{x}}}_k$ 的最大后验（MAP）估计：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{\ast }& =\underset{{\tilde{\mathbf{x}}}_k}{\mathrm{argmin}}(\Vert {\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k{\Vert }_{{\hat{\mathbf{P}}}_k}+\sum _{j=1}^{m_C}\Vert {\mathbf{z}}_C+{\mathbf{H}}_j^C{\tilde{\mathbf{x}}}_k{\Vert }_{{\mathbf{\Sigma }}_{{\mathbf{\eta }}_j^C}}+\sum _{i=1}^{m_S}\Vert {\mathbf{z}}_S+{\mathbf{H}}_i^S{\tilde{\mathbf{x}}}_k{\Vert }_{{\mathbf{\Sigma }}_{{\mathbf{\eta }}_i^S}})\end{array}\text{\hspace{1em}(20)}$$