很多朋友学视觉SLAM时前端特征匹配跑得挺顺里程计也能凑合出轨迹一到“后端优化”就开始发怵。尤其是图优化翻开讲义就是李代数、指数映射、海塞矩阵一串符号下来整个人是懵的。其实图优化要干的事特别朴素你手里有一堆对机器人轨迹的“局部判断”这些判断之间还互相打架图优化就是把这些矛盾摊开算一版大家都能接受的全局轨迹。这篇文章我打算用一个能拿笔在纸上算完的最小例子从误差怎么定义、雅可比怎么求、海塞矩阵每一项怎么填到增量方程怎么解完整走一遍。全文不引入复杂的李代数推导只讲从矩阵层面能感知到的那部分。适合已经跑通视觉里程计、但还没把图优化内部机制搞清楚的初学者也适合想手推一遍验证代码实现的同学。1. 视觉SLAM里图优化到底在解什么问题1.1 一张图讲清楚节点、边、误差视觉SLAM分为前端和后端。前端负责“我看到了什么、我动了多少”后端负责“把关键帧位姿放到一起做全局一致性修正”。在位姿图优化的语境下节点是某个时刻的相机位姿或者机器人位姿。边是两个节点之间的相对运动约束。相邻帧之间通常有里程计边回环检测会给不相邻的节点之间加一条回环边。误差是“这条边声称的相对运动”和“当前节点位姿实际算出来的相对运动”之间的差。图优化这个名字很形象把这些节点和边画在纸上就是一张图。优化过程就是调整节点位姿让每条边上的误差都尽量小。注意不是所有误差都能归零因为约束之间本身矛盾最终结果必然是折中哪条边的信息矩阵大谁的话语权就大。视觉SLAM里的误差来源主要有三类里程计边提供相邻帧相对运动约束回环边提供长时间漂移校正先验边提供绝对位置参考。如果误差函数里加入了路标点节点还包括地图点这就是Bundle Adjustment但底层求解框架和图优化完全一样。1.2 图优化的目标和“手算”有什么关系图优化目标函数的标准写法χ* argmin_χ Σ e_ij^T Ω_ij e_ij其中χ是所有节点位姿e_ij是边ij上的误差向量Ω_ij是这条边的信息矩阵也就是观测协方差矩阵的逆。这本质上是高斯噪声假设下的最大似然估计等价于加权最小二乘。求解一般用高斯牛顿法或LM算法每次迭代都要解一个线性方程H δ -b其中H J^T Ω Jb J^T Ω e。J是所有误差对节点位姿的雅可比矩阵Ω是各边信息矩阵组成的块对角矩阵。几乎所有图优化库做的事情就是帮你组装J、H、b然后解这个线性方程。如果不亲手装配一次代码里“添加边”“添加顶点”对你来说永远是黑盒。本文后面的全部内容都围绕这两个式子展开用一个小例子把H和b的物理含义算出来。2. 搭一个可以手算的最小例子2.1 场景设计三段里程计加一条回环用一个平面机器人轨迹作为例子。为了手算方便先把问题降到二维平移不优化旋转角度。这个简化对应“所有位姿朝向都相同”的特殊情况真实SLAM的位姿边会带角度第4章会补上。设机器人从原点出发节点p1初始位置设为(0,0)固定不动。机器人先向右走1米前端估计p2在(1,0)。再向右上走一段前端估计p3在(2,1)。里程计测量存在误差回环检测又给出一个p3相对于p1的全局约束。于是构造三条边边p1-p2观测相对位移为(1,0.1)。边p2-p3观测相对位移为(1,0)。边p1-p3回环观测相对位移为(2,1.1)。这三条边存在明显矛盾按前两条边推算p3的y坐标应该是0.1但回环边要求p3的y坐标是1.1。这就是典型的前端轨迹漂移与回环修正之间的矛盾。信息矩阵三条边暂时都设为单位阵I。这样做的目的是让计算量小到能手算真实场景必须按传感器协方差来设具体放到第5章讲。2.2 误差怎么定义信息矩阵怎么理解误差采用最直观的定义e_ij z_ij - (p_j - p_i)也就是“观测到的相对位移”减去“当前位姿计算出来的相对位移”。当误差为零时这条边被完全满足不为零优化就要通过调整p2、p3把误差压下去。信息矩阵Ω是协方差Σ的逆。二维情况下Ω Σ^{-1}误差平方e^T Ω e本质上是一个马氏距离。如果某个方向的测量很准噪声方差小Σ对应项就小Ω对应项就大优化时会优先照顾这个方向。单位阵是最偷懒的假设写代码时千万别当默认。假设某条边在x方向上观测精度约0.01米y方向上约0.1米那么信息矩阵大致为diag(10000, 100)而不是单位阵。同一个displacement不同方向权重差100倍优化结果就完全不同。3. 一步一步把矩阵算出来3.1 每条边的误差与雅可比先把三条边的初始误差算出来e12 (1,0.1) - ((1,0) - (0,0)) (0,0.1)e23 (1,0) - ((2,1) - (1,0)) (0,-1)e13 (2,1.1) - ((2,1) - (0,0)) (0,0.1)由于误差定义e z p_i - p_j对p_i的雅可比是I对p_j的雅可比是-I。逐条边整理边误差向量∂e/∂p2∂e/∂p3e12(0,0.1)-I0e23(0,-1)I-Ie13(0,0.1)0-I这里I是2×2单位矩阵。雅可比矩阵的维度等于误差维度乘以节点状态维度。误差是二维节点状态也是二维所以每项都是2×2块矩阵。3.2 组装海塞矩阵和右侧向量高斯牛顿的增量方程是H δ -b其中H J^T Ω Jb J^T Ω e。因为信息矩阵暂时都是单位阵先退化成H J^T Jb J^T e。按分块累加H22 (-I)^T (-I) I^T I 2IH23 I^T (-I) -IH32 (-I)^T I -IH33 (-I)^T (-I) (-I)^T (-I) 2I所以H是一个4×4矩阵按p2、p3各占2维分块H [ 2I, -I; -I, 2I ]再看右侧b2 (-I)^T e12 I^T e23 -e12 e23 (0,-0.1) (0,-1) (0,-1.1)b3 (-I)^T e23 (-I)^T e13 -e23 - e13 (0,1) (0,-0.1) (0,0.9)于是增量方程H δ -b写成对于p22I δp2 - I δp3 (0,1.1)对于p3-I δp2 2I δp3 (0,-0.9)注意这里右侧用的是-bb只是中间量符号特别容易出错后面第5章会专门讲这个坑。3.3 解增量方程并更新位姿这个方程在x和y两个方向是独立的可以拆开解。x方向2δp2x - δp3x 0-δp2x 2δp3x 0解出δp2x 0δp3x 0。x方向所有约束本来就一致不需要调整。y方向2δp2y - δp3y 1.1-δp2y 2δp3y -0.9由第一个方程得δp3y 2δp2y - 1.1代入第二个方程-δp2y 2(2δp2y - 1.1) -0.93δp2y 1.3δp2y 0.4333δp3y 2 × 0.4333 - 1.1 -0.2333更新位姿p2 (1, 0.4333)p3 (2, 0.7667)重新计算误差e12 (1,0.1) - (1,0.4333) (0,-0.3333)e23 (1,0) - ((2,0.7667) - (1,0.4333)) (0,-0.3333)e13 (2,1.1) - (2,0.7667) (0,0.3333)误差平方和从初始的1.02降到了0.3333。三条边的信息矩阵相同所以最终每条边的误差大小一样一个正的、两个负的形成平衡状态。这直观展示了图优化“折中”的本质不是让某条边完全满意而是让所有边一起分担矛盾。由于这个例子的误差是状态的线性函数增量方程一次迭代就收敛了再算一次δ会得到零。真实SLAM中误差函数是非线性的不会这么幸运所以需要迭代多次这也是第4章要重点说明的内容。4. 从纯平移例子走到真实SLAM位姿边4.1 位姿边误差的完整形式前面例子把位姿简化成了二维坐标。真实SLAM中节点位姿通常是(x, y, θ)或者更高维表达两个节点之间的相对运动不是简单坐标差而是变换矩阵的复合T_ij T_i^{-1} T_j误差定义常用e_ij log(z_ij^{-1} ∘ T_ij)这里的log是把相对变换映射到李代数空间得到一个向量两个平移分量加一个角度分量。对数映射听起来吓人但在角度差小、位置误差也小的入门场景下可以近似写成t_err R_z^T ( R_i^T ( t_j - t_i ) - t_z )e_θ θ_j - θ_i - θ_z其中R_z是观测相对姿态的旋转矩阵R_i是节点i当前的旋转矩阵。先算当前相对位置t_j - t_i旋转到i坐标系再旋转到观测坐标系最后和观测平移比较角度误差直接相减。这个公式的关键在于误差变成了非线性。R_i里包含θ_iR_i^T作用于位置差时会把角度信息混合进去。高斯牛顿法是对当前位置做一阶线性近似每迭代一次才向真实最小值靠近一点所以无法一步收敛。4.2 一个带旋转的雅可比数值演示用简化误差公式做一个带旋转的数值演示。设节点i在原点当前θ_i 0节点j位置在(1,0)θ_j 0.1。观测边认为节点j相对节点i的位姿应该是(1,0,0)信息矩阵为单位阵。如果只固定i把j作为优化变量按近似误差公式t_err (1,0) - (1,0) (0,0)e_θ 0.1位置误差为零只有角度误差0.1这一步很平淡体现不出旋转和位置的耦合。真正有耦合的是节点i自身的旋转扰动。考虑这样一个更一般的雅可比公式。误差t_err对节点i角度的偏导是∂t_err / ∂θ_i R_z^T R_i^T A ( t_j - t_i )其中A是一个90度旋转矩阵乘以二维向量相当于让向量旋转90度。取θ_i 0R_z It_j - t_i (1,0)则这一项是A (1,0) (0,-1)意思是节点i的姿态每转动一个微小角度观测坐标系下位置误差的y方向就会产生几乎同比例的负向变化。这个非零项就是海塞矩阵中的平移-旋转交叉块。真实SLAM里这类交叉块到处都是这也是为什么位姿的x、y、θ不能分开独立优化必须作为一个整体进入矩阵。如果只把节点j作为可优化变量雅可比比较简单J_j [ I, 0; 0^T, 1 ]位置部分的雅可比是2×2单位阵角度部分对θ_j的导数为1。信息矩阵为单位阵时H diag(1,1,1)b (0,0.1,0.1)一步迭代就把误差清零了。但一旦i也是可优化节点交叉项就会被填充进H矩阵整个系统就不再块对角需要多次迭代。注意不同图优化库对误差定义和扰动方式不一样有的用左扰动有的用右扰动雅可比的具体形式会有差异但最终位姿修正基本一致。你只需要在自己的代码里保证误差函数和雅可比来自同一个定义即可。4.3 为什么真实SLAM要迭代很多次旋转矩阵是非线性映射误差函数通常不是二次的。一个观测约束可能同时耦合多个节点的平移和旋转导致目标函数在参数空间中是一个弯曲的曲面。高斯牛顿法每次迭代都在当前点附近做二次近似所以需要多步逼近。SLAM后端常用LM算法而不是纯高斯牛顿法原因是LM在增量方程里加了阻尼项λ(H λI) δ -b当λ大时退化成梯度下降稳但慢当λ小时近似高斯牛顿快但可能发散。视觉SLAM里噪声大、初值未必好LM的保守策略更稳妥。单目视觉SLAM里的重投影误差比位姿边更复杂因为误差同时涉及三维路标点的坐标和相机位姿耦合更严重。不过迭代框架完全一样只是雅可比的长度更长矩阵的稀疏性需要利用得更好。5. 我踩过的坑图优化入门最容易被忽略的四个问题5.1 固定节点怎么选才不会翻车图优化问题如果只由相对约束构成没有任何绝对先验那么整条轨迹可以整体平移、旋转而保持误差不变这叫规范自由度也叫零空间。如果不固定至少一个节点增量方程的H矩阵就是奇异的线性求解器直接报错。常见做法是固定第一帧完整位姿即在支持的维度上把增量置零。但要注意如果只固定位置不固定角度某些坐标系下旋转零空间依然存在。经验是用手算例子验证H是否奇异把H矩阵打印出来看看对角线有没有明显异常。如果有GPS、IMU先验信息也可以用先验边来消除零空间而不是硬性固定节点。比如一条绝对位置边给定p1的位置另一条绝对角度边给定初始朝向效果上比“删行删列”更自然也更容易加权重。5.2 信息矩阵不是让你随便填的单位阵很多入门代码喜欢把所有信息矩阵设成单位阵能跑但结果不对。信息矩阵是观测噪声协方差的逆它代表“这条边在这个方向上有多可信”。里程计在前进方向的噪声通常比横向小视觉回环在垂直视角方向的约束可能较弱这些差异都要体现在Ω里。如果一个传感器在某方向误差方差是0.01米^2Ω对应分量就是100另一个方向方差是0.04米^2Ω就是25。两者权重的比值是4倍不是单位阵假设下的一视同仁。调图优化时最先该怀疑的就是信息矩阵没设对而不是调整节点数或者优化策略。5.3 误差符号与雅可比的一致性问题误差定义成e 观测 - 预测雅可比是∂e/∂x如果改成e 预测 - 观测雅可比符号会整体反转。单独看每个式子都没问题但一旦混用b就会算错。更隐蔽的错误是代码里误差函数用的A约定雅可比却是按B约定推导的。比如误差写成e z - h(x)雅可比却用了∂h/∂x而不是-∂h/∂x。手算例子里e12对p2的雅可比是-I如果错写成IH会缺项增量方向就反了。排查办法就是单步调试先固定p1打印第一次迭代前所有边的误差、雅可比、H、b随后手动核一遍。误差是否下降、下降方向是否合理都能立刻看出问题。5.4 真正决定性能的稀疏结构和鲁棒核图优化之所以快是因为H矩阵是非零块只出现在共有一条边的节点对之间。脱离“图”的结构去直接堆一个大稠密矩阵1000个节点就难以实时求解了。利用稀疏性做线性求解是所有图优化库能跑起来的关键。另一个很容易被忽视的是鲁棒核函数。如果一条回环边匹配错误它的残差会特别大普通二次损失里这个大残差会把整个优化结果带偏。Huber核或者柯西核能把大残差的影响从平方降为线性甚至饱和这在真实SLAM里几乎是必需品。我的习惯是不管回环检测多么有把握回环边都先套一个Huber核损失权重不要一开始拉满。6. 图优化在视觉SLAM里的应用到底用到什么程度6.1 位姿图优化和BA的分工位姿图优化只优化位姿边是相对位姿约束适合回环修正和全局轨迹调整。BA同时优化位姿和路标点边是重投影误差精度更高但矩阵维度大很多。工程里常见流程是回环闭合之后先做一次全局位姿图优化把轨迹拉回一致再做一次BA把路标点也“擦干净”。位姿图矩阵小速度快适合作为第一层修正BA精度高适合作为第二层精化。本文手算的位姿图思想是理解BA的前置课BA只是把一部分变量换成三维点误差从相对位姿换成投影残差迭代框架还是一模一样。6.2 从局部窗口到全局回环的工程习惯实时SLAM不会每帧都做全局优化通常是维护一个滑动窗口或者局部地图在里面做优化保持精度和实时性的平衡。关键帧机制也是从工程角度减少节点数量只在关键帧之间建边。回环检测成功后全局图优化会一次性修正长时间累积的漂移。修正后的轨迹可能和原来差很多工程上通常要加姿态平滑处理避免地图跳变。初学者上手可以先复现本文例子把每一步矩阵打印出来和手算结果对比随后在模拟数据上加100个节点制造逐渐漂移的轨迹观察回环边是如何把整个轨迹拉回闭合状态的。最后说点我在实际调试中的个人体会。图优化入门容易卡在两件事上一是不知道矩阵从哪来二是知道矩阵怎么来但不知道代码往哪儿填。我的建议很笨但很管用把本文这个例子写成可打印的单元测试第一次迭代的H、b、δ全部和手算对一遍对上之后再放开手脚。这个方法帮我过滤掉了一大堆低级错误。另一个小经验是每次加新的边类型先跑“只有一条边”的用例因为单边情况下数学退化成最简单形式哪一步错了立刻能看出来。视觉SLAM的图优化说白了就是一个带权重的矛盾调和器亲手算过一次之后后面那些库写的东西就都只是实现细节了。 
长河君 · 2026/10/16 4:11:45 · 阅读 1.2k 手算视觉SLAM图优化：从误差到海塞矩阵的完整推导
很多朋友学视觉SLAM时前端特征匹配跑得挺顺里程计也能凑合出轨迹一到“后端优化”就开始发怵。尤其是图优化翻开讲义就是李代数、指数映射、海塞矩阵一串符号下来整个人是懵的。其实图优化要干的事特别朴素你手里有一堆对机器人轨迹的“局部判断”这些判断之间还互相打架图优化就是把这些矛盾摊开算一版大家都能接受的全局轨迹。这篇文章我打算用一个能拿笔在纸上算完的最小例子从误差怎么定义、雅可比怎么求、海塞矩阵每一项怎么填到增量方程怎么解完整走一遍。全文不引入复杂的李代数推导只讲从矩阵层面能感知到的那部分。适合已经跑通视觉里程计、但还没把图优化内部机制搞清楚的初学者也适合想手推一遍验证代码实现的同学。1. 视觉SLAM里图优化到底在解什么问题1.1 一张图讲清楚节点、边、误差视觉SLAM分为前端和后端。前端负责“我看到了什么、我动了多少”后端负责“把关键帧位姿放到一起做全局一致性修正”。在位姿图优化的语境下节点是某个时刻的相机位姿或者机器人位姿。边是两个节点之间的相对运动约束。相邻帧之间通常有里程计边回环检测会给不相邻的节点之间加一条回环边。误差是“这条边声称的相对运动”和“当前节点位姿实际算出来的相对运动”之间的差。图优化这个名字很形象把这些节点和边画在纸上就是一张图。优化过程就是调整节点位姿让每条边上的误差都尽量小。注意不是所有误差都能归零因为约束之间本身矛盾最终结果必然是折中哪条边的信息矩阵大谁的话语权就大。视觉SLAM里的误差来源主要有三类里程计边提供相邻帧相对运动约束回环边提供长时间漂移校正先验边提供绝对位置参考。如果误差函数里加入了路标点节点还包括地图点这就是Bundle Adjustment但底层求解框架和图优化完全一样。1.2 图优化的目标和“手算”有什么关系图优化目标函数的标准写法χ* argmin_χ Σ e_ij^T Ω_ij e_ij其中χ是所有节点位姿e_ij是边ij上的误差向量Ω_ij是这条边的信息矩阵也就是观测协方差矩阵的逆。这本质上是高斯噪声假设下的最大似然估计等价于加权最小二乘。求解一般用高斯牛顿法或LM算法每次迭代都要解一个线性方程H δ -b其中H J^T Ω Jb J^T Ω e。J是所有误差对节点位姿的雅可比矩阵Ω是各边信息矩阵组成的块对角矩阵。几乎所有图优化库做的事情就是帮你组装J、H、b然后解这个线性方程。如果不亲手装配一次代码里“添加边”“添加顶点”对你来说永远是黑盒。本文后面的全部内容都围绕这两个式子展开用一个小例子把H和b的物理含义算出来。2. 搭一个可以手算的最小例子2.1 场景设计三段里程计加一条回环用一个平面机器人轨迹作为例子。为了手算方便先把问题降到二维平移不优化旋转角度。这个简化对应“所有位姿朝向都相同”的特殊情况真实SLAM的位姿边会带角度第4章会补上。设机器人从原点出发节点p1初始位置设为(0,0)固定不动。机器人先向右走1米前端估计p2在(1,0)。再向右上走一段前端估计p3在(2,1)。里程计测量存在误差回环检测又给出一个p3相对于p1的全局约束。于是构造三条边边p1-p2观测相对位移为(1,0.1)。边p2-p3观测相对位移为(1,0)。边p1-p3回环观测相对位移为(2,1.1)。这三条边存在明显矛盾按前两条边推算p3的y坐标应该是0.1但回环边要求p3的y坐标是1.1。这就是典型的前端轨迹漂移与回环修正之间的矛盾。信息矩阵三条边暂时都设为单位阵I。这样做的目的是让计算量小到能手算真实场景必须按传感器协方差来设具体放到第5章讲。2.2 误差怎么定义信息矩阵怎么理解误差采用最直观的定义e_ij z_ij - (p_j - p_i)也就是“观测到的相对位移”减去“当前位姿计算出来的相对位移”。当误差为零时这条边被完全满足不为零优化就要通过调整p2、p3把误差压下去。信息矩阵Ω是协方差Σ的逆。二维情况下Ω Σ^{-1}误差平方e^T Ω e本质上是一个马氏距离。如果某个方向的测量很准噪声方差小Σ对应项就小Ω对应项就大优化时会优先照顾这个方向。单位阵是最偷懒的假设写代码时千万别当默认。假设某条边在x方向上观测精度约0.01米y方向上约0.1米那么信息矩阵大致为diag(10000, 100)而不是单位阵。同一个displacement不同方向权重差100倍优化结果就完全不同。3. 一步一步把矩阵算出来3.1 每条边的误差与雅可比先把三条边的初始误差算出来e12 (1,0.1) - ((1,0) - (0,0)) (0,0.1)e23 (1,0) - ((2,1) - (1,0)) (0,-1)e13 (2,1.1) - ((2,1) - (0,0)) (0,0.1)由于误差定义e z p_i - p_j对p_i的雅可比是I对p_j的雅可比是-I。逐条边整理边误差向量∂e/∂p2∂e/∂p3e12(0,0.1)-I0e23(0,-1)I-Ie13(0,0.1)0-I这里I是2×2单位矩阵。雅可比矩阵的维度等于误差维度乘以节点状态维度。误差是二维节点状态也是二维所以每项都是2×2块矩阵。3.2 组装海塞矩阵和右侧向量高斯牛顿的增量方程是H δ -b其中H J^T Ω Jb J^T Ω e。因为信息矩阵暂时都是单位阵先退化成H J^T Jb J^T e。按分块累加H22 (-I)^T (-I) I^T I 2IH23 I^T (-I) -IH32 (-I)^T I -IH33 (-I)^T (-I) (-I)^T (-I) 2I所以H是一个4×4矩阵按p2、p3各占2维分块H [ 2I, -I; -I, 2I ]再看右侧b2 (-I)^T e12 I^T e23 -e12 e23 (0,-0.1) (0,-1) (0,-1.1)b3 (-I)^T e23 (-I)^T e13 -e23 - e13 (0,1) (0,-0.1) (0,0.9)于是增量方程H δ -b写成对于p22I δp2 - I δp3 (0,1.1)对于p3-I δp2 2I δp3 (0,-0.9)注意这里右侧用的是-bb只是中间量符号特别容易出错后面第5章会专门讲这个坑。3.3 解增量方程并更新位姿这个方程在x和y两个方向是独立的可以拆开解。x方向2δp2x - δp3x 0-δp2x 2δp3x 0解出δp2x 0δp3x 0。x方向所有约束本来就一致不需要调整。y方向2δp2y - δp3y 1.1-δp2y 2δp3y -0.9由第一个方程得δp3y 2δp2y - 1.1代入第二个方程-δp2y 2(2δp2y - 1.1) -0.93δp2y 1.3δp2y 0.4333δp3y 2 × 0.4333 - 1.1 -0.2333更新位姿p2 (1, 0.4333)p3 (2, 0.7667)重新计算误差e12 (1,0.1) - (1,0.4333) (0,-0.3333)e23 (1,0) - ((2,0.7667) - (1,0.4333)) (0,-0.3333)e13 (2,1.1) - (2,0.7667) (0,0.3333)误差平方和从初始的1.02降到了0.3333。三条边的信息矩阵相同所以最终每条边的误差大小一样一个正的、两个负的形成平衡状态。这直观展示了图优化“折中”的本质不是让某条边完全满意而是让所有边一起分担矛盾。由于这个例子的误差是状态的线性函数增量方程一次迭代就收敛了再算一次δ会得到零。真实SLAM中误差函数是非线性的不会这么幸运所以需要迭代多次这也是第4章要重点说明的内容。4. 从纯平移例子走到真实SLAM位姿边4.1 位姿边误差的完整形式前面例子把位姿简化成了二维坐标。真实SLAM中节点位姿通常是(x, y, θ)或者更高维表达两个节点之间的相对运动不是简单坐标差而是变换矩阵的复合T_ij T_i^{-1} T_j误差定义常用e_ij log(z_ij^{-1} ∘ T_ij)这里的log是把相对变换映射到李代数空间得到一个向量两个平移分量加一个角度分量。对数映射听起来吓人但在角度差小、位置误差也小的入门场景下可以近似写成t_err R_z^T ( R_i^T ( t_j - t_i ) - t_z )e_θ θ_j - θ_i - θ_z其中R_z是观测相对姿态的旋转矩阵R_i是节点i当前的旋转矩阵。先算当前相对位置t_j - t_i旋转到i坐标系再旋转到观测坐标系最后和观测平移比较角度误差直接相减。这个公式的关键在于误差变成了非线性。R_i里包含θ_iR_i^T作用于位置差时会把角度信息混合进去。高斯牛顿法是对当前位置做一阶线性近似每迭代一次才向真实最小值靠近一点所以无法一步收敛。4.2 一个带旋转的雅可比数值演示用简化误差公式做一个带旋转的数值演示。设节点i在原点当前θ_i 0节点j位置在(1,0)θ_j 0.1。观测边认为节点j相对节点i的位姿应该是(1,0,0)信息矩阵为单位阵。如果只固定i把j作为优化变量按近似误差公式t_err (1,0) - (1,0) (0,0)e_θ 0.1位置误差为零只有角度误差0.1这一步很平淡体现不出旋转和位置的耦合。真正有耦合的是节点i自身的旋转扰动。考虑这样一个更一般的雅可比公式。误差t_err对节点i角度的偏导是∂t_err / ∂θ_i R_z^T R_i^T A ( t_j - t_i )其中A是一个90度旋转矩阵乘以二维向量相当于让向量旋转90度。取θ_i 0R_z It_j - t_i (1,0)则这一项是A (1,0) (0,-1)意思是节点i的姿态每转动一个微小角度观测坐标系下位置误差的y方向就会产生几乎同比例的负向变化。这个非零项就是海塞矩阵中的平移-旋转交叉块。真实SLAM里这类交叉块到处都是这也是为什么位姿的x、y、θ不能分开独立优化必须作为一个整体进入矩阵。如果只把节点j作为可优化变量雅可比比较简单J_j [ I, 0; 0^T, 1 ]位置部分的雅可比是2×2单位阵角度部分对θ_j的导数为1。信息矩阵为单位阵时H diag(1,1,1)b (0,0.1,0.1)一步迭代就把误差清零了。但一旦i也是可优化节点交叉项就会被填充进H矩阵整个系统就不再块对角需要多次迭代。注意不同图优化库对误差定义和扰动方式不一样有的用左扰动有的用右扰动雅可比的具体形式会有差异但最终位姿修正基本一致。你只需要在自己的代码里保证误差函数和雅可比来自同一个定义即可。4.3 为什么真实SLAM要迭代很多次旋转矩阵是非线性映射误差函数通常不是二次的。一个观测约束可能同时耦合多个节点的平移和旋转导致目标函数在参数空间中是一个弯曲的曲面。高斯牛顿法每次迭代都在当前点附近做二次近似所以需要多步逼近。SLAM后端常用LM算法而不是纯高斯牛顿法原因是LM在增量方程里加了阻尼项λ(H λI) δ -b当λ大时退化成梯度下降稳但慢当λ小时近似高斯牛顿快但可能发散。视觉SLAM里噪声大、初值未必好LM的保守策略更稳妥。单目视觉SLAM里的重投影误差比位姿边更复杂因为误差同时涉及三维路标点的坐标和相机位姿耦合更严重。不过迭代框架完全一样只是雅可比的长度更长矩阵的稀疏性需要利用得更好。5. 我踩过的坑图优化入门最容易被忽略的四个问题5.1 固定节点怎么选才不会翻车图优化问题如果只由相对约束构成没有任何绝对先验那么整条轨迹可以整体平移、旋转而保持误差不变这叫规范自由度也叫零空间。如果不固定至少一个节点增量方程的H矩阵就是奇异的线性求解器直接报错。常见做法是固定第一帧完整位姿即在支持的维度上把增量置零。但要注意如果只固定位置不固定角度某些坐标系下旋转零空间依然存在。经验是用手算例子验证H是否奇异把H矩阵打印出来看看对角线有没有明显异常。如果有GPS、IMU先验信息也可以用先验边来消除零空间而不是硬性固定节点。比如一条绝对位置边给定p1的位置另一条绝对角度边给定初始朝向效果上比“删行删列”更自然也更容易加权重。5.2 信息矩阵不是让你随便填的单位阵很多入门代码喜欢把所有信息矩阵设成单位阵能跑但结果不对。信息矩阵是观测噪声协方差的逆它代表“这条边在这个方向上有多可信”。里程计在前进方向的噪声通常比横向小视觉回环在垂直视角方向的约束可能较弱这些差异都要体现在Ω里。如果一个传感器在某方向误差方差是0.01米^2Ω对应分量就是100另一个方向方差是0.04米^2Ω就是25。两者权重的比值是4倍不是单位阵假设下的一视同仁。调图优化时最先该怀疑的就是信息矩阵没设对而不是调整节点数或者优化策略。5.3 误差符号与雅可比的一致性问题误差定义成e 观测 - 预测雅可比是∂e/∂x如果改成e 预测 - 观测雅可比符号会整体反转。单独看每个式子都没问题但一旦混用b就会算错。更隐蔽的错误是代码里误差函数用的A约定雅可比却是按B约定推导的。比如误差写成e z - h(x)雅可比却用了∂h/∂x而不是-∂h/∂x。手算例子里e12对p2的雅可比是-I如果错写成IH会缺项增量方向就反了。排查办法就是单步调试先固定p1打印第一次迭代前所有边的误差、雅可比、H、b随后手动核一遍。误差是否下降、下降方向是否合理都能立刻看出问题。5.4 真正决定性能的稀疏结构和鲁棒核图优化之所以快是因为H矩阵是非零块只出现在共有一条边的节点对之间。脱离“图”的结构去直接堆一个大稠密矩阵1000个节点就难以实时求解了。利用稀疏性做线性求解是所有图优化库能跑起来的关键。另一个很容易被忽视的是鲁棒核函数。如果一条回环边匹配错误它的残差会特别大普通二次损失里这个大残差会把整个优化结果带偏。Huber核或者柯西核能把大残差的影响从平方降为线性甚至饱和这在真实SLAM里几乎是必需品。我的习惯是不管回环检测多么有把握回环边都先套一个Huber核损失权重不要一开始拉满。6. 图优化在视觉SLAM里的应用到底用到什么程度6.1 位姿图优化和BA的分工位姿图优化只优化位姿边是相对位姿约束适合回环修正和全局轨迹调整。BA同时优化位姿和路标点边是重投影误差精度更高但矩阵维度大很多。工程里常见流程是回环闭合之后先做一次全局位姿图优化把轨迹拉回一致再做一次BA把路标点也“擦干净”。位姿图矩阵小速度快适合作为第一层修正BA精度高适合作为第二层精化。本文手算的位姿图思想是理解BA的前置课BA只是把一部分变量换成三维点误差从相对位姿换成投影残差迭代框架还是一模一样。6.2 从局部窗口到全局回环的工程习惯实时SLAM不会每帧都做全局优化通常是维护一个滑动窗口或者局部地图在里面做优化保持精度和实时性的平衡。关键帧机制也是从工程角度减少节点数量只在关键帧之间建边。回环检测成功后全局图优化会一次性修正长时间累积的漂移。修正后的轨迹可能和原来差很多工程上通常要加姿态平滑处理避免地图跳变。初学者上手可以先复现本文例子把每一步矩阵打印出来和手算结果对比随后在模拟数据上加100个节点制造逐渐漂移的轨迹观察回环边是如何把整个轨迹拉回闭合状态的。最后说点我在实际调试中的个人体会。图优化入门容易卡在两件事上一是不知道矩阵从哪来二是知道矩阵怎么来但不知道代码往哪儿填。我的建议很笨但很管用把本文这个例子写成可打印的单元测试第一次迭代的H、b、δ全部和手算对一遍对上之后再放开手脚。这个方法帮我过滤掉了一大堆低级错误。另一个小经验是每次加新的边类型先跑“只有一条边”的用例因为单边情况下数学退化成最简单形式哪一步错了立刻能看出来。视觉SLAM的图优化说白了就是一个带权重的矛盾调和器亲手算过一次之后后面那些库写的东西就都只是实现细节了。