光束法平差 (Bundle Adjustment)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 补）把规模推到百万个点，怎样抵御误匹配，以及解有多可靠。

Walk a camera around a room and use the feature matching of Lesson 11.5 to find corresponding points across frames. Now you want two things at once: where the camera was at every frame (the poses) and where those feature points are in 3D (the structure). The tools you have each solve half: triangulation computes points when the poses are known, PnP computes a camera when the points are known. Real mapping knows neither, so it bootstraps: epipolar geometry (11.1) and triangulation give a rough structure, PnP registers the next frame, triangulation adds new points, and so on. Every step carries a little error, the errors stack up, and the map bends.

The idea of bundle adjustment (BA) is to stop "fix one side, solve the other". Treat all camera poses and all 3D points as unknowns, and ask that the projection of every point into every camera that saw it lands close to the pixel actually measured: minimize the total reprojection error. The name comes from the "bundles" of light rays from each 3D point to the camera centres, which are adjusted until they all pass through the pixels they were observed at. Under Gaussian pixel noise (Lesson 4.6) this is the maximum-likelihood estimate, hence the title "gold standard" of multi-view geometry.

It is why the maps of COLMAP and ORB-SLAM are accurate, and the camera poses needed before NeRF or 3D Gaussian splatting training mostly come out of it too (Lesson 12.4). As an algorithm BA has nothing new: it is the nonlinear least squares of Lesson 6.9 with the reprojection error as residual, solved by Levenberg-Marquardt. This lesson does not repeat that theory. It covers what is specific to BA: what the residual and Jacobian actually look like, why there are 7 superfluous degrees of freedom, how sparsity (the Schur complement) pushes the size to millions of points, how to survive mismatches, and how far to trust the answer.

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

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

Picture this: 5 cameras on an arc looking at 30 points in space. Each camera sees every point (or most of them) as a pixel in its image: the observation (purple). If your guess for the camera poses and the point positions is wrong, projecting the guessed point back into that camera lands on a different pixel: the reprojection (amber). The vector between them is the residual \(\hl{res}{\mathbf r_{ij}}\) (red line); think of it as a rubber band. The total cost \(F\) is the elastic energy of all the bands.

The unknowns are the poses of 5 cameras (6 degrees of freedom each) and 30 points (3 coordinates each): 120 numbers. There are 111 observations, each giving two equations (\(u\) and \(v\)): 222 in total. More equations than unknowns, so in general no setting relaxes every band at once, but there is a best compromise, and BA is the search for it. Below you can ruin the guess yourself and watch the bands stretch.

下面的图运行的就是真正的算法，没有预先算好的动画。每一步：(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 节），两者给出同一个步长。

The figure below runs the genuine algorithm, no pre-computed animation. Each step: (1) linearize all residuals at the current estimate, giving for every observation a \(2\times6\) camera Jacobian block and a \(2\times3\) point block (analytic formulas in Section 5, also checked against finite differences in the page); (2) solve the damped normal equations \((J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r\); (3) update the camera poses by \(T\leftarrow\Exp(\boldsymbol\delta_i)T\) (Lesson 6.10: the exponential map on \(\SE\) instead of addition) and add the step to the points; (4) if the cost drops, accept and divide \(\lambda\) by 3, otherwise reject and multiply \(\lambda\) by 4 (the LM of Lessons 6.6 and 6.9).

Two new things. First, the gauge: images have pixels but no absolute coordinates, so moving the whole scene rigidly and then scaling it up or down changes no reprojection. There are \(6+1=7\) directions along which the cost is flat. A unique answer requires "fixing the gauge": here we freeze the pose of camera 1 (6 numbers) and hold the baseline length between cameras 1 and 2 (1 number: in the code, one translation component along the baseline is frozen and the baseline is renormalized after each step, a similarity transform that leaves the cost exactly unchanged). Switch to "free" to see what happens, and open the spectrum of \(J^\top J\). Second, the linear solver: "dense" solves all \(6c+3p\) unknowns at once, "Schur" eliminates the points first (Section 4); both give the same step.

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

The 120 unknowns of FIG 2 are fine, but a real SfM job has thousands of cameras and millions of points, tens of millions of unknowns, too many even to store as a dense matrix. Yet the BA Jacobian is extremely sparse: each observation (a \(2\times n\) row block of \(J\)) touches only the 6 columns of one camera and the 3 columns of one point; everything else is zero. So the normal-equation matrix \(H=J^\top J\) has a fixed "arrowhead" shape:

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

Notice that \(C\) is block diagonal: two different points never appear in the same observation, so they are not directly coupled. Eliminating all the points therefore needs only the inversion of \(p\) small \(3\times3\) matrices (about 30 operations each) followed by one small camera-only system of size \(6c\times6c\): this is the Schur complement (derived in Section 5). Below, compare the sizes. The matrix is ordered "cameras first, points last"; the dashed box marks where the Schur complement \(\hl{schur}{S}\) lives. Cameras with a shared point are the only ones whose block \(S_{ik}\) is nonzero, so on a long trajectory \(S\) is close to banded.

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

Feature matching (Lesson 11.5) always lets some wrong correspondences through. In BA a wrong observation is a rubber band pulling the wrong way; under the squared loss its pull grows with the error, and a few of them are enough to bend the whole map. The cure is to replace \(\tfrac12e^2\) with a slower-growing \(\hl{rho}{\rho(e)}\) and solve by IRLS: each observation gets a weight \(w=\rho'(e)/e\) from its current residual, so a large residual means a small weight (Lessons 6.9 and 11.5). Below, a few of the 111 observations are replaced at random by wrong pixels (displaced by 35 to 90 px); same start, same algorithm, only the loss changes.

相机 \(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\) 是所有观测的集合。重投影残差与总代价是

The extrinsics of camera \(i\) are \({}^{c_i}T_w=(R_i,\mathbf t_i)\in\SE\), taking a world point to the camera frame: \(\mathbf x_c=R_i X_j+\mathbf t_i\) (note this is the inverse of the camera pose \({}^{w}T_{c}\); for drawing we invert it to get the camera position \(C_i=-R_i^\top\mathbf t_i\) in the world). The intrinsics \(f_x,f_y,c_x,c_y\) are known and shared. Points are \(X_j\in\R^3\), observations \(\mathbf x_{ij}\in\R^2\) (pixels), and \(\mathcal O\) is the set of all observations. The reprojection residual and the total cost are

\(\|\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 是下面的规范自由度。

\(\|\mathbf r\|_\Sigma=\sqrt{\mathbf r^\top\Sigma^{-1}\mathbf r}\) is the length "whitened" by the pixel-noise covariance (this page uses \(\Sigma=I\), i.e. 1 px units); \(\rho(e)=\tfrac12e^2\) is the Gaussian maximum likelihood (Lesson 4.6), and Huber or Cauchy give the robust versions. There are \(6c+3p\) unknowns and \(2|\mathcal O|\) equations; a solution needs \(2|\mathcal O|\ge 6c+3p-7\), where 7 is the gauge freedom below.

相机用左扰动 \(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)\)）：

Cameras use the left perturbation \(T\leftarrow\Exp(\boldsymbol\delta)T\) with \(\boldsymbol\delta=(\boldsymbol\omega,\mathbf v)\) (angular part first, as everywhere in the course). Lesson 5.7 says that for small perturbations \(\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\), so the derivative of the camera-frame point with respect to the perturbation is \([-[\mathbf x_c]_\times\ \ I]\) (\(3\times6\)). A point perturbation is added straight onto \(X_j\), and \(\mathbf x_c\) changes by \(R_i\) per unit. Multiplying by the derivative of the projection (with \(\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\) 的对角）：

Stack all observations (the IRLS weights \(w_k\) go into a diagonal matrix \(W\)); one LM step solves \((J^\top WJ+\lambda D)\boldsymbol\delta=-J^\top W\mathbf r\). Split the unknowns into the camera part \(\boldsymbol\delta_c\) and the point part \(\boldsymbol\delta_p\) and the equations take block form (\(\mathbf g=J^\top W\mathbf r\) is the gradient; the damping is absorbed in the diagonals of \(B\) and \(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}\) 量级），因为它们只是同一个线性方程的两种消元顺序。

The derivation is Gaussian elimination on the second block row: from \(E^\top\boldsymbol\delta_c+C\boldsymbol\delta_p=-\mathbf g_p\) we get \(\boldsymbol\delta_p=-C^{-1}(\mathbf g_p+E^\top\boldsymbol\delta_c)\), and substituting into the first block row \(B\boldsymbol\delta_c+E\boldsymbol\delta_p=-\mathbf g_c\) gives the equation for \(\boldsymbol\delta_c\) alone. \(S\) is called the reduced camera system; its size \(6c\times6c\) does not depend on the number of points, and it is exactly the information matrix of the cameras after marginalizing all points out. In FIG 2 the "dense" and "Schur" routes give the same step (difference of order \(10^{-13}\)) because they are just two elimination orders of the same linear equation.

取任意相似变换：旋转 \(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\) 奇异。

Take any similarity transform with rotation \(R_g\), translation \(\mathbf t_g\) and scale \(s>0\), and set \(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\). Then \(R_i'X_j'+\mathbf t_i'=s(R_iX_j+\mathbf t_i)\): the camera-frame coordinates are all scaled by \(s\), and \(\pi\) is invariant to such a scaling (\(x/z\) does not change), so every reprojection residual is unchanged and so is \(F\). This 7-parameter family \((R_g,\mathbf t_g,s)\) is a 7-dimensional flat valley of the cost, \(H\) therefore has a 7-dimensional null space, and the spectrum in FIG 2 shows exactly that. The usual way to fix the gauge: freeze the pose of the first camera (6 dimensions), then freeze one more quantity to set the scale (here the baseline length; one could also fix the depth of one point, or one component of the second camera's translation). Extra information fixes more: a stereo rig or any system with a known baseline brings its own scale and leaves 6; an IMU (gravity direction), GPS or an object of known size fixes part of the rest.

Monocular BA cannot recover absolute scale; this is a physical limit, not a defect of the algorithm: scale the whole world by two and the photographs are identical. Whether the structure can be recovered also depends on geometry: each point must be seen by at least two cameras with enough angle between the rays (parallax); a purely rotating camera has no parallax on any point, depths are completely unobservable and \(C_j\) is singular.

在最优点附近，参数估计的协方差近似是 \(\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。

Near the optimum the covariance of the estimate is approximately \(\operatorname{Cov}(\hat{\boldsymbol\theta})\approx\sigma^2(J^\top J)^{-1}\) (Lessons 4.4 and 4.6; it is also the Cramér-Rao bound). \(J^\top J\) is invertible only once the gauge is fixed, so the numbers depend on which gauge you picked: "uncertainty of a point relative to camera 1" and "uncertainty of the point cloud after removing the global similarity" are different quantities. FIG 5 uses the latter: invert in a fixed gauge, then project out the 7 similarity directions of the point cloud (the minimum-trace gauge). We checked it against 1500 Monte-Carlo repetitions: the ratio of measured to predicted point-error variance is 0.99.

Relation to pose graphs: after eliminating all points only the cameras remain, and the reduced system \(S\) is the information matrix of the camera poses, with nonzero blocks between co-visible cameras. A pose graph (the SLAM back end of Part IV) goes further and replaces those blocks by relative-pose measurements between cameras, i.e. it marginalizes the structure and approximates the result: far cheaper, but the structure detail is lost, which is why a full BA is often run once more after a loop closure.

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

BA gives not only a solution but also how far to trust it. A robot walks along a wall and every so often picks a "keyframe" for the map. Pick them too densely and neighbouring frames have almost no parallax, so newly triangulated points have poor depth; pick them sparsely and parallax is big but feature matching gets harder. The figure turns the BA above around to ask this question: given the width of the camera arc, the number of cameras and the pixel noise, how large is the uncertainty of each point (amber ellipsoids, 3σ, with the global similarity removed)? The ellipsoids come from \(\sigma^2(J^\top J)^{-1}\); no random trials are run.