C++AI算法
FAST-LIVO2 体素地图：八叉树数据结构深度解析
本文档详细推导了 FAST-LIVO2 中基于八叉树的体素地图构建与更新原理。内容包括八叉树节点结构、点云协方差特征分解与平面拟合、平面参数不确定性传播、地图插入与自适应更新流程、点到平面残差构建及权重计算，以及在迭代扩展卡尔曼滤波（IEKF）中的观测模型与状态更新方法。该设计通过递归空间划分和局部平面拟合实现紧凑地图表示，利用不确定性信息提高定位鲁棒性。
本文档详细推导了 FAST-LIVO2 中基于八叉树的体素地图构建与更新原理。内容包括八叉树节点结构、点云协方差特征分解与平面拟合、平面参数不确定性传播、地图插入与自适应更新流程、点到平面残差构建及权重计算，以及在迭代扩展卡尔曼滤波（IEKF）中的观测模型与状态更新方法。该设计通过递归空间划分和局部平面拟合实现紧凑地图表示，利用不确定性信息提高定位鲁棒性。
本文档详细推导 FAST-LIVO2 中基于八叉树的体素地图（VoxelMap）的构建、更新及其在激光–惯性–视觉里程计中用于状态估计的数学原理。该地图将三维空间递归划分为不同分辨率的体素，每个体素内拟合局部平面，并提供点到平面的残差及其不确定性，用于迭代扩展卡尔曼滤波（IEKF）的更新。
地图由一系列根体素（root voxel）组成，每个根体素对应一个固定大小的空间立方体（大小为 voxel_size）。每个根体素内部递归地构建一棵八叉树，树的每个节点 VoxelOctoTree 包含以下关键信息：
layer：根节点为 0，子节点层数递增，最大层数 max_layer_ 限制树的深度。voxel_center_：该节点所代表立方体的几何中心。quater_length_：立方体边长的一半（即从中心到边界的距离），子节点边长为父节点的一半。octo_state_：标识该节点是否为平面（0）或非平面（1）。非平面节点将继续向下划分。plane_ptr_：指向一个 VoxelPlane 结构，存储拟合的平面参数（法向量、中心、协方差等）。temp_points_：存储尚未触发平面拟合或节点分割的原始点（包含坐标和协方差）。leaves_[8]：指向八个子节点。当某个节点内的点数超过预设阈值 points_size_threshold_ 时，触发平面拟合，计算该节点内所有点的平面参数及其不确定性。
设节点内有 n 个点，每个点的世界坐标为 $\mathbf{p}_i$。首先计算点集的中心：
$$\bar{\mathbf{p}} = \frac{1}{n}\sum_{i=1}^{n} \mathbf{p}_i$$
然后计算协方差矩阵：
$$\mathbf{C} = \frac{1}{n}\sum_{i=1}^{n} (\mathbf{p}_i - \bar{\mathbf{p}})(\mathbf{p}_i - \bar{\mathbf{p}})^\top$$
对 $\mathbf{C}$ 进行特征值分解：
$$\mathbf{C} = \mathbf{V} \mathbf{\Lambda} \mathbf{V}^\top, \quad \mathbf{\Lambda} = \mathrm{diag}(\lambda_1, \lambda_2, \lambda_3), \quad \lambda_1 \le \lambda_2 \le \lambda_3$$
对应的特征向量为 $\mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3$。
若最小特征值 $\lambda_1$ 小于阈值 planer_threshold_，则认为这些点构成一个平面。此时：
平面参数 $\boldsymbol{\theta} = [\mathbf{n}^\top, \mathbf{c}^\top]^\top \in \mathbb{R}^6$ 的不确定性来源于每个点的测量噪声。假设每个点 $\mathbf{p}i$ 的测量协方差为 $\mathbf{\Sigma}{\mathbf{p}i}$（由激光雷达测距和角度误差传播得到），则平面参数协方差 $\mathbf{\Sigma}{\boldsymbol{\theta}}$ 可通过一阶线性传播近似。
关键思想：将平面参数视为点集的函数 $\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}}$。该协方差用于后续匹配时的不确定性加权。
地图构建分为两个阶段：初始化插入和在线更新。
当一个新点（包含世界坐标 $\mathbf{p}w$ 和测量协方差 $\mathbf{\Sigma}{\mathbf{p}_b}$，在 body 系下）到来时：
UpdateOctoTree(pv) 方法将点插入八叉树。每个节点根据当前状态处理新点：
temp_points_，当点数超过初始化阈值时，执行 init_octo_tree()：
cut_octo_tree() 将当前点集划分到八个子节点，递归初始化子节点。temp_points_，当累积的新点数超过更新阈值时，重新进行平面拟合，更新平面参数。若总点数超过最大点数，则禁用更新（平面固定）。该机制保证了地图能够自适应地根据局部点的分布调整分辨率，平面区域用大平面表示，非平面（如角点、边缘）区域细分到更小体素。
在状态估计中，需要为每个有效激光点找到地图中对应的平面，并计算残差。这一过程在 BuildResidualListOMP 中并行完成。
对于世界坐标系下的一个点 $\mathbf{p}_w$：
find_correspond 向下搜索，直到遇到平面节点或叶子节点（最大层数）。设找到的平面对应参数 $\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{常数}}$$
作为测量噪声的逆协方差，用于后续卡尔曼增益计算。
在递归搜索过程中，可能会找到多个候选平面（例如父节点和子节点同时满足范围条件）。此时，算法计算每个候选的概率：
$$\text{prob} = \frac{1}{\sigma_e} \exp\left( -\frac{e^2}{2\sigma_e^2} \right)$$
选择概率最大的平面作为匹配对象。这一策略有助于在模糊情况下选择最可靠的平面。
在 StateEstimation 函数中，利用构建好的点到平面残差进行迭代卡尔曼滤波更新。
首先，将当前帧的激光点从 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$ 为反对称矩阵。该公式将点的测量噪声和状态不确定性融合为点的世界协方差，用于残差方差计算。
残差 $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) << 点反对称矩阵作用于法向量的结果，法向量分量。
每次迭代中，计算所有有效点的残差 $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}$。
FAST-LIVO2 的八叉树地图模块通过递归空间划分、局部平面拟合以及不确定性传播，构建了一个概率性的平面地图。在状态估计中，利用点到平面的距离作为观测，结合一阶误差传播计算权重，在 IEKF 框架下优化状态。这种设计既保证了地图的紧凑性（用平面表示大量点），又充分利用了不确定性信息，提高了定位的鲁棒性和精度。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML 转 Markdown 互为补充。 在线工具，Markdown 转 HTML在线工具，online