添加BetweenFactorPose3回环因子后GTSAM优化器破坏地图

问题背景 #

基于iPhone LiDAR传感器搭建SLAM系统，使用GTSAM实现回环检测：

• 连续扫描点云对齐到全局地图后，将点云与世界坐标系位姿传入GTSAM；
• 多帧LiDAR扫描合并为一个关键帧，连续关键帧间添加BetweenFactor，delta取新旧关键帧首帧扫描的位姿差；
• 移除回环因子时，优化器可正常重建全局地图，逻辑无问题；
• 添加回环因子后，虽ICP匹配结果视觉验证良好，但优化后最新关键帧被拖至错误方向，全局地图彻底破坏，调整噪声参数无效。

核心问题排查方向 #

1. 回环因子的变换方向完全错误 #

GTSAM的BetweenFactor<Pose3>(i, j, delta)数学定义为：pose_j = pose_i * delta，即delta是从i到j的相对变换。如果ICP得到的变换方向与该定义相反，优化器会直接输出完全错误的位姿更新。

比如：

• 若ICP计算的是将回环关键帧j的点云对齐到当前关键帧i的点云的变换T_j_i（即p_i = T_j_i * p_j，p_j为j系下的点），世界系下位姿关系推导为pose_i = pose_j * T_j_i，对应的BetweenFactor应为BetweenFactor<Pose3>(j, i, T_j_i)，而非(i, j, T_j_i)。

验证方法：手动计算当前GTSAM中i和j的位姿估计值，检查pose_i是否等于pose_j * delta，若偏差极大，说明变换方向搞反了。

2. ICP变换的噪声模型与实际精度不匹配 #

很多开发者会直接使用固定对角噪声矩阵，但LiDAR的ICP变换旋转、平移不确定性并不对称，且ICP实际精度需结合匹配残差、点云数量、LiDAR硬件参数计算。

若回环因子噪声设得过小，优化器会优先强制满足回环约束，忽略连续帧累积误差，导致全局位姿被强行拉扯变形；若噪声设得过大，回环因子又起不到作用。

解决方法：

• 从ICP算法中获取匹配的协方差矩阵（如PCL等库会返回变换协方差）；
• 用该协方差矩阵构建GTSAM的噪声模型，而非固定对角矩阵：

  Matrix6d icp_covariance = ...; // 从ICP结果中获取
  auto noise_model = noiseModel::Gaussian::Covariance(icp_covariance);
  graph.add(BetweenFactor<Pose3>(j, i, delta, noise_model));
  

3. 关键帧的位姿与点云坐标系不一致 #

将多帧LiDAR扫描合并为关键帧时，若未将所有子帧点云正确转换到关键帧局部坐标系（以首帧扫描位姿为基准），会导致关键帧点云与GTSAM记录的世界位姿不匹配。此时ICP得到的变换基于错误点云坐标系，必然引发优化错误。

验证方法：将回环关键帧的点云用GTSAM当前估计的世界位姿转换到全局坐标系，与目标关键帧的全局系点云可视化对比，若和ICP匹配结果差异较大，说明关键帧的点云-位姿对齐存在问题。

4. 全局优化的增量处理不当 #

若每次添加回环因子后都执行全量LM优化，当回环约束与当前位姿估计偏差较大时，优化器可能直接跳到错误的局部最优解。使用GTSAM的ISAM2增量优化器，可逐步更新位姿估计，避免一次性引入强约束导致的崩溃。

解决方法：

• 替换全局LM优化为ISAM2：

  ISAM2Params params;
  ISAM2 isam(params);
  // 每次添加因子后更新ISAM2
  isam.update(graph, initial_estimate);
  isam.update(); // 执行增量优化
  auto current_estimate = isam.calculateEstimate();
  

代码片段检查要点 #

针对回环因子添加的代码，重点确认：

// 假设ICP得到的变换是T_j_i（将j的点云转到i的点云）
Pose3 T_j_i = icp.getTransformation();
// 检查位姿关系：pose_i 是否等于 pose_j * T_j_i
Pose3 pose_i = current_estimate.at<Pose3>(i);
Pose3 pose_j = current_estimate.at<Pose3>(j);
Pose3 pose_j_times_T = pose_j.compose(T_j_i);
if ((pose_i.translation() - pose_j_times_T.translation()).norm() > 0.5) {
  // 偏差过大，说明变换方向或坐标系有误
  std::cout << "回环变换方向错误" << std::endl;
}
// 正确添加回环因子
graph.add(BetweenFactor<Pose3>(j, i, T_j_i, noise_model));

可视化辅助排查 #

• 优化前全局地图：
• 优化后全局地图：
  （注：从图中可见最新关键帧被拉扯至错误区域，核心原因大概率是回环因子的变换方向或噪声模型错误）

内容的提问来源于stack exchange，提问作者aProfessionalHelpNeeder