1. 这个文件整体作用
mapOptimization 是 LeGO-LOAM 后端建图优化模块。前面的 FeatureAssociation 已经完成了前端帧间匹配，输出当前帧的角点、平面点、离群点和激光里程计 /laser_odom_to_init。mapOptimization 接收这些数据后，不再只和上一帧匹配，而是把当前帧特征点和历史关键帧构成的局部地图做匹配，也就是 scan-to-map optimization。
整体流程可以概括为：
FeatureAssociation 输出当前帧特征点 + 前端 odometry
↓
mapOptimization 接收角点、平面点、outlier、laser odometry
↓
transformAssociateToMap() 根据前端增量和上次后端位姿生成当前帧地图系初值
↓
extractSurroundingKeyFrames() 选取附近历史关键帧，构建局部地图
↓
downsampleCurrentScan() 对当前帧特征点降采样
↓
cornerOptimization() 构建点到线残差
↓
surfOptimization() 构建点到面残差
↓
LMOptimization() 构造 matA / matB，求解当前帧位姿增量
↓
saveKeyFramesAndFactor() 保存关键帧，并加入 GTSAM 位姿图
↓
loopClosureThread() 后台检测回环，成功后添加回环 BetweenFactor
↓
correctPoses() 回环后修正历史关键帧轨迹这个文件有两层优化：第一层是 每一帧的 scan-to-map 点云几何优化 ，优化对象是当前帧位姿 transformTobeMapped；第二层是 关键帧级别的 GTSAM/iSAM2 位姿图优化 ，优化对象是所有关键帧的全局位姿 T_0, T_1, ..., T_k。
2. 输入、输出和核心变量
构造函数中订阅了 4 个关键话题：
subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>(
"/laser_cloud_corner_last", 2,
&mapOptimization::laserCloudCornerLastHandler, this);
subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>(
"/laser_cloud_surf_last", 2,
&mapOptimization::laserCloudSurfLastHandler, this);
subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>(
"/outlier_cloud_last", 2,
&mapOptimization::laserCloudOutlierLastHandler, this);
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>(
"/laser_odom_to_init", 5,
&mapOptimization::laserOdometryHandler, this);/laser_cloud_corner_last 是当前帧角点，/laser_cloud_surf_last 是当前帧平面点，/outlier_cloud_last 是前端输出的离群点，/laser_odom_to_init 是前端激光里程计估计。注意这里的 outlier 不是完全没用的垃圾点，它会被降采样后和 surf 点合并，作为 surface 类约束的一部分参与后端优化。
代码中最重要的 6 个位姿数组是：
float transformLast[6];
float transformSum[6];
float transformIncre[6];
float transformTobeMapped[6];
float transformBefMapped[6];
float transformAftMapped[6];它们的含义如下：
transformSum 是前端 FeatureAssociation 发布的当前激光里程计位姿，也就是 /laser_odom_to_init。它来自前端帧间匹配，属于较高频的短期 odometry。
transformBefMapped 是上一次 mapOptimization 处理时，对应的前端 odometry 位姿。它不是后端优化位姿，而是"上次后端处理时刻的前端位姿"。
transformAftMapped 是上一次 mapOptimization 优化完成后的后端修正位姿。这个不是增量，而是机器人在地图坐标系 /camera_init 下的全局位姿，也就是上一次 scan-to-map 和 GTSAM 更新后得到的后端 pose。
transformIncre 是当前前端 odometry 相对上一次前端 odometry 的增量。
transformTobeMapped 是当前帧准备拿去 scan-to-map 优化的位姿变量。它先由 transformAssociateToMap() 计算初值，然后在 LMOptimization() 中不断被更新，最后变成当前帧优化后的地图位姿。
3. transformAftMapped 和 transformTobeMapped 的关系
你前面问得很关键：transformAftMapped 到底是增量还是全局位姿？答案是：
transformAftMapped 是上一次地图优化后的全局位姿，不是增量。它表示上一次后端修正后机器人在地图坐标系下的位置和姿态。
代码在 transformUpdate() 结尾处这样写：
for (int i = 0; i < 6; i++) {
transformBefMapped[i] = transformSum[i];
transformAftMapped[i] = transformTobeMapped[i];
}这说明每次当前帧 scan-to-map 优化结束后，transformTobeMapped 就变成当前帧后端优化位姿，然后被保存到 transformAftMapped，供下一帧使用。下一帧进入 transformAssociateToMap() 时，代码会用当前前端 odometry transformSum 和上一次前端 odometry transformBefMapped 算一个相对增量，再把这个增量接到上一次后端全局位姿 transformAftMapped 上。
公式可以写成：
T_tobeMapped = T_aftMapped_last · (T_befMapped_last^{-1} · T_sum_current)其中 T_sum_current 是当前前端 odometry 位姿，T_befMapped_last 是上一次后端处理时保存的前端 odometry 位姿，T_aftMapped_last 是上一次后端优化后的全局位姿。括号里的 T_befMapped_last^{-1} · T_sum_current 是前端 odometry 的相对增量，左乘到 T_aftMapped_last 上，就得到当前帧在地图坐标系下的 scan-to-map 初值。
所以 transformTobeMapped 的完整生命周期是：
transformAssociateToMap()：
根据前端 odometry 增量 + 上次后端全局位姿，生成当前帧初值。
scan2MapOptimization()：
当前帧点云根据 transformTobeMapped 投到地图系，
构建点到线、点到面残差，
LMOptimization() 迭代更新 transformTobeMapped。
transformUpdate()：
把优化后的 transformTobeMapped 保存成 transformAftMapped。T_befMapped_last^{-1} · T_sum_current 的意思就是从两个"前端累计位姿"里面算出一段"相对运动增量"。其中 T_sum_current 是当前帧前端 odometry 从起点 /camera_init 累计到现在的总位姿，T_befMapped_last 是上一次后端处理时保存的前端累计总位姿，不一定严格是上一帧，而是上一次 mapOptimization 成功处理那一帧对应的前端位姿。T_befMapped_last^{-1} 表示 T_befMapped_last 的逆变换，可以理解成把上一次前端累计位姿"反过来抵消掉"。所以 T_befMapped_last^{-1} · T_sum_current 就是在问：当前前端总位姿相对于上一次前端总位姿，又新增了多少运动。简单一维里它类似于 12m - 10m = 2m，但在三维 SLAM 里位姿包含旋转和平移，不能直接相减，必须用逆变换乘当前位姿来求相对位姿。得到这个新增运动量后，再接到上一次后端优化后的全局位姿 T_aftMapped_last 上，就得到当前帧后端 scan-to-map 的初始位姿。
4. pointAssociateToMap()：当前帧点如何变到地图坐标系
pointAssociateToMap() 的作用是把当前帧 LiDAR 坐标系下的点变换到地图坐标系。它用的就是当前迭代下的 transformTobeMapped。
核心代码：
void pointAssociateToMap(PointType const * const pi, PointType * const po)
{
float x1 = cYaw * pi->x - sYaw * pi->y;
float y1 = sYaw * pi->x + cYaw * pi->y;
float z1 = pi->z;
float x2 = x1;
float y2 = cRoll * y1 - sRoll * z1;
float z2 = sRoll * y1 + cRoll * z1;
po->x = cPitch * x2 + sPitch * z2 + tX;
po->y = y2 + tY;
po->z = -sPitch * x2 + cPitch * z2 + tZ;
po->intensity = pi->intensity;
}这个函数对应的数学公式是：
p_map = R(transformTobeMapped) · p_lidar + t(transformTobeMapped)其中 p_lidar 是当前帧坐标系下的点，p_map 是投到地图坐标系后的点，R 是由 roll、pitch、yaw 组成的旋转矩阵，t 是平移。注意这里的 transformTobeMapped 不是固定不变的，它在 scan-to-map 迭代中不断变化。每次 cornerOptimization() 和 surfOptimization() 执行前都会根据当前 transformTobeMapped 更新三角函数，然后把当前点投到地图坐标系，再去局部地图中找最近邻。
5. 局部地图构建：为什么不是直接用地面点 BFS 簇？
extractSurroundingKeyFrames() 的作用是从历史关键帧中选出当前机器人附近的一批关键帧，并把这些关键帧保存的特征点变换到地图坐标系，组成局部地图。这里要区分几个阶段。
ImageProjection 里的 BFS 聚类只是前端预处理，用来把原始点云分割成地面点、有效非地面点和 outlier。它的结果不是直接给后端当地图。之后 FeatureAssociation 会在 segmented cloud 上计算曲率、遮挡关系，并提取角点和平面点，最后发布：
/laser_cloud_corner_last
/laser_cloud_surf_last
/outlier_cloud_lastmapOptimization 构建局部地图用的是这些特征点，而不是 BFS 的 cluster。代码中保存了三类关键帧点云：
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> outlierCloudKeyFrames;构建局部地图时，代码会把关键帧点云变换到地图坐标系：
构建局部地图时，代码就是通过每个历史关键帧保存下来的位姿 cloudKeyPoses6D[thisKeyInd]，把该关键帧自己的点云从"关键帧局部 LiDAR 坐标系"变换到统一的"地图坐标系"。关键帧保存时，cornerCloudKeyFrames、surfCloudKeyFrames、outlierCloudKeyFrames 里面的点云本身仍然是在该关键帧采集时的局部坐标系下，并不是直接保存成全局坐标；同时系统会在 cloudKeyPoses6D 中保存这个关键帧对应的全局 6D 位姿。后面 extractSurroundingKeyFrames() 构建局部地图时，会先取出某个关键帧编号 thisKeyInd，再取出它的位姿 cloudKeyPoses6D->points[thisKeyInd]，然后调用 transformPointCloud()，按照公式 p_map = R_keyframe · p_keyframe + t_keyframe，把这个关键帧的角点、平面点和离群点全部变换到地图坐标系，最后再拼接进 laserCloudCornerFromMap 和 laserCloudSurfFromMap。所以局部地图不是把历史点云原样堆在一起，而是每个关键帧点云都先根据自己的关键帧位姿转换到同一个全局地图坐标系下，再进行拼接，这样当前帧点云投到地图系后，才能和这些历史地图点做最近邻匹配。
*laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i];也就是说，局部角点地图 laserCloudCornerFromMap 只加入历史角点；局部平面地图 laserCloudSurfFromMap 加入历史平面点和历史 outlier 点。
为什么 outlier 也加入 surf map？因为这里的 outlier 不是完全不能用的噪声，而是前端没有归入主要角点/平面点的一些稀疏结构点，其中可能包含弱边缘、非地面稀疏面、障碍物表面等。后端不会直接信任它们，而是先降采样，再在 surfOptimization() 中通过最近邻距离、平面拟合有效性、残差权重继续筛选。换句话说，outlier 在这里是"弱 surface 候选"，不是直接当可靠地图点。
构建完成后会降采样：
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);这样得到的 laserCloudCornerFromMapDS 和 laserCloudSurfFromMapDS 就是当前 scan-to-map 优化使用的局部地图。
6. 当前帧降采样：downsampleCurrentScan()
当前帧的角点、平面点和 outlier 也需要降采样，避免点太多导致优化慢。
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerLastDS);
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfLastDS);
downSizeFilterOutlier.setInputCloud(laserCloudOutlierLast);
downSizeFilterOutlier.filter(*laserCloudOutlierLastDS);然后把当前帧的 surf 点和 outlier 点合并：
*laserCloudSurfTotalLast += *laserCloudSurfLastDS;
*laserCloudSurfTotalLast += *laserCloudOutlierLastDS;
downSizeFilterSurf.setInputCloud(laserCloudSurfTotalLast);
downSizeFilterSurf.filter(*laserCloudSurfTotalLastDS);所以当前帧参与优化的数据分成两类：
laserCloudCornerLastDS：
当前帧角点，用于点到线残差。
laserCloudSurfTotalLastDS：
当前帧平面点 + outlier，用于点到面残差。7. scan-to-map 主优化流程
scan2MapOptimization() 是当前帧后端匹配的核心函数。
void scan2MapOptimization(){
if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 10; iterCount++) {
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization(iterCount);
surfOptimization(iterCount);
if (LMOptimization(iterCount) == true)
break;
}
transformUpdate();
}
}这里有三个关键点。
第一，只有局部地图中的角点数量大于 10，平面点数量大于 100，才进行优化。特征太少时约束不足，优化不可靠。
第二，cornerOptimization() 和 surfOptimization() 只是构建残差，把原始点放入 laserCloudOri，把残差系数放入 coeffSel。它们不直接更新位姿。
第三，真正更新 transformTobeMapped 的是 LMOptimization()。它把所有残差统一构造成线性系统，解出 6 自由度位姿增量。
8. cornerOptimization()：点到线残差如何构建
角点优化的流程是：
当前帧角点 pointOri
↓
pointAssociateToMap() 投到地图坐标系，得到 pointSel
↓
在地图角点中找最近 5 个点
↓
判断这 5 个点是否近似成线
↓
如果成线，构建 pointSel 到这条线的距离残差
↓
把 pointOri 和 coeff 存入 laserCloudOri / coeffSel核心代码：
pointOri = laserCloudCornerLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
kdtreeCornerFromMap->nearestKSearch(
pointSel, 5, pointSearchInd, pointSearchSqDis);
if (pointSearchSqDis[4] < 1.0) {
// 计算 5 个地图角点的中心
// 构建协方差矩阵 matA1
// 对 matA1 做特征值分解
cv::eigen(matA1, matD1, matV1);
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
// 认为这 5 个点呈线状分布
}
}这里 pointSearchSqDis[4] < 1.0 表示第 5 近的地图角点距离也小于 1m，说明附近有足够可靠的地图角点。然后代码计算 5 个点的协方差矩阵并做特征值分解。如果最大特征值明显大于第二大特征值：
λ₁ > 3λ₂说明这 5 个点主要沿一个方向分布，也就是像一条线。代码用最大特征值对应的特征向量作为线方向，构造线上的两个点 p1、p2。
点到线残差公式是：
r_edge = ||(p0 - p1) × (p0 - p2)|| / ||p1 - p2||其中 p0 是当前角点投到地图系后的点 pointSel，p1、p2 是地图线特征上的两个点。代码中：
float ld2 = a012 / l12;a012 对应叉乘模长 ||(p0-p1)×(p0-p2)||，l12 对应线段长度 ||p1-p2||，所以 ld2 就是点到线距离。
然后代码计算 la、lb、lc：
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;这里 coeff.x、coeff.y、coeff.z 是残差对地图点坐标的梯度方向，coeff.intensity 是加权后的点到线距离。s = 1 - 0.9 * fabs(ld2) 是一个简单鲁棒权重，残差越大权重越小，s > 0.1 才保留这条约束。
9. surfOptimization()：点到面残差如何构建
平面点优化流程是：
当前帧平面点 pointOri
↓
pointAssociateToMap() 投到地图坐标系，得到 pointSel
↓
在地图平面点中找最近 5 个点
↓
用 5 个点拟合平面
↓
检查 5 个点是否真的都接近平面
↓
如果平面有效，构建当前点到平面的距离残差核心代码：
pointOri = laserCloudSurfTotalLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(
pointSel, 5, pointSearchInd, pointSearchSqDis);
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
matA0.at<float>(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0.at<float>(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0.at<float>(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);
}代码拟合的平面是：
pa·x + pb·y + pc·z + pd = 0因为 matB0 初始化为 -1，代码相当于解：
[x y z] [pa pb pc]^T = -1然后令 pd = 1，得到平面参数。之后归一化：
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps; pb /= ps; pc /= ps; pd /= ps;归一化后 n = [pa, pb, pc]^T 是单位法向量，当前点到平面的有符号距离就是：
r_surf = pa·x0 + pb·y0 + pc·z0 + pd代码对应：
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;其中 pd2 是点到面的残差，coeff.x/y/z 是平面法向量，也就是残差对点坐标的梯度。代码还会检查 5 个邻近点到拟合平面的距离是否都小于 0.2m，如果有点离平面太远，就认为平面无效，不加入优化。
10. LMOptimization()：如何把残差转成位姿增量
这是你最关心的部分。cornerOptimization() 和 surfOptimization() 得到的是很多条几何约束，每条约束都可以抽象成一个残差：
r_i(T)其中 T 是当前待优化位姿，也就是 transformTobeMapped。由于 T 包含旋转，所以残差是非线性的。LM/Gauss-Newton 的做法是在当前位姿附近线性化：
r_i(T ⊕ Δx) ≈ r_i(T) + J_i Δx其中：
Δx = [δroll, δpitch, δyaw, δx, δy, δz]^TJ_i 是第 i 个残差对 6 个位姿变量的偏导：
J_i = [∂r_i/∂roll, ∂r_i/∂pitch, ∂r_i/∂yaw, ∂r_i/∂x, ∂r_i/∂y, ∂r_i/∂z]所有残差叠起来就是：
J Δx = -r正规方程是：
(J^T J) Δx = -J^T r代码里：
matA = J
matB = -r
matX = Δx核心代码如下：
int laserCloudSelNum = laserCloudOri->points.size();
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
for (int i = 0; i < laserCloudSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float arx = ...; // 残差对 roll 的偏导
float ary = ...; // 残差对 pitch 的偏导
float arz = ...; // 残差对 yaw 的偏导
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = ary;
matA.at<float>(i, 2) = arz;
matA.at<float>(i, 3) = coeff.x;
matA.at<float>(i, 4) = coeff.y;
matA.at<float>(i, 5) = coeff.z;
matB.at<float>(i, 0) = -coeff.intensity;
}这一行线性方程就是：
[arx ary arz coeff.x coeff.y coeff.z] · Δx = -coeff.intensity前三列 arx、ary、arz 是旋转雅可比。因为点坐标经过欧拉角旋转后再代入残差，所以对 roll、pitch、yaw 求导会得到一大串三角函数。后三列 coeff.x、coeff.y、coeff.z 是平移雅可比，因为点坐标对平移的导数就是单位方向，而残差对点坐标的梯度已经存在 coeff 里。
以点到面残差为例：
r = n^T (R p + t) + d其中 n = [coeff.x, coeff.y, coeff.z]^T，所以：
∂r/∂t = n也就是：
∂r/∂x = coeff.x
∂r/∂y = coeff.y
∂r/∂z = coeff.z构造完 matA 和 matB 后：
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);对应：
matAtA = J^T J
matAtB = J^T(-r)
matX = Δx求出 matX 后更新当前位姿：
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);所以 LMOptimization() 的本质就是：把每个点到线/点到面残差线性化成一行方程，所有方程组成 matA Δx = matB，再解正规方程得到当前帧位姿增量。
11. 退化检测：为什么要处理小特征值
代码第一次迭代时会对 J^T J 做特征值分解：
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;如果某些特征值很小，说明当前环境对某些自由度约束不足。例如长走廊里，某些平移方向或 yaw 方向可能没有足够约束；大平面场景里，沿平面方向也可能不可观。代码把这些方向对应的特征向量置零，再用 matP 投影掉不可靠方向的更新：
if (isDegenerate) {
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}这样可以避免位姿在退化方向上被错误残差拉飞。
12. scan-to-map 总目标函数
当前帧 scan-to-map 优化的目标函数可以写成：
min_T Σ ||r_edge(T)||² + Σ ||r_surf(T)||²其中：
r_edge(T) = ||(T·p_i - p_a) × (T·p_i - p_b)|| / ||p_a - p_b||p_i 是当前帧角点，p_a、p_b 是地图中拟合出的线特征上的两个点。
r_surf(T) = n_j^T (T·p_j) + d_jp_j 是当前帧平面点，n_j 是地图平面法向量，d_j 是平面常数项。这个目标函数优化的是当前帧位姿 T，也就是 transformTobeMapped。
注意：点云残差没有交给 Ceres 或 GTSAM 优化，而是代码自己构造 matA 和 matB，用 OpenCV 解正规方程。GTSAM/iSAM2 只用于关键帧级别的位姿图优化。
13. saveKeyFramesAndFactor()：关键帧保存和 GTSAM 因子图
scan-to-map 优化完后，saveKeyFramesAndFactor() 会判断当前帧是否保存为关键帧。
currentRobotPosPoint.x = transformAftMapped[3];
currentRobotPosPoint.y = transformAftMapped[4];
currentRobotPosPoint.z = transformAftMapped[5];
if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)^2
+(previousRobotPosPoint.y-currentRobotPosPoint.y)^2
+(previousRobotPosPoint.z-currentRobotPosPoint.z)^2) < 0.3){
saveThisKeyFrame = false;
}如果距离上一关键帧小于 0.3m，就不保存，避免关键帧太密。
如果是第一帧，添加先验因子：
gtSAMgraph.add(PriorFactor<Pose3>(
0,
Pose3(Rot3::RzRyRx(transformTobeMapped[2],
transformTobeMapped[0],
transformTobeMapped[1]),
Point3(transformTobeMapped[5],
transformTobeMapped[3],
transformTobeMapped[4])),
priorNoise));先验残差公式是：
r_prior = Log(Z_0^{-1} · T_0)Z_0 是第一帧先验位姿，T_0 是图中的第一帧变量。这个因子的作用是固定地图参考系，否则整张图可以任意平移和旋转。
如果不是第一帧，添加相邻关键帧之间的 odometry BetweenFactor：
gtsam::Pose3 poseFrom = Pose3(
Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
Point3(transformLast[5], transformLast[3], transformLast[4]));
gtsam::Pose3 poseTo = Pose3(
Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
gtSAMgraph.add(BetweenFactor<Pose3>(
cloudKeyPoses3D->points.size()-1,
cloudKeyPoses3D->points.size(),
poseFrom.between(poseTo),
odometryNoise));这里 poseFrom.between(poseTo) 是相邻关键帧相对位姿观测：
Z_{k-1,k} = poseFrom^{-1} · poseToBetweenFactor 的残差公式是：
r_odom = Log( Z_{k-1,k}^{-1} · (T_{k-1}^{-1} · T_k) )其中 T_{k-1} 和 T_k 是 GTSAM 中待优化的关键帧位姿，Z_{k-1,k} 是 scan-to-map 结果提供的相邻关键帧相对位姿观测。这个残差的意义是：图优化推出来的相邻关键帧相对运动，要接近 scan-to-map 观测到的相对运动。
然后执行 iSAM2 增量优化：
isam->update(gtSAMgraph, initialEstimate);
isam->update();
gtSAMgraph.resize(0);
initialEstimate.clear();
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);GTSAM 关键帧图的总目标函数可以写成：
min_{T_0,...,T_k}
||Log(Z_0^{-1} · T_0)||²_{Σ_prior^{-1}}
+ Σ ||Log(Z_{i,i+1}^{-1} · (T_i^{-1} · T_{i+1}))||²_{Σ_odom^{-1}}
+ Σ ||Log(Z_loop_{m,n}^{-1} · (T_m^{-1} · T_n))||²_{Σ_loop^{-1}}第一项是初始先验，第二项是相邻关键帧 odometry 约束，第三项是回环约束。回环没有发生时，第三项为空。
优化后，代码把最新关键帧位姿保存进 cloudKeyPoses3D 和 cloudKeyPoses6D，并保存当前关键帧的角点、平面点和 outlier 点云，供以后构建局部地图和回环使用。
14. 回环检测完整流程
mapOptimization 里的回环检测是在单独线程中运行的，频率是 1Hz，也就是每秒检查一次有没有可能发生回环：
void loopClosureThread(){
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(1);
while (ros::ok()){
rate.sleep();
performLoopClosure();
}
}这里 loopClosureEnableFlag 控制是否开启回环。如果关闭，就直接退出线程；如果开启，就周期性调用 performLoopClosure()。所以回环不是每一帧都做，而是后台低频检测，避免影响主线程 scan-to-map 优化。
14.1 先找空间上接近的历史关键帧
回环检测第一步是在历史关键帧位姿 cloudKeyPoses3D 中查找当前机器人附近的关键帧：
kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
kdtreeHistoryKeyPoses->radiusSearch(
currentRobotPosPoint,
historyKeyframeSearchRadius,
pointSearchIndLoop,
pointSearchSqDisLoop,
0);这里所谓"距离够近"，不是代码里写死的 10m，而是由参数 historyKeyframeSearchRadius 决定。也就是说，代码会在以当前机器人位置 currentRobotPosPoint 为圆心、historyKeyframeSearchRadius 为半径的范围内搜索历史关键帧。这个具体数值要看你的 utility.h 或参数配置文件，本文件里只看到它被使用，没有看到具体赋值。很多 LeGO-LOAM 配置中这个值常见是几米级，例如 7m 左右，但你这个工程最终是多少，要以你项目里的 historyKeyframeSearchRadius 定义为准。这里还要注意，icp.setMaxCorrespondenceDistance(100) 不是回环候选搜索半径，它只是 ICP 内部找对应点时允许的最大对应距离，不能把它理解成"100m 内算回环候选"。代码里回环候选的空间范围由 historyKeyframeSearchRadius 控制。
搜索到空间上接近的关键帧后，代码还会判断时间差：
closestHistoryFrameID = -1;
for (int i = 0; i < pointSearchIndLoop.size(); ++i){
int id = pointSearchIndLoop[i];
if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0){
closestHistoryFrameID = id;
break;
}
}
if (closestHistoryFrameID == -1){
return false;
}这个条件的意思是：候选历史关键帧不仅要空间上接近，还必须和当前帧时间差大于 30 秒。这样可以避免刚刚经过的相邻关键帧被误认为回环。比如机器人刚走过 1 秒前的某个关键帧，空间上当然很近，但那只是相邻帧，不是真正"绕了一圈回到老地方"，所以要通过时间差过滤。
因此，回环候选帧必须满足两个条件：
1. 空间距离小于 historyKeyframeSearchRadius
2. 与当前帧时间差大于 30 秒代码会从搜索结果里选择第一个满足时间差条件的历史关键帧，保存为：
closestHistoryFrameID = id;它就是后面 ICP 的历史候选中心帧。
14.2 是不是先选一个候选帧，再取它前后关键帧构建历史局部地图？
对，就是这个流程。
先通过 radiusSearch() 找到一个空间接近且时间足够久远的历史关键帧 closestHistoryFrameID，然后以这个候选帧为中心，取它前后若干个关键帧，拼成历史局部地图。取多少个由 historyKeyframeSearchNum 控制。
代码如下：
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){
if (closestHistoryFrameID + j < 0 ||
closestHistoryFrameID + j > latestFrameIDLoopCloure)
continue;
*nearHistorySurfKeyFrameCloud += *transformPointCloud(
cornerCloudKeyFrames[closestHistoryFrameID+j],
&cloudKeyPoses6D->points[closestHistoryFrameID+j]);
*nearHistorySurfKeyFrameCloud += *transformPointCloud(
surfCloudKeyFrames[closestHistoryFrameID+j],
&cloudKeyPoses6D->points[closestHistoryFrameID+j]);
}这里 j 从 -historyKeyframeSearchNum 到 +historyKeyframeSearchNum，所以如果 historyKeyframeSearchNum = 25，那就是取候选帧前后各 25 个关键帧，加上候选帧自己，一共最多 51 个关键帧。但具体是不是 25，也要看你项目里 historyKeyframeSearchNum 的配置。本文件只体现了这个参数怎么用，没有展示它的具体数值。
注意这里历史局部地图不是只用一个候选帧，而是用候选帧附近一段历史关键帧拼起来。原因是单个关键帧点云太稀疏，ICP 容易不稳定；取前后若干关键帧后，历史局部地图更完整，墙面、地面、柱子、路沿等结构更多，ICP 匹配更可靠。
这个过程可以理解成：
当前机器人位置
↓
在历史关键帧轨迹中搜索半径 historyKeyframeSearchRadius 内的候选帧
↓
找到一个时间差 > 30s 的历史关键帧 closestHistoryFrameID
↓
以 closestHistoryFrameID 为中心
↓
取 [closestHistoryFrameID - historyKeyframeSearchNum,
closestHistoryFrameID + historyKeyframeSearchNum] 之间的关键帧
↓
把这些关键帧点云通过各自 cloudKeyPoses6D 位姿变换到地图坐标系
↓
拼接成 nearHistorySurfKeyFrameCloud
↓
体素降采样成 nearHistorySurfKeyFrameCloudDS最后还会降采样：
downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud);
downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS);nearHistorySurfKeyFrameCloudDS 就是后面 ICP 的 target，也就是历史候选区域的局部地图。
14.3 当前帧 source 是怎么构造的？
当前帧 ICP 的 source 是最新关键帧点云。代码先取最新关键帧 ID：
latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;然后把最新关键帧的角点和平面点都按照它当前保存的全局位姿变换到地图坐标系：
*latestSurfKeyFrameCloud += *transformPointCloud(
cornerCloudKeyFrames[latestFrameIDLoopCloure],
&cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
*latestSurfKeyFrameCloud += *transformPointCloud(
surfCloudKeyFrames[latestFrameIDLoopCloure],
&cloudKeyPoses6D->points[latestFrameIDLoopCloure]);虽然变量名叫 latestSurfKeyFrameCloud，但它里面实际上放了当前最新关键帧的 corner + surf 点云。这里也没有加入 outlier，只用了角点和平面点。之后代码还会过滤掉 intensity < 0 的点：
pcl::PointCloud<PointType>::Ptr hahaCloud(new pcl::PointCloud<PointType>());
int cloudSize = latestSurfKeyFrameCloud->points.size();
for (int i = 0; i < cloudSize; ++i){
if ((int)latestSurfKeyFrameCloud->points[i].intensity >= 0){
hahaCloud->push_back(latestSurfKeyFrameCloud->points[i]);
}
}
latestSurfKeyFrameCloud->clear();
*latestSurfKeyFrameCloud = *hahaCloud;所以 ICP 输入可以总结为：
source：
当前最新关键帧的 corner + surf 点云，
通过当前关键帧 cloudKeyPoses6D 位姿变换到地图坐标系。
target：
历史候选帧 closestHistoryFrameID 前后若干关键帧的 corner + surf 点云，
通过各自 cloudKeyPoses6D 位姿变换到地图坐标系后拼接，并降采样。也就是说，source 和 target 在进入 ICP 前，已经都被放到了当前估计的地图坐标系里。ICP 做的是在这个基础上进一步计算"当前 source 应该再怎么修正，才能更好贴到历史 target 上"。
14.4 ICP 得到的到底是什么变换？
代码中 ICP 是这样设置的：
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
icp.setInputSource(latestSurfKeyFrameCloud);
icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);PCL ICP 的 getFinalTransformation() 返回的是一个变换 T_icp，它满足：
p_aligned = T_icp · p_source也就是把 source 点云变换后，使它尽量对齐 target 点云。ICP 优化目标可以写成
min_T_icp Σ || T_icp · p_current - q_history ||²其中 p_current 是当前最新关键帧 source 中的点，q_history 是历史局部地图 target 中的对应点。
但是这里要注意：因为 source 点云在进入 ICP 前已经用当前关键帧的全局位姿 cloudKeyPoses6D[latestFrameIDLoopCloure] 变换到了地图坐标系，所以 ICP 得到的 T_icp 更准确地说是：
当前关键帧"已按旧全局位姿放到地图中以后"，
还需要再乘上的一个修正变换。所以它不是原始 LiDAR 当前帧坐标系到历史帧坐标系的完整变换，也不是直接的 T_current^{-1}T_history。它更像是：
当前关键帧旧全局位姿的修正增量代码后面也正是这么用它的。
先取 ICP 结果：
Eigen::Affine3f correctionCameraFrame;
correctionCameraFrame = icp.getFinalTransformation();
pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);然后做坐标约定转换：
Eigen::Affine3f correctionLidarFrame =
pcl::getTransformation(z, x, y, yaw, roll, pitch);再取当前关键帧原本的全局位姿，也就是还没有被回环修正前的错误位姿：
Eigen::Affine3f tWrong =
pclPointToAffine3fCameraToLidar(
cloudKeyPoses6D->points[latestFrameIDLoopCloure]);然后用 ICP 修正它：
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;这个式子非常关键。它说明 ICP 的结果 correctionLidarFrame 被当成一个"修正增量"，左乘到当前关键帧原本的全局位姿 tWrong 上，得到修正后的当前关键帧全局位姿 tCorrect。所以回答你这个问题：
ICP 得到的不是直接的"当前帧到历史帧的 BetweenFactor 观测"，而是一个把当前关键帧旧全局位姿修正到历史局部地图上的增量变换。代码先用它修正当前帧全局位姿，得到 tCorrect，再用 tCorrect 和历史关键帧位姿构造回环相对位姿。
14.5 回环 BetweenFactor 是怎么构造的？
ICP 修正当前帧位姿后，代码把 tCorrect 转成 GTSAM 的 poseFrom：
pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom =
Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));poseFrom 表示：当前关键帧经过 ICP 修正后的全局位姿。
历史候选帧的位姿是：
gtsam::Pose3 poseTo =
pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);poseTo 表示：历史候选关键帧的全局位姿。
然后代码构造两者之间的相对位姿：
poseFrom.between(poseTo)对应公式：
Z_loop = poseFrom^{-1} · poseTo这个 Z_loop 才是最终加入 GTSAM 的回环相对位姿观测。它表示：从修正后的当前关键帧位姿看，历史候选关键帧应该在什么相对位置。
然后设置回环约束噪声：
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore,
noiseScore, noiseScore, noiseScore;
constraintNoise = noiseModel::Diagonal::Variances(Vector6);ICP fitness score 越小，说明匹配越好，噪声越小，回环约束越强；fitness score 越大，说明匹配不稳定，噪声越大，约束越弱。当然代码前面已经用 historyKeyframeFitnessScore 做了一次阈值过滤，太差的 ICP 会直接丢弃。
最后加入 GTSAM：
gtSAMgraph.add(BetweenFactor<Pose3>(
latestFrameIDLoopCloure,
closestHistoryFrameID,
poseFrom.between(poseTo),
constraintNoise));
isam->update(gtSAMgraph);
isam->update();
gtSAMgraph.resize(0);
aLoopIsClosed = true;回环残差公式是：
r_loop = Log( Z_loop^{-1} · (T_current^{-1} · T_history) )其中 Z_loop 是由 ICP 修正结果构造出来的相对位姿观测，T_current 是 GTSAM 图中当前关键帧的待优化位姿，T_history 是历史候选关键帧的待优化位姿。这个残差的意思是：GTSAM 优化出来的当前帧和历史帧之间的相对位姿，应该接近 ICP 给出的相对位姿观测。
14.6 重新串起来的完整流程
这一段完整逻辑可以这样理解：
1. 回环线程 1Hz 运行 performLoopClosure()
2. detectLoopClosure() 先用当前机器人位置 currentRobotPosPoint
在历史关键帧 cloudKeyPoses3D 中做半径搜索，
搜索半径是 historyKeyframeSearchRadius。
3. 在搜索结果中找一个时间差大于 30 秒的历史关键帧，
记为 closestHistoryFrameID。
这个就是回环候选中心帧。
4. 取当前最新关键帧 latestFrameIDLoopCloure，
把它的 corner + surf 点云根据当前关键帧位姿变换到地图系，
作为 ICP source。
5. 以 closestHistoryFrameID 为中心，
取前后 historyKeyframeSearchNum 个关键帧，
把这些历史关键帧的 corner + surf 点云分别根据各自位姿变换到地图系，
拼接并降采样，作为 ICP target。
6. ICP 计算 source 到 target 的对齐变换。
因为 source 已经在地图坐标系中，所以 ICP 结果本质上是
"当前关键帧旧全局位姿的修正增量"。
7. 用 correctionLidarFrame * tWrong 得到 tCorrect，
也就是当前关键帧经过回环 ICP 修正后的全局位姿。
8. 用 tCorrect 构造 poseFrom，
用 closestHistoryFrameID 的历史位姿构造 poseTo。
9. 用 poseFrom.between(poseTo) 得到回环相对位姿观测 Z_loop。
10. 把 Z_loop 作为 BetweenFactor 加入 GTSAM，
约束 current keyframe 和 history keyframe 的相对关系。
11. iSAM2 更新后，aLoopIsClosed = true，
后续 correctPoses() 会把优化后的所有关键帧位姿写回。最关键的两点就是：
距离够近：
由 historyKeyframeSearchRadius 控制，不是代码固定 10m。
本文件里看不到具体数值，要去 utility.h 或参数文件查。
ICP 得到的变换：
不是直接的"当前帧到历史帧 BetweenFactor 观测"。
它先作为当前关键帧旧全局位姿的修正增量，
得到 tCorrect；
然后用 tCorrect 和历史帧 poseTo 再构造 Z_loop。所以更准确地说，回环不是简单"找到一个历史帧，然后 ICP 得到当前到历史的变换就直接加图"。它实际是：先根据空间半径和时间差找一个历史候选中心帧，再用这个候选帧前后若干关键帧构造历史局部地图；然后用当前最新关键帧点云去和这段历史局部地图做 ICP；ICP 输出的是当前关键帧旧全局位姿的修正量；最后根据修正后的当前关键帧全局位姿和历史候选帧全局位姿，构造回环 BetweenFactor 加入 iSAM2。
15. 回环后 correctPoses() 如何修正历史轨迹
回环成功后，aLoopIsClosed = true。主线程调用 correctPoses()：
if (aLoopIsClosed == true){
recentCornerCloudKeyFrames.clear();
recentSurfCloudKeyFrames.clear();
recentOutlierCloudKeyFrames.clear();
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i){
cloudKeyPoses3D->points[i].x =
isamCurrentEstimate.at<Pose3>(i).translation().y();
cloudKeyPoses3D->points[i].y =
isamCurrentEstimate.at<Pose3>(i).translation().z();
cloudKeyPoses3D->points[i].z =
isamCurrentEstimate.at<Pose3>(i).translation().x();
cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
cloudKeyPoses6D->points[i].roll =
isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
cloudKeyPoses6D->points[i].pitch =
isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
cloudKeyPoses6D->points[i].yaw =
isamCurrentEstimate.at<Pose3>(i).rotation().roll();
}
aLoopIsClosed = false;
}这里修正的是每个关键帧的全局 pose，而不是永久修改每个关键帧内部点云坐标。关键帧点云仍然保存在各自局部坐标系中：
cornerCloudKeyFrames[i]
surfCloudKeyFrames[i]
outlierCloudKeyFrames[i]真正被更新的是：
cloudKeyPoses3D[i]
cloudKeyPoses6D[i]后续构建局部地图时，代码会重新使用更新后的 cloudKeyPoses6D[i] 把关键帧点云变换到地图坐标系。因此，回环修正的本质是：
回环因子加入 GTSAM
↓
iSAM2 重新优化所有关键帧位姿
↓
correctPoses() 把优化后的关键帧 pose 写回
↓
之后局部地图构建、全局地图发布都使用修正后的关键帧 pose
↓
历史轨迹和地图整体被拉回一致清空 recentCornerCloudKeyFrames、recentSurfCloudKeyFrames、recentOutlierCloudKeyFrames 的原因是：这些缓存里的点云已经按旧 pose 变换到地图坐标系了，回环后关键帧 pose 变了，旧缓存不再可靠，必须重新构建。
16. 主流程 run()
run() 是整个后端主线程的入口。它先检查角点、平面点、outlier 和 odometry 是否都到了，并且时间戳足够接近：
if (newLaserCloudCornerLast &&
std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast &&
std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast &&
std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry)满足条件后才执行：
transformAssociateToMap();
extractSurroundingKeyFrames();
downsampleCurrentScan();
scan2MapOptimization();
saveKeyFramesAndFactor();
correctPoses();
publishTF();
publishKeyPosesAndFrames();
clearCloud();这个顺序非常关键：先根据前端 odometry 生成当前帧地图初值，再构建局部地图，再做 scan-to-map 优化，然后保存关键帧并更新 GTSAM 图，最后如果有回环就修正历史轨迹并发布结果。
17. 完整总结
这个 mapOptimization 文件是 LeGO-LOAM 后端建图和全局轨迹维护的核心模块。它不是前端特征提取，也不是单纯的点云预处理，而是在前端 FeatureAssociation 已经输出角点、平面点、outlier 和 /laser_odom_to_init 的基础上，进一步把当前帧和历史关键帧组成的局部地图进行匹配，从而获得更稳定、更准确的地图系位姿。整个文件可以分成两层优化：第一层是当前帧级别的 scan-to-map 几何优化，第二层是关键帧级别的 GTSAM / iSAM2 位姿图优化。前者关注"当前这一帧怎么和局部地图对齐"，后者关注"所有关键帧轨迹怎么保持全局一致"。
在当前帧级别，transformAssociateToMap() 首先根据前端 odometry 的短期增量和上一次后端优化后的全局位姿，生成当前帧的 transformTobeMapped 初值。这里 transformAftMapped 是上一次 mapOptimization 优化后的全局位姿，不是增量；transformBefMapped 是上一次后端处理时对应的前端 odometry 位姿；transformSum 是当前前端 odometry 位姿。代码本质上用 T_aftMapped_last · (T_befMapped_last^{-1} · T_sum_current) 的思想，把前端短期运动增量接到后端全局地图坐标系上。其中 T_befMapped_last^{-1} · T_sum_current 表示当前前端累计位姿相对于上一次前端累计位姿新增出来的运动量，而不是直接把当前前端累计位姿重复叠加到后端位姿上。得到的 transformTobeMapped 就是当前帧在地图坐标系下的待优化位姿，它在后续 scan-to-map 迭代中不断被点到线、点到面残差修正。
extractSurroundingKeyFrames() 用历史关键帧构建局部地图。这里需要明确，它不是直接使用 ImageProjection 里的 BFS cluster，也不是直接把地面点簇拿来建图。BFS 聚类只是前端点云分割和噪声过滤步骤，真正进入后端的是 FeatureAssociation 提取出的角点、平面点和 outlier。后端保存历史关键帧时，分别保存 cornerCloudKeyFrames、surfCloudKeyFrames 和 outlierCloudKeyFrames。这些关键帧点云本身仍然保存在各自关键帧的局部 LiDAR 坐标系下，同时系统会在 cloudKeyPoses6D 中保存每个关键帧的全局 6D 位姿，也就是 [x, y, z, roll, pitch, yaw]。构建局部地图时，代码通过每个关键帧自己的位姿，把该关键帧点云按照 p_map = R_keyframe · p_keyframe + t_keyframe 变换到地图坐标系，再拼接成局部地图。其中角点关键帧组成 laserCloudCornerFromMap，平面点和 outlier 一起组成 laserCloudSurfFromMap。这里的 outlier 并不是完全无用噪声，而是弱结构候选点，后面还会经过降采样、平面拟合有效性判断、残差权重筛选等步骤继续过滤，因此不会直接破坏优化。
真正的 scan-to-map 优化在 scan2MapOptimization() 中进行。它先设置局部地图角点和平面点 KD-tree，然后最多迭代 10 次。每次迭代中，cornerOptimization() 会把当前帧角点通过当前 transformTobeMapped 投到地图系，在局部角点地图中找 5 个最近邻，通过协方差矩阵和特征值分解判断这 5 个点是否成线。如果最大特征值明显大于第二大特征值，就认为它们构成线特征，并构造点到线残差 r_edge = ||(T·p - p1) × (T·p - p2)|| / ||p1 - p2||。surfOptimization() 则把当前帧平面点投到地图系，在局部平面地图中找 5 个最近邻，拟合平面 pa·x + pb·y + pc·z + pd = 0，再构造点到面残差 r_surf = n^T(T·p) + d。这两类残差共同约束当前帧的 6 自由度位姿，使当前帧特征点尽量贴合历史地图中的线结构和平面结构。
LMOptimization() 是把残差真正转成位姿增量的地方。每个残差都会被线性化为 r_i(T⊕Δx) ≈ r_i(T) + J_iΔx。代码中的 matA 就是雅可比矩阵 J，matB 是负残差 -r，matX 是要求解的位姿增量 Δx = [δroll, δpitch, δyaw, δx, δy, δz]^T。matA 每一行对应一个点到线或点到面残差，前三列 arx、ary、arz 是残差对旋转的偏导，后三列 coeff.x、coeff.y、coeff.z 是残差对平移的偏导。matB 每一行是 -coeff.intensity，也就是负的当前残差。代码通过 (J^T J)Δx = -J^T r 求解出 matX，并累加到 transformTobeMapped。因此，虽然函数名叫 LM，但实际点云残差优化没有调用 Ceres 或 GTSAM，而是代码自己构造线性系统，用 OpenCV 解正规方程。GTSAM 只负责关键帧级别的位姿图。
关键帧级别的优化在 saveKeyFramesAndFactor() 中完成。当前帧经过 scan-to-map 优化后，并不是每一帧都无条件保存为关键帧，而是会先判断当前机器人位置和上一个关键帧位置的距离。如果移动距离小于阈值，例如代码里判断小于 0.3m，就不保存这一帧，避免关键帧太密、地图冗余过多、图优化负担增加。只有当前帧相对上一关键帧移动足够明显时，才会作为新的关键帧加入系统。第一帧会加入 PriorFactor，相当于给整个地图一个起始参考坐标系，其残差可以写成 r_prior = Log(Z_0^{-1}T_0)，其中 Z_0 是第一帧先验观测，T_0 是图优化中的第一帧位姿变量。这个先验因子非常关键，因为如果没有它，整条轨迹可以整体平移或旋转，因子图没有固定参考。
对于后续关键帧，代码会根据上一关键帧位姿 poseFrom 和当前关键帧位姿 poseTo 构造相邻关键帧之间的相对位姿观测 Z_{k-1,k} = poseFrom^{-1} · poseTo，然后添加 GTSAM 的 BetweenFactor<Pose3>。这个因子的残差是 r_odom = Log(Z_{k-1,k}^{-1}(T_{k-1}^{-1}T_k))。这里 Z_{k-1,k} 来自当前 scan-to-map 优化后的关键帧相对运动，T_{k-1} 和 T_k 是 GTSAM 图中待优化的关键帧位姿变量。这个残差的含义是：图优化后两个关键帧之间的相对位姿，应该尽量接近 scan-to-map 给出的相对位姿观测。每加入一个新关键帧，系统就把新的 BetweenFactor 和当前关键帧初值插入 initialEstimate，然后调用 isam->update(gtSAMgraph, initialEstimate) 进行增量优化。iSAM2 不需要每次从头优化整张图，而是在已有线性化结果基础上增量更新，因此适合在线 SLAM。优化完成后，代码会从 isamCurrentEstimate 中取出最新的关键帧位姿，把它写入 cloudKeyPoses3D 和 cloudKeyPoses6D。同时当前关键帧的角点、平面点和 outlier 点云也被保存到 cornerCloudKeyFrames、surfCloudKeyFrames 和 outlierCloudKeyFrames，这些数据后面会继续用于局部地图构建、全局地图发布和回环检测。
关键帧图优化的总目标可以概括为：min_{T_0,...,T_k} ||Log(Z_0^{-1}T_0)||² + Σ||Log(Z_{i,i+1}^{-1}(T_i^{-1}T_{i+1}))||² + Σ||Log(Z_loop^{-1}(T_m^{-1}T_n))||²。第一项是第一帧先验约束，第二项是相邻关键帧 odometry 约束，第三项是回环约束。没有回环时，图优化主要依靠相邻关键帧 BetweenFactor 维持轨迹连续；一旦回环成功，就会添加额外的非相邻关键帧约束，把长期累计漂移拉回来。
回环检测由后台线程执行，频率通常是 1Hz。它不是每帧都做 ICP，而是周期性检查当前机器人是否回到了历史上曾经到过的位置。第一步，代码用 KD-tree 在历史关键帧位置 cloudKeyPoses3D 中搜索当前机器人附近的历史关键帧，搜索半径由参数 historyKeyframeSearchRadius 控制，不是代码里固定的 10m；具体数值需要看配置文件或 utility.h。搜索到空间上接近的关键帧后，还要要求该历史关键帧和当前帧的时间差大于 30 秒，这是为了避免把刚刚经过的相邻关键帧误判为回环。也就是说，回环候选必须同时满足两个条件：空间距离足够近，时间间隔足够久。
找到候选历史关键帧 closestHistoryFrameID 后，代码不是只拿这一帧和当前帧直接 ICP，而是以这个候选帧为中心，取它前后若干个关键帧，数量由 historyKeyframeSearchNum 控制。比如如果参数是 25，那么就会尝试取候选帧前后各 25 个关键帧，加上候选帧本身，拼成一段历史局部地图。每个历史关键帧的角点和平面点都会通过它自己的 cloudKeyPoses6D 位姿变换到地图坐标系，然后拼接成 nearHistorySurfKeyFrameCloud，再经过体素降采样得到 nearHistorySurfKeyFrameCloudDS，作为 ICP 的 target。当前最新关键帧则把它自己的角点和平面点通过当前保存的全局位姿变换到地图坐标系，组成 latestSurfKeyFrameCloud，作为 ICP 的 source。也就是说，ICP 的 source 是当前最新关键帧点云，target 是历史候选区域附近一段关键帧组成的局部地图。
ICP 的目标可以写成 min_{T_icp} Σ||T_icp·p_current - q_history||²。这里 p_current 是当前关键帧 source 中的点，q_history 是历史局部地图 target 中的对应点。需要注意的是，source 和 target 在进入 ICP 前都已经通过各自关键帧位姿变换到了地图坐标系，所以 ICP 得到的 T_icp 不是原始当前 LiDAR 坐标系到历史帧坐标系的完整变换，而是当前关键帧"已经按旧全局位姿放到地图中以后"还需要再乘上的一个修正变换。代码中会取 icp.getFinalTransformation() 得到 correctionCameraFrame，再转换成 correctionLidarFrame，然后用 tCorrect = correctionLidarFrame * tWrong 修正当前关键帧原本的全局位姿。这里 tWrong 是当前关键帧原来的全局位姿，tCorrect 是 ICP 修正后的当前关键帧全局位姿。
随后，代码用 tCorrect 构造 poseFrom，用历史候选关键帧的全局位姿构造 poseTo，再用 poseFrom.between(poseTo) 得到回环相对位姿观测 Z_loop = poseFrom^{-1} · poseTo。这个 Z_loop 才是真正加入 GTSAM 的回环观测。回环因子的残差是 r_loop = Log(Z_loop^{-1}(T_current^{-1}T_history))，其中 T_current 和 T_history 是图优化中的当前关键帧和历史关键帧变量。这个残差的作用是约束当前关键帧和历史关键帧之间的相对位姿，使优化后的两帧关系接近 ICP 认为正确的关系。代码还会用 ICP 的 fitnessScore 设置回环约束噪声，匹配越好，噪声越小，约束越强；匹配太差则会被 historyKeyframeFitnessScore 直接过滤掉，不加入图优化。
回环成功后，iSAM2 会更新整张关键帧位姿图，并设置 aLoopIsClosed = true。之后主线程调用 correctPoses()，把 iSAM2 优化后的所有关键帧位姿重新写回 cloudKeyPoses3D 和 cloudKeyPoses6D。这一步修正的是关键帧的全局 pose，而不是直接修改每个关键帧内部保存的点云坐标。每个关键帧的角点、平面点、outlier 仍然保存在各自局部坐标系中；只是它们对应的全局位姿被更新了。后续无论是构建局部地图还是发布全局地图，代码都会用修正后的 cloudKeyPoses6D 把关键帧点云重新变换到地图坐标系。因此，回环后的地图修正不是"重算所有点云特征"，而是"重写关键帧位姿，再用新 pose 重新拼接地图"。同时代码会清空 recent keyframes 缓存，因为这些缓存点云是按旧位姿变换过的，回环后已经不可靠，必须重新根据新的关键帧位姿构建。
因此，LeGO-LOAM 后端的完整逻辑可以概括为：前端提供短期 odometry 和特征点，mapOptimization 先用 transformAssociateToMap() 生成当前帧地图系初值，再通过局部地图中的角点和平面点构造点到线、点到面残差，用 OpenCV 解线性系统优化当前帧位姿；当当前帧移动足够远时保存为关键帧，并用 GTSAM/iSAM2 添加先验因子、相邻关键帧 BetweenFactor 和回环 BetweenFactor 来维护全局轨迹；当 ICP 回环检测成功时，系统将当前关键帧和历史关键帧之间的相对约束加入因子图，并通过 correctPoses() 更新所有关键帧位姿，从而修正长期累计漂移。这个模块既负责局部几何匹配，也负责全局轨迹一致性，是 LeGO-LOAM 后端建图精度和回环修正能力的核心。
版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！
欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。