做三维重建的朋友应该都有过这种经历：一套稀疏重建流程跑下来，特征匹配看着没问题，三角化出来的点云也是那个形状，可一叠加到画面上怎么都对不齐，模型边缘发虚，相机轨迹飘得离谱。你查了很久，最后发现问题是出在缺了一次全局优化，或者更准确地说，缺了一次调参到位的“光束平差法（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_{i=1}^{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适合相机数量不大（例如几百个）的BA，SPARSE_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_iter=50, tol=1e-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大部分问题都能顺利解决。
