做三维重建的朋友应该都有过这种经历一套稀疏重建流程跑下来特征匹配看着没问题三角化出来的点云也是那个形状可一叠加到画面上怎么都对不齐模型边缘发虚相机轨迹飘得离谱。你查了很久最后发现问题是出在缺了一次全局优化或者更准确地说缺了一次调参到位的“光束平差法Bundle Adjustment, BA”。在三维重建、SLAM、摄影测量这些领域BA不是“锦上添花”的步骤而是决定重建精度上限的关键环节。我最早接触BA时也被它那一堆矩阵求导、雅可比、海森矩阵吓住后来在OpenMVG、Colmap、Ceres这些开源工程里反复折腾才慢慢把它的原理和工程实现对上号。这篇笔记不打太极直接讲清楚三件事BA到底在优化什么、数学上怎么迭代求解、工程落地时哪里容易炸。你如果是刚开始接触相机标定或稀疏重建这篇文章可以帮你把概念理顺如果你已经在跑Colmap这类工具后面关于稀疏性结构、鲁棒核和参数化的避坑经验应该能让你少走一些弯路。1. 三维重建流程中BA到底站在哪一环很多教程喜欢一上来就贴公式但你连BA解决的是哪个环节的问题都没搞清公式看了也只是记住符号而已。我先把整个稀疏重建流程摊开看看BA在里面的真实位置。1.1 从特征匹配到全局优化的完整链路一个典型的增量式Incremental三维重建流程大致是这样第一步对每张图像提取特征点常用SIFT、ORB、SuperPoint等然后用描述子暴力匹配或近似最近邻匹配建立图像间的对应关系。第二步利用对极几何估计两张视图之间的本质矩阵或基础矩阵解出相对位姿作为初始种子对。第三步用三角化生成初始3D点。第四步当新图像加入时通过PnP估计新相机的位姿再用三角化补充新的3D点。第五步就是这个流程的收尾和核心把已经估计出的相机位姿、相机内参、3D点坐标放在一起做一次全局的Bundle Adjustment让所有参数在“最小化重投影误差”的意义下达到最优。第六步如果场景大还要做位姿图优化、回环检测后的全局BA、以及稠密重建和纹理映射。你看BA处在所有稀疏几何估计完成之后它是一个“全局精修”的角色。前面的每一步都是局部估计——三角化只管两个视图的几何关系PnP只管当前相机的位姿它们都没能把所有相机和所有3D点放在一个统一的优化框架里做整体调整。而BA做的恰恰是这个整体调整。从数学上看它就是一个大规模非线性最小二乘问题涉及到的变量是全部相机参数和全部3D点约束是所有的特征点观测。1.2 BA的输入、输出分别是什么这个听起来有点基础但我发现很多初学者在写代码时搞混了BA的输入输出导致数据结构设计得一团糟。BA的输入包含三部分相机参数包括内参fx, fy, cx, cy以及畸变系数也包括每个相机的外参旋转R和平移t。如果内参已经标定好可以在BA中固定不优化但绝大多数三维重建系统会把内参也放进待优化变量里尤其是用视频序列重建时内参往往有轻微变化。3D点坐标由初始三角化得到的稀疏3D点在BA中这些点也是待优化的变量。观测数据即特征点在图像上的像素坐标以及它的“身份”——这个特征点对应哪台相机的哪个3D点。观测数据是整个优化问题的“监督信号”是误差计算的目标值。输出则是优化后的相机内参、外参和3D点坐标。这里要特别强调BA并不会改变观测数据本身它只是调整模型参数让模型的预测结果更接近观测。1.3 为什么初值对于BA来说几乎是生死攸关的一句话BA是非线性优化它做的是“局部寻优”不是“全局搜索”。目标函数关于相机参数和3D点坐标是高度非线性的存在大量局部极小值。如果你给的初值离全局最优太远迭代过程很可能收敛到一个局部极小点甚至直接发散得到一团完全不可用的参数。我在工程里见过不少这种崩溃案例三角化初值因为基线太短导致深度估计严重错误喂给BA之后不仅3D点没修好反而把原本差不多的相机位姿也给带偏了。所以所有成熟的BA系统都非常依赖初值质量。这也是为什么增量式重建流程里要精心设计选帧顺序和三角化筛选策略尽量保证每一帧接入时初值已经“够好”。2. 数学视角重投影误差与目标函数明白了BA在流程中的位置下面进入它真正的核心——数学建模。这部分并不需要你看懂每一个矩阵求导的细节但你要理解它解决问题的思路。2.1 重投影误差3D点投影到图像上与你实际看到的像素相差多少先建立直觉假设现在我们有一台相机它的内外参我们知道还知道某个3D点的坐标。那么按照相机成像模型我们可以算出这个3D点会被投影到图像的哪个像素位置这个位置叫做“预测投影位置”。与此同时特征匹配算法告诉我们这个3D点在图像上的真实观测位置在某个像素坐标。这两个位置之间通常不会完全重合因为内外参和3D点坐标都带着估计误差它们之间的差就是“重投影误差”。于是BA的目标就很清晰了调整所有的相机参数和3D点坐标让所有这些重投影误差的平方和尽可能小。这就是“最小二乘”这个名字的来源。写成公式就是[ E(\mathcal{C}, \mathcal{P}) \sum_{i1}^{N}\sum_{j \in S(i)} \rho\left( \left| z_{ij} - \pi(\mathbf{K}_i, \mathbf{R}_i, \mathbf{t}_i, \mathbf{X}_j) \right|^2 \right) ]其中(\mathcal{C})表示所有相机参数集合(\mathcal{P})表示所有3D点集合(z_{ij})是第(i)个相机观察到第(j)个3D点的像素坐标(\pi(\cdot))是投影函数(\rho(\cdot))是鲁棒核函数后面会讲。如果你暂时不想管鲁棒核函数可以先把它当成一个恒等函数目标就退化成平方误差和。2.2 相机投影模型从世界坐标到像素坐标的完整链路要算重投影误差就得把投影函数(\pi)写清楚。这个过程分三步第一把世界坐标系下的3D点(\mathbf{X} (X, Y, Z)^T)变换到相机坐标系[ \mathbf{X}_{cam} \mathbf{R} \mathbf{X} \mathbf{t} ]其中(\mathbf{R})是旋转矩阵(\mathbf{t})是平移向量。第二把相机坐标系下的点投影到归一化平面[ x_n \frac{X_{cam}}{Z_{cam}}, \quad y_n \frac{Y_{cam}}{Z_{cam}} ]第三考虑镜头畸变并映射到像素坐标。在不考虑畸变时[ u f_x x_n c_x, \quad v f_y y_n c_y ]如果考虑畸变一般会先用(r^2 x_n^2 y_n^2)计算径向畸变项然后按(x_{distorted} x_n(1 k_1 r^2 k_2 r^4 k_3 r^6))这些公式修正最后再乘内参矩阵。这个投影链路的每一步都有对应参数参与。BA优化的目的就是找出让这个链路对全体观测都拟合得最好的那组参数。注意这里的(\mathbf{R})如果用9元素旋转矩阵表示优化的变量带有冗余约束必须满足正交且行列式为1所以工程实现中通常用李代数(\mathfrak{so}(3))或四元数来参数化旋转后面我会展开讲。2.3 为什么偏偏用重投影误差而不是三角化误差或3D点距离误差这个疑问我当初也有既然我们已经有了3D点为什么不直接最小化3D点坐标的偏差原因是3D点坐标本来就是我们估计出来的没有一个“真值”可以当参照而且单纯在3D空间里比较点与点的距离不能反映图像观测的噪声特性。图像上的特征匹配误差是以像素为单位衡量的重投影误差直接把误差定义在像素平面上和观测噪声的统计特性一致。与“三角化误差”相比也是同理三角化本质上是求解两个视图射线的交点它的误差天然具有视角相关的退化特性基线短时误差特别大。用重投影误差做全局优化相当于把“哪个视图更可信”这件事交给了优化过程本身去权衡比人为指定的几何误差更加合理。2.4 不止是简单平方和协方差与信息矩阵实际系统中不同特征点观测的可靠性并不一样。有的特征点纹理清晰、匹配稳定像素误差可能只有0.2像素有的特征点在弱纹理区域匹配误差可能达到2像素以上。如果一视同仁地对待它们高噪声观测就会把优化结果带偏。所以BA的正规做法是引入信息矩阵(\Omega_{ij})它是观测噪声协方差矩阵的逆。目标函数变成[ E \sum \rho\left( e_{ij}^T \Omega_{ij} e_{ij} \right) ]信息矩阵本质上是一个“权重”告诉优化器这个观测的噪声小请认真对待那个观测噪声大别太当真。在Ceres等库中你可以通过设置每个残差块的协方差或权重来实现这一点。从概率角度看这相当于在假设观测噪声服从高斯分布时最大化数据的似然概率也就是最大似然估计MLE。3. 核心求解器从高斯牛顿到LM目标函数建立之后剩下的问题变成怎么在参数空间中找到让目标函数最小的那组参数这是一个无约束非线性优化问题最常用的方法分两类高斯牛顿法Gauss-Newton和列文伯格-马夸尔特法Levenberg-Marquardt, LM。我把这两者的思路和坑都过一遍。3.1 把非线性误差线性化泰勒展开与雅可比矩阵重投影误差(e(\theta))对参数(\theta)是非线性的没法直接求解析最小值所以我们采用迭代策略在当前的参数估计(\theta)附近把误差函数做一阶泰勒展开[ e(\theta \Delta\theta) \approx e(\theta) J \Delta\theta ]其中(J)是误差对参数的雅可比矩阵Jacobian每一项表示某个误差分量对某个参数的偏导数。把这个线性近似代入平方误差函数我们就得到一个关于增量(\Delta\theta)的线性最小二乘问题可以求出当前最合适的更新步长。3.2 增量方程的推导正规方程具体来说对线性化后的误差求目标函数的梯度并令其为零可以得到著名的正规方程[ (J^T J) \Delta\theta -J^T e ]令(\mathbf{H} J^T J)(\mathbf{g} -J^T e)那么每次迭代的增量就是[ \Delta\theta -\mathbf{H}^{-1} \mathbf{g} ]这就是高斯牛顿法的全部精髓。注意(\mathbf{H})是误差对参数的近似海森矩阵不是真正的海森矩阵因为它忽略了二阶导数项但它在工程上足够好用因为计算量远小于真正的海森矩阵。3.3 高斯牛顿的致命伤病态与步长失控高斯牛顿法在实际中的表现并不总是让人满意问题主要出在两个方面。第一当(\mathbf{H})接近奇异时求逆会非常不稳定增量(\Delta\theta)可能变得巨大一次迭代就把参数推出合理范围第二线性化毕竟是局部近似如果当前参数离最优解较远大步长反而会跨过极小值点导致发散。三维重建里的BA恰恰经常遇到这种情况尤其是初值质量一般时。所以实际工程中很少直接用纯高斯牛顿而更常用LM算法。3.4 LM如何救场阻尼项的作用LM算法的改动非常巧妙在高斯牛顿的增量方程中加入一个阻尼项变成[ (J^T J \lambda \operatorname{diag}(J^T J)) \Delta\theta -J^T e ]其中(\lambda)是阻尼系数(\operatorname{diag})表示取对角元素构成的对角矩阵也有的实现用单位阵(I)但取对角阵的实际效果更好。当(\lambda)很大时(\lambda \operatorname{diag}(H))占据主导地位增量退化成梯度下降方向步长变小稳定但收敛慢当(\lambda)很小时算法近似于高斯牛顿法在最优解附近收敛速度极快。LM的灵魂在于(\lambda)的自动调节。常见策略是一次迭代后如果目标函数下降就减小(\lambda)让算法更“大胆”地接近高斯牛顿如果目标函数上升就增大(\lambda)让算法更“保守”退回梯度下降方向重试。Ceres等库还把(\lambda)的更新策略做了更精细的调整但核心思想没变。正是这种自适应机制让LM在初值不是特别理想时也能稳定工作。3.5 雅可比矩阵解析式还是数值差分求解增量方程的关键是计算雅可比矩阵(J)。工程中有两种做法手推解析表达式或者用数值差分。解析表达式效率高、精度好但推导过程容易出错尤其是涉及旋转求导和链式法则时。数值差分实现简单——对每个参数分量加一个小量(\epsilon)用((e(\theta\epsilon) - e(\theta))/\epsilon)近似偏导数但计算开销大且对浮点误差敏感。常规做法是先用数值雅可比验证解析雅可比的正确性再正式使用解析版。实际上Ceres官方文档也专门建议用户用NumericDiffCostFunction做交叉验证这点我后面讲避坑时还要提。4. 稀疏性结构BA能跑起来的真正原因如果你看前面这些求解方法可能会产生一个疑问BA动辄涉及几百个相机、数万个3D点参数总量轻松到几十万甚至上百万直接求逆矩阵不是要算到天荒地老这里的关键在于BA的雅可比矩阵和正规方程具有极强的稀疏性而我们正是利用这个稀疏性来大幅降低计算量。4.1 海森矩阵的稀疏模式一个误差项只连接一个相机和一个3D点回顾一下误差的定义每个重投影误差项(e_{ij})只和第(i)个相机参数和第(j)个3D点坐标有关和其他相机、其他3D点完全无关。这意味着雅可比矩阵(J)中每个误差行只在对应的相机参数列和3D点参数列上有非零块其余位置全是零。由此得到的近似海森矩阵(\mathbf{H} J^T J)也具有天然的块结构相机对相机块、点对点块分布在对角线上相机对点块分布在非对角位置但没有直接观测关系的相机-相机或点-点之间全部为零。这个结构直观来说就是每个3D点只“围观”它被观测到的少数几台相机数量通常远小于相机总数。所以(\mathbf{H})看起来很大但实际上绝大多数元素是零。4.2 Schur补技巧与边缘化先消去3D点再解相机利用这种稀疏性最经典的加速手段是Schur补技巧也叫边缘化Marginalization。具体做法是把未知量分成两组相机参数(\mathbf{c})和3D点参数(\mathbf{p})。增量方程可以写成块矩阵形式[ \begin{bmatrix} \mathbf{B} \mathbf{E} \ \mathbf{E}^T \mathbf{C} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{c} \ \Delta\mathbf{p} \end{bmatrix}\begin{bmatrix} \mathbf{v} \ \mathbf{w} \end{bmatrix} ]其中(\mathbf{B})是相机-相机块(\mathbf{C})是点-点块(\mathbf{E})是相机-点交叉块。由于每个3D点只关联少量相机(\mathbf{C})是一个块对角矩阵求逆的代价非常低。于是我们先用(\mathbf{C})消去(\Delta\mathbf{p})[ (\mathbf{B} - \mathbf{E} \mathbf{C}^{-1} \mathbf{E}^T) \Delta\mathbf{c} \mathbf{v} - \mathbf{E} \mathbf{C}^{-1} \mathbf{w} ]这里(\mathbf{S} \mathbf{B} - \mathbf{E} \mathbf{C}^{-1} \mathbf{E}^T)叫做Schur补它是一个只涉及相机参数的缩减系统维度远小于原始问题典型场景相机数几百个3D点数几万个缩减后求解量小好几个数量级。解出(\Delta\mathbf{c})后再回代到第二个方程就能得到(\Delta\mathbf{p})。这一步就是SLAM里经常说的“边缘化”marginalization或“Schur消元”。它相当于把3D点的贡献“压缩”到相机参数的约束中先解相机位姿再按需恢复3D点增量。4.3 工程库中的对应实现Ceres Solver中的SCHUR_JACOBI预处理器g2o中的稀疏求解器以及OpenMVG、Colmap底层调用的SuiteSparse或Eigen求解器核心都是这套思路。你在这些库里看到诸如LinearSolverType::SPARSE_SCHUR、DENSE_SCHUR等选项就是在选择是否利用这种稀疏结构、采用哪种具体的线性代数求解器。作为使用者你不必亲手实现Schur补但理解这一点对于调参极有帮助。比如场景中相机数量巨大而3D点数量相对较少时选择Schur消元的顺序可以反过来先消相机保留点即“inverse Schur”再比如DENSE_SCHUR适合相机数量不大例如几百个的BASPARSE_SCHUR适合相机数量很大的场景选错了线性求解器同样规模的问题收敛速度可能差出十倍。4.4 为什么说“BA的成功是由稀疏性决定的”回头看这个问题。如果没有利用稀疏性BA的时间复杂度大约是(O(N^3))其中(N)是所有参数的数量几万个点就直接算不动了。利用块结构和Schur补后复杂度主要取决于相机数量远小于点数所以在几百个相机、数万个点的规模下也能实时或准实时求解。这也是为什么BA能成为三维重建和实时SLAM的标配算法——它不只是一个数学优化方法更是一个“可计算化”的数学优化方法。5. 工程落地中的避坑清单这一节是我写这篇笔记最想分享的部分。原理看得再明白工程中仍是处处有坑。我挑几个我踩过、也看别人反复踩的典型问题来拆解。5.1 初值给定BA不是许愿池开头讲了初值对BA的关键性这里再说细一点。BA只能做局部优化如果你的初始相机位姿错了几个像素BA能帮你修正到亚像素精度但如果初始位姿错了一整条街BA只会礼貌地告诉你“无法收敛”或直接把全部参数带崩。更隐蔽的情况是某些位置的3D点初始深度严重错误BA虽然整体收敛了但局部区域的点云还是扭曲的。所以工程上的正确用法是先做充足的初始几何估计对极几何、PnP、三角化都要有合理的筛选和验证再用BA做精修。增量式重建系统中每一帧加入后立刻做一次局部的BA或位姿图优化而不是攒一批再统一全局BA这样能避免误差累积到无法挽回的程度。5.2 鲁棒核函数没有它数据稍微脏一点就炸真实场景中特征匹配不可能百分百正确。误匹配的外点outlier如果以平方误差形式进入目标函数哪怕只有一个它的巨大残差也会“拽”着整个优化结果偏离正确位置很远。这就是为什么目标函数里要引入鲁棒核函数(\rho(\cdot))。常用的鲁棒核有Huber核和Cauchy核。Huber核在误差较小时表现为平方损失误差超过阈值(\delta)后变为线性损失这样外点的惩罚不再随误差平方增长其“破坏力”被限制住了。用生活类比来说平方误差像是一个严格的老板员工犯一次错就扣全年奖金而Huber核像一个有底线的经理小错重罚、大错最多给个警告整体上更有分寸。实际项目中Huber核的阈值(\delta)通常取在1.0到2.0像素之间具体要看你的特征点定位精度。5.3 参数化旋转矩阵不是拿来优化的旋转矩阵有9个元素但只有3个自由度把它直接作为优化变量会有严重的冗余约束问题优化过程中一不小心就得到非法的“旋转矩阵”。所以工程中几乎总是用旋转的李代数(\mathfrak{so}(3))或单位四元数来参数化旋转。Ceres中通过AngleAxisRotatePoint、四元数等类型来处理g2o里也提供了对应的旋转参数类型。一个相关但容易被忽略的问题是四元数虽然只有4个元素但它带有一个单位长度约束优化时如果不做处理同样会退化。Ceres的做法是在流形Manifold层面对这个约束做处理不直接“裸奔”优化原始四元数分量。如果你不用这些现成库而是自己写优化器一定要记得在每次增量更新后把四元数重新归一化。5.4 固定自由度规范化约束BA的误差函数存在一个天然的“规范自由度”问题如果你把所有相机和3D点一起平移或旋转重投影误差完全不变因为投影过程是相对的如果所有3D点和相机间距同时缩放像素投影也不会改变单目视觉天然存在尺度不确定性。这意味着整个优化问题是“欠约束”的正规方程中(\mathbf{H})至少奇异7维6自由度刚性变换1自由度尺度。解决方法是固定一个相机的参数不动作为世界坐标系的锚点或者在目标函数中加入关于相机位姿或3D点位置的先验约束gauge prior。很多库默认会把第一帧相机固定住。我建议你在自写BA时也这么做否则你可能发现增量方程求解器报奇异或数值不稳定根本原因就在这里。5.5 数值精度请务必用double三维重建对浮点精度极其敏感。我记得有个项目在某种环境下用了float类型存储相机参数和3D点结果BA迭代时残差死活降不下来换成double后一切恢复正常。原因很简单BA中要反复做矩阵求逆和线性化float只有大约7位有效数字而相机参数和3D点的数值范围差异可能达到几个数量级比如平移量的量级是0.1到100而3D点坐标可能是1到1000在消元过程中舍入误差会被急剧放大。使用double虽然吃内存但换来的是稳定性和精确度这笔账很划算。5.6 验证收敛看什么指标怎么判断BA跑得对不对我一般看四个指标目标函数总残差是否单调下降LM算法里允许偶尔上升并重试但整体应下降。最大单个重投影误差是否显著降低如果还有某些点的残差特别大多半是外点或错误关联没清掉。增量(\Delta\theta)的模长是否趋近于零如果迭代后期增量还很大说明还没收敛。优化前后的平均重投影误差做定性对比误差应该从几像素甚至十几像素降到亚像素级。如果某个场景反复优化后残差还是很大不要盲目加大迭代次数先回头检查数据关联是否正确。BA能优化参数但不能修复错误的观测关联——观测本身就给错了再好的优化器也白搭。6. 从零实现一个最简BA只讲原理不动手看过很容易忘。我在这里给出一个极简的BA实现思路用Python和NumPy实现LM算法的核心循环。这个简化版本只包含纯高斯牛顿和LM的骨架不涉及稀疏Schur消元适合新手在几十行代码里建立对BA的直观感受。6.1 问题设定与数据结构假设我们有一台相机暂不考虑内参优化它在不同位置拍摄了多个3D点我们可以得到一组观测。那么待优化的变量就是相机位姿这里只优化一个相机的6自由度位姿用于演示和3D点坐标。为了更贴近真实BA我下面直接以“多相机多3D点”的抽象来设计数据结构但代码示例中实现LM的一步迭代即可。定义三类数据结构import numpy as np # 相机参数这里用 [rx, ry, rz, tx, ty, tz] 表示位姿前三个是旋转向量 # 3D点参数Nx3 的坐标数组 # 观测数据一个列表每个元素是 (camera_index, point_index, u, v) class BundleAdjustmentProblem: def __init__(self, cameras, points, observations): self.cameras cameras # Mx6 数组 self.points points # Nx3 数组 self.observations observations # (cam_idx, pt_idx, u, v)6.2 投影函数与残差计算投影函数的输入是一个相机的位姿参数、内参矩阵以及一个3D点坐标输出是预测像素坐标。为简单起见这里先忽略畸变。def project(camera_params, K, point): rx, ry, rz, tx, ty, tz camera_params R, _ cv2.Rodrigues(np.array([rx, ry, rz])) t np.array([tx, ty, tz]) X_cam R point t if X_cam[2] 1e-6: return None # 点位于相机后方投影无效 x_n X_cam[0] / X_cam[2] y_n X_cam[1] / X_cam[2] u K[0, 0] * x_n K[0, 2] v K[1, 1] * y_n K[1, 2] return np.array([u, v])计算残差就是把预测投影坐标与观测像素坐标相减def compute_residuals(problem, K): residuals [] for cam_idx, pt_idx, u, v in problem.observations: cam problem.cameras[cam_idx] pt problem.points[pt_idx] proj project(cam, K, pt) if proj is None: residuals.append(1e6) # 给一个很大的惩罚 else: residuals.append(proj - np.array([u, v])) return np.array(residuals).flatten()6.3 LM迭代的核心循环有了残差函数就可以用数值雅可比配合LM公式来迭代。下面的代码是核心循环的骨架def solve_lm(problem, K, max_iter50, tol1e-8): # 把待优化变量摊平成一维向量 param_vec np.concatenate([problem.cameras.flatten(), problem.points.flatten()]) dim len(param_vec) # 观测数量 n_obs len(problem.observations) residual_matrix np.zeros((n_obs, 2)) lam 1e-3 cost_prev None for it in range(max_iter): # 数值雅可比 J np.zeros((2 * n_obs, dim)) eps 1e-6 for i in range(dim): param_vec_pos param_vec.copy() param_vec_neg param_vec.copy() h max(abs(param_vec[i]) * eps, 1e-8) param_vec_pos[i] h param_vec_neg[i] - h # 更新问题参数计算残差然后填J的列 # 实际代码需要将param_vec重新填充回problem.cameras和problem.points # 计算残差向量r和代价函数 r compute_residuals(problem, K).flatten() cost 0.5 * np.sum(r**2) # LM增量方程 (J^T J lam * diag(J^T J)) delta -J^T r H J.T J g J.T r H_reg H lam * np.diag(np.diag(H)) try: delta np.linalg.solve(H_reg, -g) except np.linalg.LinAlgError: lam * 10 continue # 试更新看代价是否下降 param_new param_vec delta # 更新problem并重新计算代价 cost_new 0.5 * np.sum(compute_residuals(problem, K).flatten()**2) if cost_new cost: param_vec param_new lam max(lam * 0.5, 1e-10) if abs(cost - cost_new) tol: break else: lam * 10把这段代码补全后你可以构造一个简单的模拟场景随机生成一些3D点和相机位姿通过投影函数生成观测再给这些观测加上噪声然后给相机位姿和3D点一个故意偏离的初值跑BA你会看到重投影误差一步步降下来参数逐步接近真实值。这个过程能非常直观地展示BA的收敛行为和LM算法的自适应调节机制。6.4 从玩具实现到工程实现当然这个玩具版本没有利用稀疏性也没有处理旋转参数化和鲁棒核所以它只能处理极小的场景。当问题规模到了几百个相机、上万个点时你需要换成Ceres或g2o这类成熟的优化库并且利用它们的自动求导、稀疏线性求解器和流形支持。用现成库的优势很明显你只需要定义好残差函数Ceres里的CostFunction或AutoDiffCostFunction、参数块以及它们之间的关联关系求解器就能高效完成增量方程求解。我强烈建议你在写完玩具实现后再在Ceres里复现同样的问题对比两者的代码量和扩展难度。理解了底层逻辑之后你会发现用库里那些“魔法接口”时不再心虚也更能知道怎么调参。7. BA在更大三维重建系统中的位置最后再往宏观看一看。BA不只是稀疏重建里的一个模块它在整个视觉计算体系里是一块通用积木。在SLAM系统中BA承担了局部地图优化和全局位姿图优化的任务。局部BA滑动窗口中的相机位姿和路标点全局BA负责在检测到回环后消除累积漂移。在刚接触SLAM的读者看来ORB-SLAM这类系统里的局部建图和回环修正经常会用到BA底层求解器也正是我们前面说的这些原理。建图时点的数量多了之后还要配合关键帧选取、地图点筛选防止BA变量规模爆炸。在摄影测量和无人机测绘领域BA的变体更多除了传统的光束法平差还有带地面控制点的高精度联合平差、支持GPS/IMU先验约束的平滑BA。它们本质上都是在重投影误差的基础上增加先验误差项让优化结果受外部测量值约束。理解了BA的基本框架后你就知道这些看似复杂的变体不过是往目标函数里“加项”而已。此外随着深度学习和可微渲染的发展基于梯度下降的端到端三维重建越来越流行BA的思想也被借用到神经网络训练中通过可微的投影层和姿态回归损失来做联合优化。但无论前端形式怎么变后端要做的依然是“让预测投影接近观测像素”这件事。所以把BA吃透对理解和设计这些新方法也有很大帮助。我自己在实际项目里常用的一个组合是拿到图像序列后先用SfM工具如Colmap或OpenMVG跑出稀疏点云和相机位姿初值然后写一个脚本把初值导出到Ceres中针对特定场景设计自定义的鲁棒核权重和参数化方式再做一轮精细化BA。这样既能利用成熟工具的自动化流程又能在关键场景上保留足够的定制空间。踩过几次坑之后我的体会是BA这玩意儿的门槛不在公式而在“你能不能判断出问题出在哪一环”——是初值太差、外点没滤干净还是参数化选错了、线性求解器不匹配。把这几个问题想清楚BA大部分问题都能顺利解决。