凸优化和SLAM的关系历来是后端调参党绕不开的一道坎手里的BA问题明明看着很规整目标函数写出来不过是残差平方和可迭代一跑就卡进某个奇怪的局部极小出来的轨迹甚至能让地图离奇扭曲。如果你去翻优化理论的书大概率会看到一句这是非凸问题理论保证有限然后就没有然后了。这篇是凸优化理论系列的第三篇前两篇我们把凸集、凸函数、KKT条件和经典迭代算法都铺了一遍这一篇不做推导复习就干一件事把凸优化这套理论真正摆到SLAM的桌面上看看它在位姿图、Bundle Adjustment、鲁棒核函数和初始化里到底站什么位置哪些地方理论能保证哪些地方理论只能给你一个局部视野以及为什么你调参数时总感觉求解器在跟你作对。适合正在学视觉SLAM后端、被非线性优化折磨过、以及想理解为什么SLAM不用凸优化这类说法的读者。1. 从求解位姿到求解凸问题SLAM后端优化的第一层解剖1.1 旋转矩阵不是凸集才是一切矛盾的原点先说一个反直觉的结论SLAM里到处都在求解最小二乘而最小二乘本身几乎总是凸的真正让整个问题变非凸的不是能量函数写错了而是变量跑到了一个不凸的空间里。旋转矩阵的集合 {R | R^T R I, det R 1} 不是凸集——你把绕某轴转90°和转-90°两个旋转矩阵取平均得到的矩阵通常不再是旋转矩阵。这意味着什么意味着你没法像在欧氏空间那样在任意两个位姿之间直接做线性插值还保证结果合法。这个几何事实是SLAM后端所有非凸糟心事的总源头。其实判断一个问题是不是凸的只需要看两件事可行域是不是凸集目标函数在可行域上是不是凸函数。如果两个条件都满足理论可以承诺解的质量但凡坏了一个就只能依赖局部方法。SLAM里的位姿优化属于两个条件都坏的典型目标函数因为残差对位姿的非线性依赖不是凸的可行域因为旋转矩阵流形不是凸的。但要注意这两个坏的严重程度并不一样理解它们的区别是后面所有工程策略的基础。1.2 目标函数接近凸可行域却把人锁死在流形上如果我们在SLAM里单独拎出一个小问题已知相机位姿估计一批地图点的三维坐标。这时变量落在普通的欧氏空间残差模型在系数固定时是线性的透视投影的齐次形式目标函数是一个标准的最小二乘也就是凸二次函数性质非常好。可是一旦把位姿也作为变量放进去同时优化问题立刻变味残差对位姿的依赖是非线性的而且位姿必须落在刚体变换流形上。于是整个优化就变成了在流形上找一个点让残差尽量小的流形优化全局凸性瞬间丢失。这就是为什么BA问题在理论上被归类为非线性非凸问题但算法设计上又处处借用凸分析工具的原因。很多博客一上来就写BA是非凸的所以没有全局解这个结论本身没错但容易让人误解成那就随便给个初值硬迭代算了。实际工程里远没那么悲观因为流形这个非凸约束其实很薄只占状态空间很小一片只要初始值落进正确的吸引域局部凸近似就能给出很好的结果。这也是我们后面几节反复围绕怎么把初值搞进正确吸引域来展开的根本原因。2. 每一步迭代里都有一个凸子问题流形上的局部线性化2.1 从流形退到切空间为什么李代数能救场SLAM工程里最常用的应对办法是每次迭代不去惊动流形本身而是把位姿在当前位置附近做局部参数化。具体说位姿 T 用一个小的增量 δ 去扰动这个 δ 住在李代数给出的切空间里。在SLAM里位姿的扰动常用左扰动模型误差对δ作一阶泰勒展开得到一个形式非常熟的线性系统r ≈ r0 J δ。这个式子把原来定义在流形上的非线性问题在当前点附近改写成普通欧氏空间里的线性最小二乘。切空间通常就是R^6旋转扰动3维加平移扰动3维它是个凸集目标函数在这个局部近似下也是凸的。于是每一步迭代我们都在解一个局部凸子问题。这样做的直观好处是你在流形上走一步必须要小心下一步还在不在流形上但在切空间里你随便走走完再用指数映射把增量拉回流形。用生活里的话说你在一片球面上走路不敢迈大步怕掉出去但你可以先站在球面上在脚下那块平滑的切平面上自由移动一小步再落回球面。所有的数学折腾都是为了把不能随便插值的空间换成可以放心做线性近似的空间。2.2 Gauss-Newton步的出现其实是在说这个局部是凸的写一下这个局部问题的最优解δ* - (J^T J)^(-1) J^T r。从凸优化视角看J^T J是目标函数在当前点残差平方和的海森近似它天然半正定。对凸问题来说只要沿着梯度的反方向走总能下降而在流形问题里这个J^T J半正定其实是我们判断局部凸性好不好用的温度计。当J^T J出现近奇异甚至奇异时对应着某些方向几乎不可观局部地形像个山谷底迭代会沿弱方向反复振荡这正是工程中加了约束才稳定的本质原因。加上一个正定扰动λI也就是LM算法的阻尼后方程变成 (J^T J λI) δ -J^T r系数矩阵被强制变成正定步长从一个流形切空间上的牛顿步变成了一个带强凸性的正则化步。这一段很有意思LM算法名义上是数值技巧实际干的是把近似海森从可能半正定非凸局部危险区修正到强正定凸保护区的活。你可以把λ想象成冰面上撒的沙子本来你在光滑的冰面上病态矩阵滑来滑去站不稳撒了沙子增加摩擦加正则化每一步走得短但稳最终也能走到目的地。没有这一步纯高斯牛顿在弱可观方向基本就是原地踏步加乱窜。2.3 先验约束的数学本质往目标函数里注入凸性调过SLAM工程的都知道后端里加运动模型先验、加IMU因子、或者给某些关键帧加位置约束常常能让本来发散的系统稳住。很多人把这归因于信息多了自然更准这当然对但从凸优化角度还有一层更直接的理解每加一个高斯先验就等于在目标函数里多了一项 μ||x - x0||²它的海森是多加了 μI正好把弱可观方向顶起来让局部二次近似变得更凸、病态程度更低。这个视角对调试极有用当你的图优化老是收敛到离谱解时与其反复调单步迭代次数不如回头看看问题里的弱方向想想在这个方向上有没有几何上的合理约束能加。我见过有同学的项目某款室内定位设备的定位漂移主要来自深度方向不可观他加了一个平面约束后收敛质量立刻提升。这就是在改造问题的凸性轮廓不是算法变了是地形变了。凸优化里强凸性这个抽象概念落到工程上就是弱方向有没有被顶住。3. 鲁棒核函数一场凸性与工程效果之间的交易3.1 外点为什么会制造出假吸引盆先看一个最常见的失败模式特征误匹配产生的大残差。普通最小二乘对残差是平方惩罚一个偏移特别大的外点会给目标函数带来一个极陡的二次盆。这个盆在数值上往往比正确位置的盆还要深于是高斯牛顿迭代会被它牵着走系统一会儿就陷进那个假的最小值。问题的关键不在外点多而在平方函数对外点太慷慨——残差越大它的惩罚增长越快主导了整个下降方向。打个比方最小二乘像一个只看平均数的记账本一个天价错账就能把整月的账面带偏。特征匹配里哪怕只有几个错误匹配只要它们的误差足够大就能在目标函数里挖出一个比真实位置更深更陡的坑优化自然往那里跑。这也是为什么SLAM系统里单纯增加匹配数量有时反而更差——多了并不意味着更健壮可能只是在给假吸引盆添砖加瓦。3.2 Huber核的前与后凸与非凸的分水岭鲁棒核函数就是为了修正这个偏见。最常见的是Huber核残差小于阈值δ时保持二次惩罚大于δ时切换成线性惩罚。做一个凸性判断Huber核本身是凸函数换了它之后新的目标函数如果其他条件不变在欧氏变量空间里依然是凸的。它抑制外点的方式是让大残差的梯度饱和不再随残差增大而无限增大结果是外点的影响被限制住了但还没有被完全剔除。另一个常用的是Tukey双权核残差超过阈值之后直接把权重置零外点相当于从优化中被物理删除。Tukey的效果更狠但它不是凸函数核函数本身会导致目标函数存在多个局部极小。所以这里出现一个经典的trade-off我整理成表格方便对比核函数凸性对外点的态度数值表现初值敏感性普通最小二乘凸平方放大外点主导干净数据效果好有外点极易崩低但容易被外点带进假盆Huber凸梯度饱和限制影响稳健能容忍一定比例外点低收敛域较宽Tukey非凸超阈值直接删除外点清除彻底但可能出现多极小高离真值太远会误删内点工程上选哪种不是看论文里吹哪个好而是看你手里的数据外点占比和初值可信度。外点少、初值稳直接Huber就很舒服外点多、且你相信初值已经在真值附近Tukey的清洗能力才有用武之地。3.3 一种稳妥的组合拳先用凸核进盆再用硬核甩点我自己的经验是在优化初期用Huber核或甚至带比较大的δ的Huber先让迭代进到正确的吸引盆里等残差分布稳定下来再切换到Tukey核跑几轮把离群点彻底删除。这个策略在多个模拟和半实际项目里都表现不错步骤大致是这样首轮用Huber核跑标准迭代把位姿和地图点初步拉到一个合理的局部区域。统计当前残差分布根据中位数和绝对偏差设定一个合理的阈值。切换到Tukey核再做几轮迭代让超过阈值的残差权重归零。这个顺序背后的道理其实很简单非凸核的局部极小值不像凸核那样每一个都不会太离谱它只在初始点离真值足够近时才有好表现所以你要先借凸核的保底能力靠近真值再用硬核做强清洗。顺序反过来的话很容易让系统在开局就把正确的内点也当外点扔掉。 提示先用凸核进盆再用硬核甩点这个顺序千万别反过来。4. 真正可以写成凸优化的SLAM子问题初始化、平均化与松弛4.1 前端几何估计里的凸问题代数投影的舞台SLAM不是只有后端BA。前端里有一大批子问题其实天生就是凸的八点法估计本质矩阵、DLT估计单应矩阵、三角化固定位姿下的地图点这些步骤的第一步都是解一个线性最小二乘目标函数凸可行域是整个欧氏空间求解可以用最小二乘的闭式解或带约束的最小二乘处理之后再做SVD把解投影回到符合几何约束的流形上。也就是说SLAM前端经常是先解一个凸问题再做一次流形投影的复合结构。理解这一点有助于减少对SLAM全是非凸的偏见非凸往往只出现在同时优化位姿和结构的后端以及旋转相关变量上。很多初学者一上来就抱着反正非凸初值随便给的念头但前端那几步凸子问题其实是有全局最优解的用好了能给后端省掉大量麻烦。比如三角化如果位姿已知先做线性最小二乘得到初值再交给带深度正则的优化基本不会出大岔子。4.2 平移平均化和旋转平均化一个凸一个不凸把问题拆开看更有意思。已知两两相对平移方向恢复一组相机位置称为平移平均化。这通常在三维平移向量空间里做变量空间是凸的如果噪声假设为高斯目标函数也是凸的可以直接使用加权最小二乘去求全局最优不需要初值。但旋转平均化就是另一回事旋转矩阵的乘积约束本质上落在SO(3)流形上全局凸性没法保证初值直接影响结果。所以很多系统里平移初始化比旋转初始化容易做得好。下面这张表把常见子问题的凸性底数整理清楚方便查阅子问题变量空间目标函数全局凸性典型处理固定位姿的三角化欧氏空间二次凸线性最小二乘正则本质矩阵/单应估计欧氏空间流形约束二次先凸后投影DLTSVD平移平均化欧氏空间二次凸加权最小二乘旋转平均化SO(3)流形非线性非凸流形迭代初值优化完整BASE(3)×欧氏空间非线性非凸局部迭代鲁棒核这也是为什么大家经常先把旋转平均化问题用别的办法搞出一个不错的初值再去跑全局迭代。凸与非凸的分野不在名称上在变量空间和目标函数的组合上。4.3 SDP松弛用更大但凸的搜索空间换一个更稳的落点再往前一步位姿图初始化可以做成一个凸松弛问题。原始问题想找一组SE(3)位姿最小化相对测量误差这是非凸的但可以把它转换成对位姿变量协方差矩阵的优化只要求矩阵半正定加一些线性约束形成一个半定规划后者是标准凸优化问题有全局最优。得到松弛解后再做低秩投影把它拉回可用的位姿估计。这个方法依然是SLAM研究里的活跃话题实际系统里用它来产生比随机初值或线性插值初值好得多的开局。用不严谨但好懂的话说SDP松弛就像你要去一个翻山越岭才能到的目的地先在整个高空平面上画一个大的圆形搜索区凸找到这个区域的中心再落回山地流形上虽然不保证一定落到全局最优但至少比从山脚乱猜强得多。某开发者在模拟项目里做过对比用这种松弛做初始化的位姿图后端迭代的收敛率比随机给初值明显更稳迭代次数也少。对工程党来说SDP的代价是计算量偏大、矩阵规模上去了之后内存压力不小但在初始化阶段用一次收益远大于开销。5. 纸上凸优化与工程求解器之间隔着一层数值线性代数5.1 稀疏性、Schur补与海森的正定性是同一条链上的事真正把一个大型BA问题丢给求解器时理论里那些半正定凸函数的漂亮话要让位给数值线性代数。后端正规方程通常是稀疏的海森矩阵分块后位姿块和地图点块分离用Schur补先消去地图点变量得到位姿块的求解方程。这个消元过程在凸分析中对应分块二次规划的化简形式上很自然。但要注意消元之后的位姿块海森往往比原始分块海森病态得多因为它把地图点的信息浓缩进去了条件数变大数值上更容易出现奇怪的振荡。很多开源系统里看起来理论很清楚一跑就数值崩第一大坑就在这里。遇到这种情况我的处理方式比较朴素先不急着换求解器而是看消元后的位姿块最小特征值有多小找到那些几乎不可观的自由度再决定是加先验还是加约束。这个排查顺序能省下大量瞎试参数的时间因为你是在根据矩阵的真实几何结构找问题不是在碰运气。5.2 梯度为0不代表解对了非凸收敛判断要防鞍点凸优化里梯度为0基本可以断言找到了全局最优在约束满足的前提下因为在凸函数上没有局部极小区别于全局极小。可惜BA是非凸的梯度为0的地方可能是局部极小点也可能是鞍点。工程上常见的错误是收敛判据只看增量范数是否够小但这在一个峡谷状的弱可观方向上很容易判停结果轨迹已经在错误局部区域里稳定了。自己的习惯是在判断收敛前顺便看下局部海森的最小特征值方向是否对应着某个实际可观的变量如果某个方向特征值几乎为0且残差还不小就说明这地方并没有真正锁死只是数值上暂时不动了。这相当于给收敛判据加了一个凸性检查真正的极小点附近海森在可观测方向上应该是正的残差也被压到噪声水平附近反之鞍点附近会有明显的零空间方向残差还可能很大。5.3 三个能直接落地的调优习惯讲了一堆理论最后给三个我实际调SLAM后端时反复用到的习惯第一先分清问题本身凸还是约束搞乱了凸性。如果是前者直接用闭式解或带权最小二乘就好别上重型迭代框架如果是后者优先给初值而不是继续堆迭代次数。很多人一上来就用LM跑二十轮但初值离谱的话二十轮只是在一个错误的局部区域里转圈。第二鲁棒核函数分阶段用Huber起步、Tukey收尾。前几轮项目里我用这个组合后外点残留率下降非常明显而且系统对阈值的选择变得宽容了很多不用再把核函数的δ调得跟炼丹一样。第三加约束的本质是改善凸性轮廓。遇到发散先找弱可观方向在那个方向上加几何先验往往比换求解器更有效。某开发者在室内模拟项目里后端初值从随机改为SDP松弛再把Huber和Tukey按阶段切换整个流水线的收敛失败率降到了原来的大约三分之一这算是我目前见过性价比最高的两个改动。最后说点个人体会。很多人觉得凸优化是一门纯理论课程跟工程调试没什么关系但我的感受恰恰相反BA里几乎每一个让你头疼的现象——局部极小、发散、需要加正则、弱可观方向震荡——用凸性眼光去看都能对应到一个非常具体的几何或代数结构。把这步迭代解了个凸子问题这句话刻在脑子里以后调参数时你会少走很多弯路。下一篇我打算聊聊鲁棒核与图优化之间的具体耦合方式顺便给出一套可参考的参数初始策略到时候再一起展开。