GTSAM 中gtsam::IndeterminantLinearSystemException详解

在 GTSAM 中进行因子图优化（特别是 ISAM2 增量优化）时，如果系统的线性化方程组是奇异的（ill-posed），就会抛出 IndeterminantLinearSystemException。这是 GTSAM 用来防止数值崩溃的重要机制。

1. 触发条件

1.1 数学背景

优化问题最终会被线性化为一个 高斯因子图 (Gaussian Factor Graph)，即线性系统：

$$H\delta x=b$$

其中：

• $H$ = 信息矩阵（近似 Hessian）
• $\delta x$ = 状态变量的增量
• $b$ = 残差项

若 $H$ 不可逆或病态，就会触发异常。

触发情况包括：

1. 欠约束 (Underdetermined system)

   • 方程数量 < 变量数量，秩不足。
   • 常见于变量缺少先验约束（例如没有对第一个位姿加 prior）。
   • 典型症状：秩 $\text{rank}(H)<\dim (x)$。

2. 不定系统 (Indefinite system)

   • Hessian 不是半正定（出现负特征值）。
   • 常见原因：噪声设置错误（负信息）、数值误差积累。
   • 在 Cholesky 分解时会直接失败。

3. 病态系统 (Poorly-conditioned system)

   • 系统理论上有解，但数值条件数极大。

   • 常见原因：

     • 测量单位差异过大（米 vs 毫米）。
     • 观测几何退化（平移/尺度模糊）。
     • 噪声协方差设置不合理。

2. 典型场景

场景 1：缺少先验

• 症状：系统整体漂移。
• 例子：第一个相机位姿没有 prior，导致尺度 / 全局参考系不确定。

场景 2：几何退化

• 症状：立体视觉基线过小 → 景深不确定。
• 例子：两个相机几乎在同一位置观测同一个点。

场景 3：观测不足

• 症状：点只被一个因子约束。
• 例子：一个 landmark 只被单个投影观测，深度自由度未约束。

场景 4：数值误差

• 症状：优化迭代过程中，因子协方差设置过小 / 极大差异 → Hessian 变为 indefinite。

3. 异常信息

异常对象 IndeterminantLinearSystemException 包含：

• what()：异常描述。
• nearbyVariable()：提示在哪个变量附近检测到问题。

注意：nearbyVariable 不一定是根因，而只是优化过程中检测到失败的变量。

4. 调试手段

1. 检查变量约束

   • 第一个 pose 是否有 prior？
   • landmark 是否被足够多的因子观测？

2. 导出矩阵
   使用以下 API 分析系统矩阵：

   • GaussianFactorGraph::sparseJacobian_()
   • GaussianFactorGraph::denseJacobian()
   • GaussianFactorGraph::denseHessian()

   可以将矩阵写入文本，导入 MATLAB / NumPy 分析秩和特征值。

3. 可视化因子图
   用 GTSAM 的 graphviz 输出因子图结构，检查孤立节点。

5. 解决思路

1. 添加先验 / 约束

   • 给第一个 pose 添加 PriorFactor。
   • 给 landmark 添加更多观测或深度约束。

2. 正则化 (Damping)

   • 在问题点添加一个 小的 damping 因子，使 Hessian 对角线变大，避免数值奇异。
   • 例子：你代码中 LinearDampingFactor。

3. 检查噪声模型

   • 协方差是否为正定矩阵？
   • 单位是否合理（米 vs 毫米）。

4. 重新初始化 ISAM2

   • 遇到奇异系统时，可能需要 reset ISAM2 状态并重新插入因子图（你代码里也这样做了）。

5. 改变优化策略

   • 切换到 Dogleg 方法，可能比 LM/ GN 更稳定。
   • 调整 relinearization 参数（relinearizeSkip / relinearizeThreshold）。

6. 结论

• 出现 IndeterminantLinearSystemException，说明优化 没有可靠收敛。
• 优化结果 可能是错误的，除非你对系统加了正则化修复。
• 最根本的解决办法是 保证因子图的可观测性与约束完备性，而不是仅靠 damping 来“救场”。

7.综合示例：GTSAM ISAM2 中的奇异系统与修复

1. 初始场景：欠约束因子图

下面构建一个最小 SLAM 场景：两个相机位姿 + 一个路标。

• Pose1 没有 prior
• Pose2 通过 odometry 连接 Pose1
• Landmark 只通过 Pose2 观测

这样系统就是 欠约束 的，运行 ISAM2 时会触发 IndeterminantLinearSystemException。

#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/inference/Symbol.h>
#include <iostream>

using namespace gtsam;

int main() {
    NonlinearFactorGraph graph;
    Values initial;
    auto K = boost::make_shared<Cal3_S2>(500, 500, 0, 320, 240);

    // --- 变量 ---
    Symbol x1('x', 1), x2('x', 2), l1('l', 1);

    // 初始估计（随便设）
    initial.insert(x1, Pose3());
    initial.insert(x2, Pose3(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1, 0, 0)));
    initial.insert(l1, Point3(5, 0, 1));

    // --- 因子 ---
    // 没有 prior !!! (BUG)
    graph.emplace_shared<BetweenFactor<Pose3>>(x1, x2,
        Pose3(Rot3::RzRyRx(0,0,0), Point3(1,0,0)),
        noiseModel::Diagonal::Sigmas((Vector(6) << 0.1,0.1,0.1,0.1,0.1,0.1).finished()));

    // Landmark 只被 x2 观测
    Point2 z(320, 240);
    auto noise = noiseModel::Isotropic::Sigma(2, 1.0);
    graph.emplace_shared<GenericProjectionFactor<Pose3, Point3>>(z, noise, x2, l1, K);

    ISAM2Params params;
    ISAM2 isam(params);

    try {
        isam.update(graph, initial);
        Values result = isam.calculateEstimate();
        result.print("Estimate:\n");
    } catch(const IndeterminantLinearSystemException& e) {
        std::cerr << "❌ ISAM2 failed: " << e.what() << std::endl;
    }

    return 0;
}

运行后你会看到类似报错：

 ISAM2 failed: Indeterminant linear system detected near variable x1

2. 修复方法 1：添加先验

为 x1 添加一个弱先验，使系统有全局参考：

// 添加先验
auto priorNoise = noiseModel::Diagonal::Sigmas(
    (Vector(6) << 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3).finished());
graph.emplace_shared<PriorFactor<Pose3>>(x1, Pose3(), priorNoise);

这样 Hessian 满秩，优化可行。

3. 修复方法 2：正则化（Damping）

如果你怀疑是病态系统，可以在更新前添加 对角线 damping：

for (auto key : initial.keys()) {
    auto damping = noiseModel::Isotropic::Sigma(6, 1e-6);
    Pose3 priorPose = initial.at<Pose3>(key);
    graph.emplace_shared<PriorFactor<Pose3>>(key, priorPose, damping);
}

这样即使欠约束，也不会直接崩溃（本质上类似 LM 算法的 λI 正则化）。

4. 调试手段

(1) 打印稀疏结构

GaussianFactorGraph gfg = *graph.linearize(initial);
gfg.print("Linearized system:\n");

(2) 导出稠密 Hessian

Matrix H = gfg.hessian().first;
std::cout << "Hessian rank: " << Eigen::FullPivLU<Matrix>(H).rank() << std::endl;

如果 rank 小于变量维度 → 系统奇异。

总结

1. 触发点：系统奇异 / 欠约束 → 抛出 IndeterminantLinearSystemException。

2. 修复方法：

   • 给第一个 pose 添加 prior。
   • 给 landmark 增加观测。
   • 对角 damping 保证数值稳定。

3. 调试手段：

   • 打印 Hessian rank。
   • 可视化因子图。