光束法平差 (Bundle Adjustment)

用一台相机在房间里走一圈，再用 11.5 节的特征匹配找出各帧之间的对应点。现在你想同时知道两件事：相机每一帧在哪里（位姿），以及这些特征点在三维里的哪里（结构）。已有的工具各解决一半：三角化在相机位姿已知时算出点，PnP在点已知时算出相机。真实的建图两者都未知，只能自举：先用对极几何（11.1）和三角化得到一点粗糙的结构，再用 PnP 把新的一帧接进来，再三角化新的点……每一步都带着一点误差，误差会一层层叠上去，地图越走越歪。

光束法平差（Bundle Adjustment，BA）的想法是：别再“固定一边、求另一边”。把所有相机位姿和所有三维点都当未知数，让每个三维点投影到每个看到它的相机上的像素，同时尽量贴近实际测得的像素，即最小化总的重投影误差。名字来自“光束”：从每个三维点射向各相机中心的光线束，要一起调整到同时穿过各自观测到的像素。在高斯像素噪声下（4.6 节），它就是最大似然估计，所以被称为多视图几何的“金标准”。

COLMAP 和 ORB-SLAM 的地图之所以准，靠的就是它；NeRF 与 3D 高斯泼溅训练前需要的相机位姿，也几乎都来自它（12.4 节）。从算法上看，BA 没有新东西：它就是 6.9 节的非线性最小二乘，残差是重投影误差，用 Levenberg-Marquardt 求解。本节不重讲那套理论，只讲 BA 特有的几件事：残差与雅可比具体长什么样，为什么有 7 个多余的自由度，怎样利用稀疏结构（Schur 补）把规模推到百万个点，怎样抵御误匹配，以及解有多可靠。

想象下面的场景：5 台相机排成一道弧，看着 30 个空间点。每台相机看到的点会在它的图像上形成一个像素，这是观测（紫色）。如果你对相机位姿和点位置的猜测是错的，把猜测的点投影回这台相机，就落在另一个像素上，这是重投影（琥珀色）。两者之间的向量就是残差\(\hl{res}{\mathbf r_{ij}}\)（红线），可以想成一根橡皮筋；总代价 \(F\) 是所有橡皮筋的弹性势能。

未知数是 5 台相机的位姿（每台 6 个自由度）和 30 个点（每个 3 个坐标），共 120 个；观测有 111 条，每条给出两个方程（\(u\) 和 \(v\)），共 222 个。方程比未知数多，所以通常不存在一组解让所有橡皮筋同时松弛，但存在一组最好的折中：BA 就是去找它。下图让你自己把猜测弄坏，看橡皮筋怎样绷紧。

下面的图运行的就是真正的算法，没有预先算好的动画。每一步：(1) 在当前估计处线性化所有残差，得到每条观测的 \(2\times6\) 相机雅可比块和 \(2\times3\) 点雅可比块（解析公式见第 5 节，页面里还用有限差分验证过）；(2) 解阻尼正规方程 \((J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r\)；(3) 相机位姿用 \(T\leftarrow\Exp(\boldsymbol\delta_i)T\) 更新（6.10 节：在 \(\SE\) 上用指数映射而不是加法），点直接相加；(4) 代价下降就接受并把 \(\lambda\) 缩小为三分之一，否则拒绝并把 \(\lambda\) 乘 4（6.6、6.9 节的 LM）。

有两处新东西。第一，规范：只有图像像素没有绝对坐标，把整个场景刚体移动、再整体放大缩小，所有重投影都不变，所以有 \(6+1=7\) 个方向代价不变。要得到唯一解必须“固定规范”：这里冻结相机 1 的位姿（6 个量），并固定相机 1、2 之间的基线长度（1 个量：实现上是冻结基线方向上的一个平移分量，每步后再把基线长度归一化，这是对代价严格不变的相似变换）。切到“不固定”看会发生什么，并打开 \(J^\top J\) 的谱。第二，线性求解器：“稠密”把全部 \(6c+3p\) 个未知数一起解，“Schur 补”先消去点（第 4 节），两者给出同一个步长。

FIG 2 的 120 个未知数还好，真实的 SfM 有几千台相机、几百万个点，未知数上千万，稠密矩阵连存都存不下。但 BA 的雅可比极其稀疏：每个观测（\(J\) 的一个 \(2\times n\) 行块）只牵涉一台相机的 6 列和一个点的 3 列，其余全是零。于是正规方程矩阵 \(H=J^\top J\) 有固定的“箭头”形状：

注意 \(C\) 是块对角的：两个不同的点从不出现在同一条观测里，彼此不直接耦合。所以消去全部点只需要对 \(p\) 个 \(3\times3\) 的小矩阵求逆（每个约 30 次运算），再解一个只含相机的 \(6c\times6c\) 小方程组：这就是 Schur 补（第 5 节推导）。下面把它的规模摆出来比较。矩阵按“相机在前、点在后”排列，虚线框是 Schur 补 \(\hl{schur}{S}\) 所在的相机块；相机很多时，一个点被多台相机共同看到，才会让这些相机之间的块 \(S_{ik}\) 非零，轨迹越长，\(S\) 越接近带状。

特征匹配（11.5 节）总会混进错误对应。BA 里一个错误观测意味着一根拉向错误方向的橡皮筋，平方损失里它的“拉力”与偏离成正比，几个这样的观测就能把整张地图拽歪。解决办法是把 \(\tfrac12e^2\) 换成增长更慢的 \(\hl{rho}{\rho(e)}\)，并用 IRLS 求解：每条观测按当前残差得到权重 \(w=\rho'(e)/e\)，外点的残差大，权重就小（6.9、11.5 节）。下图在 111 条观测里随机替换若干条为错误像素（偏离 35 到 90 px），同样的初值、同样的算法，只换损失函数。

相机 \(i\) 的外参是 \({}^{c_i}T_w=(R_i,\mathbf t_i)\in\SE\)，把世界点变到相机系：\(\mathbf x_c=R_i X_j+\mathbf t_i\)（注意这是相机位姿 \({}^{w}T_{c}\) 的逆；画图时我们再取逆得到相机在世界里的位置 \(C_i=-R_i^\top\mathbf t_i\)）。内参 \(f_x,f_y,c_x,c_y\) 已知并共享。点 \(X_j\in\R^3\)，观测 \(\mathbf x_{ij}\in\R^2\)（像素），\(\mathcal O\) 是所有观测的集合。重投影残差与总代价是

\(\|\mathbf r\|_\Sigma=\sqrt{\mathbf r^\top\Sigma^{-1}\mathbf r}\) 是按像素噪声协方差“白化”后的长度（本页取 \(\Sigma=I\)，即 1 px 为单位）；\(\rho(e)=\tfrac12e^2\) 就是高斯噪声的最大似然（4.6 节），换成 Huber 或 Cauchy 就是鲁棒版。未知数共 \(6c+3p\) 个，方程 \(2|\mathcal O|\) 个；要有解需要 \(2|\mathcal O|\ge 6c+3p-7\)，其中 7 是下面的规范自由度。

相机用左扰动 \(T\leftarrow\Exp(\boldsymbol\delta)T\)，\(\boldsymbol\delta=(\boldsymbol\omega,\mathbf v)\)（角速度在前，和全课程一致）。5.7 节告诉我们小扰动下 \(\Exp(\boldsymbol\delta)\mathbf x_c\approx\mathbf x_c+\boldsymbol\omega\times\mathbf x_c+\mathbf v=\mathbf x_c-[\mathbf x_c]_\times\boldsymbol\omega+\mathbf v\)，所以相机坐标系下的点对扰动的导数是 \([-[\mathbf x_c]_\times\ \ I]\)（\(3\times6\)）。点的扰动直接加在 \(X_j\) 上，\(\mathbf x_c\) 对它的导数是 \(R_i\)。再乘上投影的导数（令 \(\mathbf x_c=(x,y,z)\)）：

把所有观测堆起来（IRLS 权重 \(w_k\) 放进对角矩阵 \(W\)），LM 的一步解 \((J^\top WJ+\lambda D)\boldsymbol\delta=-J^\top W\mathbf r\)。把未知数分成相机部分 \(\boldsymbol\delta_c\) 和点部分 \(\boldsymbol\delta_p\)，方程变成分块形式（\(\mathbf g=J^\top W\mathbf r\) 是梯度，阻尼已并入 \(B\) 和 \(C\) 的对角）：

推导就是对第二块行做高斯消元：由 \(E^\top\boldsymbol\delta_c+C\boldsymbol\delta_p=-\mathbf g_p\) 得 \(\boldsymbol\delta_p=-C^{-1}(\mathbf g_p+E^\top\boldsymbol\delta_c)\)，代入第一块行 \(B\boldsymbol\delta_c+E\boldsymbol\delta_p=-\mathbf g_c\) 即得关于 \(\boldsymbol\delta_c\) 的方程。\(S\) 叫约化相机系统（reduced camera system），大小 \(6c\times6c\)，与点数无关；它正是把所有点“边缘化”掉之后相机的信息矩阵。FIG 2 里“稠密”与“Schur”两条路给出同一步长（差 \(10^{-13}\) 量级），因为它们只是同一个线性方程的两种消元顺序。

取任意相似变换：旋转 \(R_g\)、平移 \(\mathbf t_g\)、尺度 \(s>0\)，令 \(X_j'=sR_gX_j+\mathbf t_g\)，\(R_i'=R_iR_g^\top\)，\(\mathbf t_i'=s\mathbf t_i-R_iR_g^\top\mathbf t_g\)。则 \(R_i'X_j'+\mathbf t_i'=s(R_iX_j+\mathbf t_i)\)，相机坐标整体放大 \(s\) 倍，而 \(\pi\) 对这样的缩放不变（\(x/z\) 不变），所以每一个重投影残差都不变，\(F\) 也不变。这个 7 参数族 \((R_g,\mathbf t_g,s)\) 是代价函数的一条 7 维“平坦谷”，\(H\) 因此有 7 维零空间，FIG 2 的谱画的就是这个。固定规范的常见做法：冻结第一台相机的位姿（6 维），再冻结一个量定尺度（这里是基线长度，也可以固定某一点的深度，或者把第二台相机平移的某一分量固定）。如果有别的信息，就能多固定几维：双目或带已知基线的系统自带尺度，只剩 6 维；加 IMU（重力方向）、GPS 或已知尺寸的物体，也能固定其中一部分。

单目 BA 无法恢复绝对尺度，这是物理上的限制，不是算法的缺陷：把整个世界放大两倍，照片一模一样。能否恢复结构还取决于几何：每个点至少要被两台相机看到，并且视线有足够的夹角（视差）；纯旋转的相机对所有点都没有视差，深度完全不可观测，\(C_j\) 奇异。

在最优点附近，参数估计的协方差近似是 \(\operatorname{Cov}(\hat{\boldsymbol\theta})\approx\sigma^2(J^\top J)^{-1}\)（4.4、4.6 节；它也是 Cramér-Rao 下界）。只有固定了规范 \(J^\top J\) 才可逆，所以协方差的数值依赖于你选了哪种规范：“点相对于相机 1 的不确定度”和“点云去掉整体相似变换之后的不确定度”是不同的量。FIG 5 用后者：先取固定规范下的逆，再把点云的 7 个相似方向投影掉（最小迹规范）。我们用 1500 次蒙特卡洛重复核对了它：预测的点位误差方差与实测之比为 0.99。

与位姿图的关系：消去所有点之后只剩相机，约化系统 \(S\) 就是相机位姿的信息矩阵，它在共视的相机之间有非零块。位姿图（Part IV 的 SLAM 后端）进一步用相机之间的相对位姿测量代替这些块，相当于把结构信息“边缘化后近似”掉：代价小得多，但丢掉了结构的细节，所以回环后常常再跑一次完整的 BA。

BA 给出的不只是一个解，还有这个解的可信程度。一台机器人沿墙走，每隔一段时间选一帧“关键帧”加入地图。关键帧选得太密，相邻帧之间几乎没有视差，新三角化的点深度很不准；选得稀一点，视差大了，但特征匹配会变难。下图用上面的 BA 反过来问这个问题：给定相机弧的宽度、相机数和像素噪声，每个点的位置不确定度（琥珀色椭球，3σ，已去掉整体相似变换）有多大？椭球是从 \(\sigma^2(J^\top J)^{-1}\) 里取出来的，没有跑任何随机试验。