FAST-LIVO2 体素地图：八叉树数据结构深度解析

关键思想：将平面参数视为点集的函数 $\boldsymbol{\theta} = f(\mathbf{p}_1, \dots, \mathbf{p}_n)$，其雅可比矩阵 $\mathbf{J}_i = \frac{\partial \boldsymbol{\theta}}{\partial \mathbf{p}_i}$ 可由特征值扰动理论推导。最终：

$$\mathbf{\Sigma}{\boldsymbol{\theta}} = \sum{i=1}^{n} \mathbf{J}i , \mathbf{\Sigma}{\mathbf{p}_i} , \mathbf{J}_i^\top$$

实际代码中，通过遍历每个点，利用特征向量和中心计算每个点对平面参数的雅可比，并累加得到 $\mathbf{\Sigma}_{\boldsymbol{\theta}}$。该协方差用于后续匹配时的不确定性加权。

3. 地图构建与更新流程

地图构建分为两个阶段：初始化插入和在线更新。

3.1 点的插入

当一个新点（包含世界坐标 $\mathbf{p}w$ 和测量协方差 $\mathbf{\Sigma}{\mathbf{p}_b}$，在 body 系下）到来时：

1. 根据世界坐标计算其所属根体素的索引：$(\lfloor x/v \rfloor, \lfloor y/v \rfloor, \lfloor z/v \rfloor)$，其中 $v$ 为根体素大小。
2. 查找或创建对应的根体素节点。
3. 调用该节点的 UpdateOctoTree(pv) 方法将点插入八叉树。

3.2 节点更新逻辑

每个节点根据当前状态处理新点：

• 若节点未初始化：将点存入 temp_points_，当点数超过初始化阈值时，执行 init_octo_tree()：
  • 尝试平面拟合。
  • 若成功（是平面），节点状态设为平面，若点数超过最大点数则禁用后续更新（固定平面）。
  • 若失败（非平面），节点状态设为非平面，并执行 cut_octo_tree() 将当前点集划分到八个子节点，递归初始化子节点。
• 若节点已为平面：如果允许更新（点数未达上限），将点加入 temp_points_，当累积的新点数超过更新阈值时，重新进行平面拟合，更新平面参数。若总点数超过最大点数，则禁用更新（平面固定）。
• 若节点为非平面且未达最大层数：根据点的坐标判断应属于哪个子节点，递归调用子节点的更新函数。若子节点不存在则创建。
• 若节点为非平面且已达最大层数：退化为平面节点处理（即在该层直接进行平面拟合，不再分割）。

该机制保证了地图能够自适应地根据局部点的分布调整分辨率，平面区域用大平面表示，非平面（如角点、边缘）区域细分到更小体素。

4. 点到平面残差的构建

在状态估计中，需要为每个有效激光点找到地图中对应的平面，并计算残差。这一过程在 BuildResidualListOMP 中并行完成。

4.1 对应平面查找

对于世界坐标系下的一个点 $\mathbf{p}_w$：

1. 计算其所在根体素索引，找到对应的根节点。
2. 从根节点开始，递归地调用 find_correspond 向下搜索，直到遇到平面节点或叶子节点（最大层数）。
3. 若当前节点为平面，则检查该点是否在平面范围内（点到平面中心的水平距离小于 $k \cdot r$，$k$ 通常取 3）。
4. 若在范围内，计算点到平面的距离 $d = |\mathbf{n}^\top \mathbf{p}_w + d|$ 和概率权重。
5. 若当前节点非平面但还有子节点，继续向下搜索。
6. 若在根节点内未找到合适平面，尝试相邻体素（若点靠近边界）。

4.2 残差及不确定性加权

设找到的平面对应参数 $\mathbf{n}, \mathbf{c}, d$，且已知平面参数协方差 $\mathbf{\Sigma}{\boldsymbol{\theta}}$ 和点的世界协方差 $\mathbf{\Sigma}{\mathbf{p}_w}$。

点到平面的带符号距离：

$$e = \mathbf{n}^\top \mathbf{p}_w + d$$

该距离的不确定性来源于两部分：平面参数的不确定性和点自身的不确定性。首先，将 $e$ 视为平面参数和点的函数：

$$e = g(\boldsymbol{\theta}, \mathbf{p}_w) = \mathbf{n}^\top \mathbf{p}_w + d$$

其一阶近似方差为：

$$\sigma_e^2 = \underbrace{ \left[ (\mathbf{p}w - \mathbf{c})^\top ; -\mathbf{n}^\top \right] \mathbf{\Sigma}{\boldsymbol{\theta}} \begin{bmatrix} \mathbf{p}w - \mathbf{c} \ -\mathbf{n} \end{bmatrix} }{\text{来自平面参数}} ;+; \underbrace{ \mathbf{n}^\top \mathbf{\Sigma}_{\mathbf{p}w} \mathbf{n} }{\text{来自点的测量}}$$

在状态估计中，该方差用于计算残差的权重（即信息矩阵的逆）。实际代码中，计算：

$$R^{-1} = \frac{1}{\sigma_e^2 + \text{常数}}$$

作为测量噪声的逆协方差，用于后续卡尔曼增益计算。

4.3 匹配概率与最佳平面

在递归搜索过程中，可能会找到多个候选平面（例如父节点和子节点同时满足范围条件）。此时，算法计算每个候选的概率：

$$\text{prob} = \frac{1}{\sigma_e} \exp\left( -\frac{e^2}{2\sigma_e^2} \right)$$

选择概率最大的平面作为匹配对象。这一策略有助于在模糊情况下选择最可靠的平面。

5. 状态估计中的观测模型与 IEKF 更新

在 StateEstimation 函数中，利用构建好的点到平面残差进行迭代卡尔曼滤波更新。

5.1 点的变换与协方差传播

首先，将当前帧的激光点从 body 坐标系变换到世界坐标系。记外参为 $\mathbf{R}\text{ext}, \mathbf{t}\text{ext}$（IMU 到 LiDAR），当前姿态为 $\mathbf{R}, \mathbf{t}$（世界到 IMU？需确认，代码中 state_.rot_end 可能是 IMU 在世界系下的姿态）。点从 body 到世界的过程：

$$\mathbf{p}w = \mathbf{R} (\mathbf{R}\text{ext} \mathbf{p}b + \mathbf{t}\text{ext}) + \mathbf{t}$$

每个点的 body 系测量协方差 $\mathbf{\Sigma}_{\mathbf{p}_b}$ 通过 calcBodyCov 根据测距和角度误差计算。该协方差通过旋转传播到世界系：

$$\mathbf{\Sigma}{\mathbf{p}w} = \mathbf{R} \mathbf{R}\text{ext} \mathbf{\Sigma}{\mathbf{p}b} (\mathbf{R} \mathbf{R}\text{ext})^\top + \underbrace{(-\lfloor \mathbf{R}\text{ext}\mathbf{p}b+\mathbf{t}\text{ext} \rfloor\times) \mathbf{\Sigma}\mathbf{R} (-\lfloor \mathbf{R}\text{ext}\mathbf{p}b+\mathbf{t}\text{ext} \rfloor_\times)^\top}{\text{姿态不确定性贡献}} + \mathbf{\Sigma}\mathbf{t}$$

其中 $\mathbf{\Sigma}\mathbf{R}$ 和 $\mathbf{\Sigma}\mathbf{t}$ 是当前状态协方差中旋转和平移的部分，$\lfloor\cdot\rfloor_\times$ 为反对称矩阵。该公式将点的测量噪声和状态不确定性融合为点的世界协方差，用于残差方差计算。

5.2 观测雅可比矩阵

残差 $e$ 对状态 $\mathbf{x} = [\delta \boldsymbol{\theta}^\top, \delta \mathbf{t}^\top]^\top$（通常使用右扰动）的雅可比推导如下。

首先，点 $\mathbf{p}w$ 依赖于状态。记 $\mathbf{p}{\text{imu}} = \mathbf{R}_\text{ext} \mathbf{p}b + \mathbf{t}\text{ext}$ 为 IMU 坐标系下的点。则：

$$\mathbf{p}w = \mathbf{R} \mathbf{p}{\text{imu}} + \mathbf{t}$$

对旋转的右扰动：$\mathbf{R} \leftarrow \mathbf{R} \exp(\lfloor \delta \boldsymbol{\theta} \rfloor_\times)$，对平移的加性扰动：$\mathbf{t} \leftarrow \mathbf{t} + \delta \mathbf{t}$。

扰动后的点：

$$\mathbf{p}w' \approx \mathbf{R} (\mathbf{I} + \lfloor \delta \boldsymbol{\theta} \rfloor\times) \mathbf{p}{\text{imu}} + \mathbf{t} + \delta \mathbf{t} = \mathbf{p}w + \mathbf{R} \lfloor \delta \boldsymbol{\theta} \rfloor\times \mathbf{p}{\text{imu}} + \delta \mathbf{t}$$

利用 $\lfloor \delta \boldsymbol{\theta} \rfloor_\times \mathbf{p}{\text{imu}} = -\lfloor \mathbf{p}{\text{imu}} \rfloor_\times \delta \boldsymbol{\theta}$，可得：

$$\mathbf{p}w' = \mathbf{p}w - \mathbf{R} \lfloor \mathbf{p}{\text{imu}} \rfloor\times \delta \boldsymbol{\theta} + \delta \mathbf{t}$$

因此，残差 $e = \mathbf{n}^\top \mathbf{p}_w + d$ 对状态的雅可比为：

$$\frac{\partial e}{\partial \delta \boldsymbol{\theta}} = -\mathbf{n}^\top \mathbf{R} \lfloor \mathbf{p}{\text{imu}} \rfloor\times, \quad \frac{\partial e}{\partial \delta \mathbf{t}} = \mathbf{n}^\top$$

代码中构建的 H 矩阵正是如此：Hsub.row(i) << 点反对称矩阵作用于法向量的结果，法向量分量。

5.3 IEKF 更新

每次迭代中，计算所有有效点的残差 $e_i$ 及其权重 $R_i^{-1}$，构建批量观测方程：

$$\mathbf{z} = \mathbf{H} \delta \mathbf{x} + \mathbf{v}, \quad \mathbf{v} \sim \mathcal{N}(0, \mathbf{R})$$

其中 $\mathbf{H}$ 的行对应每个点的雅可比，$\mathbf{R}$ 为对角矩阵（对角线为各点的 $\sigma_{e_i}^2$）。

采用 IEKF 更新公式：

$$\mathbf{K} = (\mathbf{H}^\top \mathbf{R}^{-1} \mathbf{H} + \mathbf{P}^{-1})^{-1} \mathbf{H}^\top \mathbf{R}^{-1}$$

$$\delta \mathbf{x} = \mathbf{K} (\mathbf{z} - \mathbf{H} \mathbf{x}_{\text{prior}})$$

但由于迭代中状态不断更新，通常使用以下等价形式（避免显式计算 K）：

$$\delta \mathbf{x} = (\mathbf{H}^\top \mathbf{R}^{-1} \mathbf{H} + \mathbf{P}^{-1})^{-1} \left( \mathbf{H}^\top \mathbf{R}^{-1} \mathbf{z} + \mathbf{P}^{-1} (\mathbf{x}{\text{prop}} - \mathbf{x}{\text{curr}}) \right)$$

其中 $\mathbf{x}{\text{curr}}$ 为当前迭代状态，$\mathbf{x}{\text{prop}}$ 为 IMU 传播的先验状态。更新后叠加到当前状态：

$$\mathbf{x}{\text{curr}} \leftarrow \mathbf{x}{\text{curr}} + \delta \mathbf{x}$$

迭代直到收敛（旋转增量小于 0.01°，平移增量小于 0.015 m）或达到最大迭代次数。收敛后，更新状态协方差：

$$\mathbf{P} \leftarrow (\mathbf{I} - \mathbf{K}\mathbf{H}) \mathbf{P} \approx (\mathbf{I} - \mathbf{G}) \mathbf{P}$$

其中 $\mathbf{G} = (\mathbf{H}^\top \mathbf{R}^{-1} \mathbf{H} + \mathbf{P}^{-1})^{-1} \mathbf{H}^\top \mathbf{R}^{-1} \mathbf{H}$。

6. 总结

FAST-LIVO2 的八叉树地图模块通过递归空间划分、局部平面拟合以及不确定性传播，构建了一个概率性的平面地图。在状态估计中，利用点到平面的距离作为观测，结合一阶误差传播计算权重，在 IEKF 框架下优化状态。这种设计既保证了地图的紧凑性（用平面表示大量点），又充分利用了不确定性信息，提高了定位的鲁棒性和精度。