【3D SLAM源码解读系列】(四) GTSAM与iSAM2——从回环约束到位姿图优化

zh路西法2026-08-16 8:51

前言

  • 前三期我们完整走通了回环检测的前半程：第一期 Scan Context 负责粗匹配，第三期 SOLiD 是它面向有限 FOV 的升级版，两者都能输出"这地方我来过"的回环候选帧与一个 yaw 偏转角
  • 第二期 small_gicp 负责精配准，把候选帧与当前帧做点云配准，输出完整的 6-DOF 相对位姿 (x, y, z, roll, pitch, yaw)
  • 至此，我们手里已经握着一堆"约束"：相邻帧之间的里程计约束、回环帧之间的回环约束。但这些约束是互相打架的 ------里程计每走一步都在累积漂移，回环约束又指出"你现在应该回到十几帧之前那个位置"，二者不可能同时精确
    满足
  • 往前内容：
  • 怎么协调这些约束？答案就是本期的主角------位姿图优化（Pose Graph Optimization, PGO） 。而在 PGO 领域，GTSAM（Georgia Tech Smoothing and Mapping）是事实上的标准库，它背后那套"因子图 + 增量平滑"的理论框架，几乎出现在每一个现代 SLAM 系统的后端里（LIO-SAM、LEGO-LOAM、Kimera 等）
  • 本文将基于 GTSAM 官方源码，从"回环约束长什么样"一路推到"因子图如何建模、如何优化、如何增量更新"，完整覆盖公式推导与代码逐行对照 ，最终给出可运行的批式 PGO 与增量 iSAM2 两个 Python 示例
  • 官方源码：https://github.com/borglab/gtsam

文章目录

    • 前言
    • [1 回顾：从回环约束到位姿图优化](#1 回顾：从回环约束到位姿图优化)
        • [1-1 前三期产出了什么：一条 6-DOF 回环约束](#1-1 前三期产出了什么：一条 6-DOF 回环约束)
        • [1-2 为什么还要位姿图优化（PGO）](#1-2 为什么还要位姿图优化（PGO）)
        • [1-3 位姿图长什么样](#1-3 位姿图长什么样)
    • [2 GTSAM：因子图库](#2 GTSAM：因子图库)
        • [2-1 GTSAM 是什么](#2-1 GTSAM 是什么)
        • [2-2 因子图与最大后验估计](#2-2 因子图与最大后验估计)
        • [2-3 直观理解PGO](#2-3 直观理解PGO)
        • [2-4 三大核心数据结构](#2-4 三大核心数据结构)
        • [2-5 两个关键因子：PriorFactor 与 BetweenFactor](#2-5 两个关键因子：PriorFactor 与 BetweenFactor)
          • [2-5-1 PriorFactor：先验因子](#2-5-1 PriorFactor：先验因子)
          • [2-5-2 BetweenFactor：相对因子](#2-5-2 BetweenFactor：相对因子)
          • [2-5-3 残差向量计算](#2-5-3 残差向量计算)
        • [2-6 噪声模型：把残差"白化"](#2-6 噪声模型：把残差"白化")
        • [2-7 非线性最小二乘与优化器](#2-7 非线性最小二乘与优化器)
        • [2-8 完整流程：从回环约束到输出](#2-8 完整流程：从回环约束到输出)
        • [2-9 完整批式 PGO 示例](#2-9 完整批式 PGO 示例)
        • [2-10 C++ API：和其他模块对接时如何按顺序调用 PGO](#2-10 C++ API：和其他模块对接时如何按顺序调用 PGO)
    • [3 iSAM2：增量平滑与建图](#3 iSAM2：增量平滑与建图)
        • [3-1 iSAM2 是什么](#3-1 iSAM2 是什么)
        • [3-2 批式优化的痛点](#3-2 批式优化的痛点)
        • [3-3 贝叶斯树与增量更新](#3-3 贝叶斯树与增量更新)
        • [3-4 流式重线性化](#3-4 流式重线性化)
        • [3-5 ISAM2 源码解读](#3-5 ISAM2 源码解读)
        • [3-6 完整增量 iSAM2 示例](#3-6 完整增量 iSAM2 示例)
        • [3-7 C++ API：实时 SLAM 里怎么对接 small_gicp 做增量 PGO](#3-7 C++ API：实时 SLAM 里怎么对接 small_gicp 做增量 PGO)
        • [3-8 批式 vs 增量对比](#3-8 批式 vs 增量对比)
    • [4 收尾：完整 SLAM 流程与 iSAM2 的用武之地](#4 收尾：完整 SLAM 流程与 iSAM2 的用武之地)
        • [4-1 把四期串成一条完整 SLAM 流程](#4-1 把四期串成一条完整 SLAM 流程)
        • [4-2 iSAM2 都用在了哪些算法](#4-2 iSAM2 都用在了哪些算法)
    • [5 附录：配套 Python 源码](#5 附录：配套 Python 源码)
        • [5-1 三大核心数据结构可视化 Python 源码](#5-1 三大核心数据结构可视化 Python 源码)
        • [5-2 PGO 完整流程可视化 Python 源码](#5-2 PGO 完整流程可视化 Python 源码)
        • [5-3 回环残差可视化 Python 源码](#5-3 回环残差可视化 Python 源码)
        • [5-4 旋转向量（轴角）可视化 Python 源码](#5-4 旋转向量（轴角）可视化 Python 源码)
        • [5-5 贝叶斯树与增量更新可视化 Python 源码](#5-5 贝叶斯树与增量更新可视化 Python 源码)
        • [5-6 iSAM2 实时增量更新连续帧可视化 Python 源码](#5-6 iSAM2 实时增量更新连续帧可视化 Python 源码)
    • 总结

1 回顾：从回环约束到位姿图优化

1-1 前三期产出了什么：一条 6-DOF 回环约束
  • 以防你忘记，本系列前三期的分工是这样一条链路：
    • 第一期 Scan Context / 第三期 SOLiD：粗匹配------输入一帧点云，输出"回环候选帧 ID + yaw 偏转角"。它只回答"这地方我来过吗"，不输出完整位姿
    • 第二期 small_gicp：精配准 ------拿到候选帧后，把当前帧点云配准到候选帧，输出完整的 6-DOF 相对变换 T ∈ S E ( 3 ) T \in SE(3) T∈SE(3)
  • 于是，每检测到一个回环，我们就得到一条这样的约束：

z i j = R i j t i j 0 1 ∈ S E ( 3 ) z_{ij} = \begin{bmatrix} R_{ij} & t_{ij} \\ 0 & 1 \end{bmatrix} \in SE(3) zij=Rij0tij1∈SE(3)

  • 其中 R i j ∈ S O ( 3 ) R_{ij} \in SO(3) Rij∈SO(3) 是 3×3 旋转矩阵， t i j ∈ R 3 t_{ij} \in \mathbb{R}^3 tij∈R3 是平移向量
  • 含义："第 j j j 帧相对于第 i i i 帧的姿态差是 z i j z_{ij} zij" ------也就是把第 i i i 帧坐标系下的东西变换到第 j j j 帧坐标系下所需要的那个变换

说人话：回环检测这条链路，本质上是在给你"发边"------每隔若干帧，它告诉你"你此刻的姿态，和十几帧之前那次，之间差了这么多（6 个自由度）"。这些边攒起来，就是一张图。接下来要做的，就是在这张图上求一个"让所有边都尽量满意"的解。

1-2 为什么还要位姿图优化（PGO）
  • 光有约束不够，还差一步"协调"。原因在于这些约束是 互相矛盾 的：
    • 里程计约束 ：相邻帧之间，由前端（ICP/GICP 或轮速、IMU 积分）给出。单条很准，但 逐帧累积误差会漂移 ------走 100 米回来，里程计可能告诉你回到了起点，实际上差了半米
    • 回环约束 ：非相邻帧之间，由回环检测 + 精配准给出。它 指出了漂移的方向和大小，但它自身也有测量噪声，不可能精确满足
  • 如果只是把里程计约束一条条拼起来（死推），轨迹会像没拉紧的橡皮筋，越走越歪；回环约束就像在橡皮筋上打一个结，告诉你"这里应该连回那里"
  • 我们要找的是 一个折中解 ：让所有约束的"违反程度"加权求和最小。这就是 PGO------把位姿当作变量，把约束当作误差项，做一个加权非线性最小二乘

说人话：里程计是"顺着走一步算一步"，回环是"突然想起这儿来过，应该和之前对得上"。PGO 就是那个"事后诸葛亮"------拿着整张约束图，把每帧位姿重新摆一遍，摆到"所有约束都尽量满意"的位置。漂移就在这一步被"平均"掉了。

1-3 位姿图长什么样
  • 位姿图是一个由两种元素构成的图：

#mermaid-svg-HMnqvu6j812f6VXe{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-HMnqvu6j812f6VXe .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-HMnqvu6j812f6VXe .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-HMnqvu6j812f6VXe .error-icon{fill:#552222;}#mermaid-svg-HMnqvu6j812f6VXe .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-HMnqvu6j812f6VXe .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-HMnqvu6j812f6VXe .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-HMnqvu6j812f6VXe .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-HMnqvu6j812f6VXe .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-HMnqvu6j812f6VXe .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-HMnqvu6j812f6VXe .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-HMnqvu6j812f6VXe .marker{fill:#333333;stroke:#333333;}#mermaid-svg-HMnqvu6j812f6VXe .marker.cross{stroke:#333333;}#mermaid-svg-HMnqvu6j812f6VXe svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-HMnqvu6j812f6VXe p{margin:0;}#mermaid-svg-HMnqvu6j812f6VXe .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-HMnqvu6j812f6VXe .cluster-label text{fill:#333;}#mermaid-svg-HMnqvu6j812f6VXe .cluster-label span{color:#333;}#mermaid-svg-HMnqvu6j812f6VXe .cluster-label span p{background-color:transparent;}#mermaid-svg-HMnqvu6j812f6VXe .label text,#mermaid-svg-HMnqvu6j812f6VXe span{fill:#333;color:#333;}#mermaid-svg-HMnqvu6j812f6VXe .node rect,#mermaid-svg-HMnqvu6j812f6VXe .node circle,#mermaid-svg-HMnqvu6j812f6VXe .node ellipse,#mermaid-svg-HMnqvu6j812f6VXe .node polygon,#mermaid-svg-HMnqvu6j812f6VXe .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-HMnqvu6j812f6VXe .rough-node .label text,#mermaid-svg-HMnqvu6j812f6VXe .node .label text,#mermaid-svg-HMnqvu6j812f6VXe .image-shape .label,#mermaid-svg-HMnqvu6j812f6VXe .icon-shape .label{text-anchor:middle;}#mermaid-svg-HMnqvu6j812f6VXe .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-HMnqvu6j812f6VXe .rough-node .label,#mermaid-svg-HMnqvu6j812f6VXe .node .label,#mermaid-svg-HMnqvu6j812f6VXe .image-shape .label,#mermaid-svg-HMnqvu6j812f6VXe .icon-shape .label{text-align:center;}#mermaid-svg-HMnqvu6j812f6VXe .node.clickable{cursor:pointer;}#mermaid-svg-HMnqvu6j812f6VXe .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-HMnqvu6j812f6VXe .arrowheadPath{fill:#333333;}#mermaid-svg-HMnqvu6j812f6VXe .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-HMnqvu6j812f6VXe .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-HMnqvu6j812f6VXe .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-HMnqvu6j812f6VXe .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-HMnqvu6j812f6VXe .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-HMnqvu6j812f6VXe .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-HMnqvu6j812f6VXe .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-HMnqvu6j812f6VXe .cluster text{fill:#333;}#mermaid-svg-HMnqvu6j812f6VXe .cluster span{color:#333;}#mermaid-svg-HMnqvu6j812f6VXe div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-HMnqvu6j812f6VXe .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-HMnqvu6j812f6VXe rect.text{fill:none;stroke-width:0;}#mermaid-svg-HMnqvu6j812f6VXe .icon-shape,#mermaid-svg-HMnqvu6j812f6VXe .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-HMnqvu6j812f6VXe .icon-shape p,#mermaid-svg-HMnqvu6j812f6VXe .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-HMnqvu6j812f6VXe .icon-shape .label rect,#mermaid-svg-HMnqvu6j812f6VXe .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-HMnqvu6j812f6VXe .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-HMnqvu6j812f6VXe .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-HMnqvu6j812f6VXe :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} x0

位姿节点
x1

位姿节点
x2

位姿节点
x3

位姿节点
先验因子

prior
里程计因子

odom
里程计因子

odom
里程计因子

odom
回环因子

loop

  • 节点（变量） ：每一帧的位姿 x i ∈ S E ( 3 ) x_i \in SE(3) xi∈SE(3)，是我们要求解的量
  • 边（因子） ：连接若干节点的约束，每个因子携带一个观测 z z z 和一个噪声模型 Σ \Sigma Σ
  • 小方块是"变量节点"（位姿），小圆圈是"因子节点"（约束）。这就是经典的双分图（bipartite graph）结构------GTSAM 里的 NonlinearFactorGraph 就是这张图
  • 注意，位姿图里没有地图点/路标，只有位姿。它和"因子图 SLAM"的完整形式（带 landmark 的）的区别就在于：PGO 把所有传感器观测都浓缩成了"位姿间的相对约束"，图更小、更干净，但足够消除漂移

说人话：位姿图就是一张"谁和谁之间差多少"的网。节点是每一帧机器人的姿态，边是"我告诉你这两帧之间差多少"。PGO 的任务就是给每个节点安排一个姿态，让网里每根边都被"拉"得最舒服。

2 GTSAM：因子图库

2-1 GTSAM 是什么
  • GTSAM（Georgia Tech Smoothing and Mapping）是佐治亚理工 Frank Dellaert 团队开发的开源 C++ 库（带 Python 绑定），核心解决 SLAM 后端的"因子图建模 + 非线性优化 + 增量平滑"问题
  • 它的历史几乎就是 SLAM 后端理论的一条主线：
    • 2006-2008 年提出 iSAM（Incremental Smoothing and Mapping），第一次让"增量更新 + 稀疏 QR 分解"跑起来
    • 2012 年提出 iSAM2（IJRR），用贝叶斯树（Bayes Tree）替代 QR 分解，实现了"增量 + 流式重线性化"------这是目前最主流的增量平滑框架
    • 同期沉淀出 NonlinearFactorGraph、Values、noiseModel 这一整套因子图建模原语
  • 为什么大家后端都爱用它？因为它的抽象和 SLAM 的数学一一对应：你把 SLAM 问题写成因子图，它就把因子图翻译成最小二乘，再翻译成稀疏线性代数，帮你解出来。你只管"建模"，不用手写 Jacobian、Hessian 和消元

说人话：GTSAM 之于 SLAM 后端，就像 Eigen 之于矩阵运算、OpenCV 之于视觉------你不用再手撸一个图优化器了。你只需要把"约束"一个个塞进图里，剩下的求 Jacobian、求 Hessian、稀疏分解、增量更新，它全包了。

2-2 因子图与最大后验估计
  • PGO 的数学本质是一个 最大后验估计（Maximum A Posteriori, MAP） 问题。设所有位姿为 X = { x 1 , ... , x n } X = \{x_1, \dots, x_n\} X={x1,...,xn}，所有观测为 Z Z Z，我们要找：

X ∗ = arg ⁡ max ⁡ X    P ( X ∣ Z ) X^* = \arg\max_X \; P(X \mid Z) X∗=argXmaxP(X∣Z)

  • 假设观测之间相互独立，联合后验可以分解为每个因子的乘积：
    P ( X ∣ Z ) ∝ ∏ i P ( z i ∣ x i ) P(X \mid Z) \propto \prod_i P(z_i \mid x_i) P(X∣Z)∝i∏P(zi∣xi)
    • 其中 P ( z i ∣ x i ) P(z_i \mid x_i) P(zi∣xi) 就是"给定某些位姿 x i x_i xi，观测到 z i z_i zi 的概率"，也就是一个似然
  • 假设每个因子都是高斯噪声，取负对数把"求最大"变成"求最小"，就得到加权最小二乘：
    X ∗ = arg ⁡ min ⁡ X ∑ i ∥ h i ( X i ) − z i ∥ Σ i 2 X^* = \arg\min_X \sum_i \left\| h_i(X_i) - z_i \right\|^2_{\Sigma_i} X∗=argXmini∑∥hi(Xi)−zi∥Σi2
    • h i ( ⋅ ) h_i(\cdot) hi(⋅)：预测函数，由当前位姿算出的"应当观测到的值"
    • z i z_i zi：实际观测值
    • ∥ e ∥ Σ 2 = e T Σ − 1 e \|e\|^2_\Sigma = e^T \Sigma^{-1} e ∥e∥Σ2=eTΣ−1e：马氏范数（用信息矩阵加权的二范数）

说人话：因子图优化 = "把所有约束的误差平方和加起来，找一个 X X X 让它最小"。 h i ( X i ) h_i(X_i) hi(Xi) 是"按当前位姿算，这两帧应该差多少"， z i z_i zi 是"实际测出来差多少"，两者之差就是误差 e e e。谁的观测越准（ Σ \Sigma Σ 越小），谁在总误差里的权重就越大。

  • 其中 h i h_i hi 通常是非线性的（位姿在 S E ( 3 ) SE(3) SE(3) 流形上，旋转不是线性量），所以这是一个 非线性最小二乘 问题------这正是 GTSAM 优化器要解的
2-3 直观理解PGO

  • PGO 到底在干什么，一句话就能说清：不断移动每一个位姿，让整张图所有约束的残差加起来最小 。这正是 2-2 那个加权最小二乘目标 ∑ i ∥ h i ( X i ) − z i ∥ Σ i 2 \sum_i \|h_i(X_i)-z_i\|^2_{\Sigma_i} ∑i∥hi(Xi)−zi∥Σi2 落到"位姿图"上的具体样子。

  • 这里的"预测"和"观测"都指相对位姿，别被"预测"两个字唬住（它不是预测未来）：

    • 观测 z i j z_{ij} zij：传感器（里程计 / 回环配准）量出来的"第 i i i 帧到第 j j j 帧的姿态差"
    • 预测 h ( x i , x j ) = x i − 1 ∘ x j h(x_i,x_j)=x_i^{-1}\circ x_j h(xi,xj)=xi−1∘xj：按 x i x_i xi、 x j x_j xj 当前取值反推出来的相对位姿
    • 残差就是这俩的差，优化器要做的就是把每个残差往零压

  • 不看抽象式子，直接看一张图------"回环处的残差"是怎么影响整张图的：

    • 左图（优化前）：机器人绕一圈回到起点附近，里程计漂移让每个位姿都偏离理想位置一点 （灰色虚线是理想轨迹），越到后面漂得越多，终点没落回起点，中间留了个 gap；回环是一条更强的约束 ，说"终点就该是起点"，这个 gap 就是回环这条约束的残差 e e e
    • 右图（优化后）：PGO 把这个 gap 分摊到整条轨迹------不是只把终点拽回去，而是每个位姿都往里挪一点，让整条轨迹平滑闭环
    • 一处回环的 gap，最终变成对整条轨迹的调整，这就是回环对整个 PGO 的影响

  • 放大到整张图，两件事最关键：

    • 迭代中残差每步都能重算 ：位姿挪到哪，就把哪当当前值，重新算 h h h、再和固定的 z z z 比，所以优化就是"挪一步 → 重算一遍所有残差 → 再挪一步"的循环
    • 回环怎么起作用 ：它不是神秘的特殊约束，就是一条更强、跨更远的 BetweenFactor （噪声更小、两帧隔得远），和里程计同公式。它的特殊只在于------它给出的相对位姿和里程计一路积分出来的结果冲突 （因为漂移），这个冲突正是优化器真正要消除的矛盾；否则纯里程计图本来就残差为零，优化器没事可干。这也点破为什么回环越多、PGO 效果越好：每多一条回环，就多一个把两段轨迹强制对齐的硬锚点，漂移能躲的地方越少，整条轨迹被约束得越紧

  • 剩下的细节一笔带过：这个"差"不能直接减矩阵，要用对数映射摊成向量 e = L o g ( z − 1 ∘ h ) e=\mathrm{Log}(z^{-1}\circ h) e=Log(z−1∘h)（见 2-5-2）；"信哪条约束"由 2-6 的噪声模型 Σ \Sigma Σ 决定，就是 2-2 里那个加权。

说人话：PGO 就是不断挪每个位姿、每挪一步重算一遍残差，直到所有约束的残差（按 2-2 的 Σ \Sigma Σ 加权后）加起来最小；回环是其中一条更硬、跨更远的约束，靠"制造冲突"把整张图往一致的方向拉。

2-4 三大核心数据结构
  • 建模一个 PGO 问题，只需要认识三样东西：NonlinearFactorGraph（图）、Values（变量值）、Factor（因子）。它们各自对应数学里的"图 / 节点取值 / 边"
  • 三者的关系，先看下面这张图一图看懂：
    • 顶部蓝框是 NonlinearFactorGraph------它本身只是一个因子列表，不含任何变量取值
    • 里面的黄色小方块是 Factor------每条因子通过整数 Key 引用它关心的变量
    • 底部绿框是 Values------一个 Key → 值 的映射，真正存着每个位姿的实际取值（Pose2(0,0,0) 等）
    • 箭头表示"因子用 Key 引用变量"：比如绿色回环因子 BetweenFactorPose2(key3, key1) 跨了很长一段，把不相邻的 key3 和 key1 连起来

  • 下面再逐个展开这三样：

  • NonlinearFactorGraph ：因子图的容器，继承自 FactorGraph<NonlinearFactor>，内部就是一个因子列表：

    • error(values) 返回整张图的当前总误差------就是 2-2 那个加权最小二乘目标函数的值
    • 在 Python 里就是 graph.error(initial)，可以直接拿来判断优化是否收敛
cpp 复制代码 
// gtsam/nonlinear/NonlinearFactorGraph.h
class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
  ...
  /** unnormalized error, \f$ \sum_i 0.5 (h_i(X_i)-z)^2 / \sigma^2 \f$ */
  virtual double error(const Values& values) const;
  /// Linearize a nonlinear factor graph
  std::shared_ptr<GaussianFactorGraph> linearize(const Values& values) const;
};
  • Values ：一个"键 → 值"的映射，存所有变量的当前取值。键 Key 是整数，值可以是任意流形元素（Pose2、Pose3、Point3...）：
    • Python 里的用法是 values.insert(1, gtsam.Pose2(0,0,0)) 和 values.atPose2(1)
    • 注意 Key 除了直接用 1, 2, 3 这样的整数，还可以用 gtsam.symbol('x', i) 生成带类型的符号键（如 x1, x2），在大图上更好调试
cpp 复制代码 
// gtsam/nonlinear/Values.h
// A values structure is a map from keys to values. It is used to specify the value
// of a bunch of variables in a factor graph.
class GTSAM_EXPORT Values {
  ...
  template<class T> bool insert(Key j, const T& val);  // 插入/更新一个变量
  template<class T> const T& at(Key j) const;          // 按类型取出一个变量
  bool exists(Key j) const;                            // 判断变量是否存在
};
  • Factor （因子）：一条边，对应一个约束。基类是 NonlinearFactor，它定义了两个关键接口：
cpp 复制代码 
// gtsam/nonlinear/NonlinearFactor.h
class GTSAM_EXPORT NonlinearFactor: public Factor {
  // 1. 误差：这个因子当前"有多不满意"
  virtual double error(const Values& c) const;
  // 2. 线性化：在当前点把非线性因子变成高斯因子（算出 Jacobian 和残差）
  virtual std::shared_ptr<GaussianFactor> linearize(const Values& c) const = 0;
  // 3. 维度：线性化后高斯因子的行数
  virtual size_t dim() const = 0;
};
  • 三个接口串起了整个优化：error 用于评价与收敛判断，linearize 用于构造正规方程，dim 决定线性系统的规模

说人话：NonlinearFactorGraph 是"约束清单"，Values 是"每个节点现在的取值"，Factor 是"清单里的一条约束"。优化器做的事就是------不断问每条因子"你现在多不满意、往哪个方向能更满意一点"，然后把所有因子的意见汇总，一起挪一挪 Values。

2-5 两个关键因子：PriorFactor 与 BetweenFactor
  • PGO 里就两类因子，吃透它们就吃透了位姿图：
因子 连接几个变量 作用 观测 z z z 是什么
PriorFactor 1 个 把某个位姿"钉"在某个值附近 该位姿的先验值
BetweenFactor 2 个 约束两个位姿之间的相对变换 相对位姿（里程计 / 回环）
2-5-1 PriorFactor：先验因子

  • 没有 PriorFactor 会怎样？整张图只在"相对位姿"上做文章，平移和绝对朝向都不可观（解可以整体平移/旋转而不改变任何约束），所以 第一帧一定要加一个先验 ，把坐标原点定下来

  • PriorFactor 是单变量版本，误差就是把"变量当前值"和"先验值"做 Local，即 L o g ( p r i o r − 1 ∘ x ) \mathrm{Log}(prior^{-1} \circ x) Log(prior−1∘x)------把变量 x x x 往 p r i o r prior prior 附近拉：

cpp 复制代码 
// gtsam/nonlinear/PriorFactor.h
template <class VALUE>
class PriorFactor : public ExtendedPriorFactor<VALUE> {
  PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr)
      : Base(key, prior, noiseModel::validOrDefault(prior, model)) {}
};
2-5-2 BetweenFactor：相对因子
  • BetweenFactor 的源码 ------误差函数 evaluateError 是理解一切的钥匙，它做了两件事：
    • Between(p1, p2) 计算相对位姿 h = p 1 − 1 ∘ p 2 h = p_1^{-1} \circ p_2 h=p1−1∘p2------"从 p1 走到 p2 要施加的变换"
    • Local(measured_, hx) 计算 h h h 相对于观测 m e a s u r e d measured measured 的"局部坐标"，也就是对数映射 L o g ( m e a s u r e d − 1 ∘ h ) \mathrm{Log}(measured^{-1} \circ h) Log(measured−1∘h)
    • 于是误差就是 e i j ( x i , x j ) = L o g  ⁣ ( z i j − 1 ∘ ( x i − 1 ∘ x j ) ) e_{ij}(x_i, x_j) = \mathrm{Log}\!\left( z_{ij}^{-1} \circ \left( x_i^{-1} \circ x_j \right) \right) eij(xi,xj)=Log(zij−1∘(xi−1∘xj))，其中 L o g \mathrm{Log} Log 是 S E ( 3 ) → R 6 SE(3) \to \mathbb{R}^6 SE(3)→R6（或 S E ( 2 ) → R 3 SE(2) \to \mathbb{R}^3 SE(2)→R3）的对数映射，把"两个刚体变换的差"变成一个切空间向量------这正是能在高斯假设下做最小二乘的量
cpp 复制代码 
// gtsam/slam/BetweenFactor.h
template<class VALUE>
class BetweenFactor : public NoiseModelFactorT<typename traits<VALUE>::TangentVector, VALUE, VALUE> {
  VALUE measured_; /** The measurement */

  /// evaluate error, returns vector of errors size of tangent space
  ErrorVector evaluateError(const T& p1, const T& p2,
                            OptionalMatrixType H1, OptionalMatrixType H2) const override {
    T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
    // manifold equivalent of h(x)-z -> log(z,h(x))
    return traits<T>::Local(measured_, hx);
  }
};
  • 关键点：误差不是在"变换矩阵"上直接做减法 （旋转矩阵没法直接减），而是先算预测相对位姿和观测相对位姿的"差"，再用对数映射把它"摊平"到切空间里。这也是 GTSAM 里所有 Lie 类型因子（Pose2、Pose3）共用的套路

说人话：BetweenFactor 问的是------"按你现在给的两个位姿 x i , x j x_i, x_j xi,xj 算出来的相对变换，和我实际测到的相对变换 z i j z_{ij} zij，差了多少？" 这个"差"不能直接减旋转矩阵，所以用对数映射 L o g \mathrm{Log} Log 把它变成一个 6 维向量（Pose3）或 3 维向量（Pose2），再交给噪声模型去加权。

2-5-3 残差向量计算
  • 看到这里你可能要问：具体到某一条约束，残差向量到底是怎么一步步算出来的？ 把位姿图里最常见的三类约束摆在一起对照：
约束 连接的变量 观测 z z z 残差 e e e
先验 x 1 x_1 x1 先验位姿 p p p L o g ( p − 1 ∘ x 1 ) \mathrm{Log}(p^{-1} \circ x_1) Log(p−1∘x1)
里程计 x i , x i + 1 x_i,\ x_{i+1} xi, xi+1（相邻） 相邻帧相对位姿 z i , i + 1 z_{i,i+1} zi,i+1 L o g ( z i , i + 1 − 1 ∘ x i − 1 ∘ x i + 1 ) \mathrm{Log}(z_{i,i+1}^{-1} \circ x_i^{-1} \circ x_{i+1}) Log(zi,i+1−1∘xi−1∘xi+1)
回环 x i , x j x_i,\ x_j xi, xj（不相邻） 回环相对位姿 z i j z_{ij} zij L o g ( z i j − 1 ∘ x i − 1 ∘ x j ) \mathrm{Log}(z_{ij}^{-1} \circ x_i^{-1} \circ x_j) Log(zij−1∘xi−1∘xj)

  • 这张表点破一个关键：里程计和回环是同一个 BetweenFactor ，公式一模一样，差别只在"连接的两个 Key 是否相邻"和" z z z、噪声 Σ \Sigma Σ 取多少"------回环一般配更小的噪声（更信它），因为它是前三期专门配准出来的硬约束。

  • 上表里的 L o g ( ⋅ ) \mathrm{Log}(\cdot) Log(⋅) 就是对数映射（Logmap） ：它把"两帧之间的相对误差"这个刚体变换（还待在李群上、没法直接拿来加权）摊平成一个切空间向量。接下来就看 GTSAM 是怎么一步步实现这个 L o g \mathrm{Log} Log 的------先看最简单的 2D 位姿 Pose2：

cpp 复制代码 
// gtsam/geometry/Pose2.cpp
Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
  if (H) *H = Pose2::LogmapDerivative(p);
  const Rot2& R = p.r();
  const Point2& t = p.t();
  double w = R.theta();                    // 旋转角 θ
  if (std::abs(w) < 1e-10)                 // 近乎纯平移：退化成 (x, y, 0)
    return Vector3(t.x(), t.y(), w);
  else {
    double c_1 = R.c() - 1.0, s = R.s();   // cosθ - 1, sinθ
    double det = c_1 * c_1 + s * s;        // (cosθ-1)² + sin²θ
    Point2 p = R_PI_2 * (R.unrotate(t) - t);  // R_PI_2 = [[0,-1],[1,0]]
    Point2 v = (w / det) * p;
    return Vector3(v.x(), v.y(), w);       // 旋转分量就是 θ
  }
}
  • 这个 else 分支看着绕，其实是在算一个很关键的矩阵 V − 1 ( θ ) V^{-1}(\theta) V−1(θ)。先记住结论：

V ( θ ) = 1 θ sin ⁡ θ cos ⁡ θ − 1 1 − cos ⁡ θ sin ⁡ θ , L o g ( R ( θ ) t 0 1 ) = ( V − 1 ( θ )   t θ ) V(\theta)=\frac{1}{\theta}\begin{bmatrix}\sin\theta & \cos\theta-1\\ 1-\cos\theta & \sin\theta\end{bmatrix},\qquad \mathrm{Log}\begin{pmatrix}R(\theta)&t\\0&1\end{pmatrix}=\begin{pmatrix}V^{-1}(\theta)\,t\\ \theta\end{pmatrix} V(θ)=θ1sinθ1−cosθcosθ−1sinθ,Log(R(θ)0t1)=(V−1(θ)tθ)

  • 也就是说，残差的平移部分并不是 t t t 本身，而是 V − 1 ( θ )   t V^{-1}(\theta)\,t V−1(θ)t ------旋转角 θ \theta θ 越大，平移分量被"拧"得越厉害。只有当 θ → 0 \theta\to 0 θ→0 时 V − 1 ( θ ) → I V^{-1}(\theta)\to I V−1(θ)→I，才退化成 Vector3(t.x(), t.y(), w) 里那种直接拿 t t t 的写法。这正是很多人第一次读源码时忽略掉的"旋转---平移耦合"。

  • 3D 的旋转部分由 SO3::Logmap 负责，核心是轴角（axis-angle） 换算：

cpp 复制代码 
// gtsam/geometry/SO3.cpp（省略 π 分支与泰勒展开，保留主干）
Vector3 SO3::Logmap(const SO3& R) {
  double tr = R.matrix().trace();
  double theta = acos((tr - 1.0) / 2.0);          // 旋转角 θ
  double magnitude = theta / (2.0 * sin(theta));  // θ / 2sinθ
  // 由反对称部分提取旋转轴，再乘 θ/2sinθ 得到旋转向量
  return magnitude * Vector3(R(2, 1) - R(1, 2),
                             R(0, 2) - R(2, 0),
                             R(1, 0) - R(0, 1));
}
  • 它的公式是：

L o g ( R ) = ω = θ   n , θ = arccos ⁡ t r ( R ) − 1 2 \mathrm{Log}(R)=\omega=\theta\,n,\qquad \theta=\arccos\frac{\mathrm{tr}(R)-1}{2} Log(R)=ω=θn,θ=arccos2tr(R)−1

  • 其中 n n n 是旋转轴（单位向量）， ω \omega ω 就是旋转向量 ------方向是转轴，长度是转角。代码里 R(2,1)-R(1,2) 那三行，就是从旋转矩阵里"抠"出 2 sin ⁡ θ   n 2\sin\theta\,n 2sinθn，再乘上 θ / 2 sin ⁡ θ \theta/2\sin\theta θ/2sinθ 得到 ω \omega ω。当 θ ≈ 0 \theta\approx 0 θ≈0 时 sin ⁡ θ \sin\theta sinθ 趋于 0，源码里另走泰勒展开分支；当 θ = π \theta=\pi θ=π 时则按特例处理（这里都省略，不影响理解主线）。

  • 一图看清：旋转矩阵 R R R 绕转轴 n n n 转过 θ \theta θ，它的对数映射 ω = θ n \omega=\theta n ω=θn，就是那根沿 n n n、长度等于 θ \theta θ 的箭头（方向 = 转轴，长度 = 转角）：

  • 把 3D 的旋转和平移拼起来，Pose3 的对数映射就是：

L o g ( R t 0 1 ) = ( ω V − 1 ( ω )   t ) ∈ R 6 \mathrm{Log}\begin{pmatrix}R&t\\0&1\end{pmatrix}=\begin{pmatrix}\omega\\ V^{-1}(\omega)\,t\end{pmatrix}\in\mathbb{R}^6 Log(R0t1)=(ωV−1(ω)t)∈R6

  • 注意这里又出现了 V − 1 ( ⋅ ) V^{-1}(\cdot) V−1(⋅)，只是这一次是 3D 的 3×3 版本，作用在平移向量 t t t 上。无论 2D 还是 3D，残差向量的平移部分都要过一遍这个"反左雅可比"，旋转部分就是轴角/转角本身。 到这里，一条约束从"两个位姿"到"一个可加权的向量"的完整路径就清楚了：先按上表复合出误差位姿（回环是 z i j − 1 ∘ x i − 1 ∘ x j z_{ij}^{-1}\circ x_i^{-1}\circ x_j zij−1∘xi−1∘xj），再 L o g \mathrm{Log} Log 摊平。

说人话：残差不是"两个位姿矩阵直接相减"，而是先算"预测的相对位移"和"观测的相对位移"的差，再把这个差（一个刚体变换）用对数映射摊平成向量。2D 下这个向量就是 ( Δ x , Δ y , Δ θ ) (\Delta x,\ \Delta y,\ \Delta\theta) (Δx, Δy, Δθ)；3D 下是 6 维，旋转部分要换算成旋转向量。里程计和回环是同一个公式，只有连接的帧不一样。

2-6 噪声模型：把残差"白化"
  • 2-5-3 只把残差"摊平"成了向量 e e e，但还没回答"这条约束有多可信"。本节就用噪声模型 Σ \Sigma Σ 给每条残差加权------这一步叫白化，白化完的残差才真正"能进优化器"。
  • 每个因子都绑定一个噪声模型 noiseModel，它回答一个问题：这条约束我信几分？
  • GTSAM 的 noiseModel 是一个类层次，最常用的是 Diagonal，由标准差的向量构造：
python 复制代码 
# 噪声模型：先验 (x, y, theta) 各方向标准差
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
# 里程计/回环噪声 (dx, dy, dtheta)
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
  • 对于 3D（Pose3），切空间是 6 维，顺序是 先旋转后平移 ，标准差向量对应 (rx, ry, rz, tx, ty, tz)：
python 复制代码 
PRIOR_NOISE_3D = gtsam.noiseModel.Diagonal.Sigmas(np.array([
    rpy_sigma, rpy_sigma, rpy_sigma,   # roll, pitch, yaw
    xyz_sigma, xyz_sigma, xyz_sigma]))  # x, y, z
  • 噪声模型在数学上的核心动作是 白化（whiten） 。看源码里的注释，Gaussian 模型的本质就是：
cpp 复制代码 
// gtsam/linear/NoiseModel.h
/**
 * Gaussian implements the mathematical model
 *  |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
 * where
 *   y = whiten(x) = R*x
 *   x = unwhiten(x) = inv(R)*y
 */
class GTSAM_EXPORT Gaussian: public Base {
  ...
  static shared_ptr Covariance(const Matrix& covariance);   // 由协方差构造
  static shared_ptr Information(const Matrix& M);           // 由信息矩阵构造
  static shared_ptr SqrtInformation(const Matrix& R);       // 由信息矩阵平方根构造
  virtual Vector whiten(const Vector& v) const;             // 白化
};
  • 这里的关键关系是：
    Σ − 1 = R T R \Sigma^{-1} = R^T R Σ−1=RTR
    • Σ \Sigma Σ：协方差矩阵，衡量"不确定性"
    • Σ − 1 \Sigma^{-1} Σ−1：信息矩阵，衡量"确定性"------协方差越小，信息越大，约束越硬
    • R R R：信息矩阵的"平方根"，白化就是左乘 R R R
  • 白化的意义在于，把"带权重的马氏范数"变成一个"普通的二范数"（ ∥ y ∥ 2 = y T y = ∑ i y i 2 \|y\|^2 = y^T y = \sum_i y_i^2 ∥y∥2=yTy=∑iyi2，即不加权、只平方求和）：
    ∥ e ∥ Σ 2 = e T Σ − 1 e = e T R T R e = ∥ R e ∥ 2 = ∥ y ∥ 2 \|e\|^2_\Sigma = e^T \Sigma^{-1} e = e^T R^T R e = \|R e\|^2 = \|y\|^2 ∥e∥Σ2=eTΣ−1e=eTRTRe=∥Re∥2=∥y∥2
    • 其中 y = R e y = R e y=Re 就是白化后的残差
    • 对 Diagonal.Sigmas([σx, σy, σθ])， R = d i a g ( 1 / σ x , 1 / σ y , 1 / σ θ ) R = \mathrm{diag}(1/\sigma_x, 1/\sigma_y, 1/\sigma_\theta) R=diag(1/σx,1/σy,1/σθ)，白化就是"每个分量除以自己的标准差"------把误差统一到"几个标准差"的量纲上
  • 这就是为什么上一期 small_gicp 里 GICP 的马氏距离、和这里 GTSAM 的噪声模型，是同一套东西：不确定度高的方向给低权重，不确定度低的方向给高权重

说人话：噪声模型就是给每条约束配的"可信度"。白化（除以标准差）之后，所有误差都被换算成"差了几个标准差"的无量纲数，不同单位的约束（米、弧度）才能公平地加在一起求最小二乘。

2-7 非线性最小二乘与优化器
  • 有了因子和噪声模型，整张图的总误差就是：

E ( X ) = 1 2 ∑ i ∥ h i ( X i ) − z i ∥ Σ i 2 = 1 2 ∑ i ∥ R i e i ( X i ) ∥ 2 E(X) = \frac12 \sum_i \left\| h_i(X_i) - z_i \right\|^2_{\Sigma_i} = \frac12 \sum_i \| R_i e_i(X_i) \|^2 E(X)=21i∑∥hi(Xi)−zi∥Σi2=21i∑∥Riei(Xi)∥2

  • 求解它的思路和上一期 small_gicp 一模一样------Gauss-Newton / Levenberg-Marquardt。回顾一下四步：

    • 第 1 步 线性化 ：每个残差在当前点一阶泰勒展开 e ( x + δ ) ≈ e ( x ) + J δ e(x + \delta) \approx e(x) + J \delta e(x+δ)≈e(x)+Jδ
    • 第 2 步 白化 ： y ( δ ) ≈ R e ( x ) + R J δ y(\delta) \approx R e(x) + R J \delta y(δ)≈Re(x)+RJδ，把马氏范数变成普通二范数
    • 第 3 步 正规方程 ：令导数归零，得到 ( J T Σ − 1 J )   δ = − J T Σ − 1 e (J^T \Sigma^{-1} J)\,\delta = -J^T \Sigma^{-1} e (JTΣ−1J)δ=−JTΣ−1e，记作 H δ = − b H \delta = -b Hδ=−b
    • 第 4 步 更新 ： x ← x ⊕ δ x \leftarrow x \oplus \delta x←x⊕δ（流形上的 retract，旋转用指数映射更新），回到第 1 步

  • 其中 H = ∑ i J i T Σ i − 1 J i H = \sum_i J_i^T \Sigma_i^{-1} J_i H=∑iJiTΣi−1Ji 是 Hessian 的近似（信息矩阵）， b = ∑ i J i T Σ i − 1 e i b = \sum_i J_i^T \Sigma_i^{-1} e_i b=∑iJiTΣi−1ei 是梯度。这是 GTSAM 每个优化器内部都在做的一件事

  • 和 small_gicp 的关键区别在于规模 ：配准只有 6 个变量（一个位姿），而 PGO 有成千上万个变量（每帧一个位姿）。但好在这张图是稀疏 的------每个因子只连 1~2 个变量，所以 H H H 是稀疏矩阵，GTSAM 用消元 + 稀疏分解（COLAMD 排序 + Cholesky/QR）来高效求解，而不是暴力求逆

  • GTSAM 提供了两个优化器，接口一致：

python 复制代码 
# Gauss-Newton
params = gtsam.GaussNewtonParams()
params.setRelativeErrorTol(1e-5)
params.setMaxIterations(100)
result = gtsam.GaussNewtonOptimizer(graph, initial, params).optimize()

# Levenberg-Marquardt（对初值更鲁棒，默认首选）
lm_params = gtsam.LevenbergMarquardtParams()
result = gtsam.LevenbergMarquardtOptimizer(graph, initial, lm_params).optimize()
  • 两者对应源码里的两个类（GaussNewtonOptimizer、LevenbergMarquardtOptimizer），都继承自 NonlinearOptimizer，核心是一个 optimize() 循环：反复"线性化 → 求解 → 更新"，直到误差变化小于阈值或达到最大迭代次数
  • LM 与 GN 的区别只在求解那一步------GN 解 H δ = − b H\delta = -b Hδ=−b，LM 解 ( H + λ I ) δ = − b (H + \lambda I)\delta = -b (H+λI)δ=−b，用阻尼 λ \lambda λ 在"GN 大步快走"和"梯度下降小步稳走"之间自适应切换。上一期 Small_gicp 第 3 章 已经完整推导过，这里不再展开

说人话：GTSAM 的优化器就是"大号的小型配准优化器"。配准是 6 个变量求一次最优变换，PGO 是几万个变量求整条轨迹------但底层都是同一套 GN/LM：线性化、白化、拼稀疏正规方程、解出来、更新。区别只是 PGO 的 Hessian 是稀疏的，要用稀疏分解而不是 6×6 的 LDLT。

2-8 完整流程：从回环约束到输出

  • 把前面所有概念串成一条流水线，PGO 的完整流程就是四步：回环检测 → 构造输入 → 优化 → 取回输出 。先看总图：

  • 这张图的关键在「第 1 步」到「第 2 步」的连接处------前三期的回环，正是在这里才真正"用"起来：

    • 第 1 步 回环检测 ：Scan Context/SOLiD 粗匹配找到候选帧，small_gicp 精配准算出两帧之间的 6-DOF 相对变换 z i j ∈ S E ( 3 ) z_{ij} \in SE(3) zij∈SE(3)。这一步只输出"一条回环约束"，此时它还没参与任何优化
    • 第 2 步 构造输入 ：把这条回环约束 z i j z_{ij} zij 变成一条 BetweenFactor，和里程计约束、先验一起塞进因子图。回环约束的特别之处，是它连接的两个 Key 不相邻（比如 key5 连回 key2），正好把漂移"拉住"
    • 第 3 步 优化 ：把因子图 + 初值丢给 LevenbergMarquardtOptimizer（或 GN），内部反复"线性化 → 白化 → 正规方程 → 更新"直到收敛
    • 第 4 步 取回输出 ：从返回的 result 里按 Key 取优化后的位姿、看总误差、取边际协方差
2-9 完整批式 PGO 示例
  • 现在把前面所有概念串成一个完整可运行的例子：5 个 Pose2 位姿 + 1 个回环约束，用 LM 一次性优化。代码对应前面的每一个小节：
python 复制代码 
import math
import numpy as np
import gtsam

# ===== 1. 噪声模型 =====
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))     # 先验
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))  # 里程计/回环

# ===== 2. 因子图容器 =====
graph = gtsam.NonlinearFactorGraph()

# ===== 2a. 第一个位姿的先验（钉住坐标原点） =====
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))

# ===== 2b. 里程计约束（BetweenFactor） =====
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))

# ===== 2c. 回环约束：位姿 5 连回位姿 2 =====
graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))

# ===== 3. 故意给一个错的初始值 =====
initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))
initial.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))
initial.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))

print("初始误差:", graph.error(initial))

# ===== 4. LM 优化 =====
params = gtsam.LevenbergMarquardtParams()
result = gtsam.LevenbergMarquardtOptimizer(graph, initial, params).optimize()
print("优化后误差:", graph.error(result))

# ===== 5. 取回结果 =====
for i in range(1, 6):
    print(f"X{i}: {result.atPose2(i)}")

# ===== 6. 边际协方差（反映每个位姿的不确定性） =====
marginals = gtsam.Marginals(graph, result)
print("X5 位置协方差:\n", marginals.marginalCovariance(5)[0:2, 0:2])
  • 运行结果（关键数值）------误差从 20.1 降到 8e-18，几乎精确收敛到零：
    • 这是一个"无噪声、约束自洽"的玩具图，LM 能精确满足所有约束
    • X5 的位置协方差约 0.2 m²，比单条里程计噪声（0.2m 标准差 → 0.04 m² 方差）大------因为 X5 离先验锚点最远，误差逐帧累积，即便有回环修正，不确定性仍比 X1 大
text 复制代码 
初始误差: 20.10855822349009
优化后误差: 8.219125041390439e-18
X5 位置协方差: [[0.202, 0.036], [0.036, 0.260]]
  • 对应的可视化（左：初始漂移轨迹；右：LM 优化后，回环约束把轨迹拉回正确的矩形，蓝色椭圆是边际协方差）：
    • 左图虚线是"死推"出来的轨迹，已经明显漂移、回不到起点
    • 右图优化后，轨迹被拉成规整的直角矩形，X5 精确回到 X2 附近（绿色回环边被满足），椭圆尺寸反映各帧的不确定度

说人话：上面这段就是"批式 PGO"的完整流程------建图、加先验、加里程计、加回环、给初值、LM 一把梭。几十行代码，回环约束就把漂移的轨迹"掰"回了正确的形状。

2-10 C++ API：和其他模块对接时如何按顺序调用 PGO

  • 2-9 那段是 Python 一把梭，适合快速验证思路；但工程里 GTSAM 后端是 C++ 写的，还要跟前端（里程计、回环检测、精配准）持续交互。这一节看两件事：C++ 里 PGO 长什么样 ，以及数据按什么顺序流进来。

  • 调用顺序和 2-9 完全同构，一共六步，每步的输入来源也一并标出：

    • 第 1 步：建噪声模型 ------ noiseModel::Diagonal::Sigmas；σ 来自传感器精度 / 配准质量
    • 第 2 步：建因子图 ------ NonlinearFactorGraph，按"先验 → 里程计 → 回环"依次 add
    • 第 3 步：给初值 ------ Values；来自死推 / 里程计积分的当前位姿
    • 第 4 步：优化 ------ LevenbergMarquardtOptimizer（批式）或 ISAM2（增量，见第 3 章）
    • 第 5 步：取回位姿 ------ result.at<Pose2>(key)
    • 第 6 步：取协方差 ------ Marginals，反映每帧的不确定度

  • 完整 C++ 代码（和 2-9 逐行对应，X(i) 是 Symbol('x', i) 的简写）：

cpp 复制代码 
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>

using namespace gtsam;
using symbol_shorthand::X;   // X(i) == Symbol('x', i)

int main() {
  // ===== 1. 噪声模型 =====
  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  auto odomNoise  = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

  // ===== 2. 因子图：先验 → 里程计 → 回环 =====
  NonlinearFactorGraph graph;
  graph.add(PriorFactor<Pose2>(X(1), Pose2(0, 0, 0), priorNoise));              // 2a 先验
  graph.add(BetweenFactor<Pose2>(X(1), X(2), Pose2(2, 0, 0), odomNoise));       // 2b 里程计
  graph.add(BetweenFactor<Pose2>(X(2), X(3), Pose2(2, 0, M_PI / 2), odomNoise));
  graph.add(BetweenFactor<Pose2>(X(3), X(4), Pose2(2, 0, M_PI / 2), odomNoise));
  graph.add(BetweenFactor<Pose2>(X(4), X(5), Pose2(2, 0, M_PI / 2), odomNoise));
  graph.add(BetweenFactor<Pose2>(X(5), X(2), Pose2(2, 0, M_PI / 2), odomNoise)); // 2c 回环

  // ===== 3. 初值 =====
  Values initial;
  initial.insert(X(1), Pose2(0.5, 0.0, 0.2));
  initial.insert(X(2), Pose2(2.3, 0.1, -0.2));
  initial.insert(X(3), Pose2(4.1, 0.1, M_PI / 2));
  initial.insert(X(4), Pose2(4.0, 2.0, M_PI));
  initial.insert(X(5), Pose2(2.1, 2.1, -M_PI / 2));

  // ===== 4. LM 优化 =====
  LevenbergMarquardtParams params;
  Values result = LevenbergMarquardtOptimizer(graph, initial, params).optimize();

  // ===== 5. 取回位姿 =====
  std::cout << "X5: " << result.at<Pose2>(X(5)) << std::endl;

  // ===== 6. 边际协方差 =====
  Marginals marginals(graph, result);
  std::cout << "X5 cov:\n" << marginals.marginalCovariance(X(5)) << std::endl;
  return 0;
}
  • 和其他模块对接，关键是搞清"每一步的数据是谁给的"。把前三期串进来，回环这条边的交接长这样：
cpp 复制代码 
// ============ 回环回调：前端检测到回环后，交给后端 GTSAM ============
void on_loop_detected(int i, int j) {
  // ===== 阶段 1：粗匹配（第 1 期 Scan Context / 第 3 期 SOLiD）=====
  // 回调外已完成：只给出"当前帧 i 与候选帧 j 是回环"，不产出位姿。

  // ===== 阶段 2：精配准（第 2 期 small_gicp）=====
  // 把当前帧 i 配准到候选帧 j，一个 RegistrationResult 里同时装两样东西：
  //   .T ------ 6-DOF 相对位姿 z_ij
  //   .H ------ 6×6 Hessian（信息矩阵 = 协方差的逆），反映这次配准有多准
  small_gicp::RegistrationResult res = small_gicp::align(cloud[i], cloud[j]);
  Pose3  z_ij = res.T;
  Matrix H     = res.H;

  // ===== 阶段 3：后端 GTSAM 把 z_ij 变成一条回环因子 =====
  // 直接用信息矩阵构造噪声（见 2-6）：配得越差，H 越小，这条约束越松
  auto loopNoise = noiseModel::Gaussian::Information(H);
  graph.add(BetweenFactor<Pose3>(X(i), X(j), z_ij, loopNoise));
  // 注意：这里只 add 边、不优化；什么时机触发优化看下一段
}
  • 上面只讲了"一条回环边怎么进图"。放进完整的离线主流程里------graph / initial 是贯穿全程的持久状态（工程里通常是 SLAM 类的成员，on_loop_detected 也往里写同一张图），前端每帧只往里 add，攒齐后一次性 optimize：
cpp 复制代码 
// ============ 离线批式：数据攒齐后一次 optimize（和 2-9 同构） ============
NonlinearFactorGraph graph;   // 持久状态：不是每帧重建，全程复用
Values initial;               // 初值：死推位姿也全程往里塞

// 噪声只建一次，全图复用
auto priorNoise = noiseModel::Diagonal::Sigmas(
    (Vector(6) << 0.1, 0.1, 0.1, 0.3, 0.3, 0.3).finished());
auto odomNoise = noiseModel::Diagonal::Sigmas(
    (Vector(6) << 0.2, 0.2, 0.2, 0.1, 0.1, 0.1).finished());

// ===== 阶段 A：数据流式到达，只 add 边、不优化 =====
graph.add(PriorFactor<Pose3>(X(0), Pose3(), priorNoise));   // 第 0 帧钉在世界原点
initial.insert(X(0), frontend_odom_pose(0));                // 初值 = 死推位姿

for (int i = 1; i < N; ++i) {
  initial.insert(X(i), frontend_odom_pose(i));              // 死推位姿做初值
  graph.add(BetweenFactor<Pose3>(X(i - 1), X(i),
           frontend_odom_relative(i - 1, i), odomNoise));   // 里程计边：每帧一条

  if (frontend_loop_detected(i)) {                          // 回环：隔几十帧才一条
    int j = frontend_loop_candidate(i);                     // 粗匹配给的候选帧
    on_loop_detected(i, j);                                 // 复用上面：往同一张图加回环边
  }
}

// ===== 阶段 B：攒齐后一次批式优化 =====
LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, initial, params).optimize();

// ===== 阶段 C：取回修正后的整条轨迹 =====
for (int i = 0; i < N; ++i)
  Pose3 x_i = result.at<Pose3>(X(i));                        // 修正后的位姿

  • 把阶段 B 换成 ISAM2::update，就是第 3 章的在线增量版：数据一到就更新，不用攒齐。

  • 三个"对接时最容易踩的坑"：

    • Key 要对齐 ：前端里程计/回环用的是帧 ID，后端 GTSAM 用 Key。两者必须同一套映射（直接用整数帧 ID，或 Symbol('x', frame_id)），否则回环边会连错节点
    • 回环不是每帧都有 ：里程计每帧一条，回环隔几十帧才一条------前端检测到才 add，别把回环当"每帧必给"的输入
    • 噪声按配准质量定 ：回环这条边的噪声不该是拍脑袋的常数，最好拿精配准的 Hessian（信息矩阵 H）直接构造噪声，配得差就松、配得好就紧

说人话：C++ 版就是 2-9 那六步原样翻译一遍------Sigmas 建噪声、graph.add 加约束、Values 塞初值、LevenbergMarquardtOptimizer 优化、at<Pose2> 取结果。对接的难点不在 API，而在每个数据是谁、在什么时候塞进来的 ：里程计每帧塞一条，回环隔几十帧塞一条，噪声大小按配准质量定，Key 前后端对齐。真上线把第 4 步换成 ISAM2 就是增量版（第 3 章）。

3 iSAM2：增量平滑与建图

  • 第 2 章末尾的 PGO（2-9、2-10）用的是 LevenbergMarquardtOptimizer 做批式 优化：数据到齐，整张图一次算完。但 SLAM 后端跑在机器人上，数据是一帧一帧流进来的 ，不能等"全到齐"------iSAM2 就是为这个场景准备的
3-1 iSAM2 是什么
  • iSAM2 全称 Incremental Smoothing And Mapping 2 （增量式平滑与建图，第二版），是 GTSAM 里的在线、增量式 因子图求解器。它和批式 LevenbergMarquardtOptimizer 是同一件事的两种做法：
    • 批式：数据到齐 → 全图一次优化（准，但每来一条约束就得从头算一遍，慢）
    • iSAM2：每来一条约束 → 只更新它影响到的局部 → 立刻得到够用的解（快，精度逼近批式）
  • 它的核心思想一句话：不每帧重算整张图，只碰新数据影响到的局部 。落到实现上是三块，正好对应这一章后面三节：
    • 一张贝叶斯树存"已经分解好的结构"，新因子只重消元受影响的那条路径（3-3）
    • 一套流式重线性化：变量挪远了才重做泰勒展开，其余不动（3-4）
    • 一个 ISAM2 类：对外就 update() 塞约束、calculateEstimate() 取解（3-5）

说人话：iSAM2 不是一套新算法，而是把"整张图重新优化"这件事拆成"每次只修补新数据碰到的那一小块"。图还是那张图、优化还是那个优化，只是从"一次性"变成了"持续进行中"。

3-2 批式优化的痛点
  • 上一节的批式 PGO 有一个致命问题：它假设所有数据到齐后，一次性离线优化 。但真实 SLAM 是在线、实时、数据流式到达的------每一帧新的里程计/回环约束进来，你不可能从头把整张图重新优化一遍
  • 直接每帧重跑一次批式 LM，代价有多高？图的规模随帧数线性增长，而一次稀疏分解的复杂度近似与"变量数 × 填充(fill-in)"相关，最坏接近 O ( N 3 ) O(N^3) O(N3)。跑几千帧后，单次求解就要几秒甚至几十秒，实时性荡然无存
  • 于是问题变成：怎么让每次新数据到达时，只花"和新数据相关的那么一点点"计算量，就能得到和批式几乎一样的最优解？ 这就是 iSAM2 要回答的

说人话：批式优化是"每来一条约束，就把整本账从头算一遍"；iSAM2 是"只在账本上改动的那几行做局部更新"。账本越厚，后者的优势越明显------因为它只需要碰"新增约束影响到的那么一小块"。

3-3 贝叶斯树与增量更新
  • iSAM2 的核心数据结构是贝叶斯树（Bayes Tree） 。理解它，只需要抓住一条主线：消元顺序 + 增量重消元
  • 回顾批式求解：对因子图做变量消元（等价于对信息矩阵做 QR/Cholesky 分解），得到一个"上三角"的因子结构------这就是贝叶斯网络（Bayes Net）。消元的中间过程形成的"团（clique）"，按树状组织起来，就是贝叶斯树
  • 贝叶斯树的妙处在于：当新的因子加进来时，只有"受影响的那些团"需要被重新消元，树的其他部分原封不动 。具体地：
    • 新因子只涉及少数几个变量，这些变量在树里对应到某个"团"
    • 只有从这些团一路往上（向根方向）的那条路径需要重算
    • 为什么偏偏是"往上到根"？因为消元顺序决定了依赖是单向 的：先消元的变量把信息"贡献"给后消元的变量，所以树里一个团只依赖它的父（消元顺序更靠后的变量所在团），不依赖兄弟、也不依赖子孙。新因子只能顺着这条父链向上传播、到根为止，横向和向下都够不着------这就是"只碰一条路径"的根因
    • 其余子树完全不动，直接复用上一次的分解结果
  • 这就是"增量更新"的本质：把"全图重新分解"变成"树上一小块局部重新分解" ，计算量从 O ( N ) O(N) O(N) 降到 O ( 受影响团的大小 ) O(\text{受影响团的大小}) O(受影响团的大小)
    !\[GTSAM/gtsam_bayes_tree.png]
  • 左图是整棵贝叶斯树------根在上、叶在下，每个团装几个变量；右图是加进一个涉及 x8 的新因子后发生的事：只有它影响的 C5 → C2 → C1 这条到根的路径要重新消元，其余子树打灰、复用上次结果。
  • 在典型 SLAM 里，新约束只影响最近几帧 + 回环触达的几帧，受影响团远小于全图，所以 iSAM2 能做到毫秒级增量更新

说人话：贝叶斯树就像一棵"分解好的递归目录树"。每次新增约束，只把"这条约束涉及的文件夹"往上重新整理一遍，其余几百个文件夹原样保留。比起"整个硬盘重新索引"，快了几个数量级。

3-4 流式重线性化
  • 光有"局部重新消元"还不够。还有一个隐藏问题：线性化点会过期
  • GN/LM 的线性化是在当前估计 x ^ \hat{x} x^ 处做的泰勒展开。增量更新后，变量被挪到了新位置，旧的线性化点就不再准确。如果永远不重新线性化，误差会随着估计漂移而累积，解的质量下降
  • iSAM2 的做法是流式重线性化（fluid relinearization） ------它监视每个变量"线性化点离当前估计有多远"（用线性 delta 的大小衡量），超过阈值就只对这些变量重新线性化，而不是全图重新线性化。有两个关键参数：
参数 默认值 含义
relinearizeThreshold 0.1 变量的线性 delta 超过这个阈值才触发重线性化（越小越频繁、越准但越慢）
relinearizeSkip 10 每多少次 update() 才允许重线性化一次（避免每帧都重线性化，节约算力）
  • 还有一个 wildfireThreshold（ISAM2GaussNewtonParams，默认 0.001）：在贝叶斯树里做 delta 回代时，只有当变量变化超过这个阈值，才继续向上传播更新，否则提前截断------进一步省算力

说人话：重线性化就是"发现某个变量挪得够远了，就把它原来那次泰勒展开作废、重新展开一次"。relinearizeThreshold 是"挪多远算够远"，relinearizeSkip 是"至少隔几帧才重做一次"。这套机制让 iSAM2 在"实时性"和"精度"之间自适应权衡。

3-5 ISAM2 源码解读
  • 先看 ISAM2 类的核心成员与入口，源码里的注释把这套设计讲得最清楚：
    • 注意它继承自 BayesTree<ISAM2Clique>------贝叶斯树就是它内部的求解引擎
    • nonlinearFactors_ 保存原始非线性因子，linearFactors_ 保存它们的线性化版本，只有"需要重线性化"的变量才更新后者
    • update() 的入参很有意思：newFactors 是本帧新增的因子，newTheta 是本帧新增变量的初值（已存在的老变量不能再塞进来），removeFactorIndices 支持删因子（做滑动窗口/固定滞后平滑时用）
cpp 复制代码 
// gtsam/nonlinear/ISAM2.h
/**
 * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
 * The typical cycle of using this class: create an instance with ISAM2Params,
 * then add measurements and variables as they arrive using the update() method.
 * At any time, calculateEstimate() may be called to obtain the current estimate.
 */
class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
  Values theta_;                       // 当前线性化点
  NonlinearFactorGraph nonlinearFactors_;  // 存所有原始非线性因子（重线性化时用）
  mutable GaussianFactorGraph linearFactors_;  // 当前线性化的因子（只按需更新）
  ISAM2Params params_;                 // 参数
  ...
  virtual ISAM2Result update(
      const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),  // 新增因子
      const Values& newTheta = Values(),                                // 新增变量的初值
      const FactorIndices& removeFactorIndices = FactorIndices(),        // 要删除的因子
      ...);
};
  • 再看 ISAM2Params 里和重线性化直接相关的字段------其中 factorization 选择用 Cholesky 还是 QR 做数值分解（Cholesky 更快但病态时可能出问题，QR 更稳但更慢；默认 Cholesky，遇到 IndefiniteLinearSystemException 再换 QR）：
cpp 复制代码 
// gtsam/nonlinear/ISAM2Params.h
struct GTSAM_EXPORT ISAM2Params {
  RelinearizationThreshold relinearizeThreshold;  // 默认 0.1
  int relinearizeSkip;                            // 默认 10
  bool enableRelinearization;                     // 默认 true
  enum Factorization { CHOLESKY, QR };
  Factorization factorization;                    // 默认 CHOLESKY
  ...
};
  • update() 返回一个 ISAM2Result，里面有两个很有用的统计量，用它们可以直观看到 iSAM2 的增量性（正常情况下它们应该远小于全图变量数）：
cpp 复制代码 
// gtsam/nonlinear/ISAM2Result.h
struct ISAM2Result {
  size_t getVariablesRelinearized() const;  // 本次 update 重线性化了的变量数
  size_t getVariablesReeliminated() const;  // 本次 update 重新消元了的变量数
  ...
};

说人话：ISAM2 对外就是三个动作------update() 塞新约束、calculateEstimate() 取最新解、getDelta() 看变量挪了多少。内部它维护一棵贝叶斯树，只对"受影响的团"重消元、只对"挪得够远"的变量重线性化。这就是它快的原因。

3-6 完整增量 iSAM2 示例
  • 下面用一个逐帧增量的例子，模拟真实 SLAM 的数据流式到达：每帧要么加一条里程计约束，要么（检测到回环时）加一条回环约束，update() 后立刻 calculateEstimate() 取结果。完整代码（省略绘图部分，完整版在 GTSAM/gtsam_pgo_demo.py）：
python 复制代码 
import math
import numpy as np
import gtsam

np.random.seed(0)
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))

# 真实里程计：直行 + 3 个直角弯 + 直行（绕一个矩形，最后回到起点附近）
true_odom = [(2, 0, 0), (2, 0, math.pi/2), (2, 0, math.pi/2),
             (2, 0, math.pi/2), (2, 0, math.pi/2)]
# 用高斯噪声污染里程计
odom_meas = [np.random.multivariate_normal(o, ODOMETRY_NOISE.covariance()) for o in true_odom]

# iSAM2 参数
params = gtsam.ISAM2Params()
params.setRelinearizeThreshold(0.1)   # 线性 delta 超过阈值才重线性化
params.relinearizeSkip = 1            # 每次 update 都允许重线性化
isam = gtsam.ISAM2(params)

# 第 0 帧：先验（第一次 update 只传先验这一条新增）
new_factors = gtsam.NonlinearFactorGraph()
new_theta = gtsam.Values()
new_factors.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
new_theta.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
isam.update(new_factors, new_theta)
current = isam.calculateEstimate()

for i in range(len(true_odom)):
    noisy = odom_meas[i]
    # 每帧新建两个容器，只装本帧新增的因子和变量（iSAM2 只吃新增，老账本自己记着）
    new_factors = gtsam.NonlinearFactorGraph()
    new_theta = gtsam.Values()
    if i == 4:
        # 回环：位姿 5 连回位姿 2
        new_factors.push_back(gtsam.BetweenFactorPose2(i + 1, 2, gtsam.Pose2(2, 0, math.pi/2), ODOMETRY_NOISE))
    else:
        # 里程计：i+1 连 i+2
        new_factors.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, gtsam.Pose2(noisy[0], noisy[1], noisy[2]), ODOMETRY_NOISE))
        # 用带噪声里程计累加出下一帧初值
        est = current.atPose2(i + 1).compose(gtsam.Pose2(noisy[0], noisy[1], noisy[2]))
        new_theta.insert(i + 2, est)

    # 增量更新：只优化受影响的部分
    result = isam.update(new_factors, new_theta)
    current = isam.calculateEstimate()
    print(f"update {i+1}: relinearized = {result.getVariablesRelinearized()}, "
          f"reeliminated = {result.getVariablesReeliminated()}")
  • 运行输出------注意 relinearized 一直是 2、不随帧数增长：每帧只有"新变量 + 它相邻的老变量"被重线性化（新变量初值给得较偏、超了阈值）；reeliminated 从 3 涨到 5，是因为回环在第 5 帧触发，涉及的团一路重消元到根、所以最后一帧最多：
    • 关键是 relinearized 不涨------这正是 iSAM2"只碰局部"的直接证据；若像批式那样每帧把整张图传进去，这个数会跟着图一起涨
    • 实际工程里 relinearizeSkip 默认是 10，重线性化不会每帧都做------这里设成 1 只是演示这两个统计量的含义
text 复制代码 
update 1: relinearized = 2, reeliminated = 3
update 2: relinearized = 2, reeliminated = 3
update 3: relinearized = 2, reeliminated = 4
update 4: relinearized = 2, reeliminated = 4
update 5: relinearized = 2, reeliminated = 5
  • 对应的可视化------不是一张最终结果，而是 6 张连续快照，展示"路径逐步延伸 → 漂移累积 → 回环触发 → 轨迹被拉正"的完整过程。注意关键区别：每一帧都是 update() 之后实时拿到的结果，而不是最后一次性优化出来的；每帧 update() 只碰新约束涉及的那一小块贝叶斯树：
  • 若想把整个例子换成 3D，只需把类型从 Pose2 换成 Pose3、噪声从 3 维换成 6 维（先旋转后平移），其余逻辑完全一样------这正是回环约束 small_gicp 输出的 6-DOF 相对位姿能直接喂进去的形式：
python 复制代码 
# 3D 版本：把 Pose2 换成 Pose3，噪声换成 6 维
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05, 0.3, 0.3, 0.3]))
graph.add(gtsam.PriorFactorPose3(1, gtsam.Pose3(), PRIOR_NOISE))
graph.add(gtsam.BetweenFactorPose3(1, 2, gtsam.Pose3(gtsam.Rot3.RzRyRx(rx, ry, rz),
                                                     gtsam.Point3(tx, ty, tz)), ODOMETRY_NOISE))

说人话：iSAM2 的使用方式就是"边收数据边 update()，随时 calculateEstimate() 拿结果"。每帧只做一点点增量更新，却能保持和批式几乎一致的最优解------这就是它在真实 SLAM 后端里无处不在的原因。

3-7 C++ API：实时 SLAM 里怎么对接 small_gicp 做增量 PGO

  • 3-6 那段是 Python 一把梭，适合快速验证思路；但真实 SLAM 后端是 C++ 写的，还要跟前端（里程计、回环检测、精配准）持续交互。这一节看两件事：C++ 里增量 PGO 长什么样 ，以及 small_gicp 输出的 6-DOF 数据怎么流进 iSAM2。

  • 和 2-10 的批式版只有一处本质区别------批式是 graph 全程复用、数据攒齐后 optimize() 一次；增量版是每帧只把"本帧新增"的因子和变量传给 update()，传完即弃 ，calculateEstimate() 随时取解。剩下的噪声构造、因子类型、Key 映射，全都和 2-10 一样。

  • 每帧的调用节奏是一个三步循环：

    • 第 1 步：收前端数据转因子 ------ 里程计每帧一条 BetweenFactor；回环隔几十帧一条，先 small_gicp 精配准拿到 .T / .H
    • 第 2 步：update(newFactors, newTheta) ------ 只传本帧新增的因子和变量初值，传完即清空
    • 第 3 步：calculateEstimate() ------ 立刻取回修正后的位姿，发布给下游（建图 / 显示）

  • 完整 C++ 代码（X(i) 是 Symbol('x', i) 的简写，帧 ID 直接做 Key）：

cpp 复制代码 
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>

using namespace gtsam;
using symbol_shorthand::X;   // X(i) == Symbol('x', i)

int main() {
  // ===== 0. 建 iSAM2 实例 + 噪声（持久状态，全程复用）=====
  ISAM2Params params;
  params.relinearizeThreshold = 0.1;   // 线性 delta 超阈值才重线性化
  params.relinearizeSkip = 1;          // 演示：每次 update 都允许重线性化
  ISAM2 isam(params);

  auto priorNoise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << 0.1, 0.1, 0.1, 0.3, 0.3, 0.3).finished());
  auto odomNoise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << 0.2, 0.2, 0.2, 0.1, 0.1, 0.1).finished());

  // 每帧新建两个容器，只装"本帧新增"的因子和变量，update 完即清空
  NonlinearFactorGraph newFactors;
  Values newTheta;

  // ===== 帧 0：先验钉住原点（第一次 update 的新增部分）=====
  newFactors.add(PriorFactor<Pose3>(X(0), Pose3(), priorNoise));
  newTheta.insert(X(0), frontend_odom_pose(0));   // 初值 = 死推位姿
  isam.update(newFactors, newTheta);              // 第 1 次增量更新
  Values current = isam.calculateEstimate();      // 立刻取当前解

  // ===== 帧 1..N-1：里程计每帧一条 + 回环隔几十帧一条 =====
  for (int i = 1; i < N; ++i) {
    newFactors.resize(0);   // 清空重来：不能重复塞老因子（iSAM2 只收新增）
    newTheta.clear();

    // ---- 里程计边 i-1 → i：i-1 是老变量，i 是本帧新变量 ----
    newFactors.add(BetweenFactor<Pose3>(X(i - 1), X(i),
        frontend_odom_relative(i - 1, i), odomNoise));
    newTheta.insert(X(i), frontend_odom_pose(i));   // 只放本帧新变量的初值

    // ---- 回环边：前端检测到才加（small_gicp 精配准）----
    if (frontend_loop_detected(i)) {
      int j = frontend_loop_candidate(i);           // 粗匹配给的候选帧
      small_gicp::RegistrationResult res = small_gicp::align(cloud[i], cloud[j]);
      Pose3  z_ij = res.T;                          // 6-DOF 相对位姿
      Matrix H     = res.H;                         // 6×6 信息矩阵
      auto loopNoise = noiseModel::Gaussian::Information(H);   // 配得越差越松
      newFactors.add(BetweenFactor<Pose3>(X(i), X(j), z_ij, loopNoise));
      // 注意：X(j) 是历史帧，早就在图里了，别塞进 newTheta
    }

    // ---- 增量更新：只碰本帧新增约束影响的局部，然后实时取解 ----
    isam.update(newFactors, newTheta);
    current = isam.calculateEstimate();

    // 本帧修正后的位姿就是 current 里的 X(i)，直接发布给下游
    Pose3 x_i = current.at<Pose3>(X(i));
  }
}

  • 三个"增量版最容易踩的坑"：

    • update() 只收"新增" ：newFactors / newTheta 每帧只能放本帧新建的因子和变量，老因子/老变量重复塞会重复计数甚至报错。所以每帧 resize(0) / clear() 重来，update() 完即弃
    • 回环边两端通常都是老变量 ：回环把当前帧 X(i) 连回历史帧 X(j)，X(j) 早就建过了------回环因子进 newFactors，但 X(j) 不能进 newTheta，否则等于"新增一个已存在的变量"
    • 帧 ID 前后端对齐 ：small_gicp 拿到的"帧 i / j"必须和后端 X(i) / X(j) 是同一套映射（直接用帧 ID 做 Key 最省心），否则回环边连错节点

说人话：增量版就是把 2-10 的第 4 步从"攒齐后 optimize() 一次"换成"每帧 update() 一小口"。真正的差别就一句------update() 只吃本帧新来的东西，老账本它自己记着 。所以每帧新建 newFactors / newTheta、只装新增、传完即弃；回环那头的历史帧早就入账了，别再塞一遍初值。剩下的 small_gicp 配准、.T / .H 转因子、Key 对齐，和批式版一字不差。

3-8 批式 vs 增量对比
  • 最后把两种模式做一次总结对比：
对比维度 批式（GTSAM LM/GN） 增量（iSAM2）
求解时机 数据到齐后一次性离线优化 每帧数据到达即增量更新
单次代价 O ( N ) O(N) O(N)~ O ( N 3 ) O(N^3) O(N3)，随图增长 O ( 受影响团 ) O(\text{受影响团}) O(受影响团)，基本恒定
数据结构 因子图 → 稀疏线性系统 因子图 → 贝叶斯树
重线性化 每轮迭代全图重线性化 只重线性化 delta 超阈值的变量
典型用途 建图后全局一致化、离线优化 在线 SLAM 后端、实时里程计修正
精度 收敛到局部最优 接近批式最优（可控差距）
  • 一个常见的工程组合是：前端里程计 + iSAM2 在线增量 + 建图结束后批式 LM 做一次全局精修。iSAM2 保证实时，批式保证最终一致------两者不是替代关系，而是配合关系

说人话：批式是"算得最准但慢"，增量是"算得够准且快"。在线跑用 iSAM2，离线收尾用批式 LM------SLAM 后端基本就是这两个工具来回切换。

4 收尾：完整 SLAM 流程与 iSAM2 的用武之地

4-1 把四期串成一条完整 SLAM 流程

  • 到这里，本系列四期拼起来，正好是一条激光 SLAM 的完整流水线。把每一期的产出接上，长这样：

    • 前端里程计 ：帧间配准逐帧累加出"死推位姿"，漂移随之累积------第二期 small_gicp 既能做回环精配准，也能做帧间配准，是同一套算法
    • 回环检测 ：第一期 Scan Context / 第三期 SOLiD 做粗匹配，回答"这地方我来过"，给出回环候选帧
    • 精配准 ：第二期 small_gicp 把当前帧配准到候选帧，输出 6-DOF 相对位姿 z i j z_{ij} zij，连同它的信息矩阵 H H H
    • 后端建模：本期把这些约束写成因子图------先验钉原点、里程计边每帧一条、回环边隔几十帧一条，噪声按配准质量定
    • 增量优化 ：iSAM2 每帧 update() 一次，只碰受影响的局部，实时吐出修正后的轨迹
    • 输出：修正后的全局位姿，交给建图 / 定位 / 导航

  • 用一张流程图把这条流水线串起来------里程计与回环是两条并行的约束来源，最终都汇入后端：

#mermaid-svg-mWT6C5rL5rGG8Icu{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-mWT6C5rL5rGG8Icu .error-icon{fill:#552222;}#mermaid-svg-mWT6C5rL5rGG8Icu .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-mWT6C5rL5rGG8Icu .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-mWT6C5rL5rGG8Icu .marker{fill:#333333;stroke:#333333;}#mermaid-svg-mWT6C5rL5rGG8Icu .marker.cross{stroke:#333333;}#mermaid-svg-mWT6C5rL5rGG8Icu svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-mWT6C5rL5rGG8Icu p{margin:0;}#mermaid-svg-mWT6C5rL5rGG8Icu .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-mWT6C5rL5rGG8Icu .cluster-label text{fill:#333;}#mermaid-svg-mWT6C5rL5rGG8Icu .cluster-label span{color:#333;}#mermaid-svg-mWT6C5rL5rGG8Icu .cluster-label span p{background-color:transparent;}#mermaid-svg-mWT6C5rL5rGG8Icu .label text,#mermaid-svg-mWT6C5rL5rGG8Icu span{fill:#333;color:#333;}#mermaid-svg-mWT6C5rL5rGG8Icu .node rect,#mermaid-svg-mWT6C5rL5rGG8Icu .node circle,#mermaid-svg-mWT6C5rL5rGG8Icu .node ellipse,#mermaid-svg-mWT6C5rL5rGG8Icu .node polygon,#mermaid-svg-mWT6C5rL5rGG8Icu .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-mWT6C5rL5rGG8Icu .rough-node .label text,#mermaid-svg-mWT6C5rL5rGG8Icu .node .label text,#mermaid-svg-mWT6C5rL5rGG8Icu .image-shape .label,#mermaid-svg-mWT6C5rL5rGG8Icu .icon-shape .label{text-anchor:middle;}#mermaid-svg-mWT6C5rL5rGG8Icu .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-mWT6C5rL5rGG8Icu .rough-node .label,#mermaid-svg-mWT6C5rL5rGG8Icu .node .label,#mermaid-svg-mWT6C5rL5rGG8Icu .image-shape .label,#mermaid-svg-mWT6C5rL5rGG8Icu .icon-shape .label{text-align:center;}#mermaid-svg-mWT6C5rL5rGG8Icu .node.clickable{cursor:pointer;}#mermaid-svg-mWT6C5rL5rGG8Icu .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-mWT6C5rL5rGG8Icu .arrowheadPath{fill:#333333;}#mermaid-svg-mWT6C5rL5rGG8Icu .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-mWT6C5rL5rGG8Icu .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-mWT6C5rL5rGG8Icu .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-mWT6C5rL5rGG8Icu .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-mWT6C5rL5rGG8Icu .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-mWT6C5rL5rGG8Icu .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-mWT6C5rL5rGG8Icu .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-mWT6C5rL5rGG8Icu .cluster text{fill:#333;}#mermaid-svg-mWT6C5rL5rGG8Icu .cluster span{color:#333;}#mermaid-svg-mWT6C5rL5rGG8Icu div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-mWT6C5rL5rGG8Icu .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-mWT6C5rL5rGG8Icu rect.text{fill:none;stroke-width:0;}#mermaid-svg-mWT6C5rL5rGG8Icu .icon-shape,#mermaid-svg-mWT6C5rL5rGG8Icu .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-mWT6C5rL5rGG8Icu .icon-shape p,#mermaid-svg-mWT6C5rL5rGG8Icu .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-mWT6C5rL5rGG8Icu .icon-shape .label rect,#mermaid-svg-mWT6C5rL5rGG8Icu .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-mWT6C5rL5rGG8Icu .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-mWT6C5rL5rGG8Icu .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-mWT6C5rL5rGG8Icu :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 死推位姿（漂移累积）
回环候选帧
6-DOF 相对位姿 z_ij + 信息矩阵 H
建图 / 定位 / 导航
激光点云
前端里程计

帧间配准（small_gicp）
回环检测

Scan Context / SOLiD 粗匹配
后端建模

因子图：先验 + 里程计 + 回环
精配准

small_gicp 配准到候选帧
增量优化

iSAM2 每帧 update()
输出

修正后的全局位姿
下游任务

  • 这条链路里，四期各司其职：前三期是"前端"------负责找到约束 （这条边在哪、有多准）；本期是"后端"------负责协调这些互相打架的约束（让整条轨迹全局一致）。前端给得越准，后端越省力；但前端再准也救不了累积漂移，必须靠后端的回环 + 优化把轨迹掰正。

说人话：回环检测负责"认出来我来过"，精配准负责"量出我该怎么对齐"，PGO 负责"把所有对齐结果揉成一条不打架的轨迹"。三者串起来，才是完整的 SLAM 后端闭环。

4-2 iSAM2 都用在了哪些算法

  • iSAM2 不是论文里的摆设，它是一整类 SLAM 后端的工业标准。几个真正把 GTSAM ISAM2 用进系统的算法（打开源码，后端都能看到 NonlinearFactorGraph + ISAM2）：

    • LIO-SAM （Shan, IROS 2020）：紧耦合激光-惯性里程计，因子图里同时塞 IMU 预积分、激光里程计、GPS、回环、地面约束，后端用 GTSAM iSAM2------是 iSAM2 最典型的工业级应用
    • LeGO-LOAM （Shan, IROS 2018）：轻量级激光 SLAM，把地面点分割与六自由度位姿估计分开，后端用 GTSAM iSAM2 做因子图优化
    • LVI-SAM （Shan, ICRA 2021）：LIO-SAM 的激光-视觉-惯性升级版，同样用 GTSAM iSAM2 融合视觉 + 激光 + IMU
    • Kimera （MIT-SPARK）：语义 SLAM，VIO 前端用 GTSAM iSAM2 做因子图优化，再叠加稠密建图与语义

  • 但 iSAM2 不是唯一路线。状态估计还有另一派------滤波 （逐帧递推、不维护全局因子图）与平滑（维护全局因子图、全局一致）之争：

    • 平滑（iSAM2）：精度高、全局一致、回环能力强，代价是维护整张图、要管理重线性化------代表就是上面的 LIO-SAM 一族
    • 滤波（EKF / ESEKF） ：逐帧递推、实时性极强、实现简单，代价是线性化误差会累积、回环能力弱------代表是 Fast-LIO / Point-LIO（误差状态卡尔曼滤波）

  • 怎么选：需要长时间、大范围、频繁回环的建图 ，选 iSAM2 平滑；需要极低延迟、纯里程计、不强调全局一致，选滤波。现在主流是"前端滤波跑得快 + 后端平滑保一致"的混合结构------前端 Fast-LIO 出里程计，后端 iSAM2 做因子图优化。

说人话：iSAM2 是"建图派"的标配------LIO-SAM 一大家子都靠它；Fast-LIO / Point-LIO 是"滤波派"，实时快但缺全局一致。工程上常见的是两者配合：滤波管前端实时性，平滑管后端全局一致性。

5 附录：配套 Python 源码

5-1 三大核心数据结构可视化 Python 源码
  • 第 2-4 节那张「三大核心数据结构」图由下面的脚本生成，完整可运行（依赖 pip install matplotlib）：
python 复制代码 
# -*- coding: utf-8 -*-
"""
GTSAM 三大核心数据结构可视化脚本
对照 CSDN 文章《【3D SLAM源码解读系列】(四) GTSAM与iSAM2------从回环约束到位姿图优化》
第 2-4 节「三大核心数据结构」配套插图

用 matplotlib 画一张图，直观展示三者的关系：
  1. NonlinearFactorGraph ------ 因子图容器（顶部大框，内部是一串因子）
  2. Factor ------ 单个约束/边（顶部大框里的每个小方块）
  3. Values ------ 键 → 值 映射（底部大框，存每个位姿的实际取值）

并画出因子如何通过整数 Key 引用 Values 里的变量。

依赖：pip install matplotlib
"""
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import FancyBboxPatch, FancyArrowPatch

# 使用系统可用的 CJK 字体，避免中文显示为方块
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Droid Sans Fallback',
                                   'AR PL UMing CN', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

# 颜色
GRAPH_C = '#dbeafe'   # 因子图容器 浅蓝
FACTOR_C = '#fef3c7'  # 因子方块 浅橙
VALUES_C = '#d1fae5'  # Values 浅绿
VALUE_C = '#ffffff'   # 变量值方块 白
LOOP_C = '#059669'    # 回环边 绿
ARROW_C = '#94a3b8'   # 普通箭头 灰

fig, ax = plt.subplots(figsize=(11, 7.2))
ax.set_xlim(0, 10)
ax.set_ylim(0, 9)
ax.axis('off')
ax.set_aspect('equal')


def rbox(x, y, w, h, fc, ec, lw=1.6, r=0.12):
    p = FancyBboxPatch((x, y), w, h,
                       boxstyle=f'round,pad=0.02,rounding_size={r}',
                       fc=fc, ec=ec, lw=lw, zorder=1)
    ax.add_patch(p)


# ============================================================
# 顶部：NonlinearFactorGraph（因子图容器）
# ============================================================
rbox(0.5, 5.3, 9.0, 3.2, GRAPH_C, '#3b82f6', lw=2.0)
ax.text(0.85, 8.05, 'NonlinearFactorGraph（因子图容器，每个小方块是一条 Factor）',
        fontsize=12.5, fontweight='bold', color='#1e3a8a', va='center')

# 4 个因子方块（每个就是一条 Factor / 约束）
factors = [
    (1.7, 'PriorFactorPose2',    'key1'),
    (3.9, 'BetweenFactorPose2',  'key1, key2'),
    (6.1, 'BetweenFactorPose2',  'key2, key3'),
    (8.3, 'BetweenFactorPose2',  'key3, key1 回环'),
]
for cx, name, keys in factors:
    rbox(cx - 0.95, 6.0, 1.9, 1.4, FACTOR_C, '#f59e0b', lw=1.8)
    ax.text(cx, 7.0, name, fontsize=9.5, fontweight='bold',
            color='#7c2d12', ha='center', va='center')
    ax.text(cx, 6.35, f'({keys})', fontsize=8.5, color='#92400e',
            ha='center', va='center')

# ============================================================
# 底部：Values（键 → 值 映射）
# ============================================================
rbox(0.5, 1.0, 9.0, 3.0, VALUES_C, '#10b981', lw=2.0)
ax.text(0.85, 3.55, 'Values（键 → 值，存每个位姿的实际取值）',
        fontsize=12.5, fontweight='bold', color='#065f46', va='center')

values = [
    (2.5, 'key1', 'Pose2(0, 0, 0°)'),
    (5.0, 'key2', 'Pose2(2, 0, 0°)'),
    (7.5, 'key3', 'Pose2(2, 2, 90°)'),
]
for cx, key, pose in values:
    rbox(cx - 1.15, 1.7, 2.3, 1.2, VALUE_C, '#059669', lw=1.6)
    ax.text(cx, 2.55, key, fontsize=10.5, fontweight='bold',
            color='#047857', ha='center', va='center')
    ax.text(cx, 2.02, pose, fontsize=9, color='#065f46', ha='center', va='center')


# ============================================================
# 箭头：因子通过整数 Key 引用 Values 里的变量
# ============================================================
def arrow(x1, y1, x2, y2, color=ARROW_C, rad=0.0, lw=1.2, label=None):
    a = FancyArrowPatch((x1, y1), (x2, y2),
                        connectionstyle=f'arc3,rad={rad}',
                        arrowstyle='-|>', mutation_scale=12,
                        lw=lw, color=color, zorder=3)
    ax.add_patch(a)
    if label is not None:
        ax.text((x1 + x2) / 2, (y1 + y2) / 2 + 0.15, label,
                fontsize=8.5, color=color, ha='center', va='bottom')


# 因子方块底部 y = 6.0，变量方块顶部 y = 2.9
arrow(1.7, 6.0, 2.5, 2.9)                        # f1 -> key1
arrow(3.6, 6.0, 2.3, 2.9)                        # f2 -> key1
arrow(4.2, 6.0, 5.0, 2.9)                        # f2 -> key2
arrow(5.8, 6.0, 5.4, 2.9)                        # f3 -> key2
arrow(6.4, 6.0, 7.5, 2.9)                        # f3 -> key3
arrow(8.1, 6.0, 7.7, 2.9, color=LOOP_C, lw=1.5)  # f4 -> key3
arrow(8.55, 6.0, 2.75, 2.9, color=LOOP_C, rad=-0.32, lw=1.8,
      label='回环约束 key3 → key1')               # f4 -> key1 回环

# 底部说明
ax.text(5.0, 0.45, '因子用整数 Key 指向变量；Values 按 Key 存取值，供因子按需取用',
        fontsize=10.5, color='#334155', ha='center', va='center',
        style='italic')

plt.tight_layout()
plt.savefig('gtsam_structures.png', dpi=150, bbox_inches='tight',
            facecolor='white')
print('已保存 gtsam_structures.png')
5-2 PGO 完整流程可视化 Python 源码
  • 第 2-8 节那张「完整流程：从回环约束到输出」图由下面的脚本生成，完整可运行（依赖 pip install matplotlib）：
python 复制代码 
# -*- coding: utf-8 -*-
"""
GTSAM 位姿图优化（PGO）完整流程可视化脚本
对照 CSDN 文章《【3D SLAM源码解读系列】(四) GTSAM与iSAM2------从回环约束到位姿图优化》
第 2-8 节「完整流程：从回环约束到输出」配套插图

用 matplotlib 画一张纵向流程图，展示从"前三期的回环检测"到"PGO 输出"的完整流水线：
  1. 前端回环检测 ------ Scan Context/SOLiD 粗匹配 + small_gicp 精配准，产出 6-DOF 回环约束 z_ij
  2. 构造输入 ------ 把 z_ij 变成 BetweenFactor，连同噪声模型、初值一起塞进因子图
  3. 优化 ------ Levenberg-Marquardt / Gauss-Newton 迭代求解
  4. 取回输出 ------ 从 result 里取位姿 / 看误差 / 取协方差

依赖：pip install matplotlib
"""
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import FancyBboxPatch, FancyArrowPatch

# 使用系统可用的 CJK 字体，避免中文显示为方块
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Droid Sans Fallback',
                                   'AR PL UMing CN', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

# 颜色
FRONT_C = '#fef3c7'   # 前端回环检测 浅橙黄
INPUT_C = '#dbeafe'   # 构造输入 浅蓝
OPT_C = '#fde68a'     # 优化 浅黄
OUT_C = '#d1fae5'     # 取回输出 浅绿
SUB_C = '#ffffff'     # 子框 白
ARROW_C = '#64748b'   # 箭头 灰

fig, ax = plt.subplots(figsize=(11, 12))
ax.set_xlim(0, 10)
ax.set_ylim(0, 12.4)
ax.axis('off')
ax.set_aspect('equal')


def rbox(x, y, w, h, fc, ec, lw=1.6, r=0.10):
    p = FancyBboxPatch((x, y), w, h,
                       boxstyle=f'round,pad=0.02,rounding_size={r}',
                       fc=fc, ec=ec, lw=lw, zorder=1)
    ax.add_patch(p)


def subbox(x, y, w, h, text, ec, fc=SUB_C, title_size=10.5, body_size=8.5,
           title_color='#334155', body_color='#475569'):
    rbox(x, y, w, h, fc, ec, lw=1.5, r=0.08)
    lines = text.split('\n')
    if len(lines) == 1:
        ax.text(x + w / 2, y + h / 2, lines[0], fontsize=title_size,
                fontweight='bold', color=title_color, ha='center', va='center')
    else:
        title_h = h * 0.42
        ax.text(x + w / 2, y + h - title_h, lines[0], fontsize=title_size,
                fontweight='bold', color=title_color, ha='center', va='center')
        body = '\n'.join(lines[1:])
        ax.text(x + w / 2, y + (h - title_h) / 2, body, fontsize=body_size,
                color=body_color, ha='center', va='center', linespacing=1.5)


def arrow(x1, y1, x2, y2, color=ARROW_C, rad=0.0, lw=1.6, label=None,
          label_dx=0.0, label_dy=0.0):
    a = FancyArrowPatch((x1, y1), (x2, y2),
                        connectionstyle=f'arc3,rad={rad}',
                        arrowstyle='-|>', mutation_scale=14,
                        lw=lw, color=color, zorder=3)
    ax.add_patch(a)
    if label is not None:
        ax.text((x1 + x2) / 2 + label_dx, (y1 + y2) / 2 + label_dy, label,
                fontsize=8.5, color=color, ha='center', va='center')


# ============================================================
# 第 1 步：前端回环检测（前三期产出 6-DOF 回环约束）
# ============================================================
rbox(0.4, 9.5, 9.2, 2.6, FRONT_C, '#d97706', lw=2.0)
ax.text(0.7, 11.7, '第 1 步　回环检测（前三期）',
        fontsize=12.5, fontweight='bold', color='#92400e', va='center')

subbox(0.8, 9.9, 2.6, 1.5, '粗匹配\nScan Context', ec='#d97706')
subbox(3.8, 9.9, 2.6, 1.5, '精配准\nsmall_gicp', ec='#d97706')
subbox(6.8, 9.9, 2.6, 1.5, '回环约束\nz_ij ∈ SE(3)', ec='#d97706',
       title_color='#92400e')
arrow(3.4, 10.65, 3.8, 10.65)
arrow(6.4, 10.65, 6.8, 10.65)

# ============================================================
# 第 2 步：构造输入（把 z_ij 变成 BetweenFactor 塞进因子图）
# ============================================================
rbox(0.4, 6.2, 9.2, 2.8, INPUT_C, '#3b82f6', lw=2.0)
ax.text(0.7, 8.55, '第 2 步　构造输入（三样东西）',
        fontsize=12.5, fontweight='bold', color='#1e3a8a', va='center')

subbox(0.8, 6.6, 2.6, 1.5, '噪声模型\nnoiseModel', ec='#3b82f6')
subbox(3.8, 6.6, 2.6, 1.5, '因子图\n塞入约束', ec='#3b82f6')
subbox(6.8, 6.6, 2.6, 1.5, '初值\nValues', ec='#3b82f6')

arrow(5.0, 9.5, 5.0, 9.0, label='z_ij 作为一条回环边')

# ============================================================
# 第 3 步：优化（LM / GN 迭代求解）
# ============================================================
rbox(0.4, 3.3, 9.2, 2.4, OPT_C, '#d97706', lw=2.0)
ax.text(0.7, 5.3, '第 3 步　优化（LM / GN 迭代）',
        fontsize=12.5, fontweight='bold', color='#92400e', va='center')

# 迭代链：线性化 → 白化 → 正规方程 → 更新
steps = [
    (1.7, '线性化\n泰勒展开'),
    (3.9, '白化\ny = Re'),
    (6.1, '正规方程\nHδ = -b'),
    (8.3, '更新\nx ⊕ δ'),
]
for cx, txt in steps:
    subbox(cx - 0.95, 3.7, 1.9, 1.1, txt, ec='#b45309', title_size=9,
           body_size=8.5)
for i in range(3):
    x1 = steps[i][0] + 0.95
    x2 = steps[i + 1][0] - 0.95
    arrow(x1, 4.25, x2, 4.25, lw=1.4)
# 迭代循环说明（不画大弧线，避免溢出框外）
ax.text(5.0, 3.5, '（四步循环迭代，直到误差收敛）',
        fontsize=9, color='#92400e', ha='center', va='center', style='italic')

arrow(5.0, 6.2, 5.0, 5.7)

# ============================================================
# 第 4 步：取回输出（位姿 / 误差 / 协方差）
# ============================================================
rbox(0.4, 0.5, 9.2, 2.4, OUT_C, '#10b981', lw=2.0)
ax.text(0.7, 2.45, '第 4 步　取回输出',
        fontsize=12.5, fontweight='bold', color='#065f46', va='center')

subbox(0.8, 0.9, 2.6, 1.2, '取位姿\nat<Pose2>(i)', ec='#10b981')
subbox(3.8, 0.9, 2.6, 1.2, '看误差\nerror(result)', ec='#10b981')
subbox(6.8, 0.9, 2.6, 1.2, '取协方差\nMarginals', ec='#10b981')

arrow(5.0, 3.3, 5.0, 2.9)

plt.tight_layout()
plt.savefig('gtsam_pgo_flow.png', dpi=150, bbox_inches='tight',
            facecolor='white')
print('已保存 gtsam_pgo_flow.png')
5-3 回环残差可视化 Python 源码
  • 第 2-3 节那张「回环残差对整张图的影响」对比图由下面的脚本生成，完整可运行（依赖 pip install matplotlib）：
python 复制代码 
# -*- coding: utf-8 -*-
"""
GTSAM 回环残差对整张位姿图的影响 可视化脚本
对照 CSDN 文章《【3D SLAM源码解读系列】(四) GTSAM与iSAM2------从回环约束到位姿图优化》
第 2-3 节「直观理解PGO」

用一张"优化前 vs 优化后"的对比图，讲清回环处的残差是怎么影响整条轨迹的：
  - 左图（优化前）：机器人绕一圈回到起点附近，里程计漂移让每个位姿都偏离理想位置一点，
    终点没落回起点，中间留了个 gap；回环是一条更强的约束，说"终点就该是起点"，
    这个 gap 就是回环这条约束的残差 e
  - 右图（优化后）：PGO 把这个 gap 分摊到整条轨迹，每个位姿都挪一点，轨迹平滑闭环

依赖：pip install matplotlib
"""
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt

# 使用系统可用的 CJK 字体，避免中文显示为方块
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Droid Sans Fallback',
                                   'AR PL UMing CN', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

START = (0.0, 0.0)          # 起点 x1

# 理想闭环（优化后）：正方形一圈，最后回到起点
IDEAL = [
    (1.0, 0.0), (2.0, 0.0), (2.0, 1.0), (2.0, 2.0),
    (1.0, 2.0), (0.0, 2.0), (0.0, 1.0),
]
# 漂移链（优化前）：每个位姿都偏离理想一点，漂移一路累积
DRIFT = [
    (1.05, 0.06), (2.10, 0.14), (2.18, 1.10), (2.22, 2.08),
    (1.10, 2.16), (0.07, 2.17), (0.02, 1.07),
]
DRIFT_END = (0.62, 0.44)    # 漂移终点（没落回起点）

fig, axes = plt.subplots(1, 2, figsize=(11.5, 5.2))

for ax in axes:
    ax.set_xlim(-0.85, 3.05)
    ax.set_ylim(-0.85, 3.05)
    ax.set_aspect('equal')
    ax.axis('off')


def draw_chain(ax, pts, color, lw=2.2, ms=6):
    """画一条位姿链：折线 + 圆点"""
    xs = [p[0] for p in pts]
    ys = [p[1] for p in pts]
    ax.plot(xs, ys, '-', color=color, lw=lw, zorder=2)
    ax.plot(xs, ys, 'o', color=color, ms=ms, zorder=4)


# ============ 左图：优化前（每个点都漂一点，回环处有 gap） ============
ax = axes[0]

# 理想闭环（灰色虚线，作为参照：每个位姿"本该"在这）
ideal_pts = [START] + IDEAL + [START]
ax.plot([p[0] for p in ideal_pts], [p[1] for p in ideal_pts], '--',
        color='#cbd5e1', lw=1.6, zorder=1)
ax.text(2.65, 2.55, '理想轨迹（本该在这）', fontsize=8.5, color='#94a3b8',
        ha='right', va='bottom')

# 漂移链（蓝色实线：里程计一路漂移）
draw_chain(ax, [START] + DRIFT + [DRIFT_END], '#2563eb')

# 起点 vs 漂移终点
ax.plot([START[0]], [START[1]], 'o', color='#1e40af', ms=10, zorder=5)
ax.plot([DRIFT_END[0]], [DRIFT_END[1]], 'o', color='#f97316', ms=10, zorder=5)

ax.annotate('起点 $x_1$', xy=START, xytext=(-0.80, -0.55),
            fontsize=10.5, color='#1e40af', fontweight='bold',
            arrowprops=dict(arrowstyle='->', color='#1e40af', lw=1.1))
ax.annotate('里程计终点（漂了）', xy=DRIFT_END, xytext=(1.85, 1.30),
            fontsize=10.5, color='#ea580c', fontweight='bold',
            arrowprops=dict(arrowstyle='->', color='#ea580c', lw=1.1))

# 回环强约束：绿色粗虚线箭头，终点应回到起点（弧线，避免和残差重叠）
ax.annotate('', xy=START, xytext=DRIFT_END,
            arrowprops=dict(arrowstyle='-|>', color='#059669', lw=2.6,
                            linestyle=(0, (5, 3)),
                            connectionstyle='arc3,rad=-0.42'))
ax.text(1.02, 1.00, '回环强约束：\n终点 = 起点', fontsize=9.5,
        color='#047857', ha='center', va='bottom', fontweight='bold')

# 残差 gap：红色双箭头（直接连起点与漂移终点）
ax.annotate('', xy=DRIFT_END, xytext=START,
            arrowprops=dict(arrowstyle='<->', color='#dc2626', lw=3.4))
ax.text(0.20, -0.34, '回环残差 $e$', fontsize=10.5,
        color='#dc2626', ha='center', va='top', fontweight='bold')

ax.set_title('优化前：每个点都漂一点，回环处留 gap', fontsize=12.5,
             color='#b91c1c', fontweight='bold', pad=8)

# ============ 右图：优化后（gap 被分摊，闭环） ============
ax = axes[1]
draw_chain(ax, [START] + IDEAL + [START], '#2563eb')
ax.annotate('起点 = 终点（闭环）', xy=START, xytext=(0.90, 0.62),
            fontsize=10.5, color='#047857', fontweight='bold',
            arrowprops=dict(arrowstyle='->', color='#047857', lw=1.1))
ax.set_title('优化后：gap 分摊到整条轨迹，闭环', fontsize=12.5,
             color='#047857', fontweight='bold', pad=8)

plt.tight_layout()
plt.savefig('gtsam_loop_residual.png', dpi=150, bbox_inches='tight',
            facecolor='white')
print('已保存 gtsam_loop_residual.png')
5-4 旋转向量（轴角）可视化 Python 源码
  • 第 2-5-3 节那张「旋转向量 / 轴角」图由下面的脚本生成，完整可运行（依赖 pip install matplotlib）：
python 复制代码 
# -*- coding: utf-8 -*-
"""
GTSAM 旋转向量（轴角）可视化脚本
对照 CSDN 文章《【3D SLAM源码解读系列】(四) GTSAM与iSAM2------从回环约束到位姿图优化》
第 2-5-3 节「残差向量计算」配套插图

用一张图讲清 SO(3) 对数映射的核心：旋转矩阵 R 可以还原成一个旋转向量
  ω = θ·n ------ 方向是转轴 n（单位向量），长度是转角 θ。
图中三样东西：
  - 转轴 n：一条过原点的细虚线（真正的"轴"，双向延伸）
  - 转角 θ：向量 v 绕 n 转到 v' 的弧
  - 旋转向量 ω = θn：一根粗实线箭头，方向 = n，长度 = θ

说明：本脚本不依赖 mpl_toolkits.mplot3d（部分环境里它与 matplotlib 版本冲突），
而是自己把 3D 点正交投影到 2D 再画，效果一样、更可控。

依赖：pip install matplotlib
"""
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.patches import FancyArrowPatch

# 使用系统可用的 CJK 字体，避免中文显示为方块
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Droid Sans Fallback',
                                   'AR PL UMing CN', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

# ---- 3D 几何量 ----
n = np.array([0.40, 0.30, 0.87])
n = n / np.linalg.norm(n)            # 转轴（单位向量）
theta = np.deg2rad(70.0)             # 转角 70°

# 平面 ⊥ n 的一组正交基
z_axis = np.array([0.0, 0.0, 1.0])
e1 = np.cross(n, z_axis)
if np.linalg.norm(e1) < 1e-6:
    e1 = np.cross(n, np.array([1.0, 0.0, 0.0]))
e1 = e1 / np.linalg.norm(e1)
e2 = np.cross(n, e1)

v = e1                                             # 旋转前的向量
vp = np.cos(theta) * e1 + np.sin(theta) * e2       # 旋转后的向量 v' = Rv
t = np.linspace(0.0, theta, 60)
arc = np.array([np.cos(x) * e1 + np.sin(x) * e2 for x in t])
omega = theta * n                                  # 旋转向量

# ---- 正交投影（自己算视角，不依赖 mplot3d） ----
azim = np.deg2rad(-60.0)
elev = np.deg2rad(30.0)
Rx = np.array([[1, 0, 0],
               [0, np.cos(elev), -np.sin(elev)],
               [0, np.sin(elev), np.cos(elev)]])
Rz = np.array([[np.cos(azim), -np.sin(azim), 0],
               [np.sin(azim), np.cos(azim), 0],
               [0, 0, 1]])
Rview = Rx @ Rz


def proj(p):
    q = Rview @ np.asarray(p, dtype=float)
    return q[0], q[1]                # 正交投影：丢掉深度


# ---- 绘图 ----
fig, ax = plt.subplots(figsize=(8.4, 7.6))
ax.set_aspect('equal')
ax.axis('off')


def line3d(ax, p0, p1, color, lw=1.4, ls='-', zorder=2):
    (x0, y0), (x1, y1) = proj(p0), proj(p1)
    ax.plot([x0, x1], [y0, y1], color=color, lw=lw, ls=ls, zorder=zorder)


def arrow3d(ax, p0, p1, color, lw=1.8, ms=16, zorder=3):
    (x0, y0), (x1, y1) = proj(p0), proj(p1)
    a = FancyArrowPatch((x0, y0), (x1, y1), arrowstyle='-|>',
                        mutation_scale=ms, color=color, lw=lw, zorder=zorder)
    ax.add_patch(a)


def label3d(ax, p, s, color, fs=11, dx=0.0, dy=0.0, ha='center', va='center'):
    x, y = proj(p)
    ax.text(x + dx, y + dy, s, color=color, fontsize=fs, ha=ha, va=va,
            fontweight='bold')


O = np.zeros(3)

# 转轴 n：细虚线双向延伸（真正的"轴"）
line3d(ax, -0.9 * n, 2.15 * n, '#94a3b8', lw=1.4, ls=(0, (5, 3)))
label3d(ax, 2.15 * n, '转轴 n', '#475569', fs=11.5)

# 向量 v → v' 与旋转弧
arrow3d(ax, O, v, '#2563eb', lw=2.4, ms=18)
arrow3d(ax, O, vp, '#059669', lw=2.4, ms=18)
ax.plot([proj(p)[0] for p in arc], [proj(p)[1] for p in arc],
        color='#f59e0b', lw=2.6, zorder=2)
label3d(ax, v * 1.22, '$v$', '#2563eb', fs=12)
label3d(ax, vp * 1.22, "$v' = Rv$", '#059669', fs=12)

# 转角 θ 标注（弧中点外侧）
mid = (v + vp)
mid = mid / np.linalg.norm(mid)
label3d(ax, mid * 1.32, '转角 $\\theta$', '#d97706', fs=12)

# 旋转向量 ω = θn（粗实线箭头）
arrow3d(ax, O, omega, '#dc2626', lw=3.4, ms=22)
# ω 标签：沿 ω 的屏幕方向再外推一点，让"ω = θn"落在箭头尖端附近
omtip = np.array(proj(omega))
omdir = omtip / np.linalg.norm(omtip)
perp = np.array([-omdir[1], omdir[0]])
ax.text(omtip[0] + omdir[0] * 0.28 + perp[0] * 0.10,
        omtip[1] + omdir[1] * 0.28 + perp[1] * 0.10,
        '$\\omega = \\theta\\,n$', color='#dc2626', fontsize=13.5,
        ha='center', va='center', fontweight='bold')

fig.text(0.5, 0.02,
         'SO3::Logmap 把旋转矩阵 R 还原成旋转向量 $\\omega = \\theta\\,n$：'
         '方向是转轴 $n$，长度是转角 $\\theta$',
         ha='center', va='bottom', fontsize=12, color='#334155')

ax.set_xlim(-1.9, 1.9)
ax.set_ylim(-1.6, 1.7)

plt.tight_layout()
plt.savefig('gtsam_axis_angle.png', dpi=150, bbox_inches='tight',
            facecolor='white')
print('已保存 gtsam_axis_angle.png')
5-5 贝叶斯树与增量更新可视化 Python 源码
  • 第 3-3 节那张「贝叶斯树 / 增量更新」左右对比图由下面的脚本生成，完整可运行（依赖 pip install matplotlib）：
python 复制代码 
# -*- coding: utf-8 -*-
"""
GTSAM 贝叶斯树与增量更新 可视化脚本
对照 CSDN 文章《【3D SLAM源码解读系列】(四) GTSAM与iSAM2------从回环约束到位姿图优化》
第 3-3 节「贝叶斯树与增量更新」配套插图

一张左右对比图，讲清 iSAM2 增量更新的两个核心直觉：
  - 左图：因子图消元后，团(clique)按树状组织成贝叶斯树------根在上、叶在下
  - 右图：新因子加进来，只重消元"它影响到的团 → 一路向上到根"这一条路径，
          其余子树原封不动、复用上次结果

依赖：pip install matplotlib
"""
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.patches import FancyBboxPatch

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Droid Sans Fallback',
                                   'AR PL UMing CN', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

# 树节点：团名 -> (装的变量, x, y)
NODES = {
    'C1': ('{x1, x2, x3}', 5.0, 8.2),
    'C2': ('{x4}',          2.6, 5.6),
    'C3': ('{x5, x6}',      7.4, 5.6),
    'C4': ('{x7}',          1.2, 3.0),
    'C5': ('{x8}',          4.0, 3.0),
    'C6': ('{x9}',          6.2, 3.0),
    'C7': ('{x10}',         8.8, 3.0),
}
# 树边（父 -> 子）
EDGES = [('C1', 'C2'), ('C1', 'C3'), ('C2', 'C4'), ('C2', 'C5'),
         ('C3', 'C6'), ('C3', 'C7')]
# 右图高亮路径：C5 -> C2 -> C1（向上到根）
PATH_EDGES = {('C2', 'C5'), ('C1', 'C2')}
PATH_NODES = {'C5', 'C2', 'C1'}

fig, axes = plt.subplots(1, 2, figsize=(13.4, 6.0))


def draw_node(ax, name, fc, ec, lw=2.0, dashed=False, textc='#1e293b',
              subc='#475569'):
    vars_, x, y = NODES[name]
    w, h = 2.6, 1.4
    p = FancyBboxPatch((x - w / 2, y - h / 2), w, h,
                       boxstyle='round,pad=0.02,rounding_size=0.12',
                       fc=fc, ec=ec, lw=lw, zorder=2)
    if dashed:
        p.set_linestyle((0, (4, 3)))
    ax.add_patch(p)
    ax.text(x, y + 0.24, name, fontsize=12.5, fontweight='bold',
            color=textc, ha='center', va='center', zorder=3)
    ax.text(x, y - 0.28, vars_, fontsize=9.5, color=subc,
            ha='center', va='center', zorder=3)


def draw_edge(ax, a, b, color, lw=1.8, zorder=1):
    _, xa, ya = NODES[a]
    _, xb, yb = NODES[b]
    ax.plot([xa, xb], [ya, yb], color=color, lw=lw, zorder=zorder)


# ---------- 左图：贝叶斯树结构 ----------
ax = axes[0]
ax.set_xlim(0, 10)
ax.set_ylim(1.4, 9.6)
ax.set_aspect('equal')
ax.axis('off')
for a, b in EDGES:
    draw_edge(ax, a, b, '#94a3b8', lw=1.8)
for name in NODES:
    fc = '#dbeafe' if name == 'C1' else '#eff6ff'
    draw_node(ax, name, fc, '#3b82f6')
ax.text(5.0, 9.15, '根', fontsize=10.5, color='#1d4ed8',
        ha='center', va='bottom', fontweight='bold')
ax.text(1.2, 2.05, '叶', fontsize=10.5, color='#64748b',
        ha='center', va='top', fontweight='bold')
ax.text(8.8, 2.05, '叶', fontsize=10.5, color='#64748b',
        ha='center', va='top', fontweight='bold')
ax.set_title('消元后：团按树组织成贝叶斯树', fontsize=13.5, color='#1e3a8a',
             fontweight='bold', pad=12)

# ---------- 右图：增量更新只碰一条路径 ----------
ax = axes[1]
ax.set_xlim(0, 10)
ax.set_ylim(0.9, 9.6)
ax.set_aspect('equal')
ax.axis('off')
for a, b in EDGES:
    if (a, b) in PATH_EDGES:
        draw_edge(ax, a, b, '#ef4444', lw=3.6)
    else:
        draw_edge(ax, a, b, '#cbd5e1', lw=1.5)
for name in NODES:
    if name in PATH_NODES:
        draw_node(ax, name, '#fee2e2', '#ef4444', lw=2.4,
                  textc='#7f1d1d', subc='#b91c1c')
    else:
        draw_node(ax, name, '#e2e8f0', '#94a3b8', lw=1.5, dashed=True,
                  textc='#64748b', subc='#94a3b8')
# 新因子标注（指向 C5）
ax.annotate('新因子 f_new\n涉及 x8', xy=(4.0, 3.6), xytext=(5.6, 1.4),
            fontsize=9.5, color='#b91c1c', fontweight='bold', ha='center',
            arrowprops=dict(arrowstyle='->', color='#ef4444', lw=1.5))
# 高亮路径标注（放在 C2-C1 段左侧，避开 C3）
ax.annotate('这条路径\n要重新消元', xy=(3.5, 6.7), xytext=(1.5, 6.9),
            fontsize=9.5, color='#b91c1c', fontweight='bold', ha='center',
            arrowprops=dict(arrowstyle='->', color='#ef4444', lw=1.5))
# 灰色复用标注
ax.text(8.8, 1.4, '其余子树：复用\n上次结果，不动', fontsize=9.5, color='#64748b',
        ha='center', va='center')
ax.set_title('新因子进来：只重算向上到根的一条路径', fontsize=13.5,
             color='#b91c1c', fontweight='bold', pad=12)

fig.text(0.5, 0.01,
         'iSAM2 的核心：把"全图重新分解"变成"树上一小块局部重新分解"，'
         '新因子只重消元它影响到的团到根的那条路径',
         ha='center', va='bottom', fontsize=11.5, color='#334155')

plt.tight_layout(rect=[0, 0.04, 1, 1])
plt.savefig('gtsam_bayes_tree.png', dpi=150, bbox_inches='tight',
            facecolor='white')
print('已保存 gtsam_bayes_tree.png')
5-6 iSAM2 实时增量更新连续帧可视化 Python 源码
  • 第 3-6 节那张「实时增量更新」6 帧连续快照图由下面的脚本生成，完整可运行（依赖 pip install matplotlib）：
python 复制代码 
# -*- coding: utf-8 -*-
"""
GTSAM iSAM2 实时增量更新 连续帧可视化脚本
对照 CSDN 文章《【3D SLAM源码解读系列】(四) GTSAM与iSAM2------从回环约束到位姿图优化》
第 3-6 节「完整增量 iSAM2 示例」配套插图

用 6 张连续快照展示 iSAM2 实时更新的过程（左 → 右为时间推进）：
  帧 1-4：里程计一帧帧进来，路径逐步延伸
  帧 5  ：5 个位姿到齐，死推轨迹已经漂移、没闭环（灰虚线是理想位置）
  帧 6  ：回环触发，iSAM2 把位姿 5 拉回位姿 2，轨迹闭环修正

依赖：pip install matplotlib
"""
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt

plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'Droid Sans Fallback',
                                   'AR PL UMing CN', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

# 理想轨迹（优化后应收敛到）与漂移轨迹（死推，逐帧漂移累积）
IDEAL = [(0, 0), (2, 0), (4, 0), (4, 2), (2, 2)]
DRIFT = [(0, 0), (2.1, 0.15), (4.2, 0.28), (4.28, 2.2), (2.32, 2.55)]
LOOP_FROM = (2, 2)   # 回环边起点（位姿 5，修正后）
LOOP_TO = (2, 0)     # 回环边终点（位姿 2）

fig, axes = plt.subplots(1, 6, figsize=(19.5, 3.6))


def setup(ax):
    ax.set_xlim(-0.7, 4.9)
    ax.set_ylim(-0.75, 3.3)
    ax.set_aspect('equal')
    ax.axis('off')


def chain(ax, pts, color, lw=2.0, ls='-', ms=6.5):
    xs = [p[0] for p in pts]
    ys = [p[1] for p in pts]
    ax.plot(xs, ys, ls=ls, color=color, lw=lw, zorder=2)
    ax.plot(xs, ys, 'o', color=color, ms=ms, zorder=3)


def label(ax, p, s, color, dx=0.12, dy=0.12, fs=8.5):
    ax.text(p[0] + dx, p[1] + dy, s, color=color, fontsize=fs,
            fontweight='bold', ha='left', va='bottom', zorder=4)


# ---- 帧 1：先验钉住原点 ----
ax = axes[0]; setup(ax)
chain(ax, DRIFT[:1], '#2563eb')
label(ax, DRIFT[0], '1', '#1e40af')
ax.text(DRIFT[0][0], DRIFT[0][1] - 0.28, '先验', fontsize=8, color='#64748b',
        ha='center', va='top')
ax.set_title('update 1\n（先验钉住原点）', fontsize=10.5, color='#1e3a8a', pad=8)

# ---- 帧 2：+位姿 2 ----
ax = axes[1]; setup(ax)
chain(ax, DRIFT[:2], '#2563eb')
label(ax, DRIFT[1], '2', '#1e40af')
ax.set_title('update 2', fontsize=10.5, color='#1e3a8a', pad=8)

# ---- 帧 3：+位姿 3 ----
ax = axes[2]; setup(ax)
chain(ax, DRIFT[:3], '#2563eb')
label(ax, DRIFT[2], '3', '#1e40af')
ax.set_title('update 3', fontsize=10.5, color='#1e3a8a', pad=8)

# ---- 帧 4：+位姿 4 ----
ax = axes[3]; setup(ax)
chain(ax, DRIFT[:4], '#2563eb')
label(ax, DRIFT[3], '4', '#1e40af')
ax.set_title('update 4', fontsize=10.5, color='#1e3a8a', pad=8)

# ---- 帧 5：5 个位姿到齐，漂移、没闭环 ----
ax = axes[4]; setup(ax)
chain(ax, IDEAL, '#cbd5e1', ls=(0, (5, 3)), lw=1.6)   # 理想参照（灰虚线）
chain(ax, DRIFT, '#2563eb')                            # 死推轨迹
label(ax, DRIFT[4], '5 漂了', '#ea580c', 0.08, 0.06)
ax.set_title('update 5\n回环前：漂移、没闭环', fontsize=10.5, color='#b91c1c',
             pad=8)

# ---- 帧 6：回环触发，轨迹被拉正 ----
ax = axes[5]; setup(ax)
chain(ax, IDEAL, '#2563eb')                            # 修正后轨迹
# 修正前的漂移位姿 5（橙叉）+ 被拉回的箭头
ax.plot([DRIFT[4][0]], [DRIFT[4][1]], 'x', color='#ea580c', ms=10, mew=2.5,
        zorder=5)
ax.annotate('', xy=IDEAL[4], xytext=DRIFT[4],
            arrowprops=dict(arrowstyle='-|>', color='#f97316', lw=1.8,
                            linestyle=(0, (4, 2))))
label(ax, DRIFT[4], '修正前', '#ea580c', 0.08, 0.08)
# 回环边（绿色，位姿 5 → 位姿 2）
ax.annotate('', xy=LOOP_TO, xytext=LOOP_FROM,
            arrowprops=dict(arrowstyle='-|>', color='#059669', lw=2.8))
ax.text(1.52, 1.0, '回环边', fontsize=8.5, color='#047857', fontweight='bold',
        ha='center', va='center')
ax.set_title('回环触发：\n位姿 5 拉回位姿 2', fontsize=10.5, color='#047857',
             pad=8)

fig.suptitle('iSAM2 实时增量更新：每帧 update() 一次，路径边增长边被修正（左 → 右为时间推进）',
             fontsize=12, color='#334155', y=0.99)

plt.tight_layout(rect=[0, 0, 1, 0.93])
plt.savefig('gtsam_isam2_sequence.png', dpi=150, bbox_inches='tight',
            facecolor='white')
print('已保存 gtsam_isam2_sequence.png')

总结

  • 本文承接前三期的回环检测链路，系统解读了 GTSAM 与 iSAM2 的数学原理与源码实现，核心要点回顾：
    • 从回环约束到 PGO （第 1 章）：Scan Context/SOLiD 粗匹配 + small_gicp 精配准输出 6-DOF 相对位姿，这些约束互不矛盾不了，需要 PGO 来协调
    • 因子图建模 （第 2-2 至 2-4 节）：PGO 本质是最大后验估计，等价于加权非线性最小二乘；NonlinearFactorGraph + Values + Factor 三大结构对应图/取值/边
    • 两大因子 （第 2-5 节）：PriorFactor 钉住原点（否则不可观），BetweenFactor 用对数映射算相对位姿误差 e = L o g ( z − 1 x i − 1 x j ) e = \mathrm{Log}(z^{-1} x_i^{-1} x_j) e=Log(z−1xi−1xj)
    • 噪声模型与白化 （第 2-6 节）： Σ − 1 = R T R \Sigma^{-1} = R^T R Σ−1=RTR，白化把马氏范数变成普通二范数，让不同单位约束公平求和
    • 优化器（第 2-7 节）：GN/LM 和配准同源，区别在于 PGO 的 Hessian 是稀疏的，用消元 + 稀疏分解求解
    • iSAM2 增量平滑 （第 3 章）：贝叶斯树 + 局部重消元 + 流式重线性化，每帧只碰受影响的团，把 O ( N ) O(N) O(N) 降到 O ( 受影响团 ) O(\text{受影响团}) O(受影响团)
  • GTSAM 之所以成为 SLAM 后端的标准，是因为它的抽象和 SLAM 的数学一一对应------你只需把问题写成因子图，求解、稀疏化、增量更新全部交给它。理解了本期这套"因子图 → 最小二乘 → 贝叶斯树增量"，再看 LIO-SAM、LEGO-LOAM 的后端代码，就会豁然开朗
  • 如有错误，欢迎指出！
  • 感谢观看！
相关推荐
01二进制代码漫游日记1 小时前
C++基础入门速通
java·开发语言·c++
tudousisi2221 小时前
二维费用01背包
c++
2601_956121971 小时前
高精度问题
c++
橙橙笔记1 小时前
C++的学习第三部分
开发语言·c++·学习
码匠许师傅10 小时前
【C++ 面试真题】聊聊 C++ 的移动语义与右值引用
java·c++·面试
小小龙学IT11 小时前
DuckDB 深度实战：用 C++ 在进程内跑一个「分析型数据库」
数据库·c++
知无不研13 小时前
lambda表达式的使用（3）
开发语言·c++·lambda
C++ 老炮儿的技术栈14 小时前
基于Qt实现轻量化本地音乐播放器
开发语言·c++·qt·c·播放器·音乐
qz_Serene16 小时前
C++：类和对象（上）
开发语言·c++
热门推荐
01GitHub 镜像站点02国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）03微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新042026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？05Agnes AI 免费 API 接入指南：文本、生图、生视频，一套接口全免费06VSCode+CodeX扩展，无法启动07历年考研数学一、数学二、数学三真题试卷及答案PDF08AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路092026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片10如何新建文件夹？ 电脑新建文件夹的4种方法