GenZ-ICP 中两次 TBB 并行与 H、g 构建核心解析

1. 整条 GenZ-ICP 匹配链路GenZ-ICP 的核心入口是Registration::RegisterFrame()。它接收当前一帧 LiDAR 点云、体素哈希地图和初始位姿然后通过多轮 ICP 迭代求出当前帧相对于地图的最终位姿。整条链路可以概括为当前一帧点云 frame ↓ 复制得到 source ↓ 用初始位姿把 source 变换到地图坐标系 ↓ 进入 ICP 迭代 ↓ 第一次 TBB 在体素哈希地图中寻找对应点 并判断平面/非平面 ↓ 得到两组对应关系数组 ↓ 计算自适应权重 α ↓ 第二次 TBB 计算平面与非平面残差、雅可比 并累加局部 H、g ↓ 归并得到全局 H、g ↓ 求解 HΔx -g ↓ 将 Δx 转成 SE(3) 位姿增量 ↓ 更新 source 和累计位姿 ↓ 检查是否收敛官方实现确实在每轮迭代中依次调用GetCorrespondences()、计算alpha、调用BuildLinearSystem()、求解dx、更新点云和累计位姿。2. 当前一帧点云最开始存在哪里RegisterFrame()接收的当前帧点云是const std::vectorEigen::Vector3d frame这就是一帧点云最原始的存储形式。可以把它理解为一个连续的三维点数组frame[0] [x₀, y₀, z₀]ᵀ frame[1] [x₁, y₁, z₁]ᵀ frame[2] [x₂, y₂, z₂]ᵀ ... frame[N-1] [xₙ₋₁, yₙ₋₁, zₙ₋₁]ᵀ这里每个元素只有三维坐标。原始frame中并没有保存is_planar is_non_planar normal target_point residual Jacobian也就是说原始一帧点云并没有提前标记哪些点是平面点、哪些点是非平面点。3.frame和source有什么区别代码不会直接修改传入的frame而是先复制一份std::vectorEigen::Vector3d source frame; TransformPoints(initial_guess, source);其中frame 当前帧的原始点云。 source 当前真正参与 ICP 匹配的点云副本。source会先通过初始位姿变换到地图坐标系。点变换可以理解为p_map R_initial · p_lidar t_initial变量含义p_lidar 原始 frame 中的 LiDAR 点。 R_initial 初始位姿中的旋转。 t_initial 初始位姿中的平移。 p_map 根据当前位姿预测后 该点在地图坐标系中的位置。之后每求出一次新的位姿增量程序还会继续更新source。因此下一轮 ICP 查询对应点时用的是已经更新后的source而不是重新直接遍历原始frame。4. 为什么必须把当前帧变换到地图坐标系体素哈希地图中的历史地图点已经处于地图坐标系。如果当前帧点还在 LiDAR 自身坐标系那么二者不能直接计算距离。只有把当前点变到地图坐标系后才能执行当前帧点 source[i] 与 地图点 map_point ↓ 计算欧氏距离 ↓ 寻找最近邻需要注意变换后的source只是当前位姿下的预测位置并不一定正确。如果当前位姿存在误差那么source和地图之间就会存在偏差。ICP 正是根据这些偏差构造残差求解位姿增量再逐步把当前帧对齐到地图。5.tbb是什么TBBoneAPI Threading Building Blocks是一个面向 C 的多线程并行计算库它不是点云算法也不是单独的线程或进程而是负责管理线程池、拆分任务、调度 CPU 核心和归并结果。程序只需要告诉 TBB“索引范围是什么、每个索引怎么处理、局部结果怎么合并”TBB 就会把大区间动态拆成多个小任务并分配给多个工作线程执行。例如在 GenZ-ICP 中第一次 TBB 可以把当前帧点云source的索引范围[0, source.size())拆分后并行查询地图对应点第二次 TBB 可以把所有平面和非平面对应关系构成的虚拟索引范围拆分后并行计算各任务的局部 HHH 和 ggg最后通过parallel_reduce合并成全局线性系统。它的主要作用是利用多核 CPU 加速大量相互独立的点计算同时避免开发者手动创建线程和管理负载均衡。下面给你一个带详细注释的 TBBparallel_reduce示例。它模拟 GenZ-ICP 中“把点云索引划分成多个任务每个任务计算局部 H、gH、gH、g最后归并为全局 H、gH、gH、g”的过程。#include tbb/blocked_range.h #include tbb/parallel_reduce.h #include Eigen/Dense #include cstddef #include iostream #include vector // // 定义线性系统的局部/全局计算结果 // // H6×6 的近似 Hessian 矩阵 // g6×1 的梯度向量 // // 在线性最小二乘中最终要解 // // H * delta_x -g // // 其中 delta_x 是 6 自由度位姿增量 // // delta_x // [delta_tx, delta_ty, delta_tz, // delta_rx, delta_ry, delta_rz]^T // struct LinearSystem { // 6×6 Hessian 矩阵初始化为全 0 Eigen::Matrixdouble, 6, 6 H Eigen::Matrixdouble, 6, 6::Zero(); // 6×1 梯度向量初始化为全 0 Eigen::Matrixdouble, 6, 1 g Eigen::Matrixdouble, 6, 1::Zero(); }; // // 使用 TBB 并行构建 H 和 g // // 参数 source // 当前需要处理的一组点云。 // 在实际 GenZ-ICP 中它可能是 // // 1. 当前完整 source 点云 // 2. 平面/非平面对应关系构成的虚拟索引集合。 // // 这里为了演示 TBB只直接遍历 source 点云。 // LinearSystem BuildLinearSystemParallel( const std::vectorEigen::Vector3d source) { // parallel_reduce 会完成三件事 // // 1. 把大索引区间拆成多个小任务 // 2. 每个任务分别计算自己的局部 H 和 g // 3. 把所有任务的局部 H、g 归并成最终结果。 return tbb::parallel_reduce( // // 第一部分定义需要并行处理的索引范围 // // source 中共有 source.size() 个点。 // 处理范围为 // // [0, source.size()) // // 这是一个左闭右开的区间。 // // 假设 source.size() 8000则处理 // // source[0] ~ source[7999] // // 最后的 256 是 grain size表示建议的任务粒度。 // 它不是严格规定每个任务必须处理 256 个点 // 只是告诉 TBB 不要把任务无限拆得过小。 // tbb::blocked_rangestd::size_t( 0, source.size(), 256), // // 第二部分每个任务的初始局部结果 // // TBB 为任务准备一个局部 LinearSystem。 // 其中 // // local.H 初始为 0 // local.g 初始为 0 // // 每个并行任务只修改自己的 local.H 和 local.g // 不直接修改共享的全局矩阵因此不需要每个点都加锁。 // LinearSystem{}, // // 第三部分每个 TBB 任务实际执行的计算 // // range // 当前任务负责的索引区间。 // // local // 当前任务自己的局部 H 和 g。 // // 例如某个任务可能收到 // // range [1000, 1500) // // 那么它只处理 // // source[1000] ~ source[1499] // [](const tbb::blocked_rangestd::size_t range, LinearSystem local) { // 遍历当前任务负责的点云索引 for (std::size_t i range.begin(); i range.end(); i) { // -------------------------------------------- // 读取当前点 // // 使用 const 引用不复制 Eigen::Vector3d。 // // 多个线程共享 source 数组进行只读访问 // 只要没有其他线程同时修改 source // 这种读取就是线程安全的。 // -------------------------------------------- const Eigen::Vector3d point source[i]; // -------------------------------------------- // 以下只是为了展示 H、g 的构造流程。 // // 实际 GenZ-ICP 中这里应该根据点类型计算 // // 平面对应关系 // r n^T(p-q) // J [n^T, (p×n)^T] // // 非平面对应关系 // r p-q // J [I, -[p]×] // // 这里用 point.norm() 构造一个简单标量残差。 // -------------------------------------------- const double residual point.norm(); // -------------------------------------------- // 构造一个 1×6 的示例雅可比 // // 实际平面残差的雅可比也是 1×6。 // // 这里前 3 项简单设为 1 // 后 3 项使用点坐标只用于演示。 // // 注意这不是实际 ICP 的正确雅可比 // 只是为了说明 TBB 如何累加 H 和 g。 // -------------------------------------------- Eigen::Matrixdouble, 1, 6 J; J 1.0, 1.0, 1.0, point.x(), point.y(), point.z(); // -------------------------------------------- // 当前残差权重 // // 实际工程中可以由以下因素共同决定 // // 1. 鲁棒核权重 // 2. 最近邻距离 // 3. 平面可靠性 // 4. 匹配置信度 // 5. 测量协方差。 // // 此处为了演示统一设置为 1。 // -------------------------------------------- const double weight 1.0; // -------------------------------------------- // 当前点对局部 Hessian 的贡献 // // 公式 // // H_i J_i^T * w_i * J_i // // 尺寸 // // J_i^T6×1 // w_i标量 // J_i1×6 // // 最终 // // H_i6×6 // // local.H 是当前任务私有矩阵 // 所以这里不会和其他线程产生写冲突。 // -------------------------------------------- local.H J.transpose() * weight * J; // -------------------------------------------- // 当前点对局部梯度的贡献 // // 公式 // // g_i J_i^T * w_i * r_i // // 尺寸 // // J_i^T6×1 // w_i标量 // r_i标量 // // 最终 // // g_i6×1 // -------------------------------------------- local.g J.transpose() * weight * residual; } // 当前任务完成后返回自己的局部 H 和 g。 // // 例如当前任务处理了 500 个点则 // // local.H H_1 H_2 ... H_500 // // local.g g_1 g_2 ... g_500 return local; }, // // 第四部分归并两个任务的局部结果 // // left // 一个任务或一组任务已经计算出的局部结果。 // // right // 另一个任务或另一组任务的局部结果。 // // TBB 会按照树形方式不断调用该函数 // // 任务1 任务2 // 任务3 任务4 // 前两个结果再相加 // // 最终形成全局 H 和 g。 // [](const LinearSystem left, const LinearSystem right) { LinearSystem merged; // 两个任务的局部 Hessian 直接相加 merged.H left.H right.H; // 两个任务的局部梯度直接相加 merged.g left.g right.g; return merged; }); } int main() { // // 构造一组示例点云 // // 实际工程中这里应来自当前一帧 LiDAR 点云 // 或者已经建立好的对应关系数组。 // std::vectorEigen::Vector3d source; source.emplace_back(1.0, 2.0, 3.0); source.emplace_back(2.0, 1.0, 0.5); source.emplace_back(3.0, 2.0, 1.0); source.emplace_back(0.5, 1.5, 2.5); // 调用 TBB 并行函数得到归并后的全局 H 和 g const LinearSystem result BuildLinearSystemParallel(source); // 打印最终全局 Hessian std::cout H \n result.H \n\n; // 打印最终全局梯度 std::cout g \n result.g \n\n; // // 求解线性方程 // // H * delta_x -g // // LDLT 适合求解对称线性系统。 // // 实际工程中求解前还应检查 // // 1. H、g 是否包含 NaN 或 Inf // 2. H 是否退化 // 3. 有效对应点数量是否足够 // 4. 求出的 delta_x 是否异常大。 // const Eigen::Matrixdouble, 6, 1 delta_x result.H.ldlt().solve(-result.g); std::cout delta_x \n delta_x \n; return 0; }这段代码执行时TBB 会把[0, source.size())拆成多个任务。每个任务只处理自己区间中的点独立计算局部H和g任务完成后再通过最后一个归并函数把所有局部结果相加。其核心数据流为source 点云索引范围 ↓ blocked_range 划分任务 ↓ 任务 1局部 H₁、g₁ 任务 2局部 H₂、g₂ 任务 3局部 H₃、g₃ ↓ parallel_reduce 归并 ↓ H_global H₁ H₂ H₃ g_global g₁ g₂ g₃ ↓ 求解 H_global · delta_x -g_global需要特别注意上面代码重点用于解释 TBB 的区间划分、局部累加和结果归并。里面的residual和J是简化示例不是 GenZ-ICP 中真正的点到平面或点到点计算公式。6. 第一次 TBB 处理的是什么第一次 TBB 出现在voxel_map.GetCorrespondences( source, max_correspondence_distance );这里传入GetCorrespondences()的points实际就是当前迭代中的source。内部并行范围是tbb::blocked_rangesize_t( 0, points.size() );假设当前帧有 8000 个点则处理范围为[0, 8000)TBB 不会把 8000 个点复制成多个子点云而只是把索引范围拆成多个任务。例如可能形成任务 A[0, 1500) 任务 B[1500, 3000) 任务 C[3000, 5000) 任务 D[5000, 6500) 任务 E[6500, 8000)每个任务共享同一个只读points/source数组但只处理自己负责的索引for (size_t i range.begin(); i ! range.end(); i) { const Eigen::Vector3d point points[i]; }所以这里是 TBB 多线程任务并行不是多个进程也不是把点云物理切成多个独立数组。官方代码使用的正是parallel_reduce和[0, points.size())的索引范围。6. 每个当前帧点如何查询体素哈希地图每个任务取到一个当前帧点以后会调用GetClosestNeighbor(point);该函数首先计算当前点所在的体素位置然后查询当前体素及周围体素。官方代码中定义了 27 个体素偏移量也就是当前体素 (0, 0, 0) 以及其周围 x、y、z 三个方向组合形成的 26 个邻近体素查询过程可以概括为当前点 point ↓ 计算体素索引 ↓ 查询当前体素及周围 26 个体素 ↓ 遍历这些体素中的地图点 ↓ 寻找距离最近的地图点 ↓ 同时累计邻域质心和协方差对于每个邻域地图点neighbor代码计算double squared_distance (neighbor - query).squaredNorm();然后保留距离最小的地图点作为closest_neighbor。与此同时邻域点还会被用于计算质心和协方差。7. 哈希表中是否提前区分平面点和非平面点不会。体素哈希表的主要作用是按照空间位置保存地图点。加入地图时只会计算点所在的体素auto voxel Voxel((point / voxel_size_).castint());然后把该点加入对应的VoxelBlock。因此哈希地图中的结构可以理解为体素哈希地图 │ ├── 体素 A │ ├── 地图点 1 │ ├── 地图点 2 │ └── 地图点 3 │ ├── 体素 B │ ├── 地图点 1 │ └── 地图点 2 │ └── 体素 C └── 地图点 1这些地图点没有永久的“平面点”或“非平面点”标签。更准确的说法是程序在当前帧点查询到一组局部地图邻域以后临时判断这一组邻域是否具有平面结构。所以判断的是当前源点对应的局部地图结构而不是读取哈希表中某个点的固定类别。8. 协方差和特征值如何判断平面假设在当前点附近找到了 KKK 个地图邻域点。程序先计算质心和协方差局部质心 q̄ (1/K) Σ qₖ 局部协方差 C (1/K) Σ(qₖ-q̄)(qₖ-q̄)ᵀ其中qₖ 第 k 个地图邻域点。 q̄ 邻域点的平均位置。 C 描述邻域点在三个空间方向上分布情况的 3×3 矩阵。程序再对协方差矩阵进行特征值分解并按照从小到大取得λ₃最小特征值 λ₂中间特征值 λ₁最大特征值 满足 λ₁ ≥ λ₂ ≥ λ₃平面判断公式为s λ₃ ────────────── λ₁ λ₂ λ₃ 如果 s planarity_threshold 则认为该邻域是平面。最小特征值对应的特征向量被作为局部平面的法向量。代码中使用的确实是最小特征值占总特征值的比例。这里还要注意一个细节这个判据主要检查邻域是否在某一个方向上“很薄”。从数学判据本身推断理想线结构也可能出现最小特征值很小的情况。因此它并不是一个严格完整的“线、面、体”三分类器而是 GenZ-ICP 中用于平面/非平面二分类的简化判据。9. 第一次 TBB 最终输出哪些数组如果当前点距离最近地图点太远if (closest_distance max_correspondence_distance) { continue; }该点直接被丢弃不进入后续优化。如果距离有效则根据邻域数量和平面性进行分类。平面对应关系保存result.source.emplace_back(point); result.target.emplace_back(closest_neighbor); result.normals.emplace_back(normal); result.planar_count;非平面对应关系保存result.non_planar_source.emplace_back(point); result.non_planar_target.emplace_back( closest_neighbor ); result.non_planar_count;因此第一次 TBB 最终生成五个数组平面对应关系 src_planar tgt_planar normals 非平面对应关系 src_non_planar tgt_non_planar其中src_planar[i] 当前帧中的平面源点。 tgt_planar[i] 地图中的对应点。 normals[i] 对应局部平面的法向量。 src_non_planar[j] 当前帧中的非平面源点。 tgt_non_planar[j] 地图中的对应点。这些数组已经不是完整的一帧点云而是从source中筛选出来的有效对应关系。10. 第一次parallel_reduce为什么不会写冲突不同线程不能安全地同时向同一个普通std::vector执行emplace_back()。因此第一次 TBB 为每个任务传递一份局部ResultTuplestruct ResultTuple { Vector3dVector source; Vector3dVector target; Vector3dVector normals; Vector3dVector non_planar_source; Vector3dVector non_planar_target; size_t planar_count 0; size_t non_planar_count 0; };每个任务只向自己的局部数组写数据。任务完成后归并函数执行数组拼接和计数相加result.source.insert(...); result.target.insert(...); result.normals.insert(...); result.non_planar_source.insert(...); result.non_planar_target.insert(...); result.planar_count other.planar_count; result.non_planar_count other.non_planar_count;所以第一次并行过程是完整 source ↓ TBB 划分多个索引任务 ↓ 任务 A 生成自己的平面/非平面数组 任务 B 生成自己的平面/非平面数组 任务 C 生成自己的平面/非平面数组 ↓ parallel_reduce 归并 ↓ 得到完整对应关系数组官方代码确实通过局部ResultTuple和operator归并五个数组及两个计数。11. 自适应权重 α\alphaα 怎么计算获得平面和非平面对应关系数量后代码计算double alpha static_castdouble(planar_count) / static_castdouble( planar_count non_planar_count );也就是平面对应关系数量 α ───────────────────────────── 平面数量 非平面数量因此平面点比例高 α 接近 1。 非平面点比例高 α 接近 0。构建线性系统时平面部分乘以 α 非平面部分乘以 1-α。这体现了 GenZ-ICP 的核心思路同时使用点到平面和点到点误差并根据环境几何结构调整两类误差的总体比例。官方论文也将其描述为结合两种误差度量并通过环境几何特征调整自适应权重。12. 第二次 TBB 使用的到底是哪组点第二次 TBB 位于BuildLinearSystem( src_planar, tgt_planar, normals, src_non_planar, tgt_non_planar, kernel, alpha );此时已经不再直接使用原始 frame 完整 source 体素哈希地图中的全部地图点。它只使用第一次 TBB 生成的有效对应关系数组。平面残差使用src_planar[i] tgt_planar[i] normals[i]非平面残差使用src_non_planar[j] tgt_non_planar[j]所以两个 TBB 的职责非常明确第一次 TBB 寻找、筛选并分类对应关系。 第二次 TBB 基于对应关系计算 H 和 g。13. 为什么要构造虚拟索引范围平面和非平面对应关系实际存放在两个不同数组中。假设src_planar.size() 5000 src_non_planar.size() 3000代码计算size_t total_size src_planar.size() src_non_planar.size();得到total_size 8000然后构造tbb::blocked_rangesize_t( 0, total_size );也就是虚拟范围[0, 8000)该虚拟范围与真实数组的映射为虚拟索引 [0, 5000) ↓ src_planar[04999] 虚拟索引 [5000, 8000) ↓ src_non_planar[02999]程序没有真的创建一个包含 8000 个元素的新数组只是用一个连续索引范围逻辑上连接两组数组。官方实现就是将两个数组长度相加后使用一次parallel_reduce。14. 平面对应关系如何计算残差和雅可比第 iii 个平面对应关系使用点到平面残差r_pl,i (src_planar[i] - tgt_planar[i]) · normals[i]写成数学形式r_pl nᵀ(p-q)变量含义p 当前帧源点。 q 地图对应点。 n 地图局部平面的单位法向量。 p-q 源点到地图对应点的三维误差。 nᵀ(p-q) 三维误差在平面法向方向上的投影。 r_pl 点到平面的标量残差。代码中的雅可比为J_planar.block1, 3(0, 0) normals[i].transpose(); J_planar.block1, 3(0, 3) ( src_planar[i] .cross(normals[i]) ).transpose();因此J_pl [ nᵀ (p×n)ᵀ ] J_pl ∈ R¹ˣ⁶前 3 列对应三维平移后 3 列对应三维旋转。这里六维增量排列是Δx [ Δt_x Δt_y Δt_z Δθ_x Δθ_y Δθ_z ]ᵀ平面残差和雅可比公式直接对应官方Registration.cpp实现。15. 非平面对应关系如何计算残差和雅可比非平面对应关系使用三维点到点残差r_po p - q展开为r_po [ p_x - q_x p_y - q_y p_z - q_z ] r_po ∈ R³代码中的雅可比为J_non_planar.block3, 3(0, 0) Eigen::Matrix3d::Identity(); J_non_planar.block3, 3(0, 3) -Sophus::SO3d::hat( src_non_planar[i] );对应J_po [ I₃×₃ -[p]× ] J_po ∈ R³ˣ⁶其中I₃×₃ 三维单位矩阵表示平移对点坐标的影响。 [p]× 点 p 对应的反对称叉乘矩阵。 -[p]× 表示小角度旋转对点坐标的影响。点到点残差保留三个方向的完整误差而点到平面残差只保留法向方向误差。两种模型分别对应结构化平面区域和局部平面结构不可靠的区域。16. 鲁棒权重如何计算代码定义double kernel_squared kernel * kernel; Weight kernel_squared / square( kernel residual_squared );对应公式k² w ───────────────── (k e²)²其中k 鲁棒核参数 kernel。 e² 残差平方。 w 当前对应关系的鲁棒权重。对于平面点e² r_pl²对于非平面点e² ||r_po||²残差较小时权重较大残差变大时权重下降。这样可以降低错误对应点、动态物体点或噪声点对最终位姿的影响。17. 每个任务如何累加局部 HHH 和 ggg第二次 TBB 的每个任务拥有一个局部结果struct ResultTuple { Eigen::Matrix6d JTJ; Eigen::Vector6d JTr; };其中JTJ 局部 Hessian尺寸 6×6。 JTr 局部梯度尺寸 6×1。平面点的单点贡献为H_pl,i α · J_pl,iᵀ · w_pl,i · J_pl,i g_pl,i α · J_pl,iᵀ · w_pl,i · r_pl,i非平面点的单点贡献为H_po,j (1-α) · J_po,jᵀ · w_po,j · J_po,j g_po,j (1-α) · J_po,jᵀ · w_po,j · r_po,j虽然平面残差是 1 维 非平面残差是 3 维但正规方程构造后两者都变成H_i6×6 g_i6×1所以可以累加到同一个局部JTJ_private和JTr_private中。官方代码分别使用alpha与1-alpha累加两类贡献。18. 一个任务跨越两类点分界时怎么处理假设平面数量 5000 非平面数量 3000 虚拟总范围 [0, 8000)某个 TBB 任务负责[4500, 5500)那么45004999 i 5000 访问 src_planar[45004999] tgt_planar[45004999] normals[45004999] 共 500 个平面对应关系。对于后半部分50005499 i ≥ 5000 实际非平面索引 j i - 5000 访问 src_non_planar[0499] tgt_non_planar[0499] 共 500 个非平面对应关系。该任务最终得到H_task Σ(i4500 到 4999) H_pl,i Σ(j0 到 499) H_po,jg_task Σ(i4500 到 4999) g_pl,i Σ(j0 到 499) g_po,j所以任务内部在计算残差和雅可比时需要区分平面和非平面但是单点贡献计算完成以后二者直接累加到同一个局部 HtaskH_{\text{task}}Htask​ 和 gtaskg_{\text{task}}gtask​ 中。19. 多个任务如何归并并求解位姿假设 TBB 产生四个任务任务 1H₁、g₁ 任务 2H₂、g₂ 任务 3H₃、g₃ 任务 4H₄、g₄归并函数执行result.JTJ left.JTJ right.JTJ; result.JTr left.JTr right.JTr;最终得到H_global H₁ H₂ H₃ H₄g_global g₁ g₂ g₃ g₄从所有对应关系角度看H_global Σ所有平面点 H_pl,i Σ所有非平面点 H_po,jg_global Σ所有平面点 g_pl,i Σ所有非平面点 g_po,j然后统一求解H_global · Δx -g_global代码为const Eigen::Vector6d dx JTJ.ldlt().solve(-JTr);再将六维增量转为 SE(3)const Sophus::SE3d estimation Sophus::SE3d::exp(dx);之后TransformPoints(estimation, source); T_icp estimation * T_icp;也就是使用本轮增量更新当前点云同时把增量累计到最终 ICP 位姿中。当dx.norm()小于收敛阈值或者达到最大迭代次数时停止。20. 核心总结GenZ-ICP 中需要重点区分四种数据。第一种是输入的原始一帧点云frame它只是一个std::vectorEigen::Vector3d每个元素保存一个三维 LiDAR 点没有平面或非平面标签。第二种是source它是frame的副本并根据初始位姿变换到地图坐标系。ICP 每轮得到新的位姿增量后都会继续更新source因此对应点搜索始终基于当前位姿下的点云位置。第三种数据是体素哈希地图。哈希地图按照体素位置保存历史地图点但不会永久标记某个地图点是平面点还是非平面点。第一次 TBB 并行处理完整source的索引范围每个任务取得自己负责的当前帧点在当前体素及周围体素中搜索地图点找到距离最近的地图对应点同时利用邻域点计算质心、协方差和特征值。程序根据最小特征值占总特征值的比例判断局部邻域是否近似平面并把最小特征值对应的特征向量作为法向量。第一次 TBB 并行的结果不是给原始点增加标签而是生成两组新的临时对应关系数组。平面对应关系包括src_planar、tgt_planar、normals非平面对应关系包括src_non_planar、tgt_non_planar。没有找到可靠地图对应点或者对应距离超过阈值的当前帧点会直接被拒绝不进入第二次线性系统构建。每个 TBB 任务拥有自己的局部数组因此不会出现多个线程同时向同一个普通vector写入的问题。任务结束后再通过parallel_reduce把各任务的局部数组和计数归并起来。完成对应关系分类后程序根据平面对应关系数量在全部有效对应关系中的比例计算自适应权重 α\alphaα。平面点比例越高点到平面误差的总体权重越高非平面点比例越高点到点误差的总体权重越高。随后进入第二次 TBB 并行。第二次并行已经不再使用完整原始帧也不再访问哈希地图而是使用第一次并行生成的两类有效对应关系数组。为了用一次parallel_reduce统一处理两类数组程序把“平面数量加非平面数量”构造成一个虚拟连续索引范围。虚拟范围前半段直接映射到平面数组后半段通过减去平面数量映射到非平面数组。TBB 可以把这个虚拟范围拆成任意多个任务因此某个任务可能全部处理平面对应关系也可能全部处理非平面对应关系还可能横跨二者的分界。任务内部会根据当前虚拟索引判断使用哪一套数组和残差模型。平面对应关系使用点到平面残差残差是源点与地图点误差在局部法向量上的投影雅可比尺寸为 1×61\times61×6。非平面对应关系使用三维点到点残差保留三个坐标方向的全部位置误差雅可比尺寸为 3×63\times63×6。虽然两种残差的维度不同但经过 JTWJJ^TWJJTWJ 和 JTWrJ^TWrJTWr 构建正规方程后每个对应关系最终都会生成一个 6×66\times66×6 的 Hessian 贡献和一个 6×16\times16×1 的梯度贡献。因此平面点和非平面点只需要在“读取数组、计算残差、计算雅可比和权重”时区分。得到单点的 HiH_iHi​ 和 gig_igi​ 后就不再需要区分可以直接累加到任务私有的局部矩阵中。每个任务最终保存的是自己负责范围内所有平面和非平面对应关系的综合贡献。所有任务结束后parallel_reduce再把局部 HHH 和 ggg 直接相加形成全局线性系统。最终程序并不是分别用平面点求一个位姿再用非平面点求另一个位姿而是将两类几何约束统一放入同一个线性系统求解同一个六自由度位姿增量。求得增量后通过Sophus::SE3d::exp()转成刚体变换更新source和累计位姿再重新进入下一轮对应点搜索、分类和优化。整个过程的核心可以归纳为第一次 TBB 完整 source → 查询哈希地图 → 建立对应关系 → 分类平面/非平面 第二次 TBB 分类后的对应关系 → 分别计算残差和雅可比 → 累加局部 H、g → 归并全局 H、g 最终 HΔx -g → 求出统一位姿增量 → 更新点云和位姿 → 进入下一轮 ICP