GTSAM中iSAM2 实时优化后做全局 LM/GN 优化时如何检测并剔除错误（outlier）因子约束详解和工程应用

原理与总体流程（高层）

1. 从 iSAM2 取出当前完整因子图（或你保留的原始因子集合）和当前估计 Values。
2. 对图中每个因子计算**“白化残差”**（whitened residual）或其范数（标量指标）。
3. 根据统计检验（Chi-square）、阈值或排序策略识别可疑因子。
4. 剔除/降权/替换为鲁棒因子/使用 switch 变量（四选一或混合）。
5. 在剔除或调整后用 LM/GN 做批量优化，再把结果回写 iSAM2（isam.update({}, lmResult)）。
6. 可选：把被剔除的因子记录到黑名单并在未来忽略，或对被剔除的项触发重关联/复核（人工/算法）。

---

方法一 — 统计检验（推荐：简单且有效）

• 计算每个因子的 白化残差向量 r = factor->whitenedError(values)（GTSAM 的 NoiseModelFactor::whitenedError），再用 r.squaredNorm() 得到标量 s.
• s 近似服从自由度 d 的 χ² 分布（在噪声模型正确且线性化近似成立时）。
• 选择置信度（例如 0.95 或 0.99），查 χ² 分位点 χ²_{d,α}。若 s > χ²_{d,α} → 判为异常（outlier）。
• 优点：有统计学意义，能自适应噪声/因子维度（d）。

示例检测代码（伪/实际结合，基于 GTSAM API）：

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NoiseModelFactor.h>
#include <boost/math/distributions/chi_squared.hpp> // optional for chi2 quantilestd::vector<size_t> detectOutlierFactorsChi2(const gtsam::NonlinearFactorGraph& graph,const gtsam::Values& values,double alpha = 0.99) // confidence
{std::vector<size_t> outlier_indices;for (size_t i = 0; i < graph.size(); ++i) {auto factor = graph[i];auto nmf = dynamic_cast<const gtsam::NoiseModelFactor*>(factor.get());if (!nmf) continue;// whitenedError returns VectorEigen::VectorXd r = nmf->whitenedError(values);double s = r.squaredNorm();int dof = r.size();// chi2 threshold (use boost math or precomputed table)boost::math::chi_squared dist(dof);double thresh = boost::math::quantile(dist, alpha); // chi2_{d,alpha}if (s > thresh) outlier_indices.push_back(i);}return outlier_indices;
}void printFactorResiduals(const NonlinearFactorGraph& graph, const Values& values) {for (size_t i = 0; i < graph.size(); i++) {auto factor = graph[i];if (!factor) continue;double err = factor->error(values);std::cout << "Factor " << i << " error = " << err << std::endl;// 如果你需要直接的残差向量，可以尝试动态转换if (auto f = boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor)) {Pose3 pose1 = values.at<Pose3>(f->keys()[0]);Pose3 pose2 = values.at<Pose3>(f->keys()[1]);Pose3 predicted = pose1.between(pose2);Pose3 measurement = f->measured();Vector6 res = Pose3::Logmap(measurement.inverse() * predicted);std::cout << "  residual vector = " << res.transpose() << std::endl;}}
}

（提示：如果不想引 boost，dof 小的时候也可以用手写查表或用 approximate threshold。）

---

方法二 — 鲁棒化（不直接删因子）

• 把有问题的高残差因子替换为带 M-estimator 的鲁棒噪声模型（Huber、Cauchy）。
• 优点：不会彻底丢数据，对可能只是偶发噪声的观测更稳健。
• 缺点：如果是恒错（错误关联），鲁棒核可能仍然影响整体解（但比不做要好）。

替换示例：

auto iso = noiseModel::Isotropic::Sigma(dim, sigma);
auto robust = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), iso);
// 创建一个与原因子同类型的新因子但使用robust，再在新图替换

---

方法三 — 迭代剔除（Trim/Rank-based）

• 对所有因子计算残差 s，把 Top-K 最大残差 或超出某分位（例如 99.5%）的因子剔除，再重做 LM；可迭代几轮直到残差收敛。
• 这是工程上常用的“trim outlier”策略（RANSAC 之外的稳健化）。
• 注意：不要一次剔除太多（可先剔除 1–5% 或最坏 20 个），记录被剔除项以便复核。

示例伪代码：

auto residuals = compute_whitened_norms(graph, values);
sort residuals by value descending;
select top_k indices to remove (e.g. k = max(1, graph.size()/200));
build newGraph excluding those indices;
optimize newGraph -> values2;
repeat up to N times or until no new outliers found.

---

方法四 — Switchable Constraints / Dynamic Covariance Scaling

• 对于回环（loop closure）或可疑匹配，使用 switch variable（一个额外变量 s ∈ [0,1] 作为约束开关）或 dynamic covariance scaling (DCS)。
• 优点：在优化里自动学习该约束是否可信，不需要硬删。
• 复杂度：需要修改因子设计或者用已有实现（研究论文/实现库）。
• 适合长期运行与多回环场景。

---

方法五 — RANSAC / 几何检验（对视觉/匹配类因子）

• 对于基于特征匹配的因子（如重投影），优先在观测端做基于几何的一致性检验（PnP RANSAC、2D-2D 基本矩阵检验）来剔除错误匹配，减少错误因子进入图。
• 这是上流过滤（推荐）。

---

工程实现细节（如何在 GTSAM 中“剔除因子”）

• 不要直接修改 iSAM2 内部的因子数组（不安全）。推荐流程：

  1. 你应该在外部保存原始测量与生成因子的映射（例如 vector<shared_ptr> original_factors; 每个因子附带 measurement_id）。
  2. 检测出要剔除的因子 index（基于 graph 中的位置或 measurement_id）。
  3. 重建一个新 NonlinearFactorGraph（从 original_factors 复制所有未被标记为 outlier 的因子）。
  4. 在新图上执行 LM/GN 优化（或 GTSAM 的 ISAM2 一次性 reset + update），得到 lmResult。
  5. 回写 iSAM2：isam.update(NonlinearFactorGraph(), lmResult);（或如果你替换了因子，还需把新增/删除因子同步到 isam）。

• 这样实现既安全又可审计、可回滚。

示例：重建图并剔除（伪代码）：

std::vector<bool> keep(graph.size(), true);
for (idx : outlier_indices) keep[idx] = false;NonlinearFactorGraph cleanGraph;
for (size_t i=0;i<original_factors.size();++i){if (keep[i]) cleanGraph.add(original_factors[i]);
}// optimize cleanGraph...
LevenbergMarquardtOptimizer opt(cleanGraph, initial);
Values lmResult = opt.optimize();// 回写 iSAM2
isam.update(NonlinearFactorGraph(), lmResult);

---

代码示例：完整检测 → 剔除 → 重优化（可直接用）

下面提供一个集成函数（伪/可编译）：

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NoiseModelFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <boost/math/distributions/chi_squared.hpp>std::vector<size_t> chi2_outliers(const gtsam::NonlinearFactorGraph& graph,const gtsam::Values& values,double alpha = 0.99) {std::vector<size_t> out;for (size_t i=0;i<graph.size();++i) {auto f = graph[i];auto nmf = dynamic_cast<const gtsam::NoiseModelFactor*>(f.get());if(!nmf) continue;Eigen::VectorXd r = nmf->whitenedError(values);double s = r.squaredNorm();int dof = r.size();boost::math::chi_squared dist(dof);double thresh = boost::math::quantile(dist, alpha);if (s > thresh) out.push_back(i);}return out;
}// main workflow
void detect_and_trim_and_optimize(const std::vector<gtsam::NonlinearFactor::shared_ptr>& original_factors,gtsam::ISAM2& isam) {auto graph = isam.getFactorsUnsafe(); // copyauto values = isam.calculateEstimate();auto outliers = chi2_outliers(graph, values, 0.995);if (outliers.empty()) return; // nothing to do// build maskstd::vector<bool> keep(graph.size(), true);for (auto idx : outliers) keep[idx] = false;gtsam::NonlinearFactorGraph newGraph;for (size_t i = 0; i < original_factors.size(); ++i) {if (keep[i]) newGraph.add(original_factors[i]);else {// optional: log or mark measurement id}}// optimize cleaned graphgtsam::LevenbergMarquardtParams params;params.setMaxIterations(50);gtsam::LevenbergMarquardtOptimizer opt(newGraph, values, params);gtsam::Values res = opt.optimize();// 回写 iSAM2（注意：如果你改变了因子集合，也需要把差异应用到 isam）isam.update(gtsam::NonlinearFactorGraph(), res);
}

---

实践与调参建议（经验）

1. 先在小数据集上做统计：打印 factor->whitenedError(values).squaredNorm() 的分布，看看合理阈值（不要盲目设很小）。
2. 优先上游过滤（RANSAC / feature matching）：尽量在构造因子前把坏观测挡掉。
3. 谨慎剔除：不要一次性移除大量因子；使用迭代剔除策略并保留回滚能力。
4. 记录被剔除因子的 measurement id，便于事后人工检查或再次验证。
5. 替换为鲁棒核优先于直接删除（尤其当怀疑只是噪声不是恒错时）。
6. 当存在系统性错误（例如标定错误）时，剔除观测只是权宜之计，应检查更上游的问题（标定、时间同步、数据关联）。

---

常见坑

• 直接根据 factor->error(values)（未白化）判断会受到噪声模型尺度影响——应用 whitenedError。
• 在 iSAM2 内部直接 delete 因子会破坏 iSAM2 内部一致性——要在外部重建图并同步。
• 只用单次 LM 结果判断 outlier 会因为初值不佳导致误判——建议用两轮：先粗检、剔除、再细检。

---