尧图网页设计 
做多传感器融合定位那阵子我最大的困惑是手里攥着里程计、视觉重投影、GPS、回环检测四五路观测每路的噪声特性都不一样画成贝叶斯网络之后边交叉得像一团毛线想加一个约束就得改一次拓扑改完还得重新推导边缘概率。后来换成因子图Factor Graph的写法图一下子干净了变量是变量观测是因子两者分开摆成二部图加一个回环就是加一个节点、连几条边的事代码里也就多一行。概率图模型里因子图算是把表示和推断这两件事拆得最彻底的一种结构——它不追求每个数字都有概率含义这点后面细讲只关心一个全局函数能不能拆成若干局部因子连乘然后能不能沿着图上的边把消息传起来。这篇东西写给两类人一类是学过贝叶斯网络、马尔可夫随机场但一到因子图就被二部图和消息传递公式劝退的另一类是做视觉里程计、组合导航、结构化预测早就在用现成的优化库但说不清楚后端到底在解什么问题的。我会从为什么需要因子图讲起把和积算法的推导一步步摊开再落到因子图优化和回环因子这两块最常碰到的工程场景最后给两份能直接跑的代码一份是纯消息传递一份是最小二乘。1. 贝叶斯网络与马尔可夫随机场卡在哪儿因子图补上了什么1.1 一张画不干净的图联合分布的结构问题先说个具体的麻烦。假设有三个随机变量 A、B、C它们的联合分布是 p(A,B,C)。如果想用贝叶斯网络表示你得挑一个方向去连边比如 A→C、B→C那么 p p(A)p(B)p(C|A,B)。这套写法在因果清晰的场景里很舒服但当条件概率 p(C|A,B) 本身没有一个简洁的参数化形式比如 C 是连续变量、A 和 B 共同决定它的分布这个条件概率表就会变得又大又难拟合。马尔可夫随机场走的是另一条路不规定方向只用团势函数描述局部相关性p (1/Z)∏ψ_c(X_c)。这个形式灵活多了但随之而来两个新问题。第一同一个联合分布势函数乘积的写法不唯一——我可以把常数 2 塞进任何一个势函数里图没变参数变了模型的物理意义被稀释。第二团的大小一旦上去势函数的维度是指数级膨胀的一个五元团、每个变量取十个值就是十万个参数根本没法估计。更深一层的难处在于表达能力的边界。贝叶斯网络对多个父节点共同作用于一个子节点这种结构刻画得不够自然因为边是单向的、每条边只承载一个成对条件概率马尔可夫随机场虽然去掉了方向却要求势函数在团上是良定义的一旦某个局部关系涉及三个以上变量的联合约束就只能硬塞进一个高维势函数里。换句话说两者都在用边表示关系这个框架里打转而现实里的约束未必能被拆成两两边的组合。1.2 因子图的解法把边升级成节点因子图的做法很直接既然一条边装不下复杂的局部关系那就不装了把它显式地变成一个节点。图里出现两种节点——圆形的变量节点和方形的因子节点边只在变量节点和因子节点之间连形成一张二部图。因子节点代表一个局部函数变量节点代表一个待求的量一条边表示这个变量是这个因子的入参。这么一改前面提到的几个痛点同时缓解了。局部关系不再受边的维度限制一个因子想管几个变量就管几个变量写成 f(x1,x2,x3,x7) 就行图里就是四条边。参数归属也清楚了每个因子独立持有一份自己的表或解析形式不存在常数往哪儿塞的歧义因为归一化统一交给全局的配分函数处理。拓扑修改也变得局部化加一个回环约束就是新增一个因子节点、连两条边其余部分原封不动。工程上最实际的收益是可组合性。我自己做组合导航的时候把里程计因子、预积分因子、GNSS 因子、零速修正因子各自写成一个独立模块每个模块只负责一件事——给定它连接的那几个变量的当前值算出一个残差和一份雅可比。想换传感器就是换一个因子模块图的结构变几条边求解器那边几乎不用动。这种接口约定清楚、彼此不耦合的写法用贝叶斯网络硬顶也能做但会绕很多弯路。1.3 二部图约束带来的三条基本规则落到实现层面因子图有三条必须守住的规则违反了图就退化或者爆炸。第一条边只能连接变量节点和因子节点变量之间、因子之间不允许有边。这条看似只是形式要求实际上保证了消息传递的双向交替消息要么从变量流向因子要么从因子流向变量不会出现同类型节点之间的消息避免了递归定义上的歧义。第二条一个因子节点连接的所有变量构成这个因子的作用域。因子节点的取值只依赖于它的作用域内的变量图里没连边的变量在数学上就是这个因子的自变量之外的东西可以放心地在对它的求和里当常数处理。这条是后面消息传递能化简的前提。第三条同一个变量可以被任意多个因子共享共享它的因子越多这个变量的信息就越丰富。这个特性在 SLAM 里体现得特别明显一个位姿变量通常同时连着前一个里程计因子、后一个里程计因子、若干个回环因子、可能还有 GNSS 因子它在解里的置信度就是这些因子共同作用的结果。反过来说如果一个变量的度数只有一那它在图上就是个叶子信息基本由那唯一的因子决定优化的时候不会对整体产生牵引作用。2. 联合概率怎么拆成因子连乘因子分解的数学骨架2.1 全局函数与配分函数各自的角色因子图的数学定义可以一句话说清给定一组变量 X {x1, ..., xn}一个全局函数 g(X) 如果能写成g(X) ∏_a f_a(X_a)其中 X_a 是 X 的一个子集那么就存在一张因子图与之对应因子节点对应各个 f_a变量节点对应各个 x_i边由x_i 是否属于 X_a决定。注意这里我写的是 g(X) 而不是 p(X)这个细节很重要。当我们谈论因子图在概率推断里的用法时通常关心的是未归一化的联合分布。真正要算边缘概率或者最大后验的时候得除以一个配分函数 Z Σ_X ∏_a f_a(X_a)。Z 这东西是个大麻烦它的计算复杂度随变量个数指数增长除了树结构和少数特殊图一般算不出来。因子图的价值恰恰在于很多时候我们根本不需要算 Z——边缘概率可以差一个常数比例因子的形式给出最大后验更是在比较不同配置的分值时Z 直接约掉了。所以在工程里我习惯把因子理解成打分函数而不是概率密度。因子值越大说明这组变量取值越符合这个因子的约束多个因子连乘就是多个约束同时投票。至于这个乘积是不是严格等于某个概率密度只要所有因子共享同一套变量定义域归一化与否不影响最终决策就不去纠结了。这个心态调整能省掉大量的理论包袱。2.2 三种因子粒度选错一个整条链路都别扭因子粒度指的是单个因子管多少个变量。我见过不少项目在这一点上反复返工所以专门拎出来说。因子粒度典型形式优点代价一元因子f(x_i)计算量最小可当先验或正则项表达能力弱只能约束单个变量成对因子f(x_i, x_j)表格规模小稀疏结构好三变量以上的联合约束要拆成多个团因子f(X_c), |X_c|≥3精确表达复杂约束表格维度高消息传递时求和开销大一元因子最常见的用法是先验和正则。比如在优化里给某个变量加一个别跑太远的惩罚就是一个一元因子在图像处理里给每个像素加一个偏置也是一元因子。成对因子是绝对主力。汉明距离、欧氏距离、平滑项、里程计约束几乎都是一对一的关系。它的表格规模是 O(K²)K 是变量取值数非常可控。团因子要慎用。除非确实存在不可拆解的多元约束否则我倾向于把它拆成若干个成对因子。举个例子三个变量要满足两两距离之和为定值这种约束硬写成一个三元因子表格是 O(K³)拆成三个成对因子加一个全局常数虽然数学上不完全等价但在大多数优化场景里效果差不多速度却快一个量级。取舍标准很朴素能拆就拆拆完精度掉得不明显就不回头。2.3 换个名字还是同一张图HMM、CRF、LDPC 都是因子图有个认知转变挺关键很多我们熟悉的结构本质上就是因子图的特例。隐马尔可夫模型的状态转移和观测摆成因子图就是一条链状态变量 x1, x2, ..., xT 排成一列每对相邻状态之间挂一个转移因子 f(x_t, x_{t1})每个状态下面挂一个观测因子 g(x_t, y_t)。前向-后向算法呢其实就是这条链状因子图上的和积算法前向消息和后向消息分别对应两个方向的递推。当年学 HMM 时死记硬背的那几个递推公式换成消息传递视角看就是变量到因子连乘、因子到变量求和两个操作的机械重复。条件随机场更直接。CRF 的定义本身就是 p(y|x) ∝ exp(Σ_c λ_c f_c(y_c, x))那个指数里的求和展开就是若干因子连乘势函数是 exp(λ_c f_c)。所谓的特征函数换个说法就是因子。低密度奇偶校验码LDPC也是。校验矩阵的每一行对应一个校验因子每一列对应一个比特变量因子图上的置信度传播就是经典的置信传播译码算法。我第一次把 LDPC 的 Tanner 图和因子图摆在一起看的时候发现它们完全同构只是学术界在两套术语体系里各说各话。认清楚这一点之后学新算法的成本会明显下降。看到一个新方法先问一句它在因子图上是什么形状链、树、还是有环的一般图消息怎么定义的基本上就能定位到已知的知识框架里。3. 和积算法两类消息怎么在树上把边缘概率算出来3.1 消息的两条流向与递归定义和积算法Sum-Product是因子图上最基础的推断方法目标是在给定全局函数 g(X) ∏_a f_a(X_a) 的前提下算出某个变量 x 的边缘函数。它维护两类消息。第一类是变量节点到因子节点的消息记作 μ_{x→f}(x)μ_{x→f}(x) ∏_{h ∈ ne(x) \ {f}} μ_{h→x}(x)含义是变量 x 把自己从除了 f 之外的所有邻居因子那里收到的消息乘起来打包发给 f。注意是除了 f避免把 f 自己发回来的消息又发回去那样就自环了。第二类是因子节点到变量节点的消息记作 μ_{f→x}(x)μ_{f→x}(x) Σ_{X_f \ {x}} f(X_f) ∏_{y ∈ ne(f) \ {x}} μ_{y→f}(y)这个稍微绕一点。因子 f 先把收到的所有变量消息同样要排除 x 自己发来的那条乘进自己的函数值里然后对作用域内除 x 外的所有变量求和剩下的就是关于 x 的函数。求和这一步是关键——它就是边缘化在局部尺度上的执行。等所有消息收敛或者传完树结构上有环图不收敛树上两遍传完就精确任意变量的边缘就可以拼出来p(x) ∝ ∏_{f ∈ ne(x)} μ_{f→x}(x)把所有指向它的因子消息乘起来归一化完事。3.2 手推一条三变量链看清每一步在干什么光看公式容易浮在表面我们手动走一遍最小例子。三个二值变量 x1, x2, x3取值都是 {0,1}。三个因子先验因子 f_1(x1)链因子 f_a(x1,x2)链因子 f_b(x2,x3)。全局函数是g(x1,x2,x3) f_1(x1) · f_a(x1,x2) · f_b(x2,x3)注意这里没有 x3 的先验所以 x3 的信息全靠 f_b 传过来。第一步算 f_1 到 x1 的消息。f_1 只连 x1作用域里没有别的变量可求和所以 μ_{f_1→x1}(x1) f_1(x1)直接就是因子本身。第二步算 x1 到 f_a 的消息。x1 的邻居只有 f_1 和 f_a排除 f_a 之后只剩 f_1所以 μ_{x1→f_a}(x1) μ_{f_1→x1}(x1) f_1(x1)。第三步算 f_a 到 x2 的消息。这是第一个真正需要求和的步骤μ_{f_a→x2}(x2) Σ_{x1} f_a(x1,x2) · μ_{x1→f_a}(x1) Σ_{x1} f_a(x1,x2) · f_1(x1)对 x1 的两个取值分别累加得到一个长度为 2 的向量。第四步x2 的邻居是 f_a 和 f_b所以 μ_{x2→f_b}(x2) μ_{f_a→x2}(x2)就是上一步的结果。第五步f_b 到 x3 的消息μ_{f_b→x3}(x3) Σ_{x2} f_b(x2,x3) · μ_{x2→f_b}(x2)。这样 x3 的边缘就拿到了等于 μ_{f_b→x3}(x3) 归一化。但故事没完。如果我还想知道 x1 的边缘光有第一步的结果不够因为 f_a 还要往回发消息。继续第六步f_b 到 x2 的反向消息μ_{f_b→x2}(x2) Σ_{x3} f_b(x2,x3)。这里 x3 没有其他邻居所以只用 f_b 自己。第七步f_a 到 x1 的反向消息μ_{f_a→x1}(x1) Σ_{x2} f_a(x1,x2) · μ_{f_b→x2}(x2)。最后 x1 的边缘 p(x1) ∝ f_1(x1) · μ_{f_a→x1}(x1)。整个流程里前五步是从左往右扫一遍后两步是从右往左扫一遍。这就是两遍扫描、树上精确的具体体现。链越长扫一遍的次数不变只是每步的向量长度不变总计算量是线性的。我第一次手推完这条链才真正理解为什么 HMM 的前向-后向算法是 O(T) 而不是 O(T·K²) 乘以更大的常数——因为每一步只做了局部求和。3.3 从和积到最大积求边缘还是求最优和积算法回答的是每个变量单独看起来分布如何但很多时候我们真正想知道的是什么组合让全局函数最大也就是最大后验估计。这时候把求和换成取最大就得到最大积算法Max-Productμ_{f→x}(x) max_{X_f \ {x}} [ f(X_f) ∏_{y ∈ ne(f) \ {x}} μ_{y→f}(y) ]变量到因子的消息形式不变还是连乘。收敛之后每个变量取使 ∏_f μ_{f→x}(x) 最大的那个值。在树结构上这个结果就是全局最优解。这里有个坑我踩过最大积算法直接对每个变量独立取最大在一般有环图上不保证得到的是全局最优配置甚至连局部最优都不保证。原因是有环图上消息会循环取最大操作不满足某些必要的代数性质。工程上常见的补救办法是跑最大积拿到一个初始解再用别的局部搜索去修补或者干脆换成基于线性规划的松紧方法。如果图本身就是链或者树那放心用没问题。另一个实践细节是数值下溢。因子连乘几十上百次之后浮点数直接归零消息全变成 0结果算出来就是 NaN。解决办法就一个转到对数域。连乘变求和求和变 log-sum-exp取最大直接变取最大对数单调不影响 argmax。log-sum-exp 的稳定写法是log(Σ exp(a_i)) max_i(a_i) log(Σ exp(a_i - max_i(a_i)))先减最大值再指数避免溢出。这个技巧看起来是纯粹的数值工程实际上决定了你的实现能处理多大规模的图。3.4 有环图上的置信度传播近似但好用现实中的因子图大多有环。SLAM 的位姿图因为有回环几乎必然有环图像的马尔可夫随机场因为二维网格上到处是圈也是有环的。有环图上跑和积算法学名叫环路置信度传播Loopy BP。它没有理论上的精确性保证甚至不保证收敛但实践中表现往往出人意料地好。我自己的经验是只要环不太短、消息更新的阻尼设置得当Loopy BP 在不少问题上能给出非常接近精确解的结果而且比采样方法快得多。让它跑得稳的几个手段一是加阻尼新消息不用完全替换旧消息而是按 μ_new (1-α)·μ_old α·μ_calc 的形式混合α 取 0.3 到 0.5 之间二是采用消息调度而不是同步更新一次只更新一部分消息剩下的用较新的值收敛通常更快三是设一个最大迭代次数和消息变化量的阈值防止无限循环。这些都不是理论上的必需但都是工程上的必需。4. 因子图优化后端求解器为什么集体倒向它4.1 从概率推断到非线性最小二乘的等价变形前面讲的都是离散变量的消息传递。因子图真正的出圈靠的是连续变量下的另一条路线——因子图优化。设定很朴素变量 X 是连续量位姿、位置、速度、偏置每个观测因子都假设是高斯的形如f_i(X_i) exp( -½ · ‖h_i(X_i) - z_i‖²_{Σ_i} )其中 h_i 是观测模型z_i 是实际观测Σ_i 是噪声协方差。全局后验就是所有因子连乘。取负对数连乘变成求和指数变成二次型于是max_X ∏_i f_i(X_i) 等价于 min_X Σ_i ‖h_i(X_i) - z_i‖²_{Σ_i}一个概率推断问题就这么变成了非线性最小二乘。这个等价关系看着平淡但它是整个现代 SLAM 后端的基石所有关于贝叶斯、后验、边缘概率的讨论最后都落到解一个稀疏最小二乘上而稀疏最小二乘有几十年积累的成熟解法。我个人觉得这个转化的最大价值在于工程可控性。概率推断里的很多操作边缘化、条件化在数学上抽象在代码里不好写换成最小二乘之后边缘化就是舒尔补条件化就是固定变量每一次操作都能在矩阵层面找到对应也能用数值线性代数的工具去诊断。4.2 高斯假设下信息矩阵为什么天然稀疏把残差在当前估计点线性化r_i ≈ r_i(x_0) J_i·δ代进目标函数整理成关于增量 δ 的二次型得到δ* argmin (½ δ^T H δ b^T δ) H Σ_i J_i^T Σ_i^{-1} J_i b Σ_i J_i^T Σ_i^{-1} r_iH 就是信息矩阵。它的稀疏性来自一个很朴素的事实J_i 只在第 i 个因子连接的变量对应的列上非零所以 J_i^T Σ_i^{-1} J_i 只会在这些变量两两对应的块位置上产生非零元素。一个里程计因子连着 x_i 和 x_{i1}那么它只贡献 H 的 (i,i)、(i,i1)、(i1,i)、(i1,i1) 四个块。图里没有边的变量对H 里对应的块就是零。这个性质有多重要一个一万个位姿的位姿图H 的大小是 6万×6万如果当成稠密矩阵处理光是存储就要 2.9 GB求逆更是天文数字。但因为它稀疏非零块的数量级跟边的数量差不多可能只有几十万个存储和求解都变得可行。稀疏性是因子图能处理大规模问题的根本原因不是优化算法本身有多神。也正因为这样变量的编号顺序就成了一个实打实的工程问题。变量编号决定了 H 的非零块分布进而决定了消元时的填充fill-in量。消元顺序选得差原本稀疏的矩阵会在消元过程中被填出大量非零元素性能直接掉一个数量级。工业界常用的启发式是 COLAMD列近似最小度排序它能显著减少填充。我做过一个对比实验同一个两万条边的位姿图自然顺序编号下求解耗时约 4.2 秒换成 COLAMD 排序后降到 0.6 秒左右差了将近七倍。4.3 增量求解与舒尔补边缘化在干什么实际系统里图不是一次性给全的而是随着新观测不断长大。每一帧都重新解一遍全图代价无法接受。这就引出增量式求解。增量式求解的核心思想是新来的观测只影响图的一小部分已经解好的部分不要推倒重来。代表性方法是 iSAM 系列它维护一个叫贝叶斯树的结构把变量按消元顺序组织成一棵树新观测进来时只在树上做局部更新。另一个绕不开的操作是舒尔补边缘化。假设把 H 按变量分块成H [ A B ] [ Bᵀ C ]想先把其中一组变量比如历史位姿消掉只需要计算S C - Bᵀ A⁻¹ B然后对 S 求解剩下的变量再回代。S 叫做舒尔补它是消息传递在连续高斯情形下的对应物——把一组变量的信息压缩后传给另一组变量。这里有个经典的工程陷阱舒尔补一旦算出来被消掉的变量的信息就被固化进了 S后续无法再修正。如果被消掉的变量后来又被新的回环因子连上就会产生不一致。实践中常见的做法有两种一是尽量推迟边缘化把可能被回环连上的变量留到最后再消二是如果确实要边缘化就把边缘化产生的先验因子单独记录后续需要时重新线性化。这两种做法各有代价选择取决于你的系统里回环出现的频率和变量规模。4.4 回环因子加入因子图的那一刻发生了什么回环检测的流程大概是当前帧和历史帧做外观匹配匹配上了估计出一个相对位姿作为回环约束然后把这个约束作为因子加进图里。听起来就是加一条边但实际发生的事情比这复杂。加回环之前位姿图是一条链信息矩阵是三对角的消元几乎不产生填充求解非常快。加回环之后图变成了带状加上若干条长程连接H 的稀疏结构被打破。更重要的是累积漂移在回环处被揭露了里程计因子告诉你从起点走到终点累计前进了 100 米回环因子却告诉你终点就在起点附近两者矛盾。这个矛盾会以残差的形式体现在优化里求解器要做的是把所有位姿一起调整让总残差最小。我拿一维情况算过一笔账。四个位置 x0 到 x3x0 固定为 0三段里程计测量都是 1 米一个回环观测说 x3 - x0 是 3.05 米。如果没有回环解就是 [0, 1, 2, 3]每段残差为零。加了回环之后设每段里程计的误差为 e0, e1, e2则 x3 3 e0 e1 e2回环残差是 e0e1e2-0.05。最小化 e0²e1²e2²(e0e1e2-0.05)²对每个 e_k 求偏导由对称性可知三段的误差应相等设为 t代入得 t 0.05 - 3t解得 t 0.0125。最终解是 [0, 1.0125, 2.025, 3.0375]漂移被均匀分摊到了每一段里程计上。这个例子很能说明问题回环不是把终点拉回去而是把所有相关变量的误差重新分配使总体代价最低。这也解释了为什么回环之后的位姿图整体形状变了——不是因为终点移动了而是因为每一个中间位姿都动了。4.5 回环误检怎么办鲁棒核与开关变量回环检测总有虚警。视觉场景里走廊、重复纹理、相似的墙面都可能导致错误的匹配产生一个错误但看起来很确定的回环因子。如果直接把这个因子丢进最小二乘由于它带着协方差权重会强行把整张图拽歪。解决思路从最小二乘那一侧入手把二次代价函数换成鲁棒核函数。标准的二次核是 ρ(r) r²/2对大的残差惩罚无限增长所以求解器会拼命迁就异常观测。换成 Huber 核ρ(r) r²/2当 |r| ≤ k ρ(r) k|r| - k²/2当 |r| k残差超过阈值 k 之后惩罚从二次变成线性增长异常观测的影响力被自动截断。其他常用的还有 Cauchy 核、Geman-McClure 核。参数 k 通常按照噪声标准差的数量级去设比如取 3σ 到 5σ 之间。更进一步的做法是引入开关变量。给每个回环因子额外挂一个连续的开关变量 s ∈ [0,1]因子写成 (s·r)² 的形式优化过程中 s 会被自动压低到接近 0对于错误回环或者保持在接近 1对于正确回环。代价是变量数增加了但鲁棒性提升明显。我在实际项目里的经验是鲁棒核负责软截断适用于误检率不高、且错误残差不会太大的情况开关变量负责硬开关适用于误检频繁、错误残差可能极大的情况。两者也可以叠加使用。但要注意鲁棒核不是万能的如果错误回环的比例超过三成任何鲁棒核都救不回来这时候应该是回头去改回环检测的阈值而不是在优化端打补丁。5. 手写最小可复现的因子图实现5.1 用邻接表描述二部图理论讲了这么多不写代码都是空的。下面这份实现不依赖任何图优化库只用 numpy目的是把消息传递的每个步骤落到可见的数组操作上。数据结构上我用两张邻接表分别记录变量连了哪些因子和因子连了哪些变量再加一个列表存每个因子的取值表。取值表的维度顺序跟该因子的变量顺序一致这一点很重要写错顺序会导致结果对不上。import numpy as np from collections import defaultdict class SumProductFG: def __init__(self, cardinalities): self.card list(cardinalities) # 每个变量的取值个数 self.n len(self.card) self.factors [] # 元素为 (变量id元组, 取值表ndarray) self.f2v {} # 因子id - 变量id列表 self.v2f defaultdict(list) # 变量id - 因子id列表 self.msg_f2v {} # (f, v) - ndarray self.msg_v2f {} # (v, f) - ndarray def add_factor(self, var_ids, table): fid len(self.factors) var_ids tuple(var_ids) table np.asarray(table, dtypefloat) assert table.shape tuple(self.card[v] for v in var_ids), 取值表维度不匹配 self.factors.append((var_ids, table)) self.f2v[fid] list(var_ids) for v in var_ids: self.v2f[v].append(fid) return fid形状断言这一行看着多余实际上救过我很多次。因子图里变量的顺序是隐式的一旦搞错程序不报错但结果会错得莫名其妙。写上断言至少能在构建阶段就发现问题。5.2 全量消息传递求解一个小型网络消息初始化为全 1。每一轮迭代先算所有因子到变量的消息再算所有变量到因子的消息然后检查消息的最大变化量低于阈值就停。def _factor_to_var(self, fid, v): var_ids, table self.factors[fid] idx_v var_ids.index(v) prod table.copy() for ax, y in enumerate(var_ids): if y v: continue m self.msg_v2f[(y, fid)] shape [1] * table.ndim shape[ax] self.card[y] prod prod * m.reshape(shape) axes_sum tuple(ax for ax in range(table.ndim) if ax ! idx_v) return prod.sum(axisaxes_sum) def _var_to_factor(self, v, fid): msgs [self.msg_f2v[(g, v)] for g in self.v2f[v] if g ! fid] out np.ones(self.card[v]) for m in msgs: out out * m return out def run(self, max_iter200, tol1e-9): for (fid, v) in [(f, v) for f, vs in self.f2v.items() for v in vs]: self.msg_f2v[(fid, v)] np.ones(self.card[v]) self.msg_v2f[(v, fid)] np.ones(self.card[v]) for it in range(max_iter): delta 0.0 new_f2v {} for fid, vs in self.f2v.items(): for v in vs: m self._factor_to_var(fid, v) s m.sum() if s 0: m m / s # 归一化防止数值爆炸 new_f2v[(fid, v)] m delta max(delta, np.abs(m - self.msg_f2v[(fid, v)]).max()) self.msg_f2v.update(new_f2v) for v, fs in self.v2f.items(): for fid in fs: m self._var_to_factor(v, fid) s m.sum() if s 0: m m / s self.msg_v2f[(v, fid)] m delta max(delta, np.abs(m - self.msg_v2f[(v, fid)]).max()) if delta tol: return it 1 return max_iter def marginal(self, v): out np.ones(self.card[v]) for fid in self.v2f[v]: out out * self.msg_f2v[(fid, v)] return out / out.sum()跑一个例子验证。三个二值变量构造链式结构f1 是先验表示 x1 偏向取 0比如表格 [3.0, 1.0]fa 是平滑因子让 x1 和 x2 倾向一致fb 也是平滑因子让 x2 和 x3 一致。fg SumProductFG([2, 2, 2]) fg.add_factor([0], [3.0, 1.0]) fg.add_factor([0, 1], [[2.0, 0.5], [0.5, 2.0]]) fg.add_factor([1, 2], [[2.0, 0.5], [0.5, 2.0]]) iters fg.run() print(迭代次数:, iters) for v in range(3): print(fx{v} 边缘分布:, np.round(fg.marginal(v), 4))因为 x1 有明显的先验偏向 0平滑因子会把这种倾向沿链传下去。跑出来 x1 的边缘应该在 0.9 以上偏向 0x2 稍微弱一点x3 再弱一点呈现信息沿链衰减的形态。如果结果不是这个趋势那多半是消息方向或者表格维度搞反了。5.3 同一份数据改用高斯-牛顿最小二乘离散场景用消息传递连续场景就得切换到最小二乘。这里给一个一维位姿图的完整实现用的是 4.4 节那个例子。import numpy as np def solve_pose_chain(edges, n_vars, fixed0, iters20, robustNone): x np.zeros(n_vars) for it in range(iters): H np.zeros((n_vars, n_vars)) b np.zeros(n_vars) for (i, j, d, sigma) in edges: r (x[j] - x[i]) - d w 1.0 / (sigma * sigma) if robust is not None: k robust if abs(r) k: w w * k / abs(r) # Huber 权重缩放 J np.zeros(n_vars) J[j] 1.0 J[i] - 1.0 H w * np.outer(J, J) b w * J * r # 固定参考变量消除规范自由度 H[fixed, :] 0.0 H[:, fixed] 0.0 H[fixed, fixed] 1.0 b[fixed] 0.0 dx np.linalg.solve(H, -b) x x dx if np.linalg.norm(dx) 1e-12: break return x, it 1 edges [(0, 1, 1.0, 0.1), (1, 2, 1.0, 0.1), (2, 3, 1.0, 0.1), (0, 3, 3.05, 0.1)] x_opt, n_it solve_pose_chain(edges, 4) print(迭代次数:, n_it) print(最优解:, np.round(x_opt, 6))跑出来应该非常接近 [0, 1.0125, 2.025, 3.0375]跟前面手算的结果对上。这个代对上了说明你的最小二乘框架搭对了。顺便说一句固定第一个变量的那三行赋值不能省。位姿图有一个整体平移的规范自由度——所有位姿同时加一个常数所有残差不变。不固定的话信息矩阵奇异求解直接报错。固定之后的矩阵是正定的可以放心用 Cholesky 分解求解。5.4 两种方法的结果互相校验我养成了一个习惯写完消息传递实现一定拿最小二乘的结果去对照。对于高斯因子图和积算法在连续情形下是高斯消息传递和最小二乘的最优解应该是同一个点只是前者给出的是后验分布的均值和协方差后者给出的是均值。如果两者对不上八成是某一侧的实现有问题。这个交叉验证的价值在于两种实现的代码路径完全不同一个在概率域里乘和加一个在矩阵域里求导和分解。同时出错的概率很低。我在调一个多传感器融合的 bug 时就是靠这个对照定位到问题——消息传递那边结果正常最小二乘那边偏了最后发现是某个观测因子的协方差矩阵写成了标准差矩阵没平方。这个错误在单一实现里很难发现因为数值都不会崩只是精度悄悄变差。6. 我在实际项目里踩过的六个坑6.1 变量编号顺序影响的不只是速度前面提过 COLAMD 排序这里补充一个更隐蔽的坑变量编号不仅影响求解速度还影响数值稳定性。消元过程中填充量越大累积的浮点误差越多极端情况下会导致信息矩阵条件数急剧恶化。我遇到过一次一个两千个位姿的图用自然顺序编号时求解时间大约 1.8 秒结果也基本正确换成另一套看似更自然的分组编号后求解时间涨到 6 秒而且协方差对角线出现了负值——这在信息矩阵里是绝对不该出现的。换回 COLAMD 排序后问题消失。所以顺序这件事别凭直觉交给成熟的排序算法。6.2 单位与量纲混用是隐形杀手角度用弧度、位置用米、时间用秒这是默认约定。但实际项目里最容易出问题的不是这几个主变量而是协方差。雅可比矩阵的量纲取决于残差的量纲。如果残差是米J 对位置的偏导就是无量纲如果残差是像素J 的单位就是像素每米。把不同量纲的观测混在同一个信息矩阵里做加法前提是每一项都已经被自己的协方差正确加权了。如果某个协方差忘了平方或者单位写成了毫米那这一项的权重就会差六个数量级求解结果完全被它主导。我的做法是在因子定义里强制写上单位注释并且在单元测试里检查每个因子的残差量级是否符合预期。多花十分钟能省掉半天的排查。6.3 收敛判据写得太松会假收敛最小二乘迭代的停止条件常见的有三种增量范数小于阈值、残差变化小于阈值、达到最大迭代次数。用一个是不够的。我见过不止一次这样的情况增量范数很小看起来收敛了但残差其实很大。原因是步长太小每次只挪一点点增量自然就小。这种情况在高斯-牛顿遇到强非线性时很常见本质是雅可比不准确导致步长被压缩。稳妥的写法是同时检查增量范数和残差范数两个都满足才退出。如果迭代次数到了上限还没满足应该输出一个警告而不是静默返回当前结果。我现在的习惯是让求解函数返回一个状态码调用方必须检查避免悄悄错了。6.4 残差可视化比看日志管用十倍调优化问题看日志里的数值变化效率很低。更好的办法是把每个因子的残差画出来。我通常做两张图一张是按因子索引画的残差绝对值一张是残差在地图上的空间分布。第一张图能一眼看出有没有离群因子——正常残差应该大致朝着噪声标准差的量级聚集如果某个因子的残差比邻居大两个数量级那它八成有问题。第二张图能看出系统性的偏差模式比如某一段区域整体残差偏大那多半是那段区域的观测模型或者标定参数有问题。对于 SLAM 这类问题还有一个很有用的指标是卡方检验。如果噪声模型假设正确归一化残差平方和除以自由度应该接近 1。远大于 1 说明模型过于乐观协方差给小了远小于 1 说明过于保守。这个比值我在每次调参之后都会看一眼作为噪声参数是否合理的一个粗判据。6.5 别把强先验当硬约束用因子图里没有真正的硬约束除非做变量消元那是另一回事。所有先验都是软约束只是权重不同。想固定一个变量你得给它挂一个协方差极小的一元因子或者干脆在求解时把对应的行列抠掉。我一开始图省事用一个大权重的一元因子去固定起点比如给位置加一个均值 0、标准差 1e-9 的因子。结果信息矩阵的条件数直接爆炸到 1e18 以上求解器给出了警告。后来改成在组装矩阵之后直接把那一行一列替换成单位行单位列问题解决。这个坑的教训是数值上的很大和数学上的无穷是两回事。想表达硬约束就用结构上的方式消元、固定列别用数值上的极端权重去模拟。6.6 鲁棒核的阈值不能拍脑袋Huber 核的阈值 k 决定了多大的残差开始被降权。设得太小正常观测也会被当异常处理模型欠拟合设得太大等于没加鲁棒核。我见过有人直接写死 k1理由是残差超过 1 米肯定不对。问题是他的系统里残差的单位是像素1 像素的阈值把几乎所有观测都降权了最后解出来的轨迹像没优化过一样。换个系统残差单位是毫米1 毫米的阈值又等于没生效。合理的做法是让 k 跟噪声模型挂钩。我一般取 k 3σσ 是该类型观测的典型噪声标准差。更进一步可以对不同类型的因子设不同的 k因为它们的残差量级本来就不同。如果系统里有明确的卡方检验机制也可以反过来用卡方分布的分位数来定 k这样更有理论依据。再分享一个调试细节加完鲁棒核之后一定要统计被降权的因子占比。如果占比超过 20%说明要么阈值设错了要么回环检测本身有问题这时候应该回去查数据而不是继续调参数。我自己就吃过这个亏盯着优化结果调了半天阈值最后发现问题出在回环检测的时间戳对齐上跟鲁棒核一点关系都没有。 因子图与和积算法：从概率图模型到SLAM后端优化
做多传感器融合定位那阵子我最大的困惑是手里攥着里程计、视觉重投影、GPS、回环检测四五路观测每路的噪声特性都不一样画成贝叶斯网络之后边交叉得像一团毛线想加一个约束就得改一次拓扑改完还得重新推导边缘概率。后来换成因子图Factor Graph的写法图一下子干净了变量是变量观测是因子两者分开摆成二部图加一个回环就是加一个节点、连几条边的事代码里也就多一行。概率图模型里因子图算是把表示和推断这两件事拆得最彻底的一种结构——它不追求每个数字都有概率含义这点后面细讲只关心一个全局函数能不能拆成若干局部因子连乘然后能不能沿着图上的边把消息传起来。这篇东西写给两类人一类是学过贝叶斯网络、马尔可夫随机场但一到因子图就被二部图和消息传递公式劝退的另一类是做视觉里程计、组合导航、结构化预测早就在用现成的优化库但说不清楚后端到底在解什么问题的。我会从为什么需要因子图讲起把和积算法的推导一步步摊开再落到因子图优化和回环因子这两块最常碰到的工程场景最后给两份能直接跑的代码一份是纯消息传递一份是最小二乘。1. 贝叶斯网络与马尔可夫随机场卡在哪儿因子图补上了什么1.1 一张画不干净的图联合分布的结构问题先说个具体的麻烦。假设有三个随机变量 A、B、C它们的联合分布是 p(A,B,C)。如果想用贝叶斯网络表示你得挑一个方向去连边比如 A→C、B→C那么 p p(A)p(B)p(C|A,B)。这套写法在因果清晰的场景里很舒服但当条件概率 p(C|A,B) 本身没有一个简洁的参数化形式比如 C 是连续变量、A 和 B 共同决定它的分布这个条件概率表就会变得又大又难拟合。马尔可夫随机场走的是另一条路不规定方向只用团势函数描述局部相关性p (1/Z)∏ψ_c(X_c)。这个形式灵活多了但随之而来两个新问题。第一同一个联合分布势函数乘积的写法不唯一——我可以把常数 2 塞进任何一个势函数里图没变参数变了模型的物理意义被稀释。第二团的大小一旦上去势函数的维度是指数级膨胀的一个五元团、每个变量取十个值就是十万个参数根本没法估计。更深一层的难处在于表达能力的边界。贝叶斯网络对多个父节点共同作用于一个子节点这种结构刻画得不够自然因为边是单向的、每条边只承载一个成对条件概率马尔可夫随机场虽然去掉了方向却要求势函数在团上是良定义的一旦某个局部关系涉及三个以上变量的联合约束就只能硬塞进一个高维势函数里。换句话说两者都在用边表示关系这个框架里打转而现实里的约束未必能被拆成两两边的组合。1.2 因子图的解法把边升级成节点因子图的做法很直接既然一条边装不下复杂的局部关系那就不装了把它显式地变成一个节点。图里出现两种节点——圆形的变量节点和方形的因子节点边只在变量节点和因子节点之间连形成一张二部图。因子节点代表一个局部函数变量节点代表一个待求的量一条边表示这个变量是这个因子的入参。这么一改前面提到的几个痛点同时缓解了。局部关系不再受边的维度限制一个因子想管几个变量就管几个变量写成 f(x1,x2,x3,x7) 就行图里就是四条边。参数归属也清楚了每个因子独立持有一份自己的表或解析形式不存在常数往哪儿塞的歧义因为归一化统一交给全局的配分函数处理。拓扑修改也变得局部化加一个回环约束就是新增一个因子节点、连两条边其余部分原封不动。工程上最实际的收益是可组合性。我自己做组合导航的时候把里程计因子、预积分因子、GNSS 因子、零速修正因子各自写成一个独立模块每个模块只负责一件事——给定它连接的那几个变量的当前值算出一个残差和一份雅可比。想换传感器就是换一个因子模块图的结构变几条边求解器那边几乎不用动。这种接口约定清楚、彼此不耦合的写法用贝叶斯网络硬顶也能做但会绕很多弯路。1.3 二部图约束带来的三条基本规则落到实现层面因子图有三条必须守住的规则违反了图就退化或者爆炸。第一条边只能连接变量节点和因子节点变量之间、因子之间不允许有边。这条看似只是形式要求实际上保证了消息传递的双向交替消息要么从变量流向因子要么从因子流向变量不会出现同类型节点之间的消息避免了递归定义上的歧义。第二条一个因子节点连接的所有变量构成这个因子的作用域。因子节点的取值只依赖于它的作用域内的变量图里没连边的变量在数学上就是这个因子的自变量之外的东西可以放心地在对它的求和里当常数处理。这条是后面消息传递能化简的前提。第三条同一个变量可以被任意多个因子共享共享它的因子越多这个变量的信息就越丰富。这个特性在 SLAM 里体现得特别明显一个位姿变量通常同时连着前一个里程计因子、后一个里程计因子、若干个回环因子、可能还有 GNSS 因子它在解里的置信度就是这些因子共同作用的结果。反过来说如果一个变量的度数只有一那它在图上就是个叶子信息基本由那唯一的因子决定优化的时候不会对整体产生牵引作用。2. 联合概率怎么拆成因子连乘因子分解的数学骨架2.1 全局函数与配分函数各自的角色因子图的数学定义可以一句话说清给定一组变量 X {x1, ..., xn}一个全局函数 g(X) 如果能写成g(X) ∏_a f_a(X_a)其中 X_a 是 X 的一个子集那么就存在一张因子图与之对应因子节点对应各个 f_a变量节点对应各个 x_i边由x_i 是否属于 X_a决定。注意这里我写的是 g(X) 而不是 p(X)这个细节很重要。当我们谈论因子图在概率推断里的用法时通常关心的是未归一化的联合分布。真正要算边缘概率或者最大后验的时候得除以一个配分函数 Z Σ_X ∏_a f_a(X_a)。Z 这东西是个大麻烦它的计算复杂度随变量个数指数增长除了树结构和少数特殊图一般算不出来。因子图的价值恰恰在于很多时候我们根本不需要算 Z——边缘概率可以差一个常数比例因子的形式给出最大后验更是在比较不同配置的分值时Z 直接约掉了。所以在工程里我习惯把因子理解成打分函数而不是概率密度。因子值越大说明这组变量取值越符合这个因子的约束多个因子连乘就是多个约束同时投票。至于这个乘积是不是严格等于某个概率密度只要所有因子共享同一套变量定义域归一化与否不影响最终决策就不去纠结了。这个心态调整能省掉大量的理论包袱。2.2 三种因子粒度选错一个整条链路都别扭因子粒度指的是单个因子管多少个变量。我见过不少项目在这一点上反复返工所以专门拎出来说。因子粒度典型形式优点代价一元因子f(x_i)计算量最小可当先验或正则项表达能力弱只能约束单个变量成对因子f(x_i, x_j)表格规模小稀疏结构好三变量以上的联合约束要拆成多个团因子f(X_c), |X_c|≥3精确表达复杂约束表格维度高消息传递时求和开销大一元因子最常见的用法是先验和正则。比如在优化里给某个变量加一个别跑太远的惩罚就是一个一元因子在图像处理里给每个像素加一个偏置也是一元因子。成对因子是绝对主力。汉明距离、欧氏距离、平滑项、里程计约束几乎都是一对一的关系。它的表格规模是 O(K²)K 是变量取值数非常可控。团因子要慎用。除非确实存在不可拆解的多元约束否则我倾向于把它拆成若干个成对因子。举个例子三个变量要满足两两距离之和为定值这种约束硬写成一个三元因子表格是 O(K³)拆成三个成对因子加一个全局常数虽然数学上不完全等价但在大多数优化场景里效果差不多速度却快一个量级。取舍标准很朴素能拆就拆拆完精度掉得不明显就不回头。2.3 换个名字还是同一张图HMM、CRF、LDPC 都是因子图有个认知转变挺关键很多我们熟悉的结构本质上就是因子图的特例。隐马尔可夫模型的状态转移和观测摆成因子图就是一条链状态变量 x1, x2, ..., xT 排成一列每对相邻状态之间挂一个转移因子 f(x_t, x_{t1})每个状态下面挂一个观测因子 g(x_t, y_t)。前向-后向算法呢其实就是这条链状因子图上的和积算法前向消息和后向消息分别对应两个方向的递推。当年学 HMM 时死记硬背的那几个递推公式换成消息传递视角看就是变量到因子连乘、因子到变量求和两个操作的机械重复。条件随机场更直接。CRF 的定义本身就是 p(y|x) ∝ exp(Σ_c λ_c f_c(y_c, x))那个指数里的求和展开就是若干因子连乘势函数是 exp(λ_c f_c)。所谓的特征函数换个说法就是因子。低密度奇偶校验码LDPC也是。校验矩阵的每一行对应一个校验因子每一列对应一个比特变量因子图上的置信度传播就是经典的置信传播译码算法。我第一次把 LDPC 的 Tanner 图和因子图摆在一起看的时候发现它们完全同构只是学术界在两套术语体系里各说各话。认清楚这一点之后学新算法的成本会明显下降。看到一个新方法先问一句它在因子图上是什么形状链、树、还是有环的一般图消息怎么定义的基本上就能定位到已知的知识框架里。3. 和积算法两类消息怎么在树上把边缘概率算出来3.1 消息的两条流向与递归定义和积算法Sum-Product是因子图上最基础的推断方法目标是在给定全局函数 g(X) ∏_a f_a(X_a) 的前提下算出某个变量 x 的边缘函数。它维护两类消息。第一类是变量节点到因子节点的消息记作 μ_{x→f}(x)μ_{x→f}(x) ∏_{h ∈ ne(x) \ {f}} μ_{h→x}(x)含义是变量 x 把自己从除了 f 之外的所有邻居因子那里收到的消息乘起来打包发给 f。注意是除了 f避免把 f 自己发回来的消息又发回去那样就自环了。第二类是因子节点到变量节点的消息记作 μ_{f→x}(x)μ_{f→x}(x) Σ_{X_f \ {x}} f(X_f) ∏_{y ∈ ne(f) \ {x}} μ_{y→f}(y)这个稍微绕一点。因子 f 先把收到的所有变量消息同样要排除 x 自己发来的那条乘进自己的函数值里然后对作用域内除 x 外的所有变量求和剩下的就是关于 x 的函数。求和这一步是关键——它就是边缘化在局部尺度上的执行。等所有消息收敛或者传完树结构上有环图不收敛树上两遍传完就精确任意变量的边缘就可以拼出来p(x) ∝ ∏_{f ∈ ne(x)} μ_{f→x}(x)把所有指向它的因子消息乘起来归一化完事。3.2 手推一条三变量链看清每一步在干什么光看公式容易浮在表面我们手动走一遍最小例子。三个二值变量 x1, x2, x3取值都是 {0,1}。三个因子先验因子 f_1(x1)链因子 f_a(x1,x2)链因子 f_b(x2,x3)。全局函数是g(x1,x2,x3) f_1(x1) · f_a(x1,x2) · f_b(x2,x3)注意这里没有 x3 的先验所以 x3 的信息全靠 f_b 传过来。第一步算 f_1 到 x1 的消息。f_1 只连 x1作用域里没有别的变量可求和所以 μ_{f_1→x1}(x1) f_1(x1)直接就是因子本身。第二步算 x1 到 f_a 的消息。x1 的邻居只有 f_1 和 f_a排除 f_a 之后只剩 f_1所以 μ_{x1→f_a}(x1) μ_{f_1→x1}(x1) f_1(x1)。第三步算 f_a 到 x2 的消息。这是第一个真正需要求和的步骤μ_{f_a→x2}(x2) Σ_{x1} f_a(x1,x2) · μ_{x1→f_a}(x1) Σ_{x1} f_a(x1,x2) · f_1(x1)对 x1 的两个取值分别累加得到一个长度为 2 的向量。第四步x2 的邻居是 f_a 和 f_b所以 μ_{x2→f_b}(x2) μ_{f_a→x2}(x2)就是上一步的结果。第五步f_b 到 x3 的消息μ_{f_b→x3}(x3) Σ_{x2} f_b(x2,x3) · μ_{x2→f_b}(x2)。这样 x3 的边缘就拿到了等于 μ_{f_b→x3}(x3) 归一化。但故事没完。如果我还想知道 x1 的边缘光有第一步的结果不够因为 f_a 还要往回发消息。继续第六步f_b 到 x2 的反向消息μ_{f_b→x2}(x2) Σ_{x3} f_b(x2,x3)。这里 x3 没有其他邻居所以只用 f_b 自己。第七步f_a 到 x1 的反向消息μ_{f_a→x1}(x1) Σ_{x2} f_a(x1,x2) · μ_{f_b→x2}(x2)。最后 x1 的边缘 p(x1) ∝ f_1(x1) · μ_{f_a→x1}(x1)。整个流程里前五步是从左往右扫一遍后两步是从右往左扫一遍。这就是两遍扫描、树上精确的具体体现。链越长扫一遍的次数不变只是每步的向量长度不变总计算量是线性的。我第一次手推完这条链才真正理解为什么 HMM 的前向-后向算法是 O(T) 而不是 O(T·K²) 乘以更大的常数——因为每一步只做了局部求和。3.3 从和积到最大积求边缘还是求最优和积算法回答的是每个变量单独看起来分布如何但很多时候我们真正想知道的是什么组合让全局函数最大也就是最大后验估计。这时候把求和换成取最大就得到最大积算法Max-Productμ_{f→x}(x) max_{X_f \ {x}} [ f(X_f) ∏_{y ∈ ne(f) \ {x}} μ_{y→f}(y) ]变量到因子的消息形式不变还是连乘。收敛之后每个变量取使 ∏_f μ_{f→x}(x) 最大的那个值。在树结构上这个结果就是全局最优解。这里有个坑我踩过最大积算法直接对每个变量独立取最大在一般有环图上不保证得到的是全局最优配置甚至连局部最优都不保证。原因是有环图上消息会循环取最大操作不满足某些必要的代数性质。工程上常见的补救办法是跑最大积拿到一个初始解再用别的局部搜索去修补或者干脆换成基于线性规划的松紧方法。如果图本身就是链或者树那放心用没问题。另一个实践细节是数值下溢。因子连乘几十上百次之后浮点数直接归零消息全变成 0结果算出来就是 NaN。解决办法就一个转到对数域。连乘变求和求和变 log-sum-exp取最大直接变取最大对数单调不影响 argmax。log-sum-exp 的稳定写法是log(Σ exp(a_i)) max_i(a_i) log(Σ exp(a_i - max_i(a_i)))先减最大值再指数避免溢出。这个技巧看起来是纯粹的数值工程实际上决定了你的实现能处理多大规模的图。3.4 有环图上的置信度传播近似但好用现实中的因子图大多有环。SLAM 的位姿图因为有回环几乎必然有环图像的马尔可夫随机场因为二维网格上到处是圈也是有环的。有环图上跑和积算法学名叫环路置信度传播Loopy BP。它没有理论上的精确性保证甚至不保证收敛但实践中表现往往出人意料地好。我自己的经验是只要环不太短、消息更新的阻尼设置得当Loopy BP 在不少问题上能给出非常接近精确解的结果而且比采样方法快得多。让它跑得稳的几个手段一是加阻尼新消息不用完全替换旧消息而是按 μ_new (1-α)·μ_old α·μ_calc 的形式混合α 取 0.3 到 0.5 之间二是采用消息调度而不是同步更新一次只更新一部分消息剩下的用较新的值收敛通常更快三是设一个最大迭代次数和消息变化量的阈值防止无限循环。这些都不是理论上的必需但都是工程上的必需。4. 因子图优化后端求解器为什么集体倒向它4.1 从概率推断到非线性最小二乘的等价变形前面讲的都是离散变量的消息传递。因子图真正的出圈靠的是连续变量下的另一条路线——因子图优化。设定很朴素变量 X 是连续量位姿、位置、速度、偏置每个观测因子都假设是高斯的形如f_i(X_i) exp( -½ · ‖h_i(X_i) - z_i‖²_{Σ_i} )其中 h_i 是观测模型z_i 是实际观测Σ_i 是噪声协方差。全局后验就是所有因子连乘。取负对数连乘变成求和指数变成二次型于是max_X ∏_i f_i(X_i) 等价于 min_X Σ_i ‖h_i(X_i) - z_i‖²_{Σ_i}一个概率推断问题就这么变成了非线性最小二乘。这个等价关系看着平淡但它是整个现代 SLAM 后端的基石所有关于贝叶斯、后验、边缘概率的讨论最后都落到解一个稀疏最小二乘上而稀疏最小二乘有几十年积累的成熟解法。我个人觉得这个转化的最大价值在于工程可控性。概率推断里的很多操作边缘化、条件化在数学上抽象在代码里不好写换成最小二乘之后边缘化就是舒尔补条件化就是固定变量每一次操作都能在矩阵层面找到对应也能用数值线性代数的工具去诊断。4.2 高斯假设下信息矩阵为什么天然稀疏把残差在当前估计点线性化r_i ≈ r_i(x_0) J_i·δ代进目标函数整理成关于增量 δ 的二次型得到δ* argmin (½ δ^T H δ b^T δ) H Σ_i J_i^T Σ_i^{-1} J_i b Σ_i J_i^T Σ_i^{-1} r_iH 就是信息矩阵。它的稀疏性来自一个很朴素的事实J_i 只在第 i 个因子连接的变量对应的列上非零所以 J_i^T Σ_i^{-1} J_i 只会在这些变量两两对应的块位置上产生非零元素。一个里程计因子连着 x_i 和 x_{i1}那么它只贡献 H 的 (i,i)、(i,i1)、(i1,i)、(i1,i1) 四个块。图里没有边的变量对H 里对应的块就是零。这个性质有多重要一个一万个位姿的位姿图H 的大小是 6万×6万如果当成稠密矩阵处理光是存储就要 2.9 GB求逆更是天文数字。但因为它稀疏非零块的数量级跟边的数量差不多可能只有几十万个存储和求解都变得可行。稀疏性是因子图能处理大规模问题的根本原因不是优化算法本身有多神。也正因为这样变量的编号顺序就成了一个实打实的工程问题。变量编号决定了 H 的非零块分布进而决定了消元时的填充fill-in量。消元顺序选得差原本稀疏的矩阵会在消元过程中被填出大量非零元素性能直接掉一个数量级。工业界常用的启发式是 COLAMD列近似最小度排序它能显著减少填充。我做过一个对比实验同一个两万条边的位姿图自然顺序编号下求解耗时约 4.2 秒换成 COLAMD 排序后降到 0.6 秒左右差了将近七倍。4.3 增量求解与舒尔补边缘化在干什么实际系统里图不是一次性给全的而是随着新观测不断长大。每一帧都重新解一遍全图代价无法接受。这就引出增量式求解。增量式求解的核心思想是新来的观测只影响图的一小部分已经解好的部分不要推倒重来。代表性方法是 iSAM 系列它维护一个叫贝叶斯树的结构把变量按消元顺序组织成一棵树新观测进来时只在树上做局部更新。另一个绕不开的操作是舒尔补边缘化。假设把 H 按变量分块成H [ A B ] [ Bᵀ C ]想先把其中一组变量比如历史位姿消掉只需要计算S C - Bᵀ A⁻¹ B然后对 S 求解剩下的变量再回代。S 叫做舒尔补它是消息传递在连续高斯情形下的对应物——把一组变量的信息压缩后传给另一组变量。这里有个经典的工程陷阱舒尔补一旦算出来被消掉的变量的信息就被固化进了 S后续无法再修正。如果被消掉的变量后来又被新的回环因子连上就会产生不一致。实践中常见的做法有两种一是尽量推迟边缘化把可能被回环连上的变量留到最后再消二是如果确实要边缘化就把边缘化产生的先验因子单独记录后续需要时重新线性化。这两种做法各有代价选择取决于你的系统里回环出现的频率和变量规模。4.4 回环因子加入因子图的那一刻发生了什么回环检测的流程大概是当前帧和历史帧做外观匹配匹配上了估计出一个相对位姿作为回环约束然后把这个约束作为因子加进图里。听起来就是加一条边但实际发生的事情比这复杂。加回环之前位姿图是一条链信息矩阵是三对角的消元几乎不产生填充求解非常快。加回环之后图变成了带状加上若干条长程连接H 的稀疏结构被打破。更重要的是累积漂移在回环处被揭露了里程计因子告诉你从起点走到终点累计前进了 100 米回环因子却告诉你终点就在起点附近两者矛盾。这个矛盾会以残差的形式体现在优化里求解器要做的是把所有位姿一起调整让总残差最小。我拿一维情况算过一笔账。四个位置 x0 到 x3x0 固定为 0三段里程计测量都是 1 米一个回环观测说 x3 - x0 是 3.05 米。如果没有回环解就是 [0, 1, 2, 3]每段残差为零。加了回环之后设每段里程计的误差为 e0, e1, e2则 x3 3 e0 e1 e2回环残差是 e0e1e2-0.05。最小化 e0²e1²e2²(e0e1e2-0.05)²对每个 e_k 求偏导由对称性可知三段的误差应相等设为 t代入得 t 0.05 - 3t解得 t 0.0125。最终解是 [0, 1.0125, 2.025, 3.0375]漂移被均匀分摊到了每一段里程计上。这个例子很能说明问题回环不是把终点拉回去而是把所有相关变量的误差重新分配使总体代价最低。这也解释了为什么回环之后的位姿图整体形状变了——不是因为终点移动了而是因为每一个中间位姿都动了。4.5 回环误检怎么办鲁棒核与开关变量回环检测总有虚警。视觉场景里走廊、重复纹理、相似的墙面都可能导致错误的匹配产生一个错误但看起来很确定的回环因子。如果直接把这个因子丢进最小二乘由于它带着协方差权重会强行把整张图拽歪。解决思路从最小二乘那一侧入手把二次代价函数换成鲁棒核函数。标准的二次核是 ρ(r) r²/2对大的残差惩罚无限增长所以求解器会拼命迁就异常观测。换成 Huber 核ρ(r) r²/2当 |r| ≤ k ρ(r) k|r| - k²/2当 |r| k残差超过阈值 k 之后惩罚从二次变成线性增长异常观测的影响力被自动截断。其他常用的还有 Cauchy 核、Geman-McClure 核。参数 k 通常按照噪声标准差的数量级去设比如取 3σ 到 5σ 之间。更进一步的做法是引入开关变量。给每个回环因子额外挂一个连续的开关变量 s ∈ [0,1]因子写成 (s·r)² 的形式优化过程中 s 会被自动压低到接近 0对于错误回环或者保持在接近 1对于正确回环。代价是变量数增加了但鲁棒性提升明显。我在实际项目里的经验是鲁棒核负责软截断适用于误检率不高、且错误残差不会太大的情况开关变量负责硬开关适用于误检频繁、错误残差可能极大的情况。两者也可以叠加使用。但要注意鲁棒核不是万能的如果错误回环的比例超过三成任何鲁棒核都救不回来这时候应该是回头去改回环检测的阈值而不是在优化端打补丁。5. 手写最小可复现的因子图实现5.1 用邻接表描述二部图理论讲了这么多不写代码都是空的。下面这份实现不依赖任何图优化库只用 numpy目的是把消息传递的每个步骤落到可见的数组操作上。数据结构上我用两张邻接表分别记录变量连了哪些因子和因子连了哪些变量再加一个列表存每个因子的取值表。取值表的维度顺序跟该因子的变量顺序一致这一点很重要写错顺序会导致结果对不上。import numpy as np from collections import defaultdict class SumProductFG: def __init__(self, cardinalities): self.card list(cardinalities) # 每个变量的取值个数 self.n len(self.card) self.factors [] # 元素为 (变量id元组, 取值表ndarray) self.f2v {} # 因子id - 变量id列表 self.v2f defaultdict(list) # 变量id - 因子id列表 self.msg_f2v {} # (f, v) - ndarray self.msg_v2f {} # (v, f) - ndarray def add_factor(self, var_ids, table): fid len(self.factors) var_ids tuple(var_ids) table np.asarray(table, dtypefloat) assert table.shape tuple(self.card[v] for v in var_ids), 取值表维度不匹配 self.factors.append((var_ids, table)) self.f2v[fid] list(var_ids) for v in var_ids: self.v2f[v].append(fid) return fid形状断言这一行看着多余实际上救过我很多次。因子图里变量的顺序是隐式的一旦搞错程序不报错但结果会错得莫名其妙。写上断言至少能在构建阶段就发现问题。5.2 全量消息传递求解一个小型网络消息初始化为全 1。每一轮迭代先算所有因子到变量的消息再算所有变量到因子的消息然后检查消息的最大变化量低于阈值就停。def _factor_to_var(self, fid, v): var_ids, table self.factors[fid] idx_v var_ids.index(v) prod table.copy() for ax, y in enumerate(var_ids): if y v: continue m self.msg_v2f[(y, fid)] shape [1] * table.ndim shape[ax] self.card[y] prod prod * m.reshape(shape) axes_sum tuple(ax for ax in range(table.ndim) if ax ! idx_v) return prod.sum(axisaxes_sum) def _var_to_factor(self, v, fid): msgs [self.msg_f2v[(g, v)] for g in self.v2f[v] if g ! fid] out np.ones(self.card[v]) for m in msgs: out out * m return out def run(self, max_iter200, tol1e-9): for (fid, v) in [(f, v) for f, vs in self.f2v.items() for v in vs]: self.msg_f2v[(fid, v)] np.ones(self.card[v]) self.msg_v2f[(v, fid)] np.ones(self.card[v]) for it in range(max_iter): delta 0.0 new_f2v {} for fid, vs in self.f2v.items(): for v in vs: m self._factor_to_var(fid, v) s m.sum() if s 0: m m / s # 归一化防止数值爆炸 new_f2v[(fid, v)] m delta max(delta, np.abs(m - self.msg_f2v[(fid, v)]).max()) self.msg_f2v.update(new_f2v) for v, fs in self.v2f.items(): for fid in fs: m self._var_to_factor(v, fid) s m.sum() if s 0: m m / s self.msg_v2f[(v, fid)] m delta max(delta, np.abs(m - self.msg_v2f[(v, fid)]).max()) if delta tol: return it 1 return max_iter def marginal(self, v): out np.ones(self.card[v]) for fid in self.v2f[v]: out out * self.msg_f2v[(fid, v)] return out / out.sum()跑一个例子验证。三个二值变量构造链式结构f1 是先验表示 x1 偏向取 0比如表格 [3.0, 1.0]fa 是平滑因子让 x1 和 x2 倾向一致fb 也是平滑因子让 x2 和 x3 一致。fg SumProductFG([2, 2, 2]) fg.add_factor([0], [3.0, 1.0]) fg.add_factor([0, 1], [[2.0, 0.5], [0.5, 2.0]]) fg.add_factor([1, 2], [[2.0, 0.5], [0.5, 2.0]]) iters fg.run() print(迭代次数:, iters) for v in range(3): print(fx{v} 边缘分布:, np.round(fg.marginal(v), 4))因为 x1 有明显的先验偏向 0平滑因子会把这种倾向沿链传下去。跑出来 x1 的边缘应该在 0.9 以上偏向 0x2 稍微弱一点x3 再弱一点呈现信息沿链衰减的形态。如果结果不是这个趋势那多半是消息方向或者表格维度搞反了。5.3 同一份数据改用高斯-牛顿最小二乘离散场景用消息传递连续场景就得切换到最小二乘。这里给一个一维位姿图的完整实现用的是 4.4 节那个例子。import numpy as np def solve_pose_chain(edges, n_vars, fixed0, iters20, robustNone): x np.zeros(n_vars) for it in range(iters): H np.zeros((n_vars, n_vars)) b np.zeros(n_vars) for (i, j, d, sigma) in edges: r (x[j] - x[i]) - d w 1.0 / (sigma * sigma) if robust is not None: k robust if abs(r) k: w w * k / abs(r) # Huber 权重缩放 J np.zeros(n_vars) J[j] 1.0 J[i] - 1.0 H w * np.outer(J, J) b w * J * r # 固定参考变量消除规范自由度 H[fixed, :] 0.0 H[:, fixed] 0.0 H[fixed, fixed] 1.0 b[fixed] 0.0 dx np.linalg.solve(H, -b) x x dx if np.linalg.norm(dx) 1e-12: break return x, it 1 edges [(0, 1, 1.0, 0.1), (1, 2, 1.0, 0.1), (2, 3, 1.0, 0.1), (0, 3, 3.05, 0.1)] x_opt, n_it solve_pose_chain(edges, 4) print(迭代次数:, n_it) print(最优解:, np.round(x_opt, 6))跑出来应该非常接近 [0, 1.0125, 2.025, 3.0375]跟前面手算的结果对上。这个代对上了说明你的最小二乘框架搭对了。顺便说一句固定第一个变量的那三行赋值不能省。位姿图有一个整体平移的规范自由度——所有位姿同时加一个常数所有残差不变。不固定的话信息矩阵奇异求解直接报错。固定之后的矩阵是正定的可以放心用 Cholesky 分解求解。5.4 两种方法的结果互相校验我养成了一个习惯写完消息传递实现一定拿最小二乘的结果去对照。对于高斯因子图和积算法在连续情形下是高斯消息传递和最小二乘的最优解应该是同一个点只是前者给出的是后验分布的均值和协方差后者给出的是均值。如果两者对不上八成是某一侧的实现有问题。这个交叉验证的价值在于两种实现的代码路径完全不同一个在概率域里乘和加一个在矩阵域里求导和分解。同时出错的概率很低。我在调一个多传感器融合的 bug 时就是靠这个对照定位到问题——消息传递那边结果正常最小二乘那边偏了最后发现是某个观测因子的协方差矩阵写成了标准差矩阵没平方。这个错误在单一实现里很难发现因为数值都不会崩只是精度悄悄变差。6. 我在实际项目里踩过的六个坑6.1 变量编号顺序影响的不只是速度前面提过 COLAMD 排序这里补充一个更隐蔽的坑变量编号不仅影响求解速度还影响数值稳定性。消元过程中填充量越大累积的浮点误差越多极端情况下会导致信息矩阵条件数急剧恶化。我遇到过一次一个两千个位姿的图用自然顺序编号时求解时间大约 1.8 秒结果也基本正确换成另一套看似更自然的分组编号后求解时间涨到 6 秒而且协方差对角线出现了负值——这在信息矩阵里是绝对不该出现的。换回 COLAMD 排序后问题消失。所以顺序这件事别凭直觉交给成熟的排序算法。6.2 单位与量纲混用是隐形杀手角度用弧度、位置用米、时间用秒这是默认约定。但实际项目里最容易出问题的不是这几个主变量而是协方差。雅可比矩阵的量纲取决于残差的量纲。如果残差是米J 对位置的偏导就是无量纲如果残差是像素J 的单位就是像素每米。把不同量纲的观测混在同一个信息矩阵里做加法前提是每一项都已经被自己的协方差正确加权了。如果某个协方差忘了平方或者单位写成了毫米那这一项的权重就会差六个数量级求解结果完全被它主导。我的做法是在因子定义里强制写上单位注释并且在单元测试里检查每个因子的残差量级是否符合预期。多花十分钟能省掉半天的排查。6.3 收敛判据写得太松会假收敛最小二乘迭代的停止条件常见的有三种增量范数小于阈值、残差变化小于阈值、达到最大迭代次数。用一个是不够的。我见过不止一次这样的情况增量范数很小看起来收敛了但残差其实很大。原因是步长太小每次只挪一点点增量自然就小。这种情况在高斯-牛顿遇到强非线性时很常见本质是雅可比不准确导致步长被压缩。稳妥的写法是同时检查增量范数和残差范数两个都满足才退出。如果迭代次数到了上限还没满足应该输出一个警告而不是静默返回当前结果。我现在的习惯是让求解函数返回一个状态码调用方必须检查避免悄悄错了。6.4 残差可视化比看日志管用十倍调优化问题看日志里的数值变化效率很低。更好的办法是把每个因子的残差画出来。我通常做两张图一张是按因子索引画的残差绝对值一张是残差在地图上的空间分布。第一张图能一眼看出有没有离群因子——正常残差应该大致朝着噪声标准差的量级聚集如果某个因子的残差比邻居大两个数量级那它八成有问题。第二张图能看出系统性的偏差模式比如某一段区域整体残差偏大那多半是那段区域的观测模型或者标定参数有问题。对于 SLAM 这类问题还有一个很有用的指标是卡方检验。如果噪声模型假设正确归一化残差平方和除以自由度应该接近 1。远大于 1 说明模型过于乐观协方差给小了远小于 1 说明过于保守。这个比值我在每次调参之后都会看一眼作为噪声参数是否合理的一个粗判据。6.5 别把强先验当硬约束用因子图里没有真正的硬约束除非做变量消元那是另一回事。所有先验都是软约束只是权重不同。想固定一个变量你得给它挂一个协方差极小的一元因子或者干脆在求解时把对应的行列抠掉。我一开始图省事用一个大权重的一元因子去固定起点比如给位置加一个均值 0、标准差 1e-9 的因子。结果信息矩阵的条件数直接爆炸到 1e18 以上求解器给出了警告。后来改成在组装矩阵之后直接把那一行一列替换成单位行单位列问题解决。这个坑的教训是数值上的很大和数学上的无穷是两回事。想表达硬约束就用结构上的方式消元、固定列别用数值上的极端权重去模拟。6.6 鲁棒核的阈值不能拍脑袋Huber 核的阈值 k 决定了多大的残差开始被降权。设得太小正常观测也会被当异常处理模型欠拟合设得太大等于没加鲁棒核。我见过有人直接写死 k1理由是残差超过 1 米肯定不对。问题是他的系统里残差的单位是像素1 像素的阈值把几乎所有观测都降权了最后解出来的轨迹像没优化过一样。换个系统残差单位是毫米1 毫米的阈值又等于没生效。合理的做法是让 k 跟噪声模型挂钩。我一般取 k 3σσ 是该类型观测的典型噪声标准差。更进一步可以对不同类型的因子设不同的 k因为它们的残差量级本来就不同。如果系统里有明确的卡方检验机制也可以反过来用卡方分布的分位数来定 k这样更有理论依据。再分享一个调试细节加完鲁棒核之后一定要统计被降权的因子占比。如果占比超过 20%说明要么阈值设错了要么回环检测本身有问题这时候应该回去查数据而不是继续调参数。我自己就吃过这个亏盯着优化结果调了半天阈值最后发现问题出在回环检测的时间戳对齐上跟鲁棒核一点关系都没有。