1. 先说清楚因子图到底解决了什么别人解决不了的问题很多人第一次听到因子图这个词是在学习概率图模型的教材里那一章往往跟贝叶斯网络、马尔可夫随机场排在一起标题通常是无向图模型或者图模型统一框架。结果学完之后脑子里只留下一个二部图的图像圆形是变量、方形是因子、边把它们连起来——然后就没有然后了。真正让我意识到这东西的价值是后来做机器人位姿估计的时候明明写下了几十个观测方程求解器也能跑但一旦要把新的观测尤其是回环缝进来整个问题的结构就乱成一团变量该保留哪些、旧信息能不能复用、噪声怎么加权全靠拍脑袋。因子图factor graph的核心贡献其实就是把一个高维联合分布拆成一堆只依赖少数变量的局部函数的乘积然后用一张二部图把这个拆解关系完整地记下来。听起来平淡但这一步做了之后三件事变得异常简单第一联合概率的分解结构一目了然你能直接看出哪些变量之间真的有耦合第二推断算法和积算法、最大和算法可以在图上按消息传递的方式自然展开第三也是最实际的一点——当这些局部函数都取高斯形式时最大后验估计会直接退化成一个稀疏非线性最小二乘问题而这正是因子图优化能在大规模建图、多传感器融合里跑得飞快的根本原因。这篇内容适合谁看如果你正在做视觉/激光建图、组合导航、多传感器标定或者任何一堆带噪声的观测约束一堆状态量的活儿那因子图基本是你绕不开的工具。如果你只是好奇概率图模型里的这个分支到底干嘛用的也可以顺着往下看——我会尽量少用测度论的符号多用这张图在算什么的直觉讲。有个常见误区得先点掉很多人把因子图和贝叶斯网络当成两种并列的建模工具实际上因子图是更底层的一种表示它可以把有向图、无向图都翻译过来。贝叶斯网络里一个条件概率 $P(x_i \mid \text{parents})$在因子图里就是一个只连着 $x_i$ 和它父节点的方形节点马尔可夫随机场里的团势函数同样也是一个因子。也就是说因子图不改变你建模的概率语义它改变的是这份概率模型被记账的方式。1.1 有向图和无向图在表达上的那点别扭贝叶斯网络用有向边表达因果关系好处是参数化直接每个节点存一张条件概率表/条件密度联合分布就是这些条件的连乘。但它在两类场景下会显得别扭。一类是循环依赖。比如图像去噪相邻像素之间的相似性是对称的你很难说左边像素决定了右边像素还是反过来。硬要用有向图表达就得人为指定一个方向并引入额外的因子图的语义被污染了。另一类是约束的异质性。同一个变量可能同时被好几种完全不同来源的约束牵扯一个位姿节点同时被里程计约束、被激光匹配约束、被GNSS约束、被回环约束牵着。在贝叶斯网络里你想给每个约束单独标定噪声会发现它们被塞进了同一个条件概率里耦合在一起没法分开调。无向图马尔可夫随机场用团势函数解决了对称性问题但随之而来的是配分函数$P(X) \frac{1}{Z}\prod_c \psi_c(X_c)$其中 $Z \sum_X \prod_c \psi_c(X_c)$。这个 $Z$ 在连续变量下是个积分一般算不出来。图越大、团越大$Z$ 越难搞而很多推断任务其实根本不需要知道 $Z$ 的精确值——归一化常数在求极值时会被消掉。因子图的做法很干脆直接承认我就是一堆局部函数的乘积前面挂个 $Z$ 表示归一化然后把这堆局部函数和它们依赖的变量画成二部图。建模的时候你只管写因子$Z$ 存在但不用管。1.2 二部图里两类节点各司其职因子图的形式定义很朴素一个二部图 $\mathcal{G} (\mathcal{X}, \mathcal{F}, \mathcal{E})$$\mathcal{X}$ 是变量节点集合画成圆$\mathcal{F}$ 是因子节点集合画成方块边 $e \in \mathcal{E}$ 只在变量和因子之间连表示这个因子依赖这个变量。一个因子 $f_i$ 连接的所有变量记作它的邻域 $\text{n}(f_i)$因子就是定义在这些变量上的一个函数。举个具体的三个变量 $x_1, x_2, x_3$联合分布写成 $$P(x_1,x_2,x_3) \propto f_a(x_1,x_2), f_b(x_2,x_3), f_c(x_3)$$ 那么图上就是三个方块、三个圆圈$f_a$ 连 $x_1,x_2$$f_b$ 连 $x_2,x_3$$f_c$ 只连 $x_3$。在工程里我习惯这样理解变量节点 所有需要被估计的未知量因子节点 每一条你手里的观测/先验/约束。里程计走过一段就是一条连接两个位姿的因子看到一次回环就是一条跨越很远的因子加速度计零偏的先验就是一个只连零偏变量的因子。这个视角转换很有用因为它把建模变成了我手里有哪些数据的问题。你有多少条测量就有多少个因子测量之间彼此独立因子就各自独立。这比设计一个联合分布要具体得多。1.3 配分函数不是被消灭了而是被搁置了必须强调一点因子图并没有取消 $Z$只是把它放在一边。$P(X) \frac{1}{Z}\prod_i f_i(X_i)$ 里的 $Z$ 依然存在。为什么可以不管它因为绝大多数工程任务问的是哪个 $X$ 最可能最大后验估计MAP或者某个变量的边缘分布长什么样边缘推断。前者只需要比较不同 $X$ 下 $\prod_i f_i$ 的大小$Z$ 是个与 $X$ 无关的常数argmax 时直接扔掉后者在树结构下可以通过消息传递把 $Z$ 的贡献分解掉只在最后归一化那一步用到。真正需要 $Z$ 的场景是模型比较、似然计算、期望传播这类任务那时候往往要靠变分方法或者采样去近似。日常做建图定位$Z$ 基本不会出现在你的代码里。我见过有人第一次用因子图时死盯着这个概率没归一化啊不放其实完全没必要。归一化常数就像做菜时的摆盘——影响观感不影响你判断这道菜该不该吃。2. 消息传递让图从一张静态结构变成一台计算装置画出因子图只是第一步真正让它动起来的是消息传递。这套东西的核心思想很反直觉但很漂亮不去直接算那个 $P(X)$而是让每个节点只跟邻居说话反复交换信息最后每个变量上收到的消息乘积再归一化就是它的边缘分布。2.1 和积算法在树上为什么是精确的先看最简单的情形整个因子图是一棵树没有环。定义两类消息。变量 $x$ 到因子 $f$ 的消息 $$\mu_{x \to f}(x) \prod_{f \in \text{n}(x) \setminus {f}} \mu_{f \to x}(x)$$因子 $f$ 到变量 $x$ 的消息 $$\mu_{f \to x}(x) \sum_{X_f \setminus {x}} f(X_f) \prod_{x \in \text{n}(f) \setminus {x}} \mu_{x \to f}(x)$$然后变量 $x$ 的边缘就是 $$p(x) \propto \prod_{f \in \text{n}(x)} \mu_{f \to x}(x)$$这两个公式看着抽象翻译成人话是这样的一个变量给某个因子发消息时把它从其他所有因子那里听到的意见汇总起来告诉这个因子一个因子给某个变量发消息时把除这个变量之外的其他变量的影响全部边沿化求和/积分掉再乘上收到的消息。边沿化这个词很关键。因子 $f(x_1, x_2)$ 要告诉 $x_1$ 信息就必须把 $x_2$ 消掉——它不能直接说我觉得 $x_1$ 应该是多少因为它的意见依赖于 $x_2$ 到底多少。这个消元过程在离散变量下是求和在连续高斯变量下是积分结果仍然是高斯的高斯对高斯积分还是高斯这正是高斯族在因子图里如此好用的原因。树上精确的根本原因在于树没有环所以每个变量的所有依赖都能被完整地切开成两支从两支传过来的消息互不干扰乘积就是完整的边缘。证明不复杂但做工程的时候其实不用证——你只要知道树 精确这个结论就够了。2.2 叶子节点是消息传递的起点实际实现时消息传递的顺序不是随便排的得从叶子往根推。叶子上的因子只连一个变量它发出的消息就是因子本身叶子上的变量只连一个因子它发出的消息就是常数 1。这个性质在代码里体现得非常明显。假设你有一个简单的链式位姿图 $x_0 \to x_1 \to x_2 \to \dots$那么从 $x_0$ 那个先验因子开始消息一路往右传再从右往左传回来两趟就完事。时间复杂度是线性的跟变量数量成正比。这也是为什么对于纯里程计没有回环的情况图就是一个链求解快到几乎不用考虑。但只要有回环链就变成了带环的图消息传递就没有起点可言了——你不知道该从哪开始因为每个节点都在等别人先说话。2.3 最大和算法把求和换成取最大推断的另一个大任务是求 MAP$\hat{X} \arg\max_X \prod_i f_i(X_i)$。这个任务用最大和算法max-sum来做公式形式上跟和积算法几乎一模一样只是把 $\sum$ 换成 $\max$$$\mu_{f \to x}(x) \max_{X_f \setminus {x}} \left[ f(X_f) \prod_{x \in \text{n}(f) \setminus {x}} \mu_{x \to f}(x) \right]$$而变量到因子的消息还是乘积形式因为乘积在 log 域下是加法取最大和加法可以交换。工程上几乎没人直接对概率密度做乘法和取最大因为数值上会下溢连乘几百个小于 1 的数double 也扛不住。标准做法是取负对数把乘积变成求和、把取最大变成取最小$$-\log \prod_i f_i(X_i) \sum_i \left[-\log f_i(X_i)\right]$$于是 MAP 变成 $$\hat{X} \arg\min_X \sum_i E_i(X_i), \quad E_i -\log f_i$$这一步是因子图优化的全部秘密。后面所有关于最小二乘高斯牛顿信息矩阵的内容本质上都是在处理这个负对数代价函数。如果你只记一个公式就记这个。2.4 图里有环的时候事情变得不精确但依然有用真实的建图问题几乎不可能是树因为回环因子的存在就是为了制造环。一个从起点走一圈回到起点的轨迹加上回环约束图上就出现了环。在有环的图上跑和积/最大和叫 loopy belief propagationLBP。它没有任何精确性保证甚至可能不收敛振动、周期解都是常事。但实践中它对很多问题效果出奇地好尤其是在环比较长、局部仍然接近树结构的图上。不过在建图领域我们其实很少直接跑 LBP而是走另一条路既然所有因子都是高斯那 MAP 问题就是一个凸的或者局部凸的非线性最小二乘直接用数值优化求解比消息传递更稳、更快、更容易做增量更新。消息传递在这里更多是提供直觉——它告诉你为什么稀疏结构重要因为消息只在图的边上流动没有边的地方不传递信息这正好对应雅可比矩阵里的零块。提示如果你在实现中发现某个变量的估计值总是被拉偏先别怀疑求解器去看这个变量在图上有几条边。如果一个位姿只有一条里程计边连着、没有任何其他约束那它的估计就只能靠外推误差累积是必然的跟算法无关。3. 因子图优化高斯因子把概率问题变成了线性代数问题现在进入最实用的部分。假设所有因子都是高斯的对绝大多数传感器模型来说这个假设够用那么因子图优化就退化成一个结构极其漂亮的最小二乘问题。3.1 一个高斯因子等价于一条马氏距离平方单个高斯因子的形式是 $$f_i(X_i) \propto \exp\left(-\frac{1}{2}\left|h_i(X_i) - z_i\right|^2_{\Sigma_i}\right)$$ 其中 $h_i$ 是测量模型把状态映射到观测空间$z_i$ 是实际测量值$\Sigma_i$ 是测量噪声协方差。这里的 $|\cdot|^2_\Sigma$ 是马氏距离平方定义为 $e^\top \Sigma^{-1} e$$e h_i(X_i) - z_i$ 是残差。取负对数、扔掉常数项得到这个因子的代价 $$E_i \frac{1}{2} e_i^\top \Omega_i, e_i, \quad \Omega_i \Sigma_i^{-1}$$$\Omega_i$ 就是信息矩阵协方差的逆。它决定了这条件约束在优化里的话语权$\Omega$ 越大说明你越相信这个观测残差稍微大一点代价就飙上去$\Omega$ 越小说明这个观测不靠谱残差大点也无所谓。整个因子图的目标函数就是所有因子代价的和 $$\hat{X} \arg\min_X \sum_i \frac{1}{2} e_i^\top \Omega_i e_i$$到这里概率推断问题已经完全变成了一个加权非线性最小二乘。没有概率论只有残差和权重。这也是为什么很多做优化出身的人上手因子图比做概率出身的人还快——他们看到的只是一个稀疏的最小二乘。3.2 雅可比、稀疏性和 H 矩阵的形状最小二乘要用高斯牛顿或者列文伯格-马夸尔特迭代。把残差在当前估计 $\bar{X}$ 附近线性化$h(X \delta) \approx h(\bar{X}) J\delta$得到正规方程 $$\left(\sum_i J_i^\top \Omega_i J_i\right)\delta -\sum_i J_i^\top \Omega_i e_i$$左边那个矩阵 $H \sum_i J_i^\top \Omega_i J_i$ 就是信息矩阵在参数空间的体现也叫 Hessian 近似。它的稀疏结构直接由因子图的连接结构决定因子 $i$ 只连着少数几个变量那 $J_i$ 就只有对应那几列非零$J_i^\top \Omega_i J_i$ 只在那几个变量的行列交叉处产生非零块。一个位姿图10000 个位姿、每个位姿 3 维$(x, y, \theta)$理论上 $H$ 是 $30000 \times 30000$。如果是稠密的光存储就要 7 GB求解更是不可能。但因为每个因子只连少量变量$H$ 的非零块数量跟因子数成正比通常不到千分之一。这个稀疏性就是因子图优化能处理百万级变量的根源。我做过一个粗略的对比同样规模的问题把它当稠密最小二乘解和利用稀疏结构解在求解时间上能差三到四个数量级。不是快一点是快几千倍。3.3 变量消元顺序为什么能决定快慢稀疏矩阵求解常用 Cholesky 分解 $H R^\top R$。分解过程中本来为零的位置可能被填上非零值这叫fill-in。fill-in 的数量强烈依赖于你消元的顺序也就是矩阵行列的排列顺序。一个直观的例子链式结构 $x_1 - x_2 - x_3 - x_4$如果按 $1,2,3,4$ 顺序消元每消一个只影响下一个几乎不产生 fill-in如果按 $1,3,2,4$ 顺序消马上就会产生一堆额外的非零块。同理对于一个有回环的图回环的长度决定了最小 fill-in 的下界。所以工程实践里有几条经验近似最小度排序COLAMD 之类的算法是最常用的通用策略够快也够好对有明显层次结构的问题比如位姿图嵌套剖分METIS往往更优如果你知道问题有特殊结构比如纯里程计链手工指定顺序比自动排序还快。在 GTSAM 里如果你不指定它会自动用 COLAMD想换的话可以在ISAM2Params或Ordering里设置。我遇到过一个案例同样 5000 个位姿的图默认排序要 1.2 秒换成嵌套剖分后降到 0.4 秒左右差距相当可观。3.4 增量式求解只重算被影响的那部分一次性求解适合批量处理比如离线建图但机器人上跑的必须是增量的每来一帧数据就要更新一次估计不能每次从零开始解 30000 维的方程。增量求解的核心思路是贝叶斯树Bayes tree把消元过程组织成一棵树新变量加入时只在树的一小部分上做局部重消元。iSAM、iSAM2 就是这条路线的代表实现。ISAM2 还引入了 fluid relinearization意思是当某个变量的线性化点偏移过大时选择性对它重新线性化而不是全部重来。实测数据一个中等规模的位姿图2000 位姿 若干回环一次性批量求解可能要几十毫秒用 ISAM2 做增量更新通常在 1-5 毫秒量级而且这个时间几乎不随图的总规模增长——只跟局部受影响的变量数有关。注意增量求解有个容易被忽略的前提——新加入的测量只影响局部。如果你在很老的时刻插入一个回环因子它会影响从回环起点到终点之间的所有变量那一块全都要重线性化单次更新耗时可能一下子涨两个数量级。这不是 bug是结构决定的。4. 回环因子加入因子图看起来简单细节全在噪声模型上回环因子加入因子图这个操作从代码上看就一行graph.add(BetweenFactor(...))。但实际项目里这一行往往是整个系统出问题最多的地方。4.1 回环因子的本质是一条跨时刻的相对位姿约束回环因子表达的是机器人在时刻 $j$ 观测到自己在时刻 $i$ 曾到过的地方通过匹配算出了一个相对位姿 $\hat{T}_{ij}$或者更准确地说$i$ 时刻机体系到 $j$ 时刻机体系的变换。这条约束把两个相隔很远的位姿节点直接连起来形成闭环。它和里程计因子的区别只有一个跨越的距离。里程计因子连接相邻两帧误差小回环因子连接可能相隔几百帧的两个位姿中间累积的漂移在回环那一刻被关闭——闭环一合上中间的轨迹就必须整体变形来满足这个约束这正是回环修正轨迹的机制。从矩阵结构上看加一条回环因子就是在 $H$ 的非对角位置第 $i$ 块行第 $j$ 块列增加一个非零块。这个块会让消元顺序的重要性陡然上升如果先消 $i, j$ 之间的变量会制造大量 fill-in好的排序算法会优先消掉叶子部分把回环留到最后。4.2 信息矩阵怎么给直接决定结果拧不拧这是最考验经验的地方。很多人第一次写回环因子随手抄了个Diagonal::Sigmas([0.1, 0.1, 0.05])就上了结果要么轨迹根本没被拉回去因为给的回环权重太低跑不过里程计的累积约束要么轨迹被拧成一个诡异的形状因为权重给太高错位的回环把整条轨迹拉歪。我的经验做法分三步。第一步让回环的 $\sigma$ 不要比里程计精细一个量级以上。里程计噪声里平移 $\sigma$ 通常在 0.01-0.1 米量级旋转在 0.01-0.05 弧度量级这取决于里程计质量。回环的平移 $\sigma$ 可以设置成 0.05-0.5 米旋转 0.02-0.2 弧度。如果回环用的是高精度激光匹配可以把平移 $\sigma$ 设到 0.02-0.05 米如果回环来自视觉词袋粗匹配保守一点设到 0.3 米以上更安全。第二步考虑用全信息矩阵而不是对角线。相对位姿的协方差在平移和旋转之间通常存在耦合旋转估计不准会导致平移方向上的不确定性沿着垂直于运动方向的轴放大。用对角矩阵等于假设这种耦合不存在会让优化结果在某些方向上过度自信。GTSAM 支持noiseModel::Gaussian::Information(Matrix)传入完整的信息矩阵代价是每条回环多存一个对称矩阵。如果回环数量不多这点内存完全值得。第三步用鲁棒核包一层。这个后面单独说。回环来源平移 σ 建议值m旋转 σ 建议值rad备注激光 ICP/NDT 匹配0.02 – 0.080.01 – 0.03匹配收敛良好时可取小值视觉特征 PnP0.05 – 0.20.02 – 0.08取决于特征质量和标定精度视觉词袋粗定位0.2 – 1.00.1 – 0.4只用于粗修正需鲁棒核外部全局传感器0.5 – 5.00.1 – 1.0视传感器精度而定这些数值不是金科玉律是我在不同项目里试出来的安全区。真正正确的做法是用实测残差反推把回环刚加进去时看它的马氏距离 $e^\top \Omega e$ 有多大。如果在优化前就大于几十说明要么这个回环本身是错的要么你的 $\Omega$ 给得过于乐观。4.3 错误回环的杀伤力和三种应对思路一个错误的回环false positive loop closure给系统带来的伤害远大于少一个正确的回环。假设归一化后的马氏距离是 100而正确回环通常在 1-5 之间那这条错误约束在总代价里就占了压倒性比重优化器会牺牲掉几十条正确的约束去迎合它整条轨迹被扭曲得面目全非。工程上有三种主流思路。思路一鲁棒核函数m-estimator。把代价函数从二次改成残差小时近似二次、残差大时增长变慢的形式。常用的有 Huber超过阈值后线性增长、Cauchy对数增长、Geman-McClure有上界。以 Cauchy 为例代价变成$$E \frac{c^2}{2}\log\left(1 \frac{e^\top \Omega e}{c^2}\right)$$$c$ 是控制点通常在 0.1-1.0 之间取值按马氏距离的尺度。残差远大于 $c$ 时梯度几乎不再增长错误回环就拉不动优化器了。这是最简单、改动最小的方案。思路二可切换约束switchable constraints。给每条回环因子配一个连续变量 $s \in [0,1]$因子形式变成 $\exp(-\frac{1}{2} s^2 e^\top \Omega e)$再加一个先验把 $s$ 往 1 上拉。$s$ 接近 1 表示接受这条回环接近 0 表示弃用。优化过程会自动决定每条回环的权重相当于让数据自己投票。代价是引入了额外变量图变大了。思路三max-mixture。把每条回环的噪声模型建模成一个混合分布以大概率 $w$ 服从一个很紧的高斯正确回环以小概率 $1-w$ 服从一个非常宽的高斯或均匀分布错误回环的垃圾桶。这样正确回环被强约束错误回环几乎不产生力。三种方法我都在不同项目里用过。经验是如果回环错误率低于 5%Huber 或 Cauchy 就够了如果错误率在 10% 以上得上可切换约束或 max-mixture如果错误率超过 20%问题根本不在因子图在回环检测环节先把检测器调好再说。见过有人试图用巨大阈值把 30% 错误率的问题压下去最后图上是没被扭曲但回环也全被拒绝了轨迹照样飘——这不是算法的胜利是把问题藏起来了。4.4 增量加入时的几个执行细节回环因子不是一次性全加进去的是边跑边加。这里有几个坑。变量索引必须对齐。回环检测返回的是当前帧匹配到第 $k$ 帧这个 $k$ 必须准确映射到你因子图里的变量索引。用统一的哈希表管理帧号 → 变量索引的映射别用数组下标硬凑一旦中间有帧被丢弃就会全乱。我在这上面栽过一次回环把两段完全不相邻的轨迹强行连在一起优化后轨迹看起来完美闭环实际上是把两段轨迹叠到了一起。加入时机影响收敛。不要在单次更新里同时加入多条回环。一条一条加、每次都重新求解比一口气加五条再求解更稳。原因是单条回环的修正量小求解器容易收敛一次加太多初始残差很大非线性优化容易跳到一个坏解上。加完后检查残差分布。我习惯在每次回环更新之后统计所有因子的马氏距离平方看它们的直方图。健康的分布应该集中在因子维数附近比如 3 维位姿因子的残差平方均值在 3 左右。如果某条回环的残差是 500其他都在个位数那基本可以断定这条是错的。5. 动手搭一个最小可用的位姿因子图理论说完落代码。我用 Python 版的 GTSAM 举例因为它表达起来最紧凑换成 C 或 Ceres 的对应关系我也标出来。5.1 因子图的三件套图、变量容器、噪声模型任何因子图代码都离不开这三样东西因子图本身NonlinearFactorGraph一个只增不减的因子列表变量容器Values当前估计值既是初始值也是输出噪声模型每条因子都要带上决定了它在优化里的权重。在 Ceres 里对应关系是因子图 Problem变量容器 ParameterBlock噪声模型 LossFunction 残差尺度的组合Ceres 的残差本身不带权重权重得自己在残差里乘或者用ScaledLoss。g2o 里则是SparseOptimizerVertexSE2EdgeSE2加信息矩阵。三者的数学完全一样只是接口风格不同。5.2 里程计因子和回环因子的代码骨架import numpy as np import gtsam from gtsam.symbol_shorthand import X # 1. 先验因子钉住第一个位姿否则整个图可以整体漂移 prior_noise gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6])) graph gtsam.NonlinearFactorGraph() graph.add(gtsam.PriorFactorPose2(X(0), gtsam.Pose2(0.0, 0.0, 0.0), prior_noise)) # 2. 里程计因子连接相邻位姿 odom_noise gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.02])) for i in range(1, len(rel_poses)): graph.add(gtsam.BetweenFactorPose2( X(i - 1), X(i), rel_poses[i], odom_noise)) # 3. 回环因子用 Cauchy 鲁棒核包一层 loop_base gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) loop_noise gtsam.noiseModel.Robust.Create( gtsam.noiseModel.mEstimator.Cauchy(1.0), loop_base) for (i, j, rel) in loop_closures: graph.add(gtsam.BetweenFactorPose2(X(i), X(j), rel, loop_noise)) # 4. 初始值先用里程计递推一路推过去 initial gtsam.Values() initial.insert(X(0), gtsam.Pose2(0.0, 0.0, 0.0)) for i in range(1, len(rel_poses)): prev initial.atPose2(X(i - 1)) initial.insert(X(i), prev.compose(rel_poses[i])) # 5. 求解 params gtsam.LevenbergMarquardtParams() params.setMaxIterations(50) params.setRelativeErrorTol(1e-6) result gtsam.LevenbergMarquardtOptimizer(graph, initial, params).optimize() print(最终误差:, graph.error(result))这段代码有几个地方值得多说。先验噪声给到 $10^{-6}$ 不是随便写的。先验因子不是真实观测它的唯一作用是固定规范自由度gauge freedom。整个位姿图在整体旋转平移下是不变的——你把所有位姿同时旋转 30 度、平移 100 米所有相对位姿约束的残差都不变。如果不钉死一个点$H$ 矩阵会奇异求解器会报错或给出任意解。所以先验的信息矩阵要足够大对应 $\sigma$ 足够小大到不可动摇但也不要给到 $10^{-12}$ 以上否则数值条件数会变差。初始值用里程计递推不要全填零。非线性优化对初值敏感。如果所有位姿都从原点开始回环因子的初始残差会非常大LM 可能一步都走不动就直接收敛到局部极小。用里程计递推得到的轨迹作为初值虽然漂移但至少拓扑是对的回环一拉就能拉回来。LM 还是 GN高斯牛顿收敛快但不保证下降列文伯格-马夸尔特加了阻尼更稳但迭代次数多。位姿图这种问题我一般用 LM因为回环加入的瞬间残差跳变很大GN 容易发散。Dogleg 是另一个选择GTSAM 里DoglegOptimizer在初始值不错的情况下效率很高。5.3 求解器参数到底影响什么setMaxIterations设小了不收敛设大了浪费时间。位姿图问题通常 10-30 次迭代就收敛。判断收敛不看迭代次数看graph.error(result)的变化连续两次迭代误差下降小于相对容差比如 $10^{-6}$就可以停。setRelativeErrorTol和setAbsoluteErrorTol建议至少设一个。有次我两个都没设跑一个 8000 位姿的图迭代到 200 次还没停误差其实在第 15 次之后就没怎么降了纯粹在空转。如果用的是 ISAM2参数就换成ISAM2Paramsisam_params gtsam.ISAM2Params() isam_params.setRelinearizeThreshold(0.1) isam_params.relinearizeSkip 1 isam gtsam.ISAM2(isam_params)RelinearizeThreshold控制线性化点偏离多远就重新线性化。设太小会频繁重算设太大会导致线性化误差累积、精度下降。0.1 是个常用起点具体值跟你状态的尺度有关——如果位姿的平移单位是米0.1 意味着 10 厘米的偏移就会触发重新线性化。5.4 怎么验证优化结果是对的优化跑完不报错不代表结果对。我会做三层检查。第一层看总误差。优化后的graph.error()应该明显小于优化前。如果只降了百分之几要么初值本来就很好纯里程计没回环的情况要么优化卡住了。第二层看单个因子的残差。遍历所有因子算它们的马氏距离平方。正常的分布应该是绝大多数在因子维度附近3 维因子 → 3 左右少数略大没有极端的离群值。如果有一条回环残差是 300其他都是个位数那这条回环有问题。第三层看轨迹形状。把优化后的位姿连成曲线画出来跟里程计递推的轨迹对比。正确的优化结果应该是一条平滑的、闭合的轨迹。如果出现尖锐的折角、自交、或者某一段被拉成锯齿状说明有错误约束在起作用。提示GTSAM 提供marginalCovariance可以算单个变量的边缘协方差。对回环修正量最大的那几个位姿算一下协方差如果某个方向的方差比周围大一个量级说明这个位置的约束太弱可能需要补数据或者调权重。6. 我在真实项目里踩过的那几个坑前面讲的都是应该怎么做这一节讲讲实际会怎么错。6.1 噪声模型拍脑袋优化器就按你给的错权重认真执行这是我踩得最深的一个坑。早期做激光里程计我把里程计因子的 $\sigma$ 随手设成了[0.01, 0.01, 0.01]回环设成[0.3, 0.3, 0.1]。看起来合理——里程计准回环不准。结果发现问题整条轨迹在局部非常硬回环拉不动它闭环误差只能靠远处的位姿整体平移来吸收导致轨迹末端出现明显的变形。原因是我把里程计的权重给得太高了——0.01 米意味着 1 厘米而实际激光里程计在转弯时的横向误差能到 5-10 厘米。优化器太相信里程计回环的信息就被淹没了。正确做法是用数据标定。跑一段直线和一段转弯把里程计的预测位姿和真值可以用全站仪或者高精度外部跟踪系统比较统计误差的均值和方差用这个方差做噪声模型。如果没有真值退而求其次用两次独立跑同一段路的轨迹差来估计。6.2 坐标系和旋转表示的约定一个不统一就全盘皆输因子图里每个位姿都是一个刚体变换涉及三个约定旋转是左乘还是右乘、主动旋转还是被动旋转、四元数的实部在前还是在后。GTSAM 用的是左乘约定Pose2::compose是 $T_1 \cdot T_2$表示先应用 $T_2$ 再应用 $T_1$在 $T_1$ 的坐标系下。ROS 的tf用的是类似但有细微差别的约定。Eigen 的Quaterniond(w, x, y, z)是实部在前而某些库是实部在后。混用这些约定的后果是代码能跑优化能收敛但结果就是差那么几度或者偏那么几厘米你看不出来。我遇到过一次回环因子加进去之后轨迹确实闭合了但相邻位姿之间的相对旋转差了 180 度——因为四元数顺序搞反了$q$ 和 $-q$ 表示同一个旋转但插值和优化过程中的表现完全不同。如何避免在项目开始时就写一个测试用例用两组已知位姿算出它们的相对位姿然后用你因子图库里的between函数算一遍看结果是否一致。这个测试只需要二十行代码但能省你两天的调试。6.3 变量太多一次性求解内存直接爆掉有一次处理一个室外大场景图里有 40000 多个位姿、20 多万条因子。用批量 LM 求解内存瞬间冲到 30 GB 以上机器直接卡死。问题出在Values容器和稀疏矩阵的存储上。虽然 $H$ 是稀疏的但在构建过程中如果用了低效的排序fill-in 会让非零元素数量暴增。解决方案是换用 ISAM2 做增量求解永远不构建完整的 $H$如果必须批量求解用嵌套剖分排序并且分块处理先解一部分把结果作为下一部分的初值检查是否所有变量都真的需要保留很多历史位姿其实可以边缘化掉marginalization只保留对后续有影响的。边缘化本身也是个坑边缘化会产生稠密因子。你把 $N$ 个老位姿消掉之后剩下的位姿之间会新增一堆互相耦合的因子图反而变稠密了。所以边缘化要谨慎不要为了省内存反而拖慢求解。6.4 调试时该看什么图不该看什么图优化不收敛的时候很多人第一反应是去翻求解器的日志看残差下降曲线。这有用但不够。我更推荐看两张图残差直方图和因子图的可视化。残差直方图能告诉你问题出在全局尺度不对还是个别因子异常。如果整条曲线整体右移所有残差都偏大是噪声模型给得偏小如果右边拖了一条长尾是个别因子有问题。因子图可视化能告诉你结构上的问题。把变量画成点、因子画成线一眼就能看出哪些变量是孤立的只连一条边哪些区域因子特别密集。孤立变量是脆弱点密集区域是可能产生错误回环的地方。我用过的一个土办法把每条回环因子按残差大小染色画到图上红色的回环一眼就能揪出来。这个方法比看数字直观得多。7. 因子图能做的事情远不止建图定位最后聊点扩展方向给正在考虑要不要投入学这套工具的读者一点参考。7.1 多传感器融合是它最自然的应用场景卡尔曼滤波的框架里一旦传感器数量多起来、更新频率不一致、还要处理延迟观测状态向量和协方差矩阵的管理就会变得极其琐碎。因子图在这方面几乎是天然优势每个传感器就是一个独立的因子类型加进来就行不影响其他部分。IMU 预积分因子、轮速计因子、视觉重投影因子、GNSS 因子、磁力计因子全都可以挂在同一个位姿变量上各自带自己的噪声模型。想做异步融合在因子图里就是这条因子在高频加、那条在低频加没有任何额外机制。而且因子图对延迟观测特别友好。你不需要像 EKF 那样维护一个状态缓冲队列去回放直接把因子加到对应时刻的变量上重新求解一次就行——增量求解器会自动处理哪些部分需要更新。7.2 结构化预测和序列标注里也有它的影子跳出机器人领域因子图在自然语言处理和计算机视觉里同样有长时间的应用历史。词性标注、命名实体识别这些序列任务早期主流的条件随机场CRF本质上就是一个链式因子图每个位置一个节点、相邻位置之间一个转移因子。用最大和算法在 CRF 里叫维特比算法求最优标注序列跟位姿图求最优轨迹在数学上是同一件事。图像分割里的图割方法也是无向图模型的一个特例只不过用的是最大流/最小割而不是消息传递。这些方法的共同点是问题本身有局部结构用图来表达能把这个结构显式地利用起来。7.3 和深度模型的结合是这几年比较有意思的方向一个明显的趋势是把因子图中的某些因子用神经网络来建模。比如测量模型 $h_i(X_i)$ 太复杂、没法用解析式写出来的时候用一个网络代替它梯度通过自动微分传回去。这在可微优化differentiable optimization这个方向上已经有相当多的工作。另一个方向是用网络来学习因子图的稀疏结构——哪些变量之间应该连边、哪些不应该。这在多目标跟踪、场景图理解里比较常见。不过说实话这条路目前的工程落地还不多主要卡在训练和推理的效率上。如果你只是想解决一个建图问题老老实实写高斯因子是最省事的。神经网络的因子适合那些模型写不出来但数据很多的场景。我在实际项目里的体会是因子图真正难的部分从来不是算法而是噪声模型的设计和错误因子的甄别。算法本身几十年前就成熟了库也都很完善但给一条回环因子该配多大的 $\sigma$这件事至今没有一个能完全自动化的解决方案还是得靠对传感器特性的理解和一点一点试出来的经验。每次接手一个新平台我第一件事就是把各种传感器的误差特性摸清楚做一组标定实验这比读十篇论文都有用。