LIO-SAM 完整链路：关键帧选择 → 关键帧 ID 检索 → 局部地图构建 → KD-Tree 最近邻 → 线面残差 → 位姿优化

先直接回答你最核心的问题：

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。

还有一棵：

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::vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames; // 每个元素是一整帧 Surf 点云 std::vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames; // 所有关键帧的位置点，用于 KD-Tree 搜索附近关键帧 pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D; // 所有关键帧完整六自由度位姿 pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;

其中第k个关键帧的数据通过相同下标绑定：

cornerCloudKeyFrames[k] ↓ 第 k 个关键帧的 Corner 点云 surfCloudKeyFrames[k] ↓ 第 k 个关键帧的 Surf 点云 cloudKeyPoses3D[k] ↓ 第 k 个关键帧的三维位置 cloudKeyPoses6D[k] ↓ 第 k 个关键帧的 x、y、z、roll、pitch、yaw

vector可以理解成自动扩容的数组。它不是直接存一个个 Corner 点，而是每个元素保存“一整帧点云对象的指针”。

cornerCloudKeyFrames │ ├── [0] → 第 0 个关键帧 Corner 点云 ├── [1] → 第 1 个关键帧 Corner 点云 ├── [2] → 第 2 个关键帧 Corner 点云 └── [k] → 第 k 个关键帧 Corner 点云

第k个点云内部又是很多点组成的数组：

cornerCloudKeyFrames[k] ↓ PointCloud<PointType> ↓ points[0], points[1], points[2], ...

每个PointType本质为：

pcl::PointXYZI

也就是：

x、y、z、intensity

这里的关键帧 Corner / Surf 点云通常保留在该关键帧自身的 LiDAR 局部坐标系中，而不是永久存成 map 坐标系点云。后续使用时，再根据该关键帧最终优化后的 6DoF 位姿变到 map 坐标系。

三、当前帧进入 mapOptimization 后的完整流程

当前帧 Corner、Surf 从前端进入mapOptimization：

pcl::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-NN：

for (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^map

SurfLocalMap = 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_shared<pcl::PointCloud<PointType>>(); auto surfKeyFrame = std::make_shared<pcl::PointCloud<PointType>>(); *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 负责找附近关键帧 ID，Corner 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 不必每一帧都与完整全局点云匹配，也能在回环修正历史轨迹后方便地重新生成一致的局部或全局地图。