先直接回答你最核心的问题LIO-SAM 的顺序就是先通过“关键帧位姿 KD-Tree”找到附近关键帧的 ID再根据这些 ID 从vector中取出对应关键帧的 Corner / Surf 点云将它们变换到 map 坐标系并拼成局部地图然后针对局部 CornerMap、SurfMap 分别建立 KD-Tree最后让当前帧点去 KD-Tree 中寻找近邻构造点到线、点到面的残差。但要注意一个源码细节严格来说extractNearby()搜索附近关键帧时查询中心是cloudKeyPoses3D-back()也就是最近一个已经保存的关键帧位置并不是当前待优化帧的transformTobeMapped。因为当前帧还没有完成 scan-to-map 优化它的精确位姿还没出来。当前帧和上一关键帧的距离通常不会太远所以用最近关键帧作为局部地图中心是合理的。一、先把 LIO-SAM 中的三类 KD-Tree 分清楚LIO-SAM 在这条 scan-to-map 链路中至少有三棵与当前问题相关的 KD-Tree。KD-Tree输入数据查询对象作用kdtreeSurroundingKeyPoses所有历史关键帧的位置点cloudKeyPoses3D最近关键帧位置找附近有哪些关键帧 IDkdtreeCornerFromMap当前局部 CornerMap当前帧 Corner 点找候选 Corner 邻点后续拟合线kdtreeSurfFromMap当前局部 SurfMap当前帧 Surf 点找候选 Surf 邻点后续拟合平面还有一棵kdtreeHistoryKeyPoses它主要用于回环检测时搜索历史关键帧候选不是当前 scan-to-map 局部匹配的主角。所以不能把所有 KD-Tree 理解成“都在找当前点附近的地图点”。它们的职责不同关键帧位姿 KD-Tree 负责找附近关键帧 ID CornerMap KD-Tree 负责给当前 Corner 点找地图 Corner 邻点 SurfMap KD-Tree 负责给当前 Surf 点找地图 Surf 邻点LIO-SAM 在mapOptmization.cpp中分别定义了关键帧位姿、CornerMap、SurfMap 的 KD-Tree当前帧处理流程依次执行局部关键帧提取、当前帧降采样、scan-to-map 优化与关键帧保存。二、长期保存的数据关键帧点云与关键帧位姿LIO-SAM 不会把所有帧的特征都长期保存。只有关键帧会进入长期地图数据库。核心数据结构可以理解成// 每个元素是一整帧 Corner 点云 std::vectorpcl::PointCloudPointType::Ptr cornerCloudKeyFrames; // 每个元素是一整帧 Surf 点云 std::vectorpcl::PointCloudPointType::Ptr surfCloudKeyFrames; // 所有关键帧的位置点用于 KD-Tree 搜索附近关键帧 pcl::PointCloudPointType::Ptr cloudKeyPoses3D; // 所有关键帧完整六自由度位姿 pcl::PointCloudPointTypePose::Ptr cloudKeyPoses6D;其中第k个关键帧的数据通过相同下标绑定cornerCloudKeyFrames[k] ↓ 第 k 个关键帧的 Corner 点云 surfCloudKeyFrames[k] ↓ 第 k 个关键帧的 Surf 点云 cloudKeyPoses3D[k] ↓ 第 k 个关键帧的三维位置 cloudKeyPoses6D[k] ↓ 第 k 个关键帧的 x、y、z、roll、pitch、yawvector可以理解成自动扩容的数组。它不是直接存一个个 Corner 点而是每个元素保存“一整帧点云对象的指针”。cornerCloudKeyFrames │ ├── [0] → 第 0 个关键帧 Corner 点云 ├── [1] → 第 1 个关键帧 Corner 点云 ├── [2] → 第 2 个关键帧 Corner 点云 └── [k] → 第 k 个关键帧 Corner 点云第k个点云内部又是很多点组成的数组cornerCloudKeyFrames[k] ↓ PointCloudPointType ↓ points[0], points[1], points[2], ...每个PointType本质为pcl::PointXYZI也就是x、y、z、intensity这里的关键帧 Corner / Surf 点云通常保留在该关键帧自身的 LiDAR 局部坐标系中而不是永久存成 map 坐标系点云。后续使用时再根据该关键帧最终优化后的 6DoF 位姿变到 map 坐标系。三、当前帧进入 mapOptimization 后的完整流程当前帧 Corner、Surf 从前端进入mapOptimizationpcl::fromROSMsg(msgIn-cloud_corner, *laserCloudCornerLast); pcl::fromROSMsg(msgIn-cloud_surface, *laserCloudSurfLast);这两个变量只是当前帧临时缓存laserCloudCornerLast 当前帧原始 Corner 点云 laserCloudSurfLast 当前帧原始 Surf 点云主调用顺序是updateInitialGuess(); extractSurroundingKeyFrames(); downsampleCurrentScan(); scan2MapOptimization(); saveKeyFramesAndFactor(); correctPoses();其完整含义是1. IMU / 上一时刻结果给当前帧一个初始位姿 2. 找附近历史关键帧 3. 从附近关键帧构建局部地图 4. 当前帧 Corner / Surf 降采样 5. 当前帧与局部地图匹配 6. 得到优化后的当前帧位姿 7. 判断是否保存为新关键帧 8. 因子图优化后修正历史关键帧位姿当前帧 Corner / Surf 在这里先是临时点云只有经过 scan-to-map 优化并满足关键帧条件后才会进入cornerCloudKeyFrames和surfCloudKeyFrames。四、第一步建立“关键帧位姿 KD-Tree”找到附近关键帧 ID这一部分就是你问的“是不是先找附近关键帧 ID”。答案是是。LIO-SAM 先不碰 Corner 点、Surf 点而是先使用所有关键帧的三维位置kdtreeSurroundingKeyPoses-setInputCloud(cloudKeyPoses3D);其中cloudKeyPoses3D 保存的是所有关键帧的位置点 KeyPose[0] (x0, y0, z0) KeyPose[1] (x1, y1, z1) KeyPose[2] (x2, y2, z2) ...每个位置点的intensity字段会被复用为关键帧编号。例如cloudKeyPoses3D-points[120].intensity 120所以它既是一个三维位置点也带着“我是第几个关键帧”的 ID。然后执行半径搜索kdtreeSurroundingKeyPoses-radiusSearch( cloudKeyPoses3D-back(), surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);这里查询中心 cloudKeyPoses3D-back() 即最近保存的关键帧位置 搜索范围 surroundingKeyframeSearchRadius 输出 pointSearchInd 即附近关键帧在 cloudKeyPoses3D 中的下标集合假设搜索后得到pointSearchInd [118, 119, 120, 121, 122]那么它表示当前局部地图需要使用 第 118 个关键帧 第 119 个关键帧 第 120 个关键帧 第 121 个关键帧 第 122 个关键帧半径搜索实际判断的是┌─────────────────────────────────────────────────────────────┐ │ d_i² (x - x_i)² (y - y_i)² (z - z_i)² │ │ │ │ 若 d_i ≤ R则第 i 个关键帧属于当前局部地图候选关键帧。 │ └─────────────────────────────────────────────────────────────┘其中x、y、z 最近关键帧的位置 x_i、y_i、z_i 第 i 个历史关键帧的位置 R surroundingKeyframeSearchRadius这一步 KD-Tree 的作用不是找 Corner 点或 Surf 点而是从所有历史关键帧里快速找出空间位置靠近当前区域的关键帧 ID。源码先用radiusSearch()返回附近关键帧位置点的下标再将这些位置点加入surroundingKeyPoses。五、为什么找到附近关键帧后还要对关键帧位置降采样找到附近关键帧后源码会执行for (int i 0; i (int)pointSearchInd.size(); i) { int id pointSearchInd[i]; surroundingKeyPoses-push_back(cloudKeyPoses3D-points[id]); } downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses); downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);含义是附近可能有很多关键帧 ↓ 若关键帧位置非常密集 ↓ 全部拿来构建局部地图会重复、耗时 ↓ 先只对“关键帧位置点”做一次体素降采样 ↓ 局部区域内每个体素保留一个代表关键帧注意这里降采样的是关键帧的位置点不是 Corner 点云也不是 Surf 点云。降采样后某个代表位置点可能已经不是原始关键帧位置而是体素内点的组合结果因此不能直接把它当关键帧 ID 使用。源码会再次执行一次 1-NNfor (auto pt : surroundingKeyPosesDS-points) { kdtreeSurroundingKeyPoses-nearestKSearch( pt, 1, pointSearchInd, pointSearchSqDis); pt.intensity cloudKeyPoses3D-points[pointSearchInd[0]].intensity; }这一步非常关键。降采样后的关键帧代表点 pt ↓ 在原始全部关键帧位置 KD-Tree 中找最近的 1 个点 ↓ 找到真实关键帧位置 ↓ 取真实关键帧的 intensity ↓ 恢复该代表点对应的关键帧 ID也就是说这里 KD-Tree 做了第二件事把降采样后的“代表关键帧位置”重新映射回一个真实关键帧 ID。1-NN 搜索对应公式┌─────────────────────────────────────────────────────────────┐ │ i* argmin_i || p - p_i ||² │ │ │ │ 即在全部关键帧位置中找到距离代表点 p 最近的真实关键帧。 │ └─────────────────────────────────────────────────────────────┘其中p 降采样后的代表关键帧位置 p_i 原始第 i 个关键帧位置 i* 最近的真实关键帧 ID此外源码还会把最近约 10 秒内的关键帧加入局部关键帧集合目的是避免机器人原地旋转或缓慢运动时空间半径搜索得到的关键帧数量不足。六、第二步根据关键帧 ID从 vector 中取 Corner / Surf 点云经过前面的搜索和降采样最终得到surroundingKeyPosesDS它里面每个点的intensity都代表一个真实关键帧 ID。例如surroundingKeyPosesDS: point[0].intensity 118 point[1].intensity 120 point[2].intensity 122接下来进入extractCloud(surroundingKeyPosesDS);其核心逻辑是laserCloudCornerFromMap-clear(); laserCloudSurfFromMap-clear(); for (int i 0; i cloudToExtract-size(); i) { int thisKeyInd (int)cloudToExtract-points[i].intensity; auto cornerCloud cornerCloudKeyFrames[thisKeyInd]; auto surfCloud surfCloudKeyFrames[thisKeyInd]; auto pose cloudKeyPoses6D-points[thisKeyInd]; // 用关键帧位姿将局部 Corner / Surf 转到 map 坐标系 auto cornerMap transformPointCloud(cornerCloud, pose); auto surfMap transformPointCloud(surfCloud, pose); // 拼进当前局部地图 *laserCloudCornerFromMap *cornerMap; *laserCloudSurfFromMap *surfMap; }这一段的含义可以写成关键帧 ID 120 ↓ cornerCloudKeyFrames[120] ↓ 取第 120 帧保存的 Corner 点云 surfCloudKeyFrames[120] ↓ 取第 120 帧保存的 Surf 点云 cloudKeyPoses6D[120] ↓ 取第 120 帧最终优化后的 6DoF 位姿 局部 Corner / Surf 点云 ↓ 按该位姿变换到 map 坐标系 ↓ 拼入当前局部地图所以你的理解是完全对的先通过关键帧位姿 KD-Tree 找附近关键帧 ID再拿这些 ID 去cornerCloudKeyFrames[id]和surfCloudKeyFrames[id]中取点最后构建局部地图。源码中的extractCloud()正是通过thisKeyInd intensity取关键帧 ID然后访问cornerCloudKeyFrames[thisKeyInd]、surfCloudKeyFrames[thisKeyInd]并通过cloudKeyPoses6D[thisKeyInd]将点云变换后拼接。七、关键帧局部点怎样变到 map 坐标系假设第k个关键帧保存的一个 Corner 或 Surf 点为┌─────────────────────────────────────────────────────────────┐ │ p_Lk [x_L, y_L, z_L]ᵀ │ │ │ │ 表示该点在第 k 个关键帧自身 LiDAR 坐标系中的坐标。 │ └─────────────────────────────────────────────────────────────┘第k个关键帧的位姿为┌─────────────────────────────────────────────────────────────┐ │ T_ML(k) [ R_k t_k ] │ │ [ 0 1 ] │ │ │ │ R_k第 k 个关键帧的旋转矩阵 │ │ t_k第 k 个关键帧在 map 系中的平移 │ └─────────────────────────────────────────────────────────────┘该点变到 map 坐标系┌─────────────────────────────────────────────────────────────┐ │ p_M R_k · p_Lk t_k │ └─────────────────────────────────────────────────────────────┘展开为┌─────────────────────────────────────────────────────────────┐ │ x_M R₀₀x_L R₀₁y_L R₀₂z_L t_x │ │ y_M R₁₀x_L R₁₁y_L R₁₂z_L t_y │ │ z_M R₂₀x_L R₂₁y_L R₂₂z_L t_z │ └─────────────────────────────────────────────────────────────┘源码中的transformPointCloud()做的就是这个过程cloudOut-points[i].x transCur(0,0) * pointFrom.x transCur(0,1) * pointFrom.y transCur(0,2) * pointFrom.z transCur(0,3); cloudOut-points[i].y transCur(1,0) * pointFrom.x transCur(1,1) * pointFrom.y transCur(1,2) * pointFrom.z transCur(1,3); cloudOut-points[i].z transCur(2,0) * pointFrom.x transCur(2,1) * pointFrom.y transCur(2,2) * pointFrom.z transCur(2,3);因此第 120 个关键帧保存的点云本身不变如果回环或因子图优化改变了第 120 个关键帧的位姿只需要更新cloudKeyPoses6D[120]。下一次使用时点云会自动按照新位姿重新变到正确 map 位置。八、第三步拼成局部 CornerMap 和 SurfMap 后再降采样附近多个关键帧拼接后得到laserCloudCornerFromMap 当前局部 Corner 地图 laserCloudSurfFromMap 当前局部 Surf 地图例如附近关键帧是[118, 120, 122]CornerLocalMap Corner_118^map ∪ Corner_120^map ∪ Corner_122^mapSurfLocalMap Surf_118^map ∪ Surf_120^map ∪ Surf_122^map对应公式┌─────────────────────────────────────────────────────────────┐ │ M_corner ⋃( R_k · C_k t_k ) │ │ k ∈ K_near │ │ │ │ M_surf ⋃( R_k · S_k t_k ) │ │ k ∈ K_near │ └─────────────────────────────────────────────────────────────┘其中C_k 第 k 个关键帧保存的 Corner 点云 S_k 第 k 个关键帧保存的 Surf 点云 K_near 通过关键帧位姿 KD-Tree 找到的附近关键帧 ID 集合拼接完后LIO-SAM 会对这份局部地图再做一次体素降采样downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap); downSizeFilterCorner.filter(*laserCloudCornerFromMapDS); downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap); downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);目的不是丢失几何结构而是减少重复点让后面的 KNN 搜索更快。局部 CornerMap / SurfMap ↓ 体素降采样 ↓ laserCloudCornerFromMapDS laserCloudSurfFromMapDS这两份降采样局部地图才是后续 KD-Tree 的输入。源码还会使用laserCloudMapContainer缓存已经变到 map 坐标系的关键帧 Corner / Surf 点云避免同一个关键帧在短时间内重复执行坐标变换。九、第四步为局部 CornerMap 和 SurfMap 建两棵 KD-Tree局部地图准备好后kdtreeCornerFromMap-setInputCloud( laserCloudCornerFromMapDS); kdtreeSurfFromMap-setInputCloud( laserCloudSurfFromMapDS);这里两棵树的职责明确不同kdtreeCornerFromMap 输入 laserCloudCornerFromMapDS 用途 当前帧 Corner 点找最近 Corner 地图点kdtreeSurfFromMap 输入 laserCloudSurfFromMapDS 用途 当前帧 Surf 点找最近 Surf 地图点注意当前帧 Corner / Surf 点没有放进这两棵 KD-Tree。KD-Tree 里面存的是历史关键帧拼出来的局部地图点。当前帧点只是查询点。所以三棵树的关系是kdtreeSurroundingKeyPoses 先找附近关键帧 ID kdtreeCornerFromMap 再给当前 Corner 点找附近 Corner 地图点 kdtreeSurfFromMap 再给当前 Surf 点找附近 Surf 地图点十、第五步当前帧点变到 map 坐标系当前帧的位姿变量为transformTobeMapped[0] roll transformTobeMapped[1] pitch transformTobeMapped[2] yaw transformTobeMapped[3] x transformTobeMapped[4] y transformTobeMapped[5] z当前帧一个 Corner 点PointType pointOri laserCloudCornerLastDS-points[i];它还在当前 LiDAR 坐标系中。通过pointAssociateToMap(pointOri, pointSel);变到 map 坐标系。┌─────────────────────────────────────────────────────────────┐ │ p_M R(roll, pitch, yaw) · p_L t │ └─────────────────────────────────────────────────────────────┘其中p_L 当前帧点在当前 LiDAR 坐标系中的坐标 R 当前待优化位姿的旋转矩阵 t 当前待优化位姿的平移 p_M 当前点在 map 坐标系中的坐标源码中的pointAssociateToMap()本质就是矩阵乘法加平移po-x transPointAssociateToMap(0,0) * pi-x transPointAssociateToMap(0,1) * pi-y transPointAssociateToMap(0,2) * pi-z transPointAssociateToMap(0,3);这一步非常重要当前帧点每一轮优化都会用最新位姿重新变到 map 坐标系。十一、Corner 点怎样通过 KD-Tree 找近邻并拟合线当前帧一个 Corner 点pointSel已经变到 map 系后kdtreeCornerFromMap-nearestKSearch( pointSel, 5, pointSearchInd, pointSearchSqDis);这一步的意思在局部 CornerMap 中找距离 pointSel 最近的 5 个地图 Corner 点距离计算是┌─────────────────────────────────────────────────────────────┐ │ d_i² (x - x_i)² (y - y_i)² (z - z_i)² │ └─────────────────────────────────────────────────────────────┘其中x、y、z 当前帧 Corner 点 pointSel 的 map 坐标 x_i、y_i、z_i 局部 CornerMap 中第 i 个点的坐标 d_i² 当前点到第 i 个局部地图点的平方欧氏距离搜索后pointSearchInd[0] 最近第 1 个点在 CornerMap 中的索引 pointSearchInd[1] 最近第 2 个点在 CornerMap 中的索引 ... pointSearchInd[4] 最近第 5 个点在 CornerMap 中的索引对应平方距离pointSearchSqDis[0] pointSearchSqDis[1] ... pointSearchSqDis[4]源码会检查第 5 个邻点距离if (pointSearchSqDis[4] 1.0) { // 说明最近 5 个点整体都比较近 // 才继续判断它们是否共线 }┌─────────────────────────────────────────────────────────────┐ │ d₅² 1.0 │ │ │ │ 若距离单位是 m则大致相当于 │ │ d₅ 1.0 m │ └─────────────────────────────────────────────────────────────┘它表达的不是“只要最近的一个点够近”而是第 5 个近邻也必须足够近证明这 5 个点整体都处在当前点附近才允许拿它们尝试拟合线。源码在 Corner 优化中对当前 Corner 点做nearestKSearch(..., 5, ...)并检查第 5 个近邻的平方距离是否小于 1。1. 计算 5 个 Corner 邻点中心设 5 个邻点为┌─────────────────────────────────────────────────────────────┐ │ q₁, q₂, q₃, q₄, q₅ │ └─────────────────────────────────────────────────────────────┘中心点为┌─────────────────────────────────────────────────────────────┐ │ q̄ (1 / 5) · Σ(j 1 → 5) q_j │ └─────────────────────────────────────────────────────────────┘它代表这 5 个候选点的平均位置。2. 构造协方差矩阵数学上可写成┌─────────────────────────────────────────────────────────────┐ │ C (1 / 5) · Σ(j 1 → 5) │ │ (q_j - q̄)(q_j - q̄)ᵀ │ └─────────────────────────────────────────────────────────────┘展开后┌─────────────────────────────────────────────────────────────┐ │ C [ C_xx C_xy C_xz ] │ │ [ C_yx C_yy C_yz ] │ │ [ C_zx C_zy C_zz ] │ └─────────────────────────────────────────────────────────────┘它描述了这 5 个邻点在三维空间中的离散方向。源码中通常计算的是没有除以 5 的散布矩阵┌─────────────────────────────────────────────────────────────┐ │ S Σ(j 1 → 5) (q_j - q̄)(q_j - q̄)ᵀ │ └─────────────────────────────────────────────────────────────┘它和协方差矩阵只差一个固定比例。因为后续判断使用的是特征值比例关系所以是否除以 5 不影响共线判断。3. 特征值分解判断是否近似共线对协方差矩阵或散布矩阵进行特征值分解┌─────────────────────────────────────────────────────────────┐ │ C v_i λ_i v_i │ └─────────────────────────────────────────────────────────────┘特征值排序┌─────────────────────────────────────────────────────────────┐ │ λ₁ ≥ λ₂ ≥ λ₃ │ └─────────────────────────────────────────────────────────────┘若满足┌─────────────────────────────────────────────────────────────┐ │ λ₁ 3λ₂ │ └─────────────────────────────────────────────────────────────┘则说明一个方向上的扩散明显大于其他方向 ↓ 这 5 个点主要沿一个方向排列 ↓ 它们可以近似看作一条地图边缘线最大特征值对应特征向量v₁就是该线的主方向。4. 构造线并计算点到线残差假设q̄ 5 个邻点中心 v₁ 主方向单位向量构造线上的两个点┌─────────────────────────────────────────────────────────────┐ │ a q̄ s · v₁ │ │ b q̄ - s · v₁ │ └─────────────────────────────────────────────────────────────┘其中s可以取一个小常数例如0.1主要目的是从“中心 方向”构造出一条明确的直线。当前 Corner 点p_M到该直线的距离┌─────────────────────────────────────────────────────────────┐ │ r_line || (p_M - a) × (p_M - b) || / || a - b || │ └─────────────────────────────────────────────────────────────┘这里(p_M - a) × (p_M - b) 叉积模长对应由当前点和线段构成的面积量 ||a - b|| 线段长度 面积 / 底边 就是当前点到该直线的垂直距离因此 Corner 残差表达的是当前帧 Corner 点经过当前位姿变换后应当落在局部地图中对应边缘线附近。十二、Surf 点怎样通过 KD-Tree 找近邻并拟合平面当前帧 Surf 点流程和 Corner 一样kdtreeSurfFromMap-nearestKSearch( pointSel, 5, pointSearchInd, pointSearchSqDis);KD-Tree 返回局部 SurfMap 中最近的 5 个点q₁、q₂、q₃、q₄、q₅然后拟合平面┌─────────────────────────────────────────────────────────────┐ │ ax by cz d 0 │ └─────────────────────────────────────────────────────────────┘其中[a, b, c]ᵀ 平面法向量 d 平面偏置项对于每一个邻点┌─────────────────────────────────────────────────────────────┐ │ a·x_j b·y_j c·z_j d ≈ 0 │ └─────────────────────────────────────────────────────────────┘可以写成最小二乘形式┌─────────────────────────────────────────────────────────────┐ │ A n ≈ -d · 1 │ │ │ │ A [ x₁ y₁ z₁ ] │ │ [ x₂ y₂ z₂ ] │ │ [ x₃ y₃ z₃ ] │ │ [ x₄ y₄ z₄ ] │ │ [ x₅ y₅ z₅ ] │ │ │ │ n [a, b, c]ᵀ │ └─────────────────────────────────────────────────────────────┘求出后会将法向量归一化┌─────────────────────────────────────────────────────────────┐ │ n [a, b, c]ᵀ / √(a² b² c²) │ └─────────────────────────────────────────────────────────────┘归一化后当前点到平面的残差就具有距离意义。源码还会检查 5 个近邻点是否都接近平面┌─────────────────────────────────────────────────────────────┐ │ |a·x_j b·y_j c·z_j d| τ │ └─────────────────────────────────────────────────────────────┘若其中某个邻点偏离很大说明这 5 个点并不属于同一平面例如混合了地面、墙面和柱角点则此次平面约束会被舍弃。当前 Surf 点p_M [x, y, z]ᵀ的点到面残差为┌─────────────────────────────────────────────────────────────┐ │ r_plane a·x b·y c·z d │ └─────────────────────────────────────────────────────────────┘若法向量已经归一化┌─────────────────────────────────────────────────────────────┐ │ √(a² b² c²) 1 │ └─────────────────────────────────────────────────────────────┘则┌─────────────────────────────────────────────────────────────┐ │ r_plane │ │ 就是当前 Surf 点到局部地图平面的有符号距离。 │ └─────────────────────────────────────────────────────────────┘Surf 残差表达的是当前帧的表面点经过正确位姿变换后应该贴合局部地图中的对应平面。十三、Corner 和 Surf 残差怎样共同优化当前帧位姿当前帧位姿变量为┌─────────────────────────────────────────────────────────────┐ │ ξ [roll, pitch, yaw, t_x, t_y, t_z]ᵀ │ └─────────────────────────────────────────────────────────────┘对每一个有效 Corner 或 Surf 约束都有一个残差┌─────────────────────────────────────────────────────────────┐ │ r_i(ξ) │ └─────────────────────────────────────────────────────────────┘在当前位姿附近做一阶线性化┌─────────────────────────────────────────────────────────────┐ │ r_i(ξ Δξ) ≈ r_i(ξ) J_i Δξ │ └─────────────────────────────────────────────────────────────┘其中J_i 当前第 i 条残差对六自由度位姿的雅可比矩阵 Δξ 当前轮需要求解的六自由度位姿增量所有残差堆叠后┌─────────────────────────────────────────────────────────────┐ │ r [r₁, r₂, ..., r_N]ᵀ │ │ │ │ J [J₁, J₂, ..., J_N]ᵀ │ └─────────────────────────────────────────────────────────────┘优化目标┌─────────────────────────────────────────────────────────────┐ │ min || JΔξ r ||² │ └─────────────────────────────────────────────────────────────┘对应正规方程┌─────────────────────────────────────────────────────────────┐ │ JᵀJΔξ -Jᵀr │ └─────────────────────────────────────────────────────────────┘求得增量后┌─────────────────────────────────────────────────────────────┐ │ ξ ← ξ Δξ │ └─────────────────────────────────────────────────────────────┘然后进入下一轮更新当前帧位姿 ↓ 重新把当前帧点变到 map 坐标系 ↓ 重新 KNN 搜索 ↓ 重新拟合线和平面 ↓ 重新构造残差 ↓ 继续优化所以局部地图 KD-Tree 在当前帧的一轮 scan-to-map 优化中基本固定变化的是当前帧点经过当前位姿变换后的 map 坐标以及由此带来的近邻关系和残差。十四、优化完成后当前帧是否保存为关键帧scan-to-map 优化后LIO-SAM 会比较当前帧位姿和上一个关键帧位姿。设上一个关键帧位姿为┌─────────────────────────────────────────────────────────────┐ │ T_ML^(k-1) │ └─────────────────────────────────────────────────────────────┘当前帧优化后位姿为┌─────────────────────────────────────────────────────────────┐ │ T_ML^cur │ └─────────────────────────────────────────────────────────────┘两者相对运动┌─────────────────────────────────────────────────────────────┐ │ T_Δ (T_ML^(k-1))⁻¹ · T_ML^cur │ └─────────────────────────────────────────────────────────────┘从中得到┌─────────────────────────────────────────────────────────────┐ │ Δx、Δy、Δz、Δroll、Δpitch、Δyaw │ └─────────────────────────────────────────────────────────────┘若当前帧相对上一关键帧的平移、旋转都小于阈值当前帧不成为关键帧 ↓ 只用于本轮定位 ↓ Corner / Surf 不长期保存若超过阈值当前帧成为关键帧 ↓ 深拷贝当前 CornerDS、SurfDS ↓ 保存到 vector ↓ 保存当前优化后关键帧位姿 ↓ 加入因子图保存概念上等价于auto cornerKeyFrame std::make_sharedpcl::PointCloudPointType(); auto surfKeyFrame std::make_sharedpcl::PointCloudPointType(); *cornerKeyFrame *laserCloudCornerLastDS; *surfKeyFrame *laserCloudSurfLastDS; cornerCloudKeyFrames.push_back(cornerKeyFrame); surfCloudKeyFrames.push_back(surfKeyFrame);深拷贝很重要。因为laserCloudCornerLastDS laserCloudSurfLastDS是当前帧临时对象下一帧会重新清空、填入新数据。如果不复制历史关键帧会和当前帧共用同一片内存下一帧进来后历史地图就会被覆盖。十五、整条链路压缩成一张流程图当前帧 Corner / Surf 输入 ↓ 当前帧临时缓存 laserCloudCornerLast laserCloudSurfLast ↓ 当前帧降采样 laserCloudCornerLastDS laserCloudSurfLastDS ↓ ──────────────────────────────────────── 关键帧位姿 KD-Tree kdtreeSurroundingKeyPoses ──────────────────────────────────────── ↓ radiusSearch 找附近关键帧位置 ↓ 获得附近关键帧 ID ↓ 对关键帧位置降采样 ↓ 1-NN 找回真实关键帧 ID ↓ ──────────────────────────────────────── 从 vector 按 ID 取历史点云 cornerCloudKeyFrames[id] surfCloudKeyFrames[id] ──────────────────────────────────────── ↓ 按 cloudKeyPoses6D[id] 转到 map 系 ↓ 拼接为 laserCloudCornerFromMap laserCloudSurfFromMap ↓ 局部地图降采样 ↓ laserCloudCornerFromMapDS laserCloudSurfFromMapDS ↓ ──────────────────────────────────────── 建立局部地图 KD-Tree kdtreeCornerFromMap kdtreeSurfFromMap ──────────────────────────────────────── ↓ 当前 Corner 点变到 map 系 ↓ Corner KD-Tree KNN 搜索 ↓ 5 个近邻拟合线 ↓ 点到线残差 ↓ 当前 Surf 点变到 map 系 ↓ Surf KD-Tree KNN 搜索 ↓ 5 个近邻拟合平面 ↓ 点到面残差 ↓ LM / 最小二乘迭代优化 6DoF 位姿 ↓ 判断是否保存为新关键帧总结LIO-SAM 这条链路最容易混淆的地方是把“关键帧检索”“局部地图构建”“当前帧特征匹配”混成一件事。实际上它分为三个层级先找哪些关键帧值得参与当前匹配再从这些关键帧中取出 Corner、Surf 点云构成局部地图最后才让当前帧的 Corner、Surf 点去局部地图里找最近邻、构造线面约束。也就是说KD-Tree 在 LIO-SAM 中不是只做一次“找最近点”而是分别服务于不同层级的问题关键帧位姿 KD-Tree 负责找附近关键帧 IDCorner KD-Tree 负责给当前 Corner 点找附近 Corner 地图点Surf KD-Tree 负责给当前 Surf 点找附近 Surf 地图点。每一帧 LiDAR 点云经过前端处理后会产生当前帧的 Corner 特征和 Surf 特征。进入mapOptimization后它们会先放入laserCloudCornerLast和laserCloudSurfLast这两份只是当前帧临时缓存。后端会再通过体素降采样得到laserCloudCornerLastDS和laserCloudSurfLastDS它们才是这一帧真正用于 scan-to-map 匹配的输入。当前帧即使最终没有成为关键帧也不是完全没用它仍然会参与本轮局部匹配、生成当前位姿只是处理完后不会进入长期地图数据库。下一帧到来时临时缓存会被新一帧数据覆盖。只有当前帧完成 scan-to-map 优化并且相对上一关键帧的位移或姿态变化达到阈值时才会被保存为关键帧。LIO-SAM 长期保存的不是一张不断追加的全局 Corner 点云和 Surf 点云而是两个按关键帧编号组织的动态数组cornerCloudKeyFrames和surfCloudKeyFrames。其中vector可以理解为能够自动扩容的数组但数组里的一个元素不是一个点而是一整帧点云。例如cornerCloudKeyFrames[120]保存的是第 120 个关键帧的全部 Corner 点surfCloudKeyFrames[120]保存的是同一个关键帧的全部 Surf 点。系统还会在cloudKeyPoses3D[120]和cloudKeyPoses6D[120]中保存第 120 个关键帧的位置和完整六自由度位姿。四份数据依靠相同下标绑定因此只要拿到关键帧 ID就能够同时得到该帧的点云和位姿。这些关键帧点云保存时通常仍处在各自关键帧的局部 LiDAR 坐标系中。这样做的意义非常大假设后端图优化、GPS 因子或者回环约束修正了某个历史关键帧的全局位姿系统不需要把该关键帧的每一个 Corner 点和 Surf 点逐一改写只需要更新该关键帧对应的cloudKeyPoses6D[k]。下次要使用该关键帧点云时再拿它的局部点云乘更新后的位姿即可得到新的 map 坐标。换句话说LIO-SAM 长期保存的是“局部观测点云”和“该观测对应的全局位姿”两部分而不是一份无法方便修正的固定全局点云。当前帧开始匹配前LIO-SAM 不会立刻把所有历史 Corner 点和 Surf 点拿出来搜索。它首先使用cloudKeyPoses3D建立kdtreeSurroundingKeyPoses。这棵 KD-Tree 中保存的不是特征点而是所有关键帧的位置点每个位置点代表“第几个关键帧位于哪里”。源码中先以最近保存的关键帧位置cloudKeyPoses3D-back()为中心调用radiusSearch()找出某一半径内的历史关键帧位置返回的下标集合就是附近关键帧候选。也就是说这一阶段 KD-Tree 的任务是解决“当前局部地图应该由哪些关键帧组成”而不是解决“当前 Corner 点最接近哪个地图 Corner 点”。找到附近关键帧候选后LIO-SAM 会先对这些关键帧位置点做体素降采样。原因是如果机器人在一个小区域内移动得很慢可能连续生成很多位置非常接近的关键帧全部拿来构建局部地图会造成重复点过多、局部地图过密、后续匹配计算量增加。关键帧位置点降采样后每个体素中只保留一个代表位置但这个代表位置可能经过体素滤波而不再严格对应某一个原始关键帧。因此源码会再次用nearestKSearch(..., 1, ...)在全部原始关键帧位置中找到距离这个代表位置最近的真实关键帧并把该真实关键帧 ID 写回代表点的intensity字段。这样局部地图候选既不会太密又仍能通过 ID 精确关联到一帧真实的 Corner 和 Surf 点云。关键帧 ID 确定后系统才真正从vector中取出点云。对于每一个附近关键帧k它会访问cornerCloudKeyFrames[k]、surfCloudKeyFrames[k]和cloudKeyPoses6D[k]。前两者是该关键帧自己的局部 Corner、Surf 点云最后一个是该关键帧当前优化后的 map 位姿。系统将 Corner 和 Surf 点分别按该位姿变换到 map 坐标系然后拼接到laserCloudCornerFromMap与laserCloudSurfFromMap。这两份对象就是当前帧附近的临时局部地图。它们不是永久全局地图而是这一轮 scan-to-map 优化专门构建出来的局部匹配区域。为了避免同一个关键帧在连续多帧中被重复变换LIO-SAM 还使用laserCloudMapContainer缓存已经转到 map 坐标系的关键帧 Corner、Surf 点云。若当前局部地图再次需要某个关键帧系统会优先从缓存中取已经变换好的结果如果缓存中没有再从关键帧vector取原始局部点云调用transformPointCloud()转到 map 系并放入缓存。缓存超过一定规模后会清空但清空的只是“已变换的 map 坐标副本”不会删除真正长期保存的cornerCloudKeyFrames和surfCloudKeyFrames。局部 CornerMap 和 SurfMap 拼接完成后系统还会进行一次体素降采样得到laserCloudCornerFromMapDS与laserCloudSurfFromMapDS。这是因为附近多个关键帧的点云叠加后局部地图中的点密度可能很高如果每个当前帧点都去几万、几十万地图点里做最近邻搜索速度会明显下降。降采样后局部地图仍保持主要边缘和平面结构但冗余点减少KD-Tree 查询和后续残差计算会更快。随后LIO-SAM 才调用kdtreeCornerFromMap-setInputCloud(...)和kdtreeSurfFromMap-setInputCloud(...)分别为局部 Corner 地图、局部 Surf 地图建立空间索引。当前帧 Corner 点和 Surf 点不会插入上述两棵 KD-Tree。它们是查询点。当前帧一个 Corner 点先根据当前待优化的六自由度位姿通过pointAssociateToMap()从当前 LiDAR 坐标系变换到 map 坐标系。然后系统调用kdtreeCornerFromMap-nearestKSearch(pointSel, 5, ...)在局部 CornerMap 中寻找距离该点最近的 5 个地图 Corner 点。这里 KD-Tree 做的事情只是 KNN 搜索即按三维欧氏距离返回最近邻点的索引和平方距离。源码会检查第 5 个近邻的平方距离是否小于 1.0这意味着不能只找到一个近点而是要求这 5 个候选点整体都在当前点附近才允许继续构造几何约束。对于 Corner 匹配最近的 5 个点并不天然就是一条线。LIO-SAM 会先计算这些邻点的中心和三维散布矩阵再做特征值分解。如果最大特征值明显大于第二大特征值说明这些点主要沿一个方向拉长分布而不是聚成一团或者铺成一片因此可以近似为一条地图边缘线。随后系统计算当前 Corner 点到该线的垂直距离把它作为点到线残差。这个残差表达的物理意义是当前帧的边缘特征在正确位姿下应该和局部历史地图中的同一条结构边缘重合或足够接近。对于 Surf 匹配系统同样会先把当前 Surf 点变到 map 坐标系再利用kdtreeSurfFromMap找最近的 5 个局部 Surf 地图点。之后使用这些近邻拟合平面并检查每个邻点是否都贴近该平面。如果近邻来自同一面墙、地面或大平面平面拟合会可靠当前点到该平面的距离就可作为点到面残差如果这些点混杂在墙角、柱子、地面和墙体交界等多个结构上拟合平面不可靠这个匹配会被丢弃。这样做避免错误的平面约束把当前位姿拉向错误方向。最终所有有效的点到线残差和点到面残差一起参与当前帧位姿优化。当前帧的位姿变量包含 roll、pitch、yaw、x、y、z每个残差都反映“当前点经过该位姿变换后与局部地图结构是否对齐”。优化器根据这些残差计算位姿增量不断修正当前帧姿态与位置。每次位姿更新后当前帧点在 map 坐标系中的位置都会变化因此下一轮会重新建立当前点与局部地图之间的关联、重新计算残差直至收敛或达到迭代上限。局部地图在当前帧优化过程中基本固定而不断变化的是当前帧点按照最新位姿变换后的坐标以及对应的最近邻关系。因此整条链路可以概括为当前帧特征先临时保存并降采样关键帧位姿 KD-Tree 先找附近关键帧 ID系统按 ID 从关键帧vector中取出历史 Corner、Surf 点云用各自关键帧位姿把它们变到 map 坐标系并拼成局部地图局部 CornerMap、SurfMap 再分别建立 KD-Tree当前帧 Corner、Surf 点变到 map 系后以查询点身份做 KNN 搜索近邻通过线拟合或平面拟合转成点到线、点到面残差残差共同优化当前帧位姿最后只有位姿变化足够大的当前帧才会被保存为新的关键帧。这种“关键帧存储—局部地图重建—局部几何匹配”的设计使 LIO-SAM 不必每一帧都与完整全局点云匹配也能在回环修正历史轨迹后方便地重新生成一致的局部或全局地图。