FAST-LIVO2中的点到平面残差计算与OpenMP优化实践

1. FAST-LIVO2框架中的点到平面残差原理

在FAST-LIVO2这个多传感器融合框架中，点到平面残差（Point-to-Plane Residual）是LiDAR数据处理的核心数学工具。其本质是通过计算激光点与对应局部平面之间的垂直距离，来量化当前位姿估计的准确度。具体实现时，系统会先对LiDAR点云进行体素八叉树（Voxel Octree）组织，快速建立环境的三维结构表征。

对于每个激光点p，算法会在其邻近体素中拟合一个局部平面π（通常采用PCA主成分分析）。平面方程表示为ax+by+cz+d=0，其中n=[a,b,c]^T是单位法向量。当传感器位姿为T时，点p的残差计算式为：

r = n^T·(T⊗p) + d

这个残差值的物理意义非常直观——它表示变换后的激光点偏离局部平面的垂直距离。在FAST-LIVO2的ESIKF（Error State Iterative Kalman Filter）优化框架中，此类残差会与视觉重投影误差、IMU预积分约束共同构成目标函数。

2. BuildResidualListOMP函数架构解析

BuildResidualListOMP是FAST-LIVO2中实现并行化残差计算的关键函数，其名称中的OMP明确指示使用了OpenMP并行技术。该函数的典型工作流程可分为三个阶段：

2.1 数据准备阶段

函数首先会检查输入点云的时空一致性，过滤掉无效点（如NaN值或超出量程的点）。同时从体素八叉树中预加载邻近体素数据，这个步骤充分利用了CPU缓存局部性原理。在实际测试中，合理设置体素加载半径能使计算效率提升30%以上。

2.2 并行计算阶段

通过OpenMP的parallel for指令实现多线程并行：

cpp复制#pragma omp parallel for num_threads(4)
for(size_t i=0; i<points.size(); ++i){
    // 每个线程独立处理一部分点云
    auto& plane = octree.findNearestPlane(points[i]);
    residuals[i] = computePointToPlaneDistance(points[i], plane);
}

这里需要注意线程数的设置