1. 三个框架的总体差异
从 回环检测 和 因子图优化 角度看，三个框架可以这样理解：
A-LOAM：
主要是 LOAM 的简洁 Ceres 实现；
原版没有完整回环检测线程；
原版没有 GTSAM/iSAM2 全局位姿图；
主要做当前帧与局部地图的点到线 / 点到面优化；
长时间运行会累计漂移，不能自动闭环拉回。
LeGO-LOAM：
在 LOAM 基础上加入地面优化和轻量化处理；
有关键帧机制；
有 GTSAM/iSAM2 位姿图；
有基于"历史关键帧距离搜索 + 时间过滤 + ICP"的回环检测；
回环成功后添加 BetweenFactor，并修正历史关键帧轨迹。
LIO-SAM：
在 LeGO-LOAM 类似后端基础上加入 IMU 预积分和 GPS 因子；
有长期 mapOptimization 图；
有短期 imuPreintegration 图；
回环检测仍主要是距离候选 + ICP，也支持外部回环约束接入；
因子图更完整，包含 LiDAR odometry、GPS、loop、IMU preintegration、bias 等因子。一句话概括：
A-LOAM：局部点云几何优化。
LeGO-LOAM：局部点云几何优化 + 关键帧 pose graph + ICP 回环。
LIO-SAM：LiDAR + IMU + GPS + Loop 的多因子图紧耦合优化。
- A-LOAM：回环检测和因子图优化
2.1 A-LOAM 回环检测怎么做？
原版 A-LOAM 基本没有原生回环检测模块。它主要是对 LOAM 的简洁化实现，代码结构通常包括：
scanRegistration.cpp：
点云去畸变、曲率计算、角点和平面点提取。
laserOdometry.cpp：
前端帧间匹配，估计相邻帧之间的 LiDAR odometry。
laserMapping.cpp：
后端 scan-to-map，把当前帧特征点和局部地图匹配，优化当前帧位姿。
kittiHelper.cpp：
KITTI 数据读取辅助。A-LOAM 没有类似 LeGO-LOAM 的：
loopClosureThread()
performLoopClosure()
detectLoopClosure()
BetweenFactor<Pose3>(current, history, loopPose, loopNoise)
ISAM2 update也就是说，机器人绕一圈回到老地方时，原版 A-LOAM 不会主动判断"这里来过"，也不会把当前帧和历史帧之间建立回环约束，更不会通过全局图优化把前面累计的漂移拉回来。它的长期地图主要靠局部 scan-to-map 累计构建，因此大范围长时间运行后会有漂移。
2.2 A-LOAM 的优化方式
A-LOAM 的核心优化在 laserOdometry.cpp 和 laserMapping.cpp 中。它的主要思想是：当前帧提取角点和平面点后，在上一帧或局部地图中寻找对应线特征和平面特征，然后构造点到线、点到面的残差，用 Ceres 优化当前帧位姿。
它的 scan-to-map 目标函数可以写成：
min_T Σ || r_edge(T) ||² + Σ || r_plane(T) ||²点到线残差：
r_edge(T) =
|| (T·p - p_a) × (T·p - p_b) || / || p_a - p_b ||其中 p 是当前帧角点，T·p 是把当前角点变换到地图坐标系后的点，p_a 和 p_b 是地图中线特征上的两个点。
点到面残差：
r_plane(T) = n^T(T·p) + d其中 p 是当前帧平面点，n 是地图平面法向量，d 是平面常数项。
A-LOAM 的典型 Ceres 优化伪代码可以写成：
// A-LOAM laserMapping.cpp / laserOdometry.cpp 等价伪代码
// 作用：构造当前帧相对于局部地图的点到线、点到面残差，用 Ceres 优化当前帧位姿
ceres::Problem problem;
// para_q：当前帧待优化旋转，一般用四元数表示
// para_t：当前帧待优化平移
double para_q[4];
double para_t[3];
// 1. 添加角点到线残差
for (auto& curr_point : currentCornerPoints)
{
// 在地图角点集合中找能构成线特征的两个点 line_point_a、line_point_b
// curr_point 是当前帧角点
// line_point_a / line_point_b 是局部地图中的线特征点
ceres::CostFunction* cost_function =
LidarEdgeFactor::Create(
curr_point, // 当前帧角点
line_point_a, // 地图线特征点 1
line_point_b, // 地图线特征点 2
weight); // 残差权重
// 使用鲁棒核函数可以降低错误匹配点影响
problem.AddResidualBlock(
cost_function,
loss_function,
para_q,
para_t);
}
// 2. 添加平面点到面残差
for (auto& curr_point : currentSurfPoints)
{
// 在地图平面点集合中拟合平面，得到法向量 plane_norm 和平面常数项 d
// 点到面残差为 n^T(Tp) + d
ceres::CostFunction* cost_function =
LidarPlaneNormFactor::Create(
curr_point, // 当前帧平面点
plane_unit_norm, // 地图平面单位法向量 n
negative_OA_dot_norm, // 平面常数项 d
weight);
problem.AddResidualBlock(
cost_function,
loss_function,
para_q,
para_t);
}
// 3. Ceres 求解当前帧位姿
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);这段逻辑说明：A-LOAM 主要是 局部非线性最小二乘优化。每次优化对象是当前帧位姿，而不是整条轨迹中的所有关键帧。它没有把历史关键帧作为图节点，也没有把相邻关键帧约束、回环约束放进 GTSAM 图里。
2.3 A-LOAM 因子图优化情况
严格说，原版 A-LOAM 没有 GTSAM 因子图优化。它没有：
PriorFactor
BetweenFactor
LoopFactor
GPSFactor
IMU Preintegration Factor
ISAM2它只有 Ceres 层面的点云几何残差优化。因此它可以理解为：
每一帧：
当前特征点和局部地图匹配
↓
构造点到线 / 点到面残差
↓
Ceres 优化当前帧 pose
↓
更新局部地图没有：
关键帧 pose graph
回环闭合
全局轨迹重优化
历史 pose 回写所以 A-LOAM 的后端更轻、更简单，也更适合学习 LOAM 的基本几何优化思想；但它不适合直接解决大规模场景中的长期漂移问题。
3. LeGO-LOAM：回环检测和因子图优化
3.1 LeGO-LOAM 回环检测怎么做？
LeGO-LOAM 的回环检测主要在：
LeGO-LOAM/LeGO-LOAM/src/mapOptmization.cpp注意常见源码文件名是 mapOptmization.cpp，中间少了一个 i。这个文件中同时做 scan-to-map、关键帧保存、GTSAM/iSAM2 图优化、ICP 回环检测和回环后 pose 修正。
LeGO-LOAM 的回环检测流程是：
1. loopClosureThread() 后台以 1Hz 运行。
2. detectLoopClosure() 在历史关键帧 cloudKeyPoses3D 中做 KD-tree 半径搜索。
3. 候选历史关键帧必须空间上接近，搜索半径由 historyKeyframeSearchRadius 控制。
4. 候选历史关键帧还要和当前帧时间差大于 30 秒，避免把刚刚经过的相邻帧误判为回环。
5. 找到 closestHistoryFrameID 后，以它为中心取前后 historyKeyframeSearchNum 个关键帧。
6. 把这些历史关键帧的角点和平面点根据各自 cloudKeyPoses6D 位姿变换到地图坐标系，拼成历史局部地图 target。
7. 把当前最新关键帧的角点和平面点根据当前关键帧位姿变换到地图坐标系，作为 ICP source。
8. 用 PCL ICP 计算 source 到 target 的对齐修正。
9. 如果 ICP 收敛且 fitness score 小于阈值，就认为回环有效。
10. ICP 输出被当成当前关键帧旧全局位姿的修正增量 correction。
11. 用 tCorrect = correction · tWrong 得到修正后的当前关键帧全局位姿。
12. 用修正后的当前关键帧 poseFrom 和历史关键帧 poseTo 构造 Z_loop。
13. 把 Z_loop 作为 BetweenFactor 加入 GTSAM/iSAM2。
14. correctPoses() 将 iSAM2 优化后的所有关键帧 pose 写回。等价伪代码如下：
// LeGO-LOAM 回环检测等价伪代码，核心逻辑来自 mapOptmization.cpp
void performLoopClosure()
{
if (cloudKeyPoses3D->points.empty())
return;
// 1. 用 KD-tree 在历史关键帧位姿中找空间接近的候选帧
kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
std::vector<int> candidateIds;
std::vector<float> candidateDists;
kdtreeHistoryKeyPoses->radiusSearch(
currentRobotPosPoint, // 当前机器人全局位置
historyKeyframeSearchRadius, // 回环候选搜索半径，由参数控制
candidateIds,
candidateDists,
0);
// 2. 时间差过滤：必须是足够久远的历史帧
int closestHistoryFrameID = -1;
for (int id : candidateIds)
{
double dt = fabs(cloudKeyPoses6D->points[id].time - timeLaserOdometry);
if (dt > 30.0) {
closestHistoryFrameID = id;
break;
}
}
if (closestHistoryFrameID == -1)
return;
// 3. 当前最新关键帧作为 ICP source
int latestFrameIDLoopClosure = cloudKeyPoses3D->points.size() - 1;
PointCloud::Ptr source(new PointCloud);
// 当前关键帧点云原本在自己的 LiDAR 局部坐标系下
// 这里通过 cloudKeyPoses6D 中保存的关键帧全局位姿变换到地图坐标系
*source += *transformPointCloud(
cornerCloudKeyFrames[latestFrameIDLoopClosure],
&cloudKeyPoses6D->points[latestFrameIDLoopClosure]);
*source += *transformPointCloud(
surfCloudKeyFrames[latestFrameIDLoopClosure],
&cloudKeyPoses6D->points[latestFrameIDLoopClosure]);
// 4. 以历史候选帧为中心，取前后若干关键帧构建 ICP target
PointCloud::Ptr target(new PointCloud);
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j)
{
int id = closestHistoryFrameID + j;
if (id < 0 || id >= latestFrameIDLoopClosure)
continue;
*target += *transformPointCloud(
cornerCloudKeyFrames[id],
&cloudKeyPoses6D->points[id]);
*target += *transformPointCloud(
surfCloudKeyFrames[id],
&cloudKeyPoses6D->points[id]);
}
// 5. 对历史局部地图降采样，提高 ICP 实时性
downSizeFilterHistoryKeyFrames.setInputCloud(target);
downSizeFilterHistoryKeyFrames.filter(*targetDS);
// 6. ICP 配准：source 对齐 target
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setInputSource(source);
icp.setInputTarget(targetDS);
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
PointCloud::Ptr unused(new PointCloud);
icp.align(*unused);
// 7. ICP 失败或 fitness 太差，则不加回环
if (!icp.hasConverged() ||
icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// 8. ICP 输出的是当前关键帧旧全局位姿的修正量
Eigen::Affine3f correction = icp.getFinalTransformation();
Eigen::Affine3f tWrong =
pclPointToAffine3fCameraToLidar(
cloudKeyPoses6D->points[latestFrameIDLoopClosure]);
// 修正当前关键帧全局位姿
Eigen::Affine3f tCorrect = correction * tWrong;
// 9. 构造回环观测
Pose3 poseFrom = affineToGtsamPose(tCorrect);
Pose3 poseTo = pclPointTogtsamPose3(
cloudKeyPoses6D->points[closestHistoryFrameID]);
Pose3 loopRelativePose = poseFrom.between(poseTo);
// 10. 添加 GTSAM 回环 BetweenFactor
gtSAMgraph.add(BetweenFactor<Pose3>(
latestFrameIDLoopClosure,
closestHistoryFrameID,
loopRelativePose,
constraintNoise));
isam->update(gtSAMgraph);
isam->update();
aLoopIsClosed = true;
}LeGO-LOAM 的 ICP 目标函数是：
min_{T_icp} Σ || T_icp · p_current - q_history ||²这里 p_current 是当前关键帧 source 中的点，q_history 是历史局部地图 target 中的对应点。由于 source 和 target 进入 ICP 前都已经被各自关键帧 pose 变换到地图坐标系，所以 T_icp 更准确地说是 当前关键帧旧全局位姿的修正量，不是原始当前帧到历史帧的完整相对位姿。
回环观测：
Z_loop = poseFrom^{-1} · poseTo其中 poseFrom 是 ICP 修正后的当前关键帧全局位姿，poseTo 是历史候选关键帧全局位姿。
回环残差：
r_loop = Log( Z_loop^{-1} · (T_current^{-1} · T_history) )这个残差的作用是约束当前关键帧和历史关键帧之间的相对位姿，使 GTSAM 优化后的两帧关系接近 ICP 给出的回环观测。
3.2 LeGO-LOAM 的因子图优化怎么做？
LeGO-LOAM 的因子图优化也在 mapOptmization.cpp 中。它的变量是关键帧 pose：
T_0, T_1, ..., T_k主要因子有三类：
PriorFactor：
第一帧先验，固定地图参考系。
Odometry BetweenFactor：
相邻关键帧之间的 LiDAR odometry 相对位姿约束。
Loop Closure BetweenFactor：
回环成功后，当前关键帧和历史关键帧之间的相对位姿约束。关键帧图目标函数为：
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^{-1} · (T_m^{-1} · T_n)) ||²_{Σ_loop^{-1}}其中 Z_0 是第一帧先验，Z_{i,i+1} 是相邻关键帧相对位姿观测，Z_loop 是回环 ICP 产生的相对位姿观测。
关键帧保存和图优化伪代码：
// LeGO-LOAM saveKeyFramesAndFactor() 等价伪代码
void saveKeyFramesAndFactor()
{
// 1. 当前帧移动不够远，不保存关键帧
if (distance(currentRobotPosPoint, previousRobotPosPoint) < 0.3 &&
!cloudKeyPoses3D->points.empty())
{
return;
}
// 2. 第一帧：加入先验因子
if (cloudKeyPoses3D->points.empty())
{
Pose3 priorPose = Pose3(
Rot3::RzRyRx(yaw, roll, pitch),
Point3(z, x, y));
gtSAMgraph.add(PriorFactor<Pose3>(
0,
priorPose,
priorNoise));
initialEstimate.insert(0, priorPose);
}
else
{
// 3. 后续关键帧：加入相邻关键帧 BetweenFactor
Pose3 poseFrom = lastKeyPose;
Pose3 poseTo = currentKeyPose;
Pose3 relativePose = poseFrom.between(poseTo);
gtSAMgraph.add(BetweenFactor<Pose3>(
lastKeyIndex,
currentKeyIndex,
relativePose,
odometryNoise));
initialEstimate.insert(currentKeyIndex, poseTo);
}
// 4. iSAM2 增量优化
isam->update(gtSAMgraph, initialEstimate);
isam->update();
gtSAMgraph.resize(0);
initialEstimate.clear();
// 5. 读取优化后的关键帧 pose
isamCurrentEstimate = isam->calculateEstimate();
// 6. 保存优化后的关键帧位姿
cloudKeyPoses3D->push_back(optimizedPose3D);
cloudKeyPoses6D->push_back(optimizedPose6D);
// 7. 保存该关键帧的点云，用于后续局部地图和回环
cornerCloudKeyFrames.push_back(currentCornerCloud);
surfCloudKeyFrames.push_back(currentSurfCloud);
outlierCloudKeyFrames.push_back(currentOutlierCloud);
}LeGO-LOAM 的图优化是典型 pose graph，不包含 IMU bias、速度、IMU 预积分状态，也没有 GPSFactor。它比 A-LOAM 强在有全局关键帧位姿图和回环约束，但相比 LIO-SAM，因子种类较少。
4. LIO-SAM：回环检测和因子图优化
4.1 LIO-SAM 回环检测怎么做？
LIO-SAM 的回环检测主要在：
LIO-SAM/src/mapOptimization.cppLIO-SAM 的回环检测思路和 LeGO-LOAM 类似，仍然是 历史关键帧距离搜索 + 时间差过滤 + ICP 验证 + 回环 BetweenFactor。但 LIO-SAM 在系统结构上更完整，它还可以接入外部回环检测结果，例如 Scan Context 或其他 place recognition 模块。LIO-SAM 官方 README 明确说其因子图可以融合多种相对和绝对测量，包括 loop closures；论文摘要也说明 LIO-SAM 是基于 factor graph 的紧耦合 LiDAR-Inertial 框架。
LIO-SAM 距离回环大致流程：
1. loopClosureThread() 后台运行。
2. detectLoopClosureDistance() 在历史关键帧中搜索空间接近的候选帧。
3. 候选帧需要满足时间差阈值 historyKeyframeSearchTimeDiff。
4. 找到当前帧 loopKeyCur 和历史帧 loopKeyPre。
5. 用 loopFindNearKeyframes() 构造当前 source 和历史 target。
6. ICP 对齐 source 和 target。
7. ICP 收敛且 fitness 合格后，计算回环相对位姿。
8. 将回环约束放入 loopIndexQueue、loopPoseQueue、loopNoiseQueue。
9. addLoopFactor() 在主图更新时统一添加 BetweenFactor。LIO-SAM 回环检测伪代码：
// LIO-SAM mapOptimization.cpp 回环检测等价伪代码
void performLoopClosure()
{
if (cloudKeyPoses3D->points.empty())
return;
int loopKeyCur = -1; // 当前关键帧 ID
int loopKeyPre = -1; // 历史候选关键帧 ID
// 1. 距离回环检测：
// 在历史关键帧位姿中找和当前帧空间接近、时间足够久远的候选帧
bool detected = detectLoopClosureDistance(&loopKeyCur, &loopKeyPre);
if (!detected)
return;
// 2. 构造当前 source
// loopKeyCur 附近的关键帧点云作为当前局部点云
PointCloud::Ptr cureKeyframeCloud(new PointCloud);
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
// 3. 构造历史 target
// loopKeyPre 附近 historyKeyframeSearchNum 个关键帧组成历史局部地图
PointCloud::Ptr prevKeyframeCloud(new PointCloud);
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
if (cureKeyframeCloud->empty() || prevKeyframeCloud->empty())
return;
// 4. ICP：当前局部点云对齐历史局部地图
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
icp.align(*unused_result);
if (!icp.hasConverged() ||
icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// 5. ICP 输出当前关键帧旧全局 pose 的修正增量
Eigen::Affine3f correction = icp.getFinalTransformation();
Eigen::Affine3f tWrong =
pclPointToAffine3f(cloudKeyPoses6D->points[loopKeyCur]);
Eigen::Affine3f tCorrect = correction * tWrong;
// 6. 用修正后的当前 pose 和历史 pose 构造 loop relative pose
Pose3 poseFrom = affineToGtsamPose(tCorrect);
Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[loopKeyPre]);
Pose3 loopRelativePose = poseFrom.between(poseTo);
// 7. LIO-SAM 通常先把 loop 约束放入队列
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(loopRelativePose);
loopNoiseQueue.push_back(loopNoise);
}和 LeGO-LOAM 的主要区别是：LeGO-LOAM 往往在 ICP 成功后直接往 gtSAMgraph 中加 BetweenFactor；LIO-SAM 更常见的实现是先把 loop 结果放进队列，然后在 addLoopFactor() 中统一加入长期图。
LIO-SAM 回环残差公式同样是：
r_loop = Log( Z_loop^{-1} · (T_current^{-1} · T_history) )4.2 LIO-SAM 长期因子图：mapOptimization.cpp
LIO-SAM 的长期因子图在：
src/mapOptimization.cpp变量是关键帧位姿：
T_0, T_1, ..., T_k主要因子包括：
PriorFactor：
第一帧位姿先验。
Lidar Odometry BetweenFactor：
相邻关键帧 LiDAR odometry 约束。
GPSFactor：
GPS 绝对位置约束。
Loop Closure BetweenFactor：
回环约束。长期图目标函数：
min_{T_0,...,T_k}
|| r_prior ||²
+ Σ || r_lidar_odom ||²
+ Σ || r_gps ||²
+ Σ || r_loop ||²展开：
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})) ||²_{Σ_lidar^{-1}}
+ Σ || p_i - p_i^{gps} ||²_{Σ_gps^{-1}}
+ Σ || Log(Z_loop^{-1} · (T_m^{-1} · T_n)) ||²_{Σ_loop^{-1}}其中 p_i 是关键帧位姿中的位置，p_i^{gps} 是 GPS 测量的位置。
长期图伪代码：
// LIO-SAM mapOptimization.cpp 等价伪代码
void saveKeyFramesAndFactor()
{
// 1. 判断是否保存关键帧
if (!saveFrame())
return;
// 2. 添加相邻 LiDAR odometry 因子
addOdomFactor();
// 3. 如果当前 GPS 有效，添加 GPSFactor
addGPSFactor();
// 4. 如果有回环队列，添加 loop closure factor
addLoopFactor();
// 5. iSAM2 增量优化长期图
isam->update(gtSAMgraph, initialEstimate);
isam->update();
gtSAMgraph.resize(0);
initialEstimate.clear();
// 6. 读取优化后的关键帧位姿
isamCurrentEstimate = isam->calculateEstimate();
// 7. 保存关键帧和优化位姿
cloudKeyPoses3D->push_back(optimizedPose3D);
cloudKeyPoses6D->push_back(optimizedPose6D);
}addLoopFactor() 伪代码：
// LIO-SAM addLoopFactor() 等价伪代码
void addLoopFactor()
{
if (loopIndexQueue.empty())
return;
for (int i = 0; i < loopIndexQueue.size(); ++i)
{
int indexFrom = loopIndexQueue[i].first; // 当前关键帧
int indexTo = loopIndexQueue[i].second; // 历史关键帧
Pose3 loopPose = loopPoseQueue[i]; // ICP 得到的相对位姿观测
auto loopNoise = loopNoiseQueue[i]; // ICP fitness 对应噪声
gtSAMgraph.add(BetweenFactor<Pose3>(
indexFrom,
indexTo,
loopPose,
loopNoise));
}
loopIndexQueue.clear();
loopPoseQueue.clear();
loopNoiseQueue.clear();
}4.3 LIO-SAM 短期 IMU 因子图：imuPreintegration.cpp
LIO-SAM 和 LeGO-LOAM 最大区别是它还有一张短期 IMU 图，文件在：
src/imuPreintegration.cppLIO-SAM 官方 README 明确说，这张图优化 IMU 和 LiDAR odometry factor，并估计 IMU bias；同时 mapOptimization.cpp 长期图优化 LiDAR odometry factor 和 GPS factor。
短期图变量包括：
X_i：IMU pose
V_i：IMU velocity
B_i：IMU bias主要因子：
IMU Preintegration Factor：
约束相邻 IMU 状态之间的运动关系。
Bias BetweenFactor：
约束 IMU bias 不要突变。
LiDAR Correction PriorFactor：
用 mapOptimization 输出的 LiDAR 位姿校正 IMU 预积分漂移。伪代码：
// LIO-SAM imuPreintegration.cpp 等价伪代码
void addIMUFactor()
{
// 1. 添加 IMU 预积分因子
graphFactors.add(ImuFactor(
X(i-1), V(i-1),
X(i), V(i),
B(i-1),
preintegratedMeasurements));
// 2. 添加 bias 随机游走因子
graphFactors.add(BetweenFactor<imuBias::ConstantBias>(
B(i-1),
B(i),
imuBias::ConstantBias(),
biasNoise));
// 3. 添加 LiDAR correction 作为 pose prior
graphFactors.add(PriorFactor<Pose3>(
X(i),
lidarPoseCorrection,
correctionNoise));
}IMU 预积分残差：
r_imu =
[
Log(ΔR_ij^{-1} · (R_i^T R_j)),
R_i^T(v_j - v_i - gΔt) - Δv_ij,
R_i^T(p_j - p_i - v_iΔt - 1/2gΔt²) - Δp_ij
]bias 残差：
r_bias = B_j - B_iLiDAR correction prior：
r_lidar_prior = Log(Z_lidar^{-1} · X_i)所以 LIO-SAM 的图优化不是只有关键帧 pose graph，而是长期图和短期图共同工作：长期图保证全局一致性，短期图保证 IMU 高频里程计实时性和 bias 估计。
5. 回环检测对比表
| 对比项 | A-LOAM | LeGO-LOAM | LIO-SAM |
|---|---|---|---|
| 原生回环检测 | 没有 | 有 | 有 |
| 回环候选搜索 | 无 | KD-tree 半径搜索历史关键帧 | KD-tree 距离回环，也可接外部回环 |
| 时间过滤 | 无 | 当前帧与历史帧时间差 > 30s | 当前帧与历史帧时间差超过配置阈值 |
| 回环验证 | 无 | 当前关键帧和历史局部地图 ICP | 当前/附近关键帧与历史局部地图 ICP |
| source | 无 | 当前最新关键帧 corner + surf | 当前关键帧或附近关键帧点云 |
| target | 无 | 历史候选帧前后若干关键帧局部地图 | 历史候选帧附近若干关键帧局部地图 |
| ICP 输出 | 无 | 当前关键帧旧全局位姿修正增量 | 当前关键帧旧全局位姿修正增量 |
| 回环因子 | 无 | BetweenFactor<Pose3> |
BetweenFactor<Pose3> |
| 回环后修正 | 无 | correctPoses() 回写关键帧 pose |
iSAM2 更新长期图，回写关键帧 pose |
| 优点 | 简单、轻量 | 有基本闭环能力 | 多传感器约束强，长期一致性更好 |
| 缺点 | 不能自动修正长期漂移 | ICP 回环朴素，大漂移可能失败 | 系统复杂，依赖 IMU/GPS 标定和参数 |
6. 因子图优化对比表
| 对比项 | A-LOAM | LeGO-LOAM | LIO-SAM |
|---|---|---|---|
| 是否使用 GTSAM | 否 | 是 | 是 |
| 是否使用 iSAM2 | 否 | 是 | 是 |
| 点云残差优化方式 | Ceres | OpenCV 正规方程 + GTSAM pose graph | scan-to-map + GTSAM 长短期双图 |
| 图变量 | 无全局图 | 关键帧 pose T_i |
长期图：关键帧 pose；短期图：pose、velocity、bias |
| 第一帧因子 | 无 | PriorFactor<Pose3> |
PriorFactor<Pose3> |
| 相邻关键帧因子 | 无 | LiDAR odometry BetweenFactor |
LiDAR odometry BetweenFactor |
| 回环因子 | 无 | Loop BetweenFactor |
Loop BetweenFactor |
| GPS 因子 | 无 | 原版无 | 有 GPSFactor |
| IMU 预积分因子 | 无 | 原版无紧耦合 IMU 图 | 有 ImuFactor |
| Bias 因子 | 无 | 无 | 有 BetweenFactor<imuBias> |
| 长期漂移修正 | 弱 | 中等，靠 ICP loop | 强，靠 IMU/GPS/Loop 多因子 |
| 工程复杂度 | 低 | 中 | 高 |
7. 三个框架核心代码文件对比
| 框架 | 主要代码文件 | 作用 |
|---|---|---|
| A-LOAM | src/scanRegistration.cpp |
点云畸变处理、曲率计算、角点/平面点提取 |
| A-LOAM | src/laserOdometry.cpp |
帧间 odometry，scan-to-scan 匹配 |
| A-LOAM | src/laserMapping.cpp |
scan-to-map 局部地图优化，Ceres 求解 |
| A-LOAM | src/kittiHelper.cpp |
KITTI 数据读取辅助 |
| LeGO-LOAM | LeGO-LOAM/src/imageProjection.cpp |
range image 投影、地面检测、点云分割 |
| LeGO-LOAM | LeGO-LOAM/src/featureAssociation.cpp |
特征提取、前端 odometry |
| LeGO-LOAM | LeGO-LOAM/src/mapOptmization.cpp |
scan-to-map、关键帧图优化、ICP 回环、GTSAM/iSAM2 |
| LeGO-LOAM | LeGO-LOAM/src/transformFusion.cpp |
前后端位姿融合与发布 |
| LIO-SAM | src/imageProjection.cpp |
点云去畸变、range image 投影 |
| LIO-SAM | src/featureExtraction.cpp |
角点/平面点提取 |
| LIO-SAM | src/mapOptimization.cpp |
scan-to-map、关键帧、GPS factor、loop factor、长期因子图 |
| LIO-SAM | src/imuPreintegration.cpp |
IMU 预积分短期因子图、bias 估计、高频 IMU odometry |
| LIO-SAM | include/utility.h |
参数、点类型、工具函数 |
8. A-LOAM 回环检测和因子图优化总结
A-LOAM 可以看作是三者里面最基础、最简洁的 LOAM 系列实现。它的主要价值在于把 LOAM 原始论文中的特征提取、前端 odometry 和后端 mapping 过程用较清晰的代码重新实现，并使用 Ceres Solver 来完成点云几何残差优化。A-LOAM 的核心并不是回环检测，也不是全局因子图，而是局部帧间匹配和局部 scan-to-map 匹配。它通过 scanRegistration.cpp 从原始点云中提取角点和平面点，然后在 laserOdometry.cpp 中进行相邻帧之间的特征匹配，估计较高频的前端里程计；再在 laserMapping.cpp 中将当前帧特征点与局部地图特征点匹配，通过点到线残差和点到面残差优化当前帧位姿。这个过程的优化目标可以写成 min_T Σ||r_edge(T)||² + Σ||r_plane(T)||²，其中点到线残差约束当前角点贴近地图中的线特征，点到面残差约束当前平面点贴近地图中的平面结构。Ceres 在这里负责求解当前帧的旋转和平移，使这些几何残差最小。
但是从回环检测角度看，A-LOAM 原版是明显不足的。它没有独立的回环检测线程，没有历史关键帧候选搜索，没有 ICP 回环验证，也没有回环约束加入全局位姿图的过程。也就是说，当机器人长时间运行后，由于每一帧优化都只依赖局部地图和局部特征匹配，误差会随着路径长度逐渐累积。如果机器人绕一圈回到起点附近，A-LOAM 原版不会主动识别"当前位置和历史某个位置是同一个地方"，更不会生成 Z_loop 这种当前关键帧到历史关键帧的回环相对位姿观测。因此它不能像 LeGO-LOAM 或 LIO-SAM 那样通过回环约束把轨迹整体拉回。很多基于 A-LOAM 的改进版本会额外加入 Scan Context、ICP 回环、GTSAM pose graph 等模块，本质上就是为了补上 A-LOAM 原版缺少全局一致性优化的问题。
从因子图角度看，A-LOAM 原版也没有 GTSAM/iSAM2。它没有 PriorFactor 固定第一帧，没有 BetweenFactor 维护相邻关键帧关系，也没有 LoopFactor 约束非相邻历史帧。它优化的是当前帧 pose，而不是一组历史关键帧 pose。换句话说，A-LOAM 的优化变量通常只是当前帧相对于地图或上一帧的位姿，而不是 T_0, T_1, ..., T_k 这样一整条轨迹。它的优点是结构清晰、依赖较少、便于理解 LOAM 的点到线/点到面几何残差；缺点是长期漂移修正能力弱，不具备原生全局图优化能力。对于学习 SLAM 来说，A-LOAM 非常适合先理解"LiDAR 特征点如何构造残差"和"Ceres 如何优化当前帧位姿"；但如果要做大范围建图、闭环修正和长期轨迹一致性，就需要在 A-LOAM 之外额外加入回环检测和全局 pose graph 优化。
9. LeGO-LOAM 回环检测和因子图优化总结
LeGO-LOAM 相比 A-LOAM 的一个重要进步，是它不仅做局部 scan-to-map 优化，还在后端加入了关键帧机制、GTSAM/iSAM2 位姿图和基于 ICP 的回环检测。它的前端通过 imageProjection.cpp 和 featureAssociation.cpp 完成 range image 投影、地面检测、点云分割、曲率计算和角点/平面点提取。进入后端 mapOptmization.cpp 后，系统会根据前端 odometry 和上一次后端优化位姿生成当前帧初值，再从历史关键帧中选取附近关键帧构建局部地图。局部地图中的角点来自 cornerCloudKeyFrames，平面点和部分 outlier 来自 surfCloudKeyFrames 与 outlierCloudKeyFrames。每个关键帧点云本身保存在自己的局部 LiDAR 坐标系下，构建局部地图时，会通过该关键帧的 cloudKeyPoses6D 全局位姿变换到地图坐标系，再拼接成局部 corner map 和 surf map。
LeGO-LOAM 的当前帧级别优化仍然是典型的点到线、点到面 scan-to-map 优化。当前帧角点投到地图系后，在局部角点地图中搜索 5 个最近邻，通过协方差矩阵和特征值分解判断这些邻近点是否构成线特征；当前平面点投到地图系后，在局部平面地图中搜索 5 个最近邻，并拟合平面。角点残差为 r_edge = ||(T·p-p1)×(T·p-p2)|| / ||p1-p2||，平面残差为 r_surf = n^T(T·p)+d。这些残差被 LMOptimization() 线性化成 JΔx=-r，代码自己构造 matA 和 matB，用 OpenCV 求解正规方程 (J^TJ)Δx=-J^Tr，最后把解出的位姿增量累加到 transformTobeMapped。因此，LeGO-LOAM 的点云残差优化不是用 Ceres，也不是直接用 GTSAM，而是手写雅可比矩阵并解线性系统。
在关键帧级别，LeGO-LOAM 使用 GTSAM/iSAM2 维护 pose graph。第一帧加入 PriorFactor<Pose3>，固定地图参考系；后续关键帧根据上一关键帧和当前关键帧之间的相对位姿加入 BetweenFactor<Pose3>。当没有回环时，图优化主要依靠相邻关键帧之间的 odometry 约束维持轨迹连续性；当回环发生时，会额外加入当前关键帧和历史关键帧之间的回环 BetweenFactor。图优化目标可以写成 min ||r_prior||² + Σ||r_odom||² + Σ||r_loop||²。这种设计比 A-LOAM 更强，因为它可以保留关键帧轨迹，并在回环发生后全局修正历史 pose。
LeGO-LOAM 的回环检测比较朴素，但流程完整。后台线程以约 1Hz 运行，先在历史关键帧位置 cloudKeyPoses3D 中做 KD-tree 半径搜索，搜索半径由 historyKeyframeSearchRadius 控制；候选帧还必须和当前时间差大于 30 秒，避免相邻帧误检。找到候选历史帧 closestHistoryFrameID 后，不是只拿这一帧做 ICP，而是取它前后 historyKeyframeSearchNum 个关键帧，拼接成历史局部地图 target；当前最新关键帧的 corner + surf 点云作为 source。source 和 target 都已经通过各自关键帧位姿变换到地图坐标系，所以 ICP 输出的不是原始 LiDAR 到历史 LiDAR 的完整变换，而是当前关键帧旧全局位姿的修正增量。代码用 tCorrect = correction * tWrong 得到修正后的当前关键帧全局位姿，再用 poseFrom.between(poseTo) 构造回环相对位姿 Z_loop，加入 GTSAM 的回环 BetweenFactor。回环成功后，correctPoses() 会把 iSAM2 优化后的所有关键帧位姿写回 cloudKeyPoses3D/6D，并清空旧的 recent keyframes 缓存。LeGO-LOAM 的优势是有闭环和全局 pose graph，缺点是回环检测主要依赖 ICP，如果初始漂移太大、环境重复结构严重或 ICP fitness 不可靠，回环容易失败或误检。
10. LIO-SAM 回环检测和因子图优化总结
LIO-SAM 是这三个框架中后端因子图最完整的一个。它不是单纯的 LiDAR SLAM，而是紧耦合 LiDAR-Inertial SLAM，并且可以融合 GPS 和回环约束。LIO-SAM 的官方系统架构明确分为两张图：一张在 mapOptimization.cpp 中长期维护，主要优化 LiDAR odometry factor、GPS factor 和 loop closure factor；另一张在 imuPreintegration.cpp 中短期维护，主要优化 IMU preintegration factor、LiDAR odometry correction factor，并估计 IMU bias。这个设计使 LIO-SAM 不仅能像 LeGO-LOAM 一样进行 scan-to-map 和关键帧图优化，还能利用 IMU 提供高频运动预测、点云去畸变和 bias 估计，利用 GPS 提供绝对位置约束。
在回环检测方面，LIO-SAM 默认流程和 LeGO-LOAM 类似，也是通过历史关键帧距离搜索找到候选回环，然后用 ICP 验证。具体来说，系统会在 cloudKeyPoses3D 中搜索与当前关键帧空间接近、时间间隔足够大的历史关键帧，得到 loopKeyCur 和 loopKeyPre。然后通过 loopFindNearKeyframes() 构造当前局部点云 source 和历史局部地图 target。source 通常是当前关键帧或当前关键帧附近的小范围点云，target 是历史候选关键帧附近若干关键帧组成的局部地图。之后使用 ICP 计算 source 到 target 的对齐变换。如果 ICP 不收敛或 fitness score 超过阈值，就丢弃这次回环；如果通过验证，就用 ICP 修正当前关键帧原来的全局位姿，得到修正后的 tCorrect，再与历史候选帧位姿构造 Z_loop。回环残差仍然是 r_loop = Log(Z_loop^{-1}(T_current^{-1}T_history))。与 LeGO-LOAM 不同的是，LIO-SAM 常见实现中会把回环约束先放入 loopIndexQueue、loopPoseQueue、loopNoiseQueue，然后在 addLoopFactor() 中统一加入 GTSAM 图。这种队列式设计让回环线程和主图优化之间解耦，更适合复杂多线程系统。
LIO-SAM 的长期图比 LeGO-LOAM 更丰富。LeGO-LOAM 的图主要包括第一帧先验、相邻关键帧 odometry 和回环因子；LIO-SAM 在此基础上增加 GPSFactor。长期图目标函数可以写成 min ||r_prior||² + Σ||r_lidar_odom||² + Σ||r_gps||² + Σ||r_loop||²。其中 r_lidar_odom 约束相邻关键帧之间的 LiDAR 相对运动，r_gps = p_i - p_i^{gps} 约束关键帧位置接近 GPS 测量，r_loop 约束当前帧和历史帧之间的回环相对位姿。GPS 因子的存在使 LIO-SAM 在开阔室外场景中可以借助绝对位置减少全局漂移，而不完全依赖回环。对于大尺度车辆轨迹，GPSFactor 通常能显著提升长期地图一致性。
LIO-SAM 最核心的优势还在短期 IMU 图。imuPreintegration.cpp 中的因子图变量包括 IMU pose X_i、速度 V_i 和 bias B_i。IMU 预积分因子约束相邻 IMU 状态之间的旋转、速度、位置变化，残差包括 Log(ΔR_ij^{-1}(R_i^TR_j))、R_i^T(v_j-v_i-gΔt)-Δv_ij 和 R_i^T(p_j-p_i-v_iΔt-1/2gΔt²)-Δp_ij。bias 因子约束 B_j-B_i 不要突变，LiDAR correction prior 则把 mapOptimization 输出的 LiDAR 位姿作为当前 IMU pose 的校正观测。这样 IMU 高频预测可以用于点云去畸变和实时 odometry，LiDAR 后端优化结果又能反过来校正 IMU bias 和累计漂移。相比 LeGO-LOAM，LIO-SAM 的优化变量不只是关键帧 pose，还包括速度和 IMU bias；因子也不只是 odometry 和 loop，还包括 IMU、GPS、LiDAR correction 等多源约束。缺点是系统复杂度更高，对 IMU 噪声参数、外参、时间同步和 GPS 质量要求更高。总体来说，A-LOAM 更适合理解 LOAM 几何残差，LeGO-LOAM 更适合理解 LiDAR 关键帧图和 ICP 回环，LIO-SAM 则更适合理解现代多传感器因子图 SLAM 的完整后端。
版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！
欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。