1 引言

在本系列（《最小二乘问题详解：目录》）的前几篇文章中，我们系统探讨了运动恢复结构（Structure from Motion, SFM）中的三个核心子问题：

PnP 问题 （《最小二乘问题详解 10：PnP 问题求解》与《最小二乘问题详解 11：基于李代数的 PnP 优化》）：在已知部分 3D 结构的前提下，通过 2D-3D 对应关系求解新图像的相机位姿；
三角化 （《最小二乘问题详解 12：三角化中的非线性优化》）：在已知相机位姿的前提下，通过多视角 2D 观测反推空间点的 3D 位置。
对极几何与本质矩阵 E （《最小二乘问题详解13：对极几何中本质矩阵求解》与《最小二乘问题详解14：鲁棒估计与5点算法求解本质矩阵》）：在相机内参已知的前提下，从两视图的 2D-2D 匹配点对中恢复相机间的相对位姿（旋转 \(\mathbf{R}\) 与平移方向 \(\mathbf{t}\)）。

其实，这些方法构成了增量式 SFM 的基本流程：先用对极几何初始化两帧，再交替使用三角化和 PnP 逐步扩展重建规模。然而，它们本质上都是局部优化方法 ------每一步仅利用当前可用的信息进行独立求解，无法感知全局几何一致性。例如：

初始两帧的 E 矩阵分解可能存在错误解或尺度模糊；
三角化结果受位姿误差和图像噪声影响；
PnP 的位姿估计会继承并放大前期误差。

随着图像数量增加，这些局部误差会不断累积，导致最终重建结果出现尺度漂移、结构扭曲甚至拓扑错误 。要解决这一问题，我们需要一种全局优化机制 ，能够同时调整所有相机位姿和所有 3D 点，使得整个观测系统在统一的几何模型下达到最优自洽。这就是束平差（Bundle Adjustment, BA） ：一种非线性最小二乘优化方法，它将所有相机位姿和所有 3D 点作为联合变量，最小化所有观测点的重投影误差总和，从而实现全局一致的高精度重建。

BA 并不取代上述局部方法，而是作为 SFM 流程的最终全局优化阶段，对由 PnP 与三角化逐步构建的初始重建结果进行一致性精修。在本文中，我们将建立 BA 的完整数学模型，分析其稀疏结构，并基于 Ceres Solver 实现一个基础但可运行的 BA 系统。

2 几何模型

束平差建立在针孔相机成像模型 基础之上。考虑一个由 \(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）尽可能交汇于同一点。

3 问题建模

束平差本质上是一个大规模非线性最小二乘问题。我们首先定义可见性集合：

\[\Omega = \left\{ (i, j) \mid \text{第 } i \text{ 个相机观测到了第 } j \text{ 个 3D 点} \right\} \]

其大小 \(K = |\Omega|\) 表示有效观测总数。

3.1 优化变量

相机位姿：共 \(N\) 个，每个用李代数 \(\boldsymbol{\xi}_i = [\boldsymbol{\phi}_i^\top, \mathbf{t}_i^\top]^\top \in \mathbb{R}^6\) 表示（\(\boldsymbol{\phi}_i\) 为旋转向量），避免直接优化带约束的旋转矩阵；
3D 点坐标：共 \(M\) 个，\(\mathbf{X}_j \in \mathbb{R}^3\)。

将所有变量拼接为全局参数向量：

\[\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} \]

3.2 残差函数

对每一对观测 \((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)\) 将旋转向量映射为旋转矩阵。

3.3 目标函数

将所有残差堆叠为总残差向量 \(\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 \]

该问题具有以下关键特性：

非线性：因透视除法与旋转指数映射引入强非线性；
稀疏性 ：每个残差 \(\mathbf{r}_{ij}\) 仅依赖两个变量块（\(\boldsymbol{\xi}_i\) 与 \(\mathbf{X}_j\)），导致雅可比矩阵呈块状稀疏结构；
大规模 ：当 \(N, M\) 较大时（如 \(N=100, M=10^4\)），变量维数可达数万，需专用稀疏求解器。

4 理论推导

在《最小二乘问题详解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}\) 的特殊稀疏结构。下面我们将具体推导单个残差块的雅可比，并揭示其全局稀疏模式。

4.1 单个残差的雅可比分解

考虑一个观测 \((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], \]

其中：

\(\mathbf{J}_{\boldsymbol{\xi}i}^{(ij)} = \partial \mathbf{r}{ij} / \partial \boldsymbol{\xi}_i \in \mathbb{R}^{2 \times 6}\)，
\(\mathbf{J}_{\mathbf{X}j}^{(ij)} = \partial \mathbf{r}{ij} / \partial \mathbf{X}_j \in \mathbb{R}^{2 \times 3}\)。

这两部分可通过链式法则推导，其核心思想是：

\(\mathbf{J}_{\boldsymbol{\xi}_i}^{(ij)}\) 描述相机微小运动（旋转+平移）对重投影的影响，涉及李代数扰动模型。
\(\mathbf{J}_{\mathbf{X}_j}^{(ij)}\) 描述 3D 点微小移动对重投影位置的影响；

4.2 全局雅可比的稀疏结构

从以上雅可比分解的推导可以看到，每个残差块仅贡献两个非零子块，其余位置全为零。当所有 \(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

每 2 行 对应一个观测 \((i,j)\)；
每行中仅有 两个非零块 ：一个在相机 \(i\) 对应的 6 列，一个在 3D 点 \(j\) 对应的 3 列；
非零元素占比通常低于 0.1% ，属于典型的块状稀疏矩阵（Block-Sparse Matrix）。

4.3 稀疏求解优势

尽管束平差（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（舒尔补） 技巧，将原系统分解并消元，从而大幅降低计算量。

4.3.1 按变量类型重排系统

我们将优化变量分为两类：

相机参数：\(\mathbf{c} = [\boldsymbol{\xi}_1^\top, \dots, \boldsymbol{\xi}_N^\top]^\top \in \mathbb{R}^{6N}\)
3D点参数：\(\mathbf{x} = [\mathbf{X}_1^\top, \dots, \mathbf{X}_M^\top]^\top \ \in \mathbb{R}^{3M}\)

相应地，将信息矩阵 \(\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}, \]

其中：

\(\mathbf{B} \in \mathbb{R}^{6N \times 6N}\)：相机-相机块，表示相机之间的耦合（通过共视点间接产生）；
\(\mathbf{C} \in \mathbb{R}^{3M \times 3M}\)：点-点块；
\(\mathbf{E} \in \mathbb{R}^{6N \times 3M}\)：相机-点交叉块。

由于每个 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 块求逆）。

4.3.2：消去 3D 点变量

原线性系统为：

\[\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) } \]

4.3.3：求解与回代

求解简化系统：

新系统的规模仅为 \(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)\) ，可带来数个数量级的加速。

5. 实际案例

基于以上的理论推导，这里设计了一个完整的束平差数值优化实现。为了能验证初始估计存在显著偏差，全局优化仍能恢复高精度重建，按照如下设计思路生成了实验数据：

构建一个由 5 个相机和 100 个 3D 点组成的简单航拍场景；
相机沿 Y 轴匀速旋转并向前平移，模拟无人机飞行；
3D 点随机分布在相机前方一定深度范围内；
对真实重投影点添加高斯噪声（标准差 = 1 像素），模拟特征检测误差；
初始估计在真值基础上叠加小量噪声（平移 ±0.1m，点位置 ±0.1m），模拟 SFM 前端输出。

完整代码如下所示：

cpp 复制代码

#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] 属于欧氏空间，无需特殊流形；
- 虽然 3D 点未显式调用 AddParameterBlock，但 Ceres 会在 AddResidualBlock 中自动将其识别为优化变量（默认欧氏流形）。仅当需要覆盖默认行为（如固定、加边界、非欧结构）时，才需显式注册。
残差函数设计 ：旋转变换使用了 Ceres 提供的 ceres::QuaternionRotatePoint(q, point, p)，而非直接调用 Eigen 的 q * point。需要说明的是，从自动微分的角度看，Eigen 的四元数乘法本身是可导的，Ceres 的 AutoDiffCostFunction 完全能够正确处理它 ------因此，并非"不能用矩阵乘法"。使用 Ceres 专门提供的 QuaternionRotatePoint 这类底层几何操作函数，主要出于以下工程考量：
- 数值鲁棒性：该函数内部会对输入四元数进行归一化处理，即使在优化过程中因浮点误差导致四元数轻微失范（|q| ≠ 1），仍能保证旋转结果有效；
- 求导链路优化 ：作为标量函数实现，其雅可比推导路径更短、更稳定，避免了 Eigen 模板在 Jet 类型上实例化带来的潜在数值噪声或性能开销；
- 编译效率：不依赖 Eigen 的泛型模板，减少编译时间与二进制体积；
- 接口一致性 ：与 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\)），大幅加速大规模问题求解。

程序输出结果如下：

text 复制代码

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

从这个输出结果可以看到：

重投影误差显著下降 ：初始 RMSE 高达 30.16 像素 （严重失准），经 5 次迭代后降至 0.86 像素，达到亚像素级精度，表明 BA 成功校正了初始估计中的系统性偏差。
位姿恢复准确 ：各相机的位姿误差（旋转角距离 + 平移范数）稳定在 0.08 量级。注意第 0 帧误差非零是因为我们固定了其初始含噪估计值作为世界坐标系，而非真实值；其余帧均相对于此参考系优化，因此误差具有可比性。
高效收敛 ：得益于 SPARSE_SCHUR 求解器对稀疏结构的利用，整个优化过程仅耗时约 10 毫秒（在普通 CPU 上），验证了理论部分关于计算复杂度优势的分析。

最小二乘问题详解15：束平差原理与基础实现