在本系列（《最小二乘问题详解：目录》）的前几篇文章中，我们系统探讨了运动恢复结构（Structure from Motion, SFM）中的三个核心子问题：
其实，这些方法构成了增量式 SFM 的基本流程：先用对极几何初始化两帧，再交替使用三角化和 PnP 逐步扩展重建规模。然而，它们本质上都是局部优化方法——每一步仅利用当前可用的信息进行独立求解，无法感知全局几何一致性。例如：
随着图像数量增加，这些局部误差会不断累积，导致最终重建结果出现尺度漂移、结构扭曲甚至拓扑错误。要解决这一问题，我们需要一种全局优化机制，能够同时调整所有相机位姿和所有 3D 点，使得整个观测系统在统一的几何模型下达到最优自洽。这就是束平差（Bundle Adjustment, BA） ：一种非线性最小二乘优化方法，它将所有相机位姿和所有 3D 点作为联合变量，最小化所有观测点的重投影误差总和，从而实现全局一致的高精度重建。
BA 并不取代上述局部方法，而是作为 SFM 流程的最终全局优化阶段，对由 PnP 与三角化逐步构建的初始重建结果进行一致性精修。在本文中，我们将建立 BA 的完整数学模型，分析其稀疏结构，并基于 Ceres Solver 实现一个基础但可运行的 BA 系统。
束平差建立在针孔相机成像模型基础之上。考虑一个由 \(N\) 个相机和 \(M\) 个 3D 空间点构成的多视图系统。对于任意一个被第 \(i\) 个相机观测到的第 \(j\) 个 3D 点，其成像过程可描述如下：
世界坐标系中的 3D 点：记为 \(\mathbf{X}_j \in \mathbb{R}^3\)；
变换至第 \(i\) 个相机坐标系：
\[\mathbf{X}_j^{(i)} = \mathbf{R}_i \mathbf{X}_j + \mathbf{t}_i \]
其中 \(\mathbf{R}_i \in SO(3)\) 和 \(\mathbf{t}_i \in \mathbb{R}^3\) 分别表示第 \(i\) 个相机的旋转矩阵和平移向量（即外参）；
透视投影至归一化平面：
\[\mathbf{x}_{ij}^{\text{norm}} = \frac{1}{Z_j^{(i)}} \begin{bmatrix} X_j^{(i)} \\ Y_j^{(i)} \end{bmatrix} \]
应用内参得到像素坐标（假设相机内参 \(\mathbf{K}\) 已知且固定）：
\[\hat{\mathbf{x}}_{ij} = \mathbf{K} \begin{bmatrix} \mathbf{x}_{ij}^{\text{norm}} \\ 1 \end{bmatrix} = \pi\left( \mathbf{K} [\mathbf{R}_i \mid \mathbf{t}_i] \begin{bmatrix} \mathbf{X}_j \\ 1 \end{bmatrix} \right) \]
其中 \(\pi(\cdot)\) 表示齐次坐标到非齐次坐标的转换操作。
在理想无噪声情况下，重投影点 \(\hat{\mathbf{x}}_{ij}\) 应与图像中实际检测到的特征点 \(\mathbf{x}_{ij}\) 完全重合。然而，由于特征提取误差、初始位姿不准、三角化偏差等因素，二者通常存在偏差。BA 的目标正是通过调整所有 \(\{\mathbf{R}_i, \mathbf{t}_i\}\) 与 \(\{\mathbf{X}_j\}\)，使这一偏差全局最小化。
在几何上，BA 可理解为：调整所有相机光心位置与朝向，以及所有 3D 点的空间位置，使得从各相机出发、穿过对应图像点的“光线束”（bundle of rays）尽可能交汇于同一点。
束平差本质上是一个大规模非线性最小二乘问题。我们首先定义可见性集合：
\[\Omega = \left\{ (i, j) \mid \text{第 } i \text{ 个相机观测到了第 } j \text{ 个 3D 点} \right\} \]
其大小 \(K = |\Omega|\) 表示有效观测总数。
将所有变量拼接为全局参数向量：
\[\mathbf{p} = \left[ \boldsymbol{\xi}_1^\top, \dots, \boldsymbol{\xi}_N^\top, \mathbf{X}_1^\top, \dots, \mathbf{X}_M^\top \right]^\top \in \mathbb{R}^{6N + 3M} \]
对每一对观测 \((i, j) \in \Omega\)，定义重投影残差：
\[\mathbf{r}_{ij}(\boldsymbol{\xi}_i, \mathbf{X}_j) = \mathbf{x}_{ij} - \pi\left( \mathbf{K} \left[ \exp([\boldsymbol{\phi}_i]_\times) \,\middle|\, \mathbf{t}_i \right] \begin{bmatrix} \mathbf{X}_j \\ 1 \end{bmatrix} \right) \in \mathbb{R}^2 \]
其中 \(\exp([\boldsymbol{\phi}_i]_\times)\) 将旋转向量映射为旋转矩阵。
将所有残差堆叠为总残差向量 \(\mathbf{r}_{\text{total}} \in \mathbb{R}^{2K}\)，束平差的目标为：
\[\min_{\mathbf{p}} \ \| \mathbf{r}_{\text{total}}(\mathbf{p}) \|^2 = \sum_{(i,j) \in \Omega} \left\| \mathbf{r}_{ij}(\boldsymbol{\xi}_i, \mathbf{X}_j) \right\|^2 \]
该问题具有以下关键特性：
在《最小二乘问题详解4：非线性最小二乘》与《最小二乘问题详解8：Levenberg-Marquardt方法》中，我们已系统介绍了 Gauss-Newton（GN）与 Levenberg-Marquardt（LM）方法求解非线性最小二乘问题的一般框架。束平差（BA）作为其典型应用场景，同样通过迭代求解如下线性化系统：
\[\left( \mathbf{J}^\top \mathbf{J} + \lambda \mathbf{I} \right) \Delta \mathbf{p} = -\mathbf{J}^\top \mathbf{r}, \]
其中 \(\mathbf{J} = \partial \mathbf{r}_{\text{total}} / \partial \mathbf{p}\) 为总残差对所有参数的雅可比矩阵，\(\Delta \mathbf{p}\) 为参数更新量。
BA 的高效求解关键在于 \(\mathbf{J}\) 的特殊稀疏结构。下面我们将具体推导单个残差块的雅可比，并揭示其全局稀疏模式。
考虑一个观测 \((i, j) \in \Omega\)，其残差为：
\[\mathbf{r}_{ij} = \mathbf{x}_{ij} - \hat{\mathbf{x}}_{ij}, \quad \hat{\mathbf{x}}_{ij} = \pi\left( \mathbf{K} [\mathbf{R}_i \mid \mathbf{t}_i] \begin{bmatrix} \mathbf{X}_j \\ 1 \end{bmatrix} \right). \]
该残差仅依赖两个变量：相机 \(i\) 的位姿 \(\boldsymbol{\xi}_i\) 和 3D 点 \(j\) 的坐标 \(\mathbf{X}_j\)。因此，其雅可比可分解为两部分：
\[\frac{\partial \mathbf{r}_{ij}}{\partial \mathbf{p}} = \left[ \underbrace{\mathbf{0} \cdots \mathbf{J}_{\boldsymbol{\xi}_i}^{(ij)} \cdots \mathbf{0}}_{\text{关于相机位姿}}, \quad \underbrace{\mathbf{0} \cdots \mathbf{J}_{\mathbf{X}_j}^{(ij)} \cdots \mathbf{0}}_{\text{关于3D点}} \right], \]
其中：
这两部分可通过链式法则推导，其核心思想是：
从以上雅可比分解的推导可以看到，每个残差块仅贡献两个非零子块，其余位置全为零。当所有 \(K\) 个残差的雅可比垂直堆叠，得到总雅可比矩阵 \(\mathbf{J} \in \mathbb{R}^{2K \times (6N + 3M)}\)。其结构如下图所示：
Bundle Adjustment Jacobian Sparsity
Columns (Optimization Variables)
---------------------------------------------------------
| Cameras (6N) | Points (3M) |
---------------------------------------------------------
Rows (Residuals)
r11 | XXXX | XXX
r12 | XXXX | XXX
r13 | XXXX | XXX
r21 | XXXX | XXX
r22 | XXXX | XXX
r31 | XXXX | XXX
r41 | XXXX| XXX
每一行只有两个非零块：
camera_i
point_j
尽管束平差（BA）的目标函数是非线性的，但在 Gauss-Newton 或 Levenberg-Marquardt 框架下，每一步迭代都归结为求解一个大型线性方程组：
\[\mathbf{H} \Delta \mathbf{p} = -\mathbf{g}, \quad \text{其中 } \mathbf{H} = \mathbf{J}^\top \mathbf{J},\ \mathbf{g} = \mathbf{J}^\top \mathbf{r}. \]
这里 \(\mathbf{H} \in \mathbb{R}^{(6N+3M) \times (6N+3M)}\) 是所谓的信息矩阵（或 Hessian 近似），\(\mathbf{g}\) 是梯度向量。
虽然原始雅可比矩阵 \(\mathbf{J}\) 是高度稀疏的，但其信息矩阵 \(\mathbf{H} = \mathbf{J}^\top \mathbf{J}\) 通常是稠密的——因为两个相机若共视同一个 3D 点，它们的参数块就会在 \(\mathbf{H}\) 中产生非零耦合项。直接对 \(\mathbf{H}\) 进行 Cholesky 分解的复杂度为 \(O((6N+3M)^3)\)，在大规模问题中不可接受。
然而，BA 的特殊结构——每个残差仅连接一个相机和一个 3D 点——使得 \(\mathbf{H}\) 具有可利用的块结构。我们可以通过 Schur Complement（舒尔补） 技巧，将原系统分解并消元，从而大幅降低计算量。
我们将优化变量分为两类：
相应地，将信息矩阵 \(\mathbf{H}\) 和梯度 \(\mathbf{g}\) 按此分块：
\[\mathbf{H} = \begin{bmatrix} \mathbf{B} & \mathbf{E} \\ \mathbf{E}^\top & \mathbf{C} \end{bmatrix}, \quad \mathbf{g} = \begin{bmatrix} \mathbf{v} \\ \mathbf{w} \end{bmatrix}, \]
其中：
由于每个 3D 点仅被一组相机观测，且不同 3D 点之间无共享残差，因此 \(\mathbf{C}\) 是一个块对角矩阵：
\[\mathbf{C} = \operatorname{diag}\left( \mathbf{C}_1, \mathbf{C}_2, \dots, \mathbf{C}_M \right), \]
其中每个 \(\mathbf{C}_j \in \mathbb{R}^{3 \times 3}\) 仅与观测到点 \(j\) 的相机有关。这意味着 \(\mathbf{C}^{-1}\) 可以并行、高效地计算（只需对每个 3×3 块求逆）。
原线性系统为：
\[\begin{cases} \mathbf{B} \Delta \mathbf{c} + \mathbf{E} \Delta \mathbf{x} = -\mathbf{v} \quad &(1)\\ \mathbf{E}^\top \Delta \mathbf{c} + \mathbf{C} \Delta \mathbf{x} = -\mathbf{w} \quad &(2) \end{cases} \]
由方程 (2) 解出 \(\Delta \mathbf{x}\)：
\[\Delta \mathbf{x} = -\mathbf{C}^{-1} (\mathbf{E}^\top \Delta \mathbf{c} + \mathbf{w}). \]
代入方程 (1)，得到仅关于 \(\Delta \mathbf{c}\) 的方程：
\[\mathbf{B} \Delta \mathbf{c} - \mathbf{E} \mathbf{C}^{-1} (\mathbf{E}^\top \Delta \mathbf{c} + \mathbf{w}) = -\mathbf{v}, \]
整理后即为 Schur Complement 系统：
\[\boxed{ \left( \mathbf{B} - \mathbf{E} \mathbf{C}^{-1} \mathbf{E}^\top \right) \Delta \mathbf{c} = -\left( \mathbf{v} - \mathbf{E} \mathbf{C}^{-1} \mathbf{w} \right) } \]
求解简化系统：
新系统的规模仅为 \(6N \times 6N\)。虽然 \(\mathbf{B} - \mathbf{E} \mathbf{C}^{-1} \mathbf{E}^\top\) 仍是稠密的（称为“填充” fill-in），但当 \(N \ll M\)（通常如此，如 100 相机 vs 10,000 点），其规模远小于原始 \((6N+3M)^2\)。
回代求 \(\Delta \mathbf{x}\)：
一旦得到 \(\Delta \mathbf{c}\)，即可通过
\[\Delta \mathbf{x} = -\mathbf{C}^{-1} (\mathbf{E}^\top \Delta \mathbf{c} + \mathbf{w}) \]
快速计算所有 3D 点的更新量。由于 \(\mathbf{C}\) 是块对角的，这一步可完全并行化。
计算复杂度对比：
与常规的 Cholesky 分解算法相比，Schur Complement 方法的复杂度从 \(O((6N + 3M)^3)\) 下降到 \(O((6N)^3 + M)\) ，可带来数个数量级的加速。
基于以上的理论推导，这里设计了一个完整的束平差数值优化实现。为了能验证初始估计存在显著偏差，全局优化仍能恢复高精度重建，按照如下设计思路生成了实验数据：
完整代码如下所示：
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include <random>
#include <vector>
using namespace std;
struct Camera {
double q[4]; // quaternion
double t[3]; // translation
};
struct Point3D {
double xyz[3];
};
struct Observation {
int cam_id;
int pt_id;
double x;
double y;
};
struct ReprojectionError {
ReprojectionError(double x, double y, double fx, double fy, double cx,
double cy)
: x_(x), y_(y), fx_(fx), fy_(fy), cx_(cx), cy_(cy) {}
template <typename T>
bool operator()(const T* const q, const T* const t, const T* const point,
T* residuals) const {
T p[3];
ceres::QuaternionRotatePoint(q, point, p);
p[0] += t[0];
p[1] += t[1];
p[2] += t[2];
T xp = p[0] / p[2];
T yp = p[1] / p[2];
T u = T(fx_) * xp + T(cx_);
T v = T(fy_) * yp + T(cy_);
residuals[0] = u - T(x_);
residuals[1] = v - T(y_);
return true;
}
static ceres::CostFunction* Create(double x, double y, double fx, double fy,
double cx, double cy) {
return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>(
new ReprojectionError(x, y, fx, fy, cx, cy));
}
double x_, y_;
double fx_, fy_, cx_, cy_;
};
double ComputeRMSE(const vector<Camera>& cams, const vector<Point3D>& pts,
const vector<Observation>& obs, double fx, double fy,
double cx, double cy) {
double err = 0;
int n = 0;
for (auto& o : obs) {
const Camera& c = cams[o.cam_id];
const Point3D& p = pts[o.pt_id];
Eigen::Quaterniond q(c.q[0], c.q[1], c.q[2], c.q[3]);
Eigen::Vector3d t(c.t[0], c.t[1], c.t[2]);
Eigen::Vector3d P(p.xyz[0], p.xyz[1], p.xyz[2]);
Eigen::Vector3d Pc = q * P + t;
double u = fx * Pc.x() / Pc.z() + cx;
double v = fy * Pc.y() / Pc.z() + cy;
double du = u - o.x;
double dv = v - o.y;
err += du * du + dv * dv;
n += 2;
}
return sqrt(err / n);
}
double PoseError(const Camera& a, const Camera& b) {
Eigen::Quaterniond qa(a.q[0], a.q[1], a.q[2], a.q[3]);
Eigen::Quaterniond qb(b.q[0], b.q[1], b.q[2], b.q[3]);
double rot = qa.angularDistance(qb);
Eigen::Vector3d ta(a.t[0], a.t[1], a.t[2]);
Eigen::Vector3d tb(b.t[0], b.t[1], b.t[2]);
double trans = (ta - tb).norm();
return rot + trans;
}
int main() {
int num_cams = 5;
int num_pts = 100;
double fx = 800, fy = 800, cx = 320, cy = 240;
vector<Camera> gt_cams(num_cams);
vector<Point3D> gt_pts(num_pts);
vector<Observation> obs;
mt19937 rng(0);
normal_distribution<double> noise(0, 1);
// 生成真实相机
for (int i = 0; i < num_cams; i++) {
double ang = i * 0.2;
Eigen::Quaterniond q(Eigen::AngleAxisd(ang, Eigen::Vector3d::UnitY()));
gt_cams[i].q[0] = q.w();
gt_cams[i].q[1] = q.x();
gt_cams[i].q[2] = q.y();
gt_cams[i].q[3] = q.z();
gt_cams[i].t[0] = i * 0.5;
gt_cams[i].t[1] = 0;
gt_cams[i].t[2] = 0;
}
// 生成3D点
for (int i = 0; i < num_pts; i++) {
gt_pts[i].xyz[0] = (rand() % 100) / 50.0 - 1;
gt_pts[i].xyz[1] = (rand() % 100) / 50.0 - 1;
gt_pts[i].xyz[2] = 4 + (rand() % 100) / 50.0;
}
// 生成观测
for (int i = 0; i < num_cams; i++) {
Eigen::Quaterniond q(gt_cams[i].q[0], gt_cams[i].q[1], gt_cams[i].q[2],
gt_cams[i].q[3]);
Eigen::Vector3d t(gt_cams[i].t[0], gt_cams[i].t[1], gt_cams[i].t[2]);
for (int j = 0; j < num_pts; j++) {
Eigen::Vector3d P(gt_pts[j].xyz[0], gt_pts[j].xyz[1], gt_pts[j].xyz[2]);
Eigen::Vector3d Pc = q * P + t;
double u = fx * Pc.x() / Pc.z() + cx;
double v = fy * Pc.y() / Pc.z() + cy;
Observation o;
o.cam_id = i;
o.pt_id = j;
o.x = u + noise(rng);
o.y = v + noise(rng);
obs.push_back(o);
}
}
vector<Camera> est_cams = gt_cams;
vector<Point3D> est_pts = gt_pts;
// 添加初始噪声
for (auto& c : est_cams) {
for (int k = 0; k < 3; k++) c.t[k] += noise(rng) * 0.1;
}
for (auto& p : est_pts) {
for (int k = 0; k < 3; k++) p.xyz[k] += noise(rng) * 0.1;
}
cout << "Initial RMSE: "
<< ComputeRMSE(est_cams, est_pts, obs, fx, fy, cx, cy) << endl;
ceres::Problem problem;
ceres::Manifold* q_manifold = new ceres::QuaternionManifold();
for (int i = 0; i < num_cams; i++) {
problem.AddParameterBlock(est_cams[i].q, 4, q_manifold);
problem.AddParameterBlock(est_cams[i].t, 3);
}
for (auto& o : obs) {
ceres::CostFunction* cost =
ReprojectionError::Create(o.x, o.y, fx, fy, cx, cy);
problem.AddResidualBlock(cost, nullptr, est_cams[o.cam_id].q,
est_cams[o.cam_id].t, est_pts[o.pt_id].xyz);
}
// Fix first camera
problem.SetParameterBlockConstant(est_cams[0].q);
problem.SetParameterBlockConstant(est_cams[0].t);
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;
options.minimizer_progress_to_stdout = true;
options.max_num_iterations = 50;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout << summary.BriefReport() << endl;
cout << "Final RMSE: " << ComputeRMSE(est_cams, est_pts, obs, fx, fy, cx, cy)
<< endl;
cout << "\nPose error\n";
for (int i = 0; i < num_cams; i++) {
cout << "cam " << i << " : " << PoseError(gt_cams[i], est_cams[i]) << endl;
}
return 0;
}
在这段代码实现中，需要注意如下关键机制：
参数化与流形注册：使用 AddParameterBlock 指定参数块的更新规则（如绑定流形、设为常量等）；使用 AddResidualBlock 则将所引用的参数内存注册为优化变量（默认采用欧氏流形），无需额外声明：
ceres::QuaternionManifold 处理旋转约束，二者在优化效果上等价。q[4] 存储，并通过 ceres::QuaternionManifold 约束其单位性；t[3] 和 3D 点 xyz[3] 属于欧氏空间，无需特殊流形；AddParameterBlock，但 Ceres 会在 AddResidualBlock 中自动将其识别为优化变量（默认欧氏流形）。仅当需要覆盖默认行为（如固定、加边界、非欧结构）时，才需显式注册。残差函数设计：旋转变换使用了 Ceres 提供的 ceres::QuaternionRotatePoint(q, point, p)，而非直接调用 Eigen 的 q * point。需要说明的是，从自动微分的角度看，Eigen 的四元数乘法本身是可导的，Ceres 的 AutoDiffCostFunction 完全能够正确处理它——因此，并非“不能用矩阵乘法”。使用 Ceres 专门提供的 QuaternionRotatePoint 这类底层几何操作函数，主要出于以下工程考量：
Jet 类型上实例化带来的潜在数值噪声或性能开销；ceres::QuaternionManifold 配套使用，确保参数更新与几何操作遵循相同的四元数约定（w, x, y, z）。消除零空间自由度：调用 SetParameterBlockConstant 固定第一帧相机的位姿（q 和 t）。这是 BA 能收敛的必要条件：由于整个场景在 3D 空间中可任意刚体变换而不改变重投影误差，Hessian 矩阵天然奇异。固定一个相机相当于设定世界坐标系原点，消除 6 个不可观测自由度（3 旋转 + 3 平移）。
稀疏求解器选择：设置 options.linear_solver_type = ceres::SPARSE_SCHUR，启用基于 Schur Complement 的稀疏求解策略。Ceres 会自动识别“相机-点”二分图结构，构建并高效求解约简系统（规模仅 \(6N \times 6N\)），大幅加速大规模问题求解。
程序输出结果如下：
Initial RMSE: 30.1646
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 4.549507e+05 0.00e+00 1.87e+06 0.00e+00 0.00e+00 1.00e+04 0 6.63e-04 3.06e-03
1 8.934163e+02 4.54e+05 6.91e+04 0.00e+00 9.99e-01 3.00e+04 1 3.16e-03 6.44e-03
2 3.666374e+02 5.27e+02 5.55e+02 2.76e-01 1.00e+00 9.00e+04 1 6.72e-04 7.77e-03
3 3.664255e+02 2.12e-01 1.75e+02 8.00e-02 1.00e+00 2.70e+05 1 6.67e-04 8.90e-03
4 3.664170e+02 8.57e-03 2.01e+01 2.69e-02 1.00e+00 8.10e+05 1 6.15e-04 9.82e-03
Ceres Solver Report: Iterations: 5, Initial cost: 4.549507e+05, Final cost: 3.664170e+02, Termination: CONVERGENCE
Final RMSE: 0.856057
Pose error
cam 0 : 0.081944
cam 1 : 0.0811149
cam 2 : 0.0821392
cam 3 : 0.083811
cam 4 : 0.0855873
从这个输出结果可以看到：
SPARSE_SCHUR 求解器对稀疏结构的利用，整个优化过程仅耗时约 10 毫秒（在普通 CPU 上），验证了理论部分关于计算复杂度优势的分析。此内容由惯性聚合(RSS阅读器)自动聚合整理，仅供阅读参考。 原文来自 — 版权归原作者所有。