LeGO-LOAM 的整体思路可以理解为：先把原始三维点云整理成规则的二维深度图，再利用地面信息和连通域分割筛选出稳定点云，之后提取角点和平面点，通过前端 scan-to-scan 求相邻帧位姿，再通过后端 scan-to-map 和关键帧地图进一步优化位姿，最后融合前端和后端结果发布连续轨迹。
它不是一上来就直接对原始点云做匹配，而是先做了比较完整的点云预处理。这个预处理非常关键，因为 LeGO-LOAM 后面的特征提取、遮挡判断、曲率计算、地面点利用、连通域分割，全部依赖前面生成的结构化点云信息。
整个源码主要可以按下面几个文件看：
utility.h
imageProjection.cpp
featureAssociation.cpp
mapOptmization.cpp
transformFusion.cpp其中：
utility.h：
定义雷达参数、话题名、点云类型、公共变量。
imageProjection.cpp：
负责原始点云投影、地面检测、点云分割。
featureAssociation.cpp：
负责曲率计算、特征提取、前端里程计估计。
mapOptmization.cpp：
负责局部地图构建、scan-to-map 优化、关键帧、回环和位姿图。
transformFusion.cpp：
负责融合前端 odometry 和后端 mapping 位姿，发布最终连续轨迹。1. utility.h：公共参数和基础配置
utility.h 是 LeGO-LOAM 里面非常重要的公共头文件。很多人看源码时容易跳过它，但实际上 LeGO-LOAM 的点云投影、地面判断、range image 大小、扫描线数量、水平角分辨率，都由这里面的参数决定。
常见参数类似下面这些：
extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2;
extern const float ang_res_y = 2.0;
extern const float ang_bottom = 15.0;
extern const int groundScanInd = 7;这些参数对应的是雷达扫描结构。
N_SCAN 表示 LiDAR 有多少根垂直线束。比如 VLP-16 就是 16 线，所以 N_SCAN = 16。Horizon_SCAN 表示水平方向一圈被划分成多少列，如果水平角分辨率是 0.2°，那么一圈 360° 对应 1800 列，所以 Horizon_SCAN = 1800。ang_res_x 是水平角分辨率，ang_res_y 是垂直角分辨率，ang_bottom 是最低线束相对水平面的角度补偿，groundScanInd 表示用前几根较低线束参与地面检测。
这些参数直接决定了点云投影矩阵的大小：
rangeMat 的大小 = N_SCAN × Horizon_SCAN也就是：
16 × 1800每个点最终都会被投影到这个二维矩阵里的某一个格子中。矩阵的行表示第几根激光线，矩阵的列表示水平角方向，矩阵里的值通常保存这个点到雷达的距离。
所以 LeGO-LOAM 的点云预处理可以理解为：
无序三维点云
↓
根据线束 ID 和水平角
↓
投影成规则二维 range image这一步非常重要，因为投影之后，点云就有了类似图像一样的上下左右邻接关系。后面判断地面点、判断遮挡点、做连通域分割、计算曲率，都可以基于二维矩阵索引快速完成。
2. imageProjection.cpp：点云投影、地面检测、点云分割
imageProjection.cpp 是 LeGO-LOAM 前端输入的第一站。这个文件的作用不是简单滤波，而是把原始点云转换成后续模块可以直接使用的结构化点云。
它主要完成下面几件事：
1. 接收原始 LiDAR 点云
2. 去除 NaN 无效点
3. 计算当前帧扫描起止角
4. 将点云投影到 range image
5. 检测地面点
6. 对非地面点做连通域分割
7. 整理 segmentedCloud 和 cloudInfo
8. 发布给 featureAssociation 模块主入口一般是 cloudHandler()。
2.1 cloudHandler()：一帧点云的总处理入口
cloudHandler() 是点云回调函数，每来一帧 LiDAR 点云，就会执行一次。它把一帧点云的处理流程串起来。
整体逻辑可以理解为：
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
copyPointCloud(laserCloudMsg);
findStartEndAngle();
projectPointCloud();
groundRemoval();
cloudSegmentation();
publishCloud();
resetParameters();
}这几个函数就是 imageProjection.cpp 的主线。也就是说，LeGO-LOAM 每收到一帧原始点云，都会先经过投影、地面检测和分割，然后再送给后面的特征提取模块。
2.2 copyPointCloud()：复制点云并去除无效点
原始点云通常是 ROS 的 sensor_msgs::PointCloud2 类型，LeGO-LOAM 需要把它转换成 PCL 点云格式，方便后续处理。
核心逻辑类似：
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);这里主要做两件事：
第一，将 ROS 点云消息转换成 PCL 点云；
第二，去除 NaN 点。NaN 点就是坐标非法的点，比如某些点的 x、y、z 不是正常数值。这种点不能计算距离，也不能参与角度投影。如果不提前去除，后面的 sqrt()、atan2()、矩阵索引计算都可能出错。
如果点云中带有 ring 字段，LeGO-LOAM 可以直接使用 ring 字段确定每个点属于第几根线束。这个非常关键，因为投影成 range image 时，行号就是由线束 ID 决定的。如果没有 ring 字段，就需要通过垂直角反推线束编号。
2.3 findStartEndAngle()：计算当前帧扫描起止方向
机械式 LiDAR 是旋转扫描的，一帧点云不是同一时刻瞬间采集出来的，而是从一帧起始角度逐渐扫到结束角度。所以 LeGO-LOAM 要先计算这一帧点云的起始方向和结束方向。
典型代码逻辑类似：
startOrientation = -atan2(firstPoint.y, firstPoint.x);
endOrientation = -atan2(lastPoint.y, lastPoint.x) + 2 * M_PI;
orientationDiff = endOrientation - startOrientation;这里：
firstPoint：
当前帧点云中的第一个点。
lastPoint：
当前帧点云中的最后一个点。
startOrientation：
当前帧开始扫描时的水平角。
endOrientation：
当前帧结束扫描时的水平角。
orientationDiff：
当前帧扫描覆盖的角度范围。atan2(y, x) 用于计算点在水平面上的方向角。代码中前面加负号，是因为 LeGO-LOAM 采用的角度方向和常规数学坐标方向有一定差异，需要统一到它内部定义的扫描方向。
为什么 endOrientation 要加 2π？因为 LiDAR 扫描一圈时，角度会存在从 π 到 -π 的跳变。如果直接相减，可能得到一个错误的小角度差。加 2π 是为了让一帧扫描的角度范围接近完整一圈。
这个角度信息后面有两个作用：
第一，用于计算点在一帧中的相对时间；
第二，用于点云运动畸变补偿。因为同一帧点云内部的点不是同一时刻采集的，所以需要知道每个点大概位于当前扫描周期的哪个位置。
2.4 projectPointCloud()：将三维点云投影为 range image
这是 LeGO-LOAM 的核心预处理步骤。
每个点都有三维坐标：
p = (x, y, z)首先计算点到雷达原点的距离：
range = sqrt(x * x + y * y + z * z);也就是：
range = √(x² + y² + z²)range 就是点到雷达的欧氏距离。如果距离太近或者太远，通常会被过滤，因为太近的点容易是车体自身或噪声，太远的点精度较差。
接着计算这个点应该放到 range image 的哪一行、哪一列。
如果点云中有 ring 字段，那么行号可以直接使用：
rowIdn = point.ring;如果没有 ring 字段，就需要通过垂直角计算：
verticalAngle = atan2(z, sqrt(x * x + y * y)) * 180 / M_PI;
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;这里：
sqrt(x² + y²)：
点在水平面上的距离。
atan2(z, sqrt(x² + y²))：
点相对于水平面的垂直角。
ang_bottom：
最低线束角度补偿。
ang_res_y：
垂直方向角分辨率。
rowIdn：
点对应的线束行号。然后计算列号。列号由水平角决定：
horizonAngle = atan2(x, y) * 180 / M_PI;
columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2;这个表达式看起来比较绕，但本质就是把水平角映射到 [0, Horizon_SCAN) 的列索引中。
最后得到一维索引：
index = columnIdn + rowIdn * Horizon_SCAN;也就是说：
fullCloud[index] = 当前点
rangeMat[rowIdn][columnIdn] = range这里的 fullCloud 虽然是点云形式，但它的排列顺序已经和二维矩阵一致了。某个点在 fullCloud 中的位置就是：
rowIdn * Horizon_SCAN + columnIdn这个设计后面非常有用。比如要访问某个点的上方线束点，只需要访问：
(rowIdn - 1, columnIdn)要访问下方线束点：
(rowIdn + 1, columnIdn)要访问左边水平相邻点：
(rowIdn, columnIdn - 1)要访问右边水平相邻点：
(rowIdn, columnIdn + 1)所以 LeGO-LOAM 不是把点云当成完全无序的点集，而是把它变成了类似图像的结构，这就是后续地面检测和连通域分割能够高效运行的基础。
2.5 groundRemoval()：地面点检测
点云投影完成后，LeGO-LOAM 会利用低线束之间的几何关系判断地面点。
地面检测的核心思路是：在同一列中，取上下相邻两根线束的点，如果这两个点之间形成的连线角度接近雷达安装角，那么可以认为它们落在同一个地面平面上。
典型逻辑如下：
diffX = upperPoint.x - lowerPoint.x;
diffY = upperPoint.y - lowerPoint.y;
diffZ = upperPoint.z - lowerPoint.z;
angle = atan2(diffZ, sqrt(diffX * diffX + diffY * diffY)) * 180 / M_PI;这里：
lowerPoint：
较低线束上的点。
upperPoint：
相邻较高线束上的点。
diffX、diffY：
两个点在水平面上的差值。
diffZ：
两个点在高度方向上的差值。
angle：
上下两个点连线相对于水平面的倾斜角。如果这个角度和传感器安装角比较接近：
if (abs(angle - sensorMountAngle) <= 10)
{
groundMat[i][j] = 1;
groundMat[i + 1][j] = 1;
}就把对应点标记为地面点。
这里有几个关键点。
第一，LeGO-LOAM 只在较低的若干根线束中做地面判断。因为地面主要出现在雷达下半部分视场，高线束更多看到墙、树、建筑或其他障碍物。
第二，地面点不是完全无用点。它们不会参与普通非地面物体的连通域分割，但后面仍然可能作为平面特征的一部分参与优化，尤其对高度和姿态估计有帮助。
第三，地面检测依赖前面的 range image。如果点没有正确投影到对应行列，那么上下线束之间的关系就不真实，地面判断也会错。
在这个阶段，groundMat 和 labelMat 会被更新：
groundMat = 1：
表示这个点是地面点。
labelMat = -1：
表示这个点已经被标记为地面或无效点，不再参与普通分割。
labelMat = 0：
表示这个点还没有被处理，后面需要做连通域分割。2.6 cloudSegmentation()：对非地面点做连通域分割
地面点处理完之后，LeGO-LOAM 会对剩余非地面点做分割。分割的目的不是做语义识别，而是把连续的点云区域找出来，过滤孤立噪声和不稳定小簇。
它的基本思想类似图像中的区域生长。因为点云已经投影成 range image，所以可以从一个未标记点开始，检查它的上下左右邻居：
上：row - 1
下：row + 1
左：col - 1
右：col + 1如果邻居点和当前点在几何上连续，就把它们归为同一个 segment。
判断两个相邻点是否连续时，会用到两点的距离和角分辨率。假设两个相邻点的距离分别是：
d1 = 较远点距离
d2 = 较近点距离
alpha = 两个点对应的角分辨率代码中会计算一个夹角
angle = atan2(d2 * sin(alpha), d1 - d2 * cos(alpha))如果这个角度大于阈值 segmentTheta，说明两个点更可能属于同一个连续表面，于是把邻居加入当前连通域。
这一步可以这样理解：
如果两个相邻激光点距离差不大，它们可能打在同一个物体表面；
如果两个相邻激光点距离突然变化很大，它们可能位于遮挡边界或不同物体上；
如果点簇太小，通常不稳定，可能是噪声或离群点。分割结束后，LeGO-LOAM 会判断当前 segment 是否有效。通常会看两个条件：
第一，点数是否足够多；
第二，是否跨越了足够多的扫描线。如果一个 segment 只有很少几个点，或者只出现在很局部的位置，它就可能被认为是不可靠点，不会作为稳定结构参与后续特征提取。
2.7 segmentedCloud 和 segMsg：给后端模块准备结构化信息
cloudSegmentation() 之后，LeGO-LOAM 会整理输出点云和辅助信息。
主要输出包括：
segmentedCloud：
经过投影、地面检测、分割之后保留下来的有效点云。
outlierCloud：
分割中被认为不稳定或离群的点。
groundCloud：
检测到的地面点。
segMsg：
每个点对应的行列、距离、地面标记、起止索引等辅助信息。segMsg 很重要，因为 featureAssociation.cpp 不只是需要点云坐标，还需要知道每个点的扫描线编号、深度值、是否为地面点、在每根线中的起止位置等信息。
典型信息包括：
startRingIndex：
每根 scan line 在 segmentedCloud 中的起始索引。
endRingIndex：
每根 scan line 在 segmentedCloud 中的结束索引。
segmentedCloudGroundFlag：
每个点是否是地面点。
segmentedCloudColInd：
每个点在 range image 中的列号。
segmentedCloudRange：
每个点到雷达的距离。
startOrientation：
当前帧起始水平角。
endOrientation：
当前帧结束水平角。这些信息会直接影响后面的曲率计算、遮挡判断和特征点提取。
3. featureAssociation.cpp：特征提取和前端里程计
featureAssociation.cpp 是 LeGO-LOAM 的前端里程计模块。它接收 imageProjection.cpp 输出的分割点云，然后计算曲率、提取角点和平面点，并估计当前帧相对上一帧的运动。
这个文件主要做下面几件事：
1. 接收 segmentedCloud 和 segMsg
2. 根据 IMU 和扫描时间做点云畸变校正
3. 计算每个点的曲率
4. 标记遮挡点和平行光束点
5. 按 scan line 分段提取角点和平面点
6. 当前帧特征点和上一帧特征点匹配
7. 构建点到线、点到面约束
8. 两阶段 LM 优化当前帧相对运动
9. 发布前端 odometry 和特征点云3.1 laserCloudHandler()：前端模块入口
laserCloudHandler() 是 featureAssociation.cpp 里面处理分割点云的主要入口。当前面 imageProjection.cpp 发布新的 segmentedCloud 和 segMsg 之后，这里就会开始处理。
整体流程可以理解为：
laserCloudHandler()
{
adjustDistortion();
calculateSmoothness();
markOccludedPoints();
extractFeatures();
updateTransformation();
integrateTransformation();
publishOdometry();
publishCloud();
}这条链路就是 LeGO-LOAM 前端的完整逻辑。
3.2 adjustDistortion()：点云运动畸变校正
机械式 LiDAR 一帧点云不是同一时刻采集的。如果机器人在扫描过程中运动，那么一帧点云内部不同点对应的雷达位姿不同。这个现象叫点云运动畸变。
比如一帧点云持续时间是 0.1s：
第一个点：t = 0.00s 时采集
中间点：t = 0.05s 时采集
最后点：t = 0.10s 时采集如果机器人在这 0.1s 内发生了运动，那么这些点其实不应该直接放在同一个坐标系下。否则点云会被拉扯、扭曲，影响后续匹配。
LeGO-LOAM 会利用当前点的水平角，估计它在一帧扫描中的相对时间：
relTime = (ori - startOri) / (endOri - startOri)其中：
ori：
当前点的水平角。
startOri：
当前帧起始角。
endOri：
当前帧结束角。
relTime：
当前点在一帧扫描周期中的相对时间，范围大致是 0 到 1。如果有 IMU，LeGO-LOAM 会利用 IMU 的 roll、pitch、yaw 辅助估计扫描过程中的姿态变化，再把点补偿到统一参考时刻。这样后面的特征点匹配才是在相对一致的坐标系下进行。
这里要注意，LeGO-LOAM 里面 IMU 的作用主要是辅助去畸变和姿态估计，不是像后面一些紧耦合系统那样把 IMU 预积分作为后端核心约束。对于 LeGO-LOAM 来说，前端主体仍然是 LiDAR 特征匹配。
3.3 calculateSmoothness()：计算点云曲率
曲率计算是特征提取的基础。LeGO-LOAM 用每个点和前后邻居点的距离变化来衡量这个点是不是处在边缘或者平面区域。
典型计算方式可以概括为：
diffRange = r[i-5] + r[i-4] + r[i-3] + r[i-2] + r[i-1]
- 10 * r[i]
+ r[i+1] + r[i+2] + r[i+3] + r[i+4] + r[i+5]
curvature = diffRange²这里：
r[i]：
当前点到雷达的距离。
r[i-5] ~ r[i-1]：
当前点前面相邻点的距离。
r[i+1] ~ r[i+5]：
当前点后面相邻点的距离。
diffRange：
当前点相对邻域的距离突变程度。
curvature：
点的平滑度或曲率指标。如果一个点位于墙面、地面这类平滑表面上，它和前后邻居的距离变化比较平缓，那么 curvature 会比较小。
如果一个点位于墙角、柱子边缘、物体轮廓处，它附近的距离变化会比较明显，那么 curvature 会比较大。
所以：
curvature 大：
更可能是角点 / 边缘点。
curvature 小：
更可能是平面点。这里的曲率不是严格数学意义上的曲率，而是 LiDAR SLAM 工程中常用的局部平滑度指标。
3.4 markOccludedPoints()：剔除遮挡点和不稳定点
不是所有曲率大的点都适合作为角点。比如遮挡边界处，距离也会突然变化，曲率可能很大，但这种点不稳定。机器人稍微换一个视角，这个边界点的位置就可能明显变化。
所以 LeGO-LOAM 会专门标记遮挡点。
它会比较同一条 scan line 中相邻点的距离：
depth1 = 当前点距离
depth2 = 下一个点距离如果：
|depth1 - depth2| 很大说明这两个点可能来自前后两个不同物体，中间存在遮挡边界。
举个直观例子：
前面是一根柱子，后面是一面墙。
激光从柱子边缘扫过去时，相邻两个点可能一个打在柱子上，一个打在墙上。
这两个点距离差很大，但这种位置不是稳定几何角点。所以代码会把遮挡边界附近的一些点标记为 cloudNeighborPicked = 1，表示后面提特征时不要选它们。
此外，如果相邻点距离变化很小但光束方向和表面几乎平行，也容易产生不稳定测量，LeGO-LOAM 也会把这类点排除掉。
这一部分的作用可以总结为：
曲率只是判断候选特征；
遮挡标记用于剔除不可靠候选；
最后留下来的点才适合作为优化约束。3.5 extractFeatures()：提取角点和平面点
LeGO-LOAM 的特征点主要分成四类：
cornerPointsSharp：
最尖锐的角点，数量少，质量高，主要用于前端里程计匹配。
cornerPointsLessSharp：
次尖锐角点，数量更多，作为后续匹配和建图使用。
surfPointsFlat：
最平坦的平面点，数量少，质量高，主要用于前端平面约束。
surfPointsLessFlat：
次平面点，数量更多，通常会降采样后送给后端建图。特征提取不是整帧点云统一排序，而是按每根 scan line 分段处理。比如每根线会划分成若干段，每段分别选角点和平面点。
这样做的原因是为了让特征点分布更均匀。如果直接对整帧点云排序，特征点可能集中在某一小块区域，比如全都集中在一个墙角附近。这样优化时约束分布不均匀，位姿估计容易不稳定。
角点提取逻辑大致是：
1. 在当前分段内按照 curvature 从大到小排序；
2. 选择曲率较大、没有被遮挡标记、没有被邻域占用的点；
3. 前几个作为 cornerPointsSharp；
4. 更多一些作为 cornerPointsLessSharp；
5. 选中一个点后，把周围相邻点标记为已占用，避免重复选择同一局部区域。平面点提取逻辑大致是：
1. 在当前分段内按照 curvature 从小到大排序；
2. 选择曲率较小、没有被遮挡标记的点；
3. 少量最平坦点作为 surfPointsFlat；
4. 其他相对平滑点作为 surfPointsLessFlat；
5. 对 surfPointsLessFlat 做体素滤波，降低点数。这里的"邻域占用"非常关键。假设一个墙角附近连续 10 个点曲率都很大，如果都选出来，其实提供的是重复约束。LeGO-LOAM 选中一个特征点后，会把它附近几个点标记掉，这样可以让特征点更分散，约束更有效。
3.6 updateTransformation()：估计当前帧相对上一帧的运动
特征点提取完成后，前端需要估计当前帧相对于上一帧的位姿变化。
也就是求：
当前帧点云 → 上一帧点云的变换关系。
LeGO-LOAM 使用当前帧的角点和平面点，分别去上一帧的特征点云中寻找对应关系。
角点对应关系：
当前帧角点 p
↓
在上一帧 cornerPointsLessSharp 中找最近邻点
↓
再找另一个不同 scan line 上的邻近点
↓
这两个点构成一条线
↓
构建点到线距离约束为什么第二个点要尽量来自不同 scan line？因为如果两个点来自同一条扫描线，它们可能只是扫描方向上的相邻点，不一定能稳定表示三维空间中的边缘线。来自不同线束的两个点更容易形成空间边缘结构。
平面点对应关系：
当前帧平面点 p
↓
在上一帧 surfPointsLessFlat 中找最近邻点
↓
再找另外两个邻近点
↓
三个点构成一个平面
↓
构建点到面距离约束如果找到的三个点不稳定，或者拟合出来的平面质量不好，这个约束就不会被使用。
3.7 点到线约束：角点如何参与优化
对于当前帧角点 p，先通过当前估计位姿把它变换到上一帧坐标系，得到 p'。在上一帧中找到两个角点 a 和 b，它们构成一条边缘线。
点到线距离可以写成：
d = ||(p' - a) × (p' - b)|| / ||a - b||这里：
p'：
当前角点变换到上一帧坐标系后的坐标。
a、b：
上一帧中对应边缘线上的两个点。
×：
向量叉乘。
||(p' - a) × (p' - b)||：
表示由 p'、a、b 构成的平行四边形面积。
||a - b||：
边缘线长度。
d：
当前点到这条边缘线的垂直距离。优化的目标是让这个距离尽量小。也就是说，当前帧的角点经过位姿变换后，应该尽量落到上一帧对应的边缘线上。
3.8 点到面约束：平面点如何参与优化
对于当前帧平面点 p，同样先通过当前估计位姿变换到上一帧坐标系，得到 p'。在上一帧中找到三个平面点 a、b、c，它们构成一个局部平面。
先计算平面法向量：
n = (b - a) × (c - a)点到平面的距离可以写成：
d = n · (p' - a) / ||n||这里：
n：
平面法向量。
p' - a：
从平面上一点 a 指向当前点 p' 的向量。
n · (p' - a)：
当前点在法向量方向上的投影。
d：
当前点到平面的距离。优化目标是让 d 尽量接近 0。也就是当前帧平面点经过位姿变换后，应该尽量落在上一帧对应的平面上。
3.9 两阶段 LM 优化
LeGO-LOAM 前端优化的一个重要特点是，它不是简单一次性优化所有 6 个自由度，而是分阶段优化。
前端位姿变量可以理解为：
roll, pitch, yaw, x, y, zLeGO-LOAM 会先利用平面特征优化一部分变量，再利用边缘特征优化另一部分变量。
大致可以理解为：
第一阶段：
主要利用平面特征，优化 roll、pitch、z。
第二阶段：
主要利用边缘特征，优化 yaw、x、y。这样做的原因是，平面结构尤其是地面和平坦表面对姿态和高度约束比较明显，而边缘结构对水平位置和航向角约束更明显。把问题拆开后，优化规模更小，计算更快，也更稳定。
每一轮优化都会构建线性方程：
A x = b其中：
A：
残差对位姿变量的雅可比矩阵。
x：
要求解的位姿增量。
b：
残差项。求解出位姿增量后，更新当前估计的帧间变换。经过多次迭代后，得到当前帧相对于上一帧的运动估计。
3.10 integrateTransformation()：累计前端里程计
updateTransformation() 得到的是当前帧相对于上一帧的增量运动。为了发布连续轨迹，需要把每一帧的相对运动累计起来。
这一步由 integrateTransformation() 完成。
可以理解为：
上一时刻全局位姿
×
当前帧相对上一帧的增量
=
当前帧全局 odometry 位姿在 LeGO-LOAM 中，前端累计位姿通常保存在类似 transformSum 这样的变量中。它表示从系统起始时刻到当前帧的前端里程计估计。
前端 odometry 的特点是频率较高，能够连续输出，但由于它主要依赖相邻帧匹配，长期运行会产生累计漂移。因此后面还需要 mapOptmization.cpp 做 scan-to-map 修正。
4. mapOptmization.cpp：后端建图和地图优化
mapOptmization.cpp 是 LeGO-LOAM 的后端部分。它接收前端输出的 odometry 和特征点云，然后把当前帧和局部地图进行匹配，进一步优化当前帧在地图中的位姿。
这个文件主要完成：
1. 接收前端角点、平面点、全分辨率点云和 odometry；
2. 根据前端 odometry 预测当前帧在地图中的位姿；
3. 提取当前位姿附近的历史关键帧，构建局部地图；
4. 将当前帧特征点和局部地图匹配；
5. 通过 scan-to-map 优化当前帧位姿；
6. 判断是否保存关键帧；
7. 添加里程计因子和回环因子；
8. 使用 GTSAM 更新关键帧位姿；
9. 发布优化后的地图位姿和点云地图。4.1 后端接收的输入
mapOptmization.cpp 会订阅前端输出的几类数据：
laserCloudCornerLast：
当前帧角点。
laserCloudSurfLast：
当前帧平面点。
laserCloudFullRes：
当前帧完整点云。
laserOdometry：
前端估计的当前帧 odometry。这些输入来自 featureAssociation.cpp。前端已经完成了 scan-to-scan 的相邻帧匹配，而后端要做的是 scan-to-map，也就是把当前帧和历史局部地图对齐。
4.2 transformAssociateToMap()：根据前端结果预测后端初值
后端优化不能没有初始值。LeGO-LOAM 会利用前端 odometry 的增量，为当前帧在地图坐标系下的位置提供一个初始估计。
这里涉及几个重要变量：
transformSum：
前端 odometry 当前累计位姿。
transformBefMapped：
上一次后端优化前，对应的前端位姿。
transformAftMapped：
上一次后端优化后的地图位姿。
transformTobeMapped：
当前帧待优化的地图位姿。直观理解如下：
transformSum：
前端认为当前帧在哪里。
transformAftMapped：
后端上一次修正后认为上一关键时刻在哪里。
transformBefMapped：
后端上一次修正时，前端当时认为在哪里。
transformTobeMapped：
综合前端增量和后端历史修正后，得到当前帧的初始地图位姿。也就是说，后端不会直接相信前端 odometry 的绝对累计结果，而是使用：
上一次后端修正后的位姿
+
从上一次修正之后到当前时刻的前端增量来预测当前帧初值。
这样可以利用前端的连续运动估计，同时保持后端修正的一致性。
4.3 extractSurroundingKeyFrames()：提取局部关键帧地图
后端 scan-to-map 不会拿当前帧和所有历史点云匹配，因为历史点云太多，计算量太大，而且远处地图和当前帧通常没有重叠。
所以 LeGO-LOAM 会根据当前位姿附近的关键帧，构建一个局部地图。
大致流程是：
1. 当前估计位姿 transformTobeMapped；
2. 在历史关键帧位姿中查找附近关键帧；
3. 取出这些关键帧对应的角点和平面点；
4. 把这些点从各自关键帧坐标系变换到地图坐标系；
5. 合并成局部 corner map 和 surface map；
6. 对局部地图进行体素滤波降采样。局部地图一般分成两类：
laserCloudCornerFromMap：
局部地图中的角点集合。
laserCloudSurfFromMap：
局部地图中的平面点集合。后面当前帧的角点会去 laserCloudCornerFromMap 中找线特征，当前帧的平面点会去 laserCloudSurfFromMap 中找面特征。
4.4 downsampleCurrentScan()：当前帧特征降采样
当前帧特征点在进入 scan-to-map 优化之前，也会做降采样。
主要对象是：
laserCloudCornerLast：
当前帧角点。
laserCloudSurfLast：
当前帧平面点。降采样后得到：
laserCloudCornerStack：
当前帧用于后端优化的角点。
laserCloudSurfStack：
当前帧用于后端优化的平面点。为什么要降采样？因为后端匹配需要 KD-tree 最近邻搜索，如果点太多，计算量会很大。而且很多相邻点提供的约束是重复的，适当降采样不会明显降低约束质量，反而能提升实时性。
4.5 scan2MapOptimization()：当前帧与局部地图匹配优化
这是 mapOptmization.cpp 的核心函数。它的目标是优化 transformTobeMapped，也就是当前帧在地图坐标系下的位姿。
优化过程主要包括两类约束：
角点约束：
当前帧角点到局部地图边缘线的距离。
平面约束：
当前帧平面点到局部地图平面的距离。4.6 当前帧角点如何和地图角点匹配
对于当前帧中的每个角点，先通过当前估计位姿把它变换到地图坐标系：
pointOri：当前帧坐标系下的点
pointSel：变换到地图坐标系后的点然后在局部角点地图 laserCloudCornerFromMap 中寻找附近点。一般会使用 KD-tree 查找最近邻。
找到若干邻近点之后，并不是直接拿最近两个点构成线，而是需要判断这些邻近点是否真的呈线状分布。
常见做法是对邻近点做协方差分析。如果这些点主要沿某一个方向分布，那么最大特征值会明显大于其他特征值，说明它们可以构成一条稳定的线特征。然后根据这条线构建点到线残差。
直观流程：
当前帧角点
↓
变换到地图坐标系
↓
KD-tree 搜索局部地图角点邻居
↓
判断邻居是否线状分布
↓
构建点到线残差如果邻居点分布不像线，比如比较散乱，那么这个角点约束就会被丢弃。
4.7 当前帧平面点如何和地图平面点匹配
对于当前帧中的每个平面点，同样先变换到地图坐标系，然后在局部平面地图 laserCloudSurfFromMap 中找邻近点。
找到若干邻近点后，用这些点拟合平面。平面一般可以写成：
ax + by + cz + d = 0其中：
a, b, c：
平面法向量分量。
d：
平面偏置项。然后检查邻近点到这个平面的距离是否都比较小。如果距离都比较小，说明这个平面拟合可靠，可以用来构建约束。如果某些邻近点离平面很远，说明这些点不在同一平面上，这个约束就不能用。
直观流程：
当前帧平面点
↓
变换到地图坐标系
↓
KD-tree 搜索局部地图平面点邻居
↓
拟合局部平面
↓
检查平面拟合质量
↓
构建点到面残差优化目标就是让当前帧平面点尽量落到地图中的对应平面上。
4.8 LMOptimization()：后端位姿迭代优化
当角点残差和平面点残差都构建好之后，LeGO-LOAM 会进行 LM 优化，求解当前位姿增量。
它会把所有有效约束组合成一个线性系统：
A x = b其中：
A：
所有残差对位姿变量的雅可比矩阵。
x：
待求解的位姿增量，包括旋转和平移增量。
b：
残差项。求解得到 x 后，更新 transformTobeMapped。然后继续下一轮迭代。
整个过程可以理解为：
1. 根据当前位姿，把当前帧点投到地图坐标系；
2. 在局部地图中寻找对应线和平面；
3. 根据对应关系计算残差和雅可比；
4. 求解位姿增量；
5. 更新当前位姿；
6. 重复迭代，直到收敛或达到最大迭代次数。后端优化完成后，transformTobeMapped 就表示当前帧经过 scan-to-map 修正后的地图位姿。
4.9 saveKeyFramesAndFactor()：保存关键帧和添加位姿图因子
后端优化完成后，LeGO-LOAM 会判断是否需要保存当前帧为关键帧。
如果当前帧相对于上一个关键帧移动或旋转足够大，就保存关键帧。关键帧通常保存：
1. 当前帧优化后的位姿；
2. 当前帧角点特征；
3. 当前帧平面特征；
4. 当前帧完整点云。同时，系统会把关键帧位姿加入 GTSAM 位姿图。
对于第一帧，通常会添加先验因子：
PriorFactor表示第一帧位姿作为图优化的起点。
对于后续关键帧，会添加相邻帧之间的里程计约束：
BetweenFactor这个约束来自当前关键帧和上一关键帧之间的相对位姿变化。
可以理解为：
关键帧 0 ------ 关键帧 1 ------ 关键帧 2 ------ 关键帧 3每两个相邻关键帧之间都有一个里程计约束。后面如果检测到回环，还会在当前关键帧和历史关键帧之间再加一条回环约束。
4.10 correctPoses()：图优化后修正历史关键帧位姿
如果没有回环，关键帧位姿基本按照里程计顺序往前延伸。如果出现回环并且 GTSAM 优化成功，历史关键帧位姿可能会被整体修正。
这时 LeGO-LOAM 需要更新保存的关键帧位姿：
cloudKeyPoses3D
cloudKeyPoses6D其中：
cloudKeyPoses3D：
保存关键帧的位置，通常用于空间搜索和可视化。
cloudKeyPoses6D：
保存关键帧完整 6D 位姿，包括 roll、pitch、yaw、x、y、z。修正历史位姿后，后续提取局部地图时，就会使用修正后的关键帧位置。这样地图整体一致性会更好。
4.11 回环线程：loopClosureThread()
LeGO-LOAM 的回环检测通常在单独线程中运行。它不会每一帧都强制做回环，因为回环检测和 ICP 配准计算量比较大。
回环检测的大致流程是：
1. 根据当前关键帧位置，在历史关键帧中找距离较近的候选帧；
2. 排除时间上太近的关键帧，避免把相邻帧当成回环；
3. 构建当前关键帧附近的局部点云；
4. 构建历史候选关键帧附近的局部点云；
5. 使用 ICP 对当前局部点云和历史局部点云配准；
6. 如果 ICP 收敛且匹配误差满足阈值，就认为检测到回环；
7. 添加当前关键帧和历史关键帧之间的回环约束；
8. 触发 GTSAM 位姿图优化；
9. 修正历史关键帧位姿。回环约束本质上也是一个 BetweenFactor，表示：
当前关键帧 和 历史关键帧 之间应该满足某个相对位姿关系这个相对位姿由 ICP 配准结果得到。
5. transformFusion.cpp：前端和后端位姿融合发布
transformFusion.cpp 是 LeGO-LOAM 最后一个输出模块。它的作用是融合前端高频 odometry 和后端低频 mapping 结果，发布连续、平滑的最终位姿。
前端 featureAssociation.cpp 输出的是：
/laser_odom_to_init它频率较高，连续性好，但是长期会有漂移。
后端 mapOptmization.cpp 输出的是：
/aft_mapped_to_init它频率较低，但经过 scan-to-map 和图优化修正，位姿更可靠。
transformFusion.cpp 的基本思想是：
用后端 mapping 位姿作为全局修正基准；
用前端 odometry 提供两次后端更新之间的短时间增量；
最终发布连续的融合位姿。可以理解为：
最终位姿 = 最近一次后端优化位姿 + 后端更新之后前端累计的相对运动这样既不会丢失前端的高频连续性，又能利用后端的地图修正结果。
它最终会发布：
odometry
path
tf这些结果可以在 RViz 中显示轨迹，也可以给导航、定位、建图模块使用。
6. LeGO-LOAM 完整流程详细总结
LeGO-LOAM 的完整运行流程可以从一帧原始 LiDAR 点云开始理解。首先，系统通过 imageProjection.cpp 接收 /velodyne_points 原始点云，将 ROS 点云转换成 PCL 点云，并去除 NaN 无效点。随后，根据当前帧第一个点和最后一个点计算扫描起始角、结束角和扫描角度差，为后续点云去畸变和相对时间计算做准备。接着，系统遍历每一个点，根据点的线束 ID 或垂直角确定它在 range image 中的行号，根据水平角确定它在 range image 中的列号，同时计算点到雷达原点的距离，并把点存入 fullCloud 和 rangeMat 中。到这一步，原本无序的三维点云就被转换成了一个规则的二维深度图结构。
投影完成后，LeGO-LOAM 利用低线束之间的几何关系做地面检测。它会比较同一列中相邻上下线束的两个点，计算它们之间连线相对于水平面的夹角。如果这个角度接近雷达安装角，就把这些点标记为地面点。地面点会在 groundMat 和 labelMat 中被标记出来，不再参与普通非地面物体的连通域分割。之后，系统对剩余非地面点进行基于 range image 的区域生长分割，从未标记点开始检查上下左右邻居，根据相邻点的距离和角分辨率判断它们是否属于连续表面。如果一个点簇足够大或者跨越足够多扫描线，就认为它是有效 segment；如果点簇太小，则认为它可能是噪声或离群点。最终，imageProjection.cpp 输出经过整理的 segmentedCloud、地面点云、离群点云，以及包含每个点行列索引、深度、地面标记、扫描线起止位置等信息的 segMsg。
然后，featureAssociation.cpp 接收这些结构化点云信息，开始进行前端里程计估计。它首先根据一帧点云中的相对扫描时间和 IMU 姿态信息，对点云进行运动畸变校正。因为机械式 LiDAR 一帧点云是旋转扫描得到的，不同点的采集时刻不同，如果机器人在这一帧内运动，点云内部就会发生扭曲。校正完成后，系统根据每个点前后若干邻居点的距离变化计算曲率。曲率大的点通常对应物体边缘、墙角、柱子边界等结构；曲率小的点通常对应墙面、地面等平滑区域。
计算曲率之后，LeGO-LOAM 会进一步标记遮挡点和平行光束点。因为遮挡边界虽然曲率大，但不是稳定特征，机器人视角稍微变化，这些点的位置就可能发生明显变化，所以不能直接拿来做优化。系统会通过相邻点距离突变来判断遮挡区域，并把这些点标记为不可选。随后，系统按每根 scan line 分段提取特征点，在每个分段内按曲率从大到小选择角点，按曲率从小到大选择平面点。选中一个特征点后，会把它附近若干点标记为已占用，避免在同一局部区域重复选择过多特征。最终得到 cornerPointsSharp、cornerPointsLessSharp、surfPointsFlat 和 surfPointsLessFlat 四类特征点。
特征提取完成后，前端开始进行 scan-to-scan 匹配。对于当前帧角点，系统会在上一帧的角点集合中寻找对应边缘线，并构建点到线距离残差；对于当前帧平面点，系统会在上一帧的平面点集合中寻找对应局部平面，并构建点到面距离残差。然后通过两阶段 LM 优化求解当前帧相对于上一帧的位姿增量。第一阶段主要利用平面特征约束姿态和高度相关变量，第二阶段主要利用边缘特征约束水平位置和航向相关变量。优化完成后，前端将帧间增量累计到 transformSum 中，得到连续的前端 odometry，并发布当前帧角点、平面点和前端位姿。
接下来，mapOptmization.cpp 接收前端发布的角点、平面点、完整点云和 odometry。它首先通过 transformAssociateToMap() 把前端 odometry 增量和上一次后端优化结果结合起来，得到当前帧在地图坐标系中的初始位姿 transformTobeMapped。然后根据这个初值，在历史关键帧中查找当前位置附近的关键帧，取出这些关键帧的角点和平面点，并变换到地图坐标系下，拼接成当前帧附近的局部角点地图和局部平面地图。为了降低计算量，局部地图和当前帧特征点都会进行体素滤波降采样。
局部地图构建完成后，后端执行 scan-to-map 优化。当前帧角点会被变换到地图坐标系，并在局部角点地图中通过 KD-tree 查找邻近点。如果邻近点呈明显线状分布，就构建点到线约束。当前帧平面点同样会被变换到地图坐标系，并在局部平面地图中查找邻近点，通过邻近点拟合局部平面，构建点到面约束。系统把所有有效的线约束和平面约束组合起来，通过 LM 迭代优化 transformTobeMapped，最终得到当前帧经过地图修正后的位姿。
后端优化完成后，系统会判断当前帧是否需要保存为关键帧。如果当前帧相对上一个关键帧的平移或旋转超过阈值，就保存当前帧位姿、角点云、平面点云和完整点云。同时，系统会把关键帧位姿加入 GTSAM 位姿图。第一帧加入先验因子，后续关键帧加入相邻帧之间的里程计因子。如果回环线程检测到当前关键帧和某个历史关键帧存在回环关系，就会通过 ICP 计算它们之间的相对位姿，并把这个回环关系作为额外约束加入位姿图。GTSAM 优化后，历史关键帧位姿会被修正，地图的一致性也会随之改善。
最后，transformFusion.cpp 接收前端高频 odometry 和后端低频 mapping 位姿。前端 odometry 连续性好，但长期漂移；后端 mapping 位姿更准确，但频率较低。融合模块使用后端优化结果作为全局修正基准，再叠加前端在两次后端更新之间的相对运动，得到连续、平滑、并且经过地图修正的最终位姿。最终系统发布 odometry、path 和 tf，供 RViz 可视化或者后续导航定位模块使用。
所以，LeGO-LOAM 的完整流程可以浓缩成一句话：imageProjection.cpp 先把原始点云投影、地面检测并分割；featureAssociation.cpp 再从分割点云中提取角点和平面点，通过 scan-to-scan 估计前端里程计；mapOptmization.cpp 进一步把当前帧和局部地图做 scan-to-map 匹配，并通过关键帧和位姿图维护地图一致性；transformFusion.cpp 最后融合前端高频位姿和后端优化位姿，发布连续轨迹**。**