1. 引言

在上一篇文章《最小二乘问题详解15：束平差原理与基础实现》中，我们从几何模型出发，系统推导了 束平差（Bundle Adjustment, BA） 的数学形式，并基于 Ceres Solver 实现了一个基础但完整的 BA 系统。在理想合成数据下，该系统能够将初始存在偏差的相机位姿与 3D 点高效优化至亚像素级精度，充分验证了 BA 作为"全局一致性精修"工具的强大能力。

然而，理论上的优雅并不总能直接转化为工程上的稳定。在真实的 BA 应用场景（ SFM / SLAM / 稀疏重建）中不会那么理想，往往会遇到各种问题。其中最为常见的就是由于误匹配造成的外点（Outliers）污染的问题。

2. 实例

为了更直观地说明外点对 BA 优化的影响，以及如何在工程实践中提升 BA 的稳健性，这里通过一个完整的实例进行演示。在该示例中，我们构造一个简单但具有代表性的 多视图重建场景。具体设置如下：

相机数量：5 个
空间点数量：100 个
相机模型：理想 pinhole 相机
观测噪声：像素级高斯噪声
外点比例：10%

在生成观测数据时，我们首先按照真实几何关系计算每个 3D 点在相机中的投影位置，并叠加 高斯像素噪声 。随后按照设定的比例随机注入 错误匹配（外点），即直接生成随机像素坐标来替代真实观测值，从而模拟真实特征匹配中不可避免的误匹配问题。具体代码实现如下：

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];
  double t[3];
};

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 ReprojectionErrorValue(const Camera& c, const Point3D& p,
                              const Observation& o, double fx, double fy,
                              double cx, double cy) {
  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;

  return sqrt(du * du + dv * dv);
}

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) {
    double e =
        ReprojectionErrorValue(cams[o.cam_id], pts[o.pt_id], o, fx, fy, cx, cy);

    err += e * e;
    n++;
  }

  return sqrt(err / n);
}

void SolveBA(vector<Camera>& cams, vector<Point3D>& pts,
             const vector<Observation>& obs, double fx, double fy, double cx,
             double cy, bool robust) {
  ceres::Problem problem;

  ceres::Manifold* q_manifold = new ceres::QuaternionManifold();

  for (auto& c : cams) {
    problem.AddParameterBlock(c.q, 4, q_manifold);
    problem.AddParameterBlock(c.t, 3);
  }

  for (auto& p : pts) problem.AddParameterBlock(p.xyz, 3);

  for (auto& o : obs) {
    ceres::CostFunction* cost =
        ReprojectionError::Create(o.x, o.y, fx, fy, cx, cy);

    ceres::LossFunction* loss = nullptr;

    if (robust) loss = new ceres::HuberLoss(1.0);

    problem.AddResidualBlock(cost, loss, cams[o.cam_id].q, cams[o.cam_id].t,
                             pts[o.pt_id].xyz);
  }

  problem.SetParameterBlockConstant(cams[0].q);
  problem.SetParameterBlockConstant(cams[0].t);

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.max_num_iterations = 50;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  cout << summary.BriefReport() << endl;
}

int main() {
  int num_cams = 5;
  int num_pts = 100;

  double fx = 800, fy = 800;
  double cx = 320, cy = 240;

  // 外点比例
  double outlier_ratio = 0.1;

  vector<Camera> gt_cams(num_cams);
  vector<Point3D> gt_pts(num_pts);
  vector<Observation> obs;

  mt19937 rng(0);

  uniform_real_distribution<double> uni(-1.0, 1.0);
  uniform_real_distribution<double> uni01(0.0, 1.0);
  uniform_real_distribution<double> pixel_x(0.0, 640.0);
  uniform_real_distribution<double> pixel_y(0.0, 480.0);

  normal_distribution<double> noise(0.0, 1.0);

  // 生成相机
  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] = uni(rng);
    gt_pts[i].xyz[1] = uni(rng);
    gt_pts[i].xyz[2] = 4.0 + uni(rng);
  }

  // 生成观测
  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);

      // 注入外点
      if (uni01(rng) < outlier_ratio) {
        o.x = pixel_x(rng);
        o.y = pixel_y(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.2;

  for (auto& p : est_pts)
    for (int k = 0; k < 3; k++) p.xyz[k] += noise(rng) * 0.2;

  cout << "Initial RMSE: "
       << ComputeRMSE(est_cams, est_pts, obs, fx, fy, cx, cy) << endl;

  cout << "\n===== BA without robust =====\n";
  SolveBA(est_cams, est_pts, obs, fx, fy, cx, cy, false);

  cout << "RMSE: " << ComputeRMSE(est_cams, est_pts, obs, fx, fy, cx, cy)
       << endl;

  cout << "\n===== BA with Huber =====\n";
  est_cams = gt_cams;
  est_pts = gt_pts;
  SolveBA(est_cams, est_pts, obs, fx, fy, cx, cy, true);

  cout << "RMSE: " << ComputeRMSE(est_cams, est_pts, obs, fx, fy, cx, cy)
       << endl;

  cout << "\n===== Outlier Filtering =====\n";

  vector<Observation> filtered;

  for (auto& o : obs) {
    double e = ReprojectionErrorValue(est_cams[o.cam_id], est_pts[o.pt_id], o,
                                      fx, fy, cx, cy);

    if (e < 5.0) filtered.push_back(o);
  }

  cout << "Observations: " << obs.size() << " -> " << filtered.size() << endl;

  SolveBA(est_cams, est_pts, filtered, fx, fy, cx, cy, true);

  cout << "Final RMSE: "
       << ComputeRMSE(est_cams, est_pts, filtered, fx, fy, cx, cy) << endl;
}

在这个案例中，我们按照 10% 的比例随机注入外点。这些外点的像素位置与真实投影完全无关，因此其重投影误差往往会达到 数百像素级别，从而对 BA 优化产生显著干扰。具体的优化流程如下：

加入像素噪声：模拟真实图像测量误差。
加入初始位姿误差：在真实相机位姿基础上叠加随机扰动，模拟 SFM / SLAM 中不准确的初始估计。
注入错误匹配（Outliers）：按 10% 的比例随机生成错误观测。
直接进行 BA 优化：不使用鲁棒核函数，观察外点对优化结果的影响。
引入鲁棒核函数（Huber Loss）：在 BA 中对大残差进行权重抑制。
BA 后进行外点剔除：根据重投影误差过滤异常观测。
再次执行 BA 优化：在清洗后的数据上进行最终优化。

整体流程可以总结为一个典型的 稳健 BA pipeline：

text 复制代码

Initial
  ↓
BA
  ↓
Robust BA
  ↓
Outlier Filtering
  ↓
Final BA

程序的实际运行结果如下所示：

text 复制代码

Initial RMSE: 295.759

===== BA without robust =====
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  2.186829e+07    0.00e+00    5.19e+06   0.00e+00   0.00e+00  1.00e+04        0    5.52e-04    1.37e-03
   1  1.102282e+09   -1.08e+09    5.19e+06   0.00e+00  -7.89e+01  5.00e+03        1    1.04e-03    2.59e-03
   2  7.761862e+10   -7.76e+10    5.19e+06   0.00e+00  -5.67e+03  1.25e+03        1    3.09e-04    3.07e-03
   3  2.597820e+08   -2.38e+08    5.19e+06   0.00e+00  -1.74e+01  1.56e+02        1    3.35e-04    3.54e-03
   4  5.889476e+09   -5.87e+09    5.19e+06   0.00e+00  -4.32e+02  9.77e+00        1    3.11e-04    3.98e-03
   5  1.061306e+07    1.13e+07    1.13e+06   0.00e+00   8.92e-01  1.88e+01        1    6.78e-04    4.77e-03
   6  9.469239e+06    1.14e+06    1.10e+06   4.45e+01   6.42e-01  1.92e+01        1    6.48e-04    5.51e-03
   7  8.256255e+06    1.21e+06    2.75e+05   1.14e+02   9.29e-01  5.24e+01        1    7.67e-04    6.45e-03
   8  1.052581e+07   -2.27e+06    2.75e+05   5.23e+02  -1.44e+00  2.62e+01        1    4.22e-04    7.20e-03
   9  8.731070e+06   -4.75e+05    2.75e+05   3.68e+02  -3.35e-01  6.54e+00        1    2.92e-04    7.66e-03
  10  7.442423e+06    8.14e+05    5.54e+05   1.51e+02   8.10e-01  8.60e+00        1    7.57e-04    8.70e-03
  11  6.690504e+06    7.52e+05    3.93e+05   2.07e+02   1.11e+00  2.58e+01        1    6.74e-04    9.51e-03
  12  1.583961e+07   -9.15e+06    3.93e+05   8.84e+02  -1.08e+01  1.29e+01        1    2.74e-04    9.88e-03
  13  1.601285e+09   -1.59e+09    3.93e+05   5.77e+02  -2.47e+03  3.23e+00        1    3.49e-04    1.05e-02
  14  6.374333e+06    3.16e+05    4.11e+05   1.92e+02   1.12e+00  9.68e+00        1    6.33e-04    1.14e-02
  15  5.985169e+06    3.89e+05    1.60e+06   3.16e+02   7.17e-01  1.05e+01        1    7.66e-04    1.25e-02
  16  4.864876e+06    1.12e+06    2.78e+05   2.13e+02   1.17e+00  3.16e+01        1    6.26e-04    1.32e-02
  17  4.497102e+06    3.68e+05    8.45e+05   3.91e+02   4.41e-01  3.16e+01        1    6.38e-04    1.42e-02
  18  4.218995e+06    2.78e+05    3.98e+05   3.12e+02   4.43e-01  3.15e+01        1    7.15e-04    1.50e-02
  19  4.120921e+06    9.81e+04    9.27e+04   3.49e+02   7.55e-01  3.63e+01        1    7.22e-04    1.58e-02
  20  4.602581e+06   -4.82e+05    9.27e+04   4.92e+02  -7.51e+00  1.81e+01        1    3.33e-04    1.63e-02
  21  4.080593e+06    4.03e+04    5.43e+04   2.72e+02   1.06e+00  5.44e+01        1    7.30e-04    1.74e-02
  22  4.029644e+06    5.09e+04    2.21e+05   6.65e+02   8.69e-01  9.12e+01        1    7.73e-04    1.82e-02
  23  3.991119e+06    3.85e+04    2.64e+05   1.01e+03   8.74e-01  1.57e+02        1    7.79e-04    1.93e-02
  24  3.969039e+06    2.21e+04    2.21e+05   1.11e+03   8.23e-01  2.14e+02        1    6.81e-04    2.02e-02
  25  3.959024e+06    1.00e+04    7.61e+04   1.17e+03   8.29e-01  2.99e+02        1    6.91e-04    2.10e-02
  26  3.955334e+06    3.69e+03    1.68e+04   1.64e+03   8.11e-01  3.93e+02        1    6.40e-04    2.18e-02
  27  3.953939e+06    1.39e+03    3.35e+03   2.22e+03   8.20e-01  5.33e+02        1    6.28e-04    2.26e-02
  28  3.953339e+06    6.00e+02    1.22e+03   2.37e+03   8.40e-01  7.77e+02        1    6.86e-04    2.35e-02
  29  3.952988e+06    3.51e+02    6.41e+02   2.46e+03   8.37e-01  1.12e+03        1    8.45e-04    2.44e-02
  30  3.952734e+06    2.54e+02    3.17e+02   2.45e+03   8.34e-01  1.59e+03        1    6.99e-04    2.55e-02
  31  3.952528e+06    2.06e+02    1.89e+02   2.47e+03   8.25e-01  2.20e+03        1    6.50e-04    2.62e-02
  32  3.952359e+06    1.69e+02    1.17e+02   2.57e+03   8.21e-01  2.99e+03        1    7.71e-04    2.71e-02
  33  3.952223e+06    1.37e+02    8.09e+01   2.74e+03   8.22e-01  4.08e+03        1    7.05e-04    2.80e-02
  34  3.952113e+06    1.10e+02    8.33e+01   3.00e+03   8.26e-01  5.65e+03        1    7.68e-04    2.90e-02
  35  3.952024e+06    8.89e+01    9.15e+01   3.26e+03   8.33e-01  8.03e+03        1    6.43e-04    3.01e-02
  36  3.951950e+06    7.44e+01    1.09e+02   3.67e+03   8.33e-01  1.14e+04        1    7.37e-04    3.11e-02
  37  3.951888e+06    6.16e+01    1.15e+02   4.13e+03   8.37e-01  1.64e+04        1    6.83e-04    3.21e-02
  38  3.951836e+06    5.17e+01    1.24e+02   4.69e+03   8.42e-01  2.42e+04        1    7.78e-04    3.35e-02
  39  3.951792e+06    4.43e+01    1.41e+02   5.44e+03   8.43e-01  3.58e+04        1    6.88e-04    3.46e-02
  40  3.951755e+06    3.68e+01    1.34e+02   6.23e+03   8.60e-01  5.72e+04        1    7.06e-04    3.57e-02
  41  3.951722e+06    3.35e+01    1.85e+02   7.66e+03   8.60e-01  9.12e+04        1    6.43e-04    3.67e-02
  42  3.951692e+06    2.99e+01    2.15e+02   9.20e+03   8.58e-01  1.44e+05        1    6.08e-04    3.75e-02
  43  3.951666e+06    2.59e+01    1.77e+02   1.07e+04   8.62e-01  2.32e+05        1    5.69e-04    3.84e-02
  44  3.951643e+06    2.31e+01    2.32e+02   1.27e+04   8.62e-01  3.74e+05        1    6.14e-04    3.93e-02
  45  3.951623e+06    2.03e+01    2.76e+02   1.47e+04   8.63e-01  6.04e+05        1    6.65e-04    4.01e-02
  46  3.951605e+06    1.78e+01    3.48e+02   1.70e+04   8.64e-01  9.82e+05        1    6.93e-04    4.11e-02
  47  3.951589e+06    1.56e+01    4.40e+02   1.94e+04   8.66e-01  1.61e+06        1    6.15e-04    4.19e-02
  48  3.951575e+06    1.36e+01    5.62e+02   2.19e+04   8.68e-01  2.68e+06        1    6.86e-04    4.27e-02
  49  3.951564e+06    1.17e+01    7.24e+02   2.46e+04   8.71e-01  4.53e+06        1    9.08e-04    4.38e-02
  50  3.951554e+06    9.88e+00    9.35e+02   2.73e+04   8.75e-01  7.82e+06        1    6.47e-04    4.47e-02
Ceres Solver Report: Iterations: 51, Initial cost: 2.186829e+07, Final cost: 3.951554e+06, Termination: NO_CONVERGENCE
RMSE: 125.723

===== BA with Huber =====
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  3.458088e+04    0.00e+00    1.05e+04   0.00e+00   0.00e+00  1.00e+04        0    4.48e-04    8.73e-04
   1  3.437237e+04    2.09e+02    1.46e+03   0.00e+00   1.30e+00  3.00e+04        1    1.09e-03    2.21e-03
   2  3.423587e+04    1.37e+02    1.13e+03   9.98e-01   1.78e+00  9.00e+04        1    7.72e-04    3.58e-03
   3  3.400336e+04    2.33e+02    9.65e+02   2.87e+00   1.48e+00  2.70e+05        1    7.87e-04    4.74e-03
   4  3.371180e+04    2.92e+02    7.20e+02   1.26e+01   1.03e+00  8.10e+05        1    6.67e-04    5.53e-03
   5  3.346516e+04    2.47e+02    7.92e+02   1.03e+02   6.37e-01  8.27e+05        1    7.58e-04    6.54e-03
   6  3.330819e+04    1.57e+02    6.80e+02   3.89e+03   3.54e-01  8.07e+05        1    7.31e-04    7.80e-03
   7  3.315351e+04    1.55e+02    1.87e+04   1.41e+04   5.75e-01  8.10e+05        1    6.95e-04    8.73e-03
   8  3.306804e+04    8.55e+01    4.06e+03   3.92e+03   3.64e-01  7.94e+05        1    7.00e-04    9.54e-03
   9  3.303941e+04    2.86e+01    3.48e+04   1.62e+04   2.48e-01  7.04e+05        1    8.07e-04    1.07e-02
  10  3.285352e+04    1.86e+02    2.05e+04   3.03e+04   1.08e+00  2.11e+06        1    6.85e-04    1.16e-02
  11  3.279611e+04    5.74e+01    3.81e+04   3.59e+03   7.09e-01  2.28e+06        1    7.13e-04    1.24e-02
  12  3.266054e+04    1.36e+02    2.74e+04   3.28e+03   1.17e+00  6.83e+06        1    6.60e-04    1.32e-02
  13  3.256993e+04    9.06e+01    1.72e+04   5.55e+03   1.34e+00  2.05e+07        1    7.28e-04    1.41e-02
  14  3.252382e+04    4.61e+01    1.69e+04   1.19e+04   1.27e+00  6.15e+07        1    8.01e-04    1.52e-02
  15  3.250368e+04    2.01e+01    2.43e+04   2.17e+04   8.23e-01  8.42e+07        1    7.96e-04    1.64e-02
  16  3.247707e+04    2.66e+01    2.04e+04   1.48e+04   1.01e+00  2.53e+08        1    7.79e-04    1.75e-02
  17  3.248113e+04   -4.06e+00    2.04e+04   3.16e+04  -2.07e-01  1.26e+08        1    3.11e-04    1.79e-02
  18  3.245748e+04    1.96e+01    2.03e+04   1.54e+04   9.99e-01  3.79e+08        1    8.18e-04    1.89e-02
  19  3.246806e+04   -1.06e+01    2.03e+04   3.37e+04  -6.02e-01  1.90e+08        1    3.57e-04    1.95e-02
  20  3.244144e+04    1.60e+01    2.21e+04   1.66e+04   9.13e-01  4.35e+08        1    6.39e-04    2.05e-02
  21  3.244195e+04   -5.12e-01    2.21e+04   2.78e+04  -2.99e-02  2.17e+08        1    3.19e-04    2.15e-02
  22  3.242364e+04    1.78e+01    1.93e+04   1.36e+04   1.04e+00  6.52e+08        1    7.44e-04    2.24e-02
  23  3.243337e+04   -9.73e+00    1.93e+04   3.26e+04  -7.28e-01  3.26e+08        1    4.20e-04    2.35e-02
  24  3.241143e+04    1.22e+01    2.17e+04   1.61e+04   9.13e-01  7.48e+08        1    7.31e-04    2.43e-02
  25  3.241300e+04   -1.56e+00    2.17e+04   2.84e+04  -1.21e-01  3.74e+08        1    3.00e-04    2.54e-02
  26  3.239821e+04    1.32e+01    1.90e+04   1.40e+04   1.02e+00  1.12e+09        1    6.04e-04    2.63e-02
  27  3.240644e+04   -8.23e+00    1.90e+04   3.40e+04  -8.53e-01  5.61e+08        1    3.82e-04    2.70e-02
  28  3.238958e+04    8.63e+00    2.11e+04   1.68e+04   8.95e-01  1.11e+09        1    7.26e-04    2.80e-02
  29  3.238695e+04    2.63e+00    3.22e+04   2.58e+04   2.98e-01  1.04e+09        1    6.80e-04    2.91e-02
  30  3.237595e+04    1.10e+01    2.15e+04   1.64e+04   9.33e-01  2.96e+09        1    6.68e-04    2.99e-02
  31  3.237954e+04   -3.59e+00    2.15e+04   3.71e+04  -6.40e-01  1.48e+09        1    2.53e-04    3.04e-02
  32  3.237076e+04    5.19e+00    2.01e+04   1.84e+04   9.27e-01  3.92e+09        1    7.26e-04    3.14e-02
  33  3.237276e+04   -2.00e+00    2.01e+04   3.78e+04  -4.86e-01  1.96e+09        1    3.03e-04    3.20e-02
  34  3.236666e+04    4.10e+00    1.89e+04   1.88e+04   9.95e-01  5.88e+09        1    6.54e-04    3.29e-02
  35  3.237015e+04   -3.49e+00    1.89e+04   4.34e+04  -1.16e+00  2.94e+09        1    2.90e-04    3.33e-02
  36  3.236388e+04    2.78e+00    2.12e+04   2.16e+04   9.29e-01  8.01e+09        1    6.84e-04    3.41e-02
  37  3.236591e+04   -2.03e+00    2.12e+04   4.34e+04  -7.93e-01  4.00e+09        1    3.73e-04    3.46e-02
  38  3.236132e+04    2.56e+00    2.11e+04   2.16e+04   1.00e+00  1.20e+10        1    7.00e-04    3.55e-02
  39  3.236405e+04   -2.73e+00    2.11e+04   4.78e+04  -1.42e+00  6.00e+09        1    2.92e-04    3.61e-02
  40  3.235946e+04    1.86e+00    2.38e+04   2.38e+04   9.67e-01  1.80e+10        1    6.15e-04    3.69e-02
  41  3.236259e+04   -3.13e+00    2.38e+04   5.06e+04  -1.93e+00  9.01e+09        1    3.22e-04    3.74e-02
  42  3.235796e+04    1.50e+00    2.75e+04   2.52e+04   9.29e-01  2.44e+10        1    7.42e-04    3.85e-02
  43  3.235998e+04   -2.02e+00    2.75e+04   4.69e+04  -1.47e+00  1.22e+10        1    2.97e-04    3.93e-02
  44  3.235662e+04    1.34e+00    2.92e+04   2.34e+04   9.74e-01  3.66e+10        1    6.41e-04    4.02e-02
  45  3.236032e+04   -3.70e+00    2.92e+04   4.92e+04  -3.53e+00  1.83e+10        1    2.72e-04    4.06e-02
  46  3.235583e+04    7.91e-01    4.01e+04   2.46e+04   7.55e-01  2.11e+10        1    6.07e-04    4.14e-02
  47  3.235484e+04    9.88e-01    3.93e+04   1.92e+04   9.59e-01  6.33e+10        1    5.75e-04    4.22e-02
  48  3.235830e+04   -3.46e+00    3.93e+04   4.25e+04  -4.96e+00  3.17e+10        1    3.02e-04    4.27e-02
  49  3.235449e+04    3.48e-01    6.21e+04   2.12e+04   4.99e-01  3.17e+10        1    7.20e-04    4.37e-02
  50  3.235378e+04    7.15e-01    5.75e+04   1.49e+04   8.70e-01  5.32e+10        1    6.76e-04    4.46e-02
Ceres Solver Report: Iterations: 51, Initial cost: 3.458088e+04, Final cost: 3.235378e+04, Termination: NO_CONVERGENCE
RMSE: 252.662

===== Outlier Filtering =====
Observations: 500 -> 412
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.013538e+02    0.00e+00    2.47e+04   0.00e+00   0.00e+00  1.00e+04        0    3.67e-04    7.70e-04
   1  3.411137e+02    6.02e+01    1.94e+05   0.00e+00   1.04e+00  3.00e+04        1    8.65e-04    1.80e-03
   2  4.370727e+02   -9.60e+01    1.94e+05   1.54e+04  -2.00e+00  1.50e+04        1    3.00e-04    2.58e-03
   3  3.280579e+02    1.31e+01    3.52e+05   8.71e+03   3.73e-01  1.48e+04        1    5.07e-04    3.25e-03
   4  3.014199e+02    2.66e+01    3.33e+05   8.45e+03   6.33e-01  1.50e+04        1    6.36e-04    4.19e-03
   5  2.779264e+02    2.35e+01    2.77e+05   7.74e+03   6.90e-01  1.59e+04        1    5.85e-04    5.13e-03
   6  2.592852e+02    1.86e+01    2.35e+05   7.14e+03   7.13e-01  1.72e+04        1    5.45e-04    6.08e-03
   7  2.434548e+02    1.58e+01    2.03e+05   6.63e+03   7.60e-01  2.01e+04        1    5.41e-04    7.05e-03
   8  2.307131e+02    1.27e+01    1.83e+05   6.27e+03   7.77e-01  2.42e+04        1    6.59e-04    7.92e-03
   9  2.206938e+02    1.00e+01    1.59e+05   5.59e+03   8.03e-01  3.11e+04        1    6.66e-04    9.01e-03
  10  2.136451e+02    7.05e+00    1.29e+05   4.88e+03   8.29e-01  4.35e+04        1    5.58e-04    9.95e-03
  11  2.087576e+02    4.89e+00    9.58e+04   4.13e+03   8.81e-01  7.81e+04        1    6.68e-04    1.13e-02
  12  2.058755e+02    2.88e+00    6.52e+04   3.34e+03   9.18e-01  1.88e+05        1    6.90e-04    1.21e-02
  13  2.044417e+02    1.43e+00    2.53e+04   2.03e+03   1.09e+00  5.63e+05        1    6.71e-04    1.31e-02
  14  2.040352e+02    4.07e-01    5.44e+03   8.63e+02   1.32e+00  1.69e+06        1    6.71e-04    1.42e-02
  15  2.039235e+02    1.12e-01    1.57e+03   3.48e+02   1.54e+00  5.07e+06        1    6.44e-04    1.53e-02
  16  2.038868e+02    3.68e-02    9.67e+02   1.58e+02   1.58e+00  1.52e+07        1    8.33e-04    1.68e-02
  17  2.038721e+02    1.46e-02    5.72e+02   7.34e+01   1.67e+00  4.56e+07        1    5.61e-04    1.80e-02
  18  2.038652e+02    6.88e-03    3.74e+02   3.87e+01   1.70e+00  1.37e+08        1    5.57e-04    1.90e-02
  19  2.038617e+02    3.51e-03    2.56e+02   2.21e+01   1.72e+00  4.10e+08        1    5.76e-04    1.97e-02
  20  2.038599e+02    1.88e-03    1.80e+02   1.34e+01   1.73e+00  1.23e+09        1    6.62e-04    2.08e-02
  21  2.038589e+02    9.39e-04    1.42e+02   8.68e+00   1.68e+00  3.69e+09        1    6.37e-04    2.16e-02
  22  2.038584e+02    4.75e-04    1.14e+02   5.90e+00   1.74e+00  1.11e+10        1    5.21e-04    2.24e-02
  23  2.038582e+02    2.59e-04    9.18e+01   3.44e+00   1.74e+00  3.32e+10        1    4.93e-04    2.31e-02
Ceres Solver Report: Iterations: 24, Initial cost: 4.013538e+02, Final cost: 2.038582e+02, Termination: CONVERGENCE
Final RMSE: 1.09353

从该输出结果可以观察到几个非常典型的现象：

首先，在未使用鲁棒核函数的 BA 中，优化过程虽然能够降低整体误差，但最终 RMSE 仍然维持在 百像素级别。这是因为外点的重投影误差非常大，在最小二乘框架下会被平方放大，从而主导整个优化目标函数，使优化结果偏离真实解。

随后，在引入 Huber Loss 后，优化结果出现了一定程度的改善。Huber 核函数会在残差较小时保持二次损失，而在残差超过阈值后转为线性增长，从而有效降低外点的影响权重。然而需要注意的是，仅依赖鲁棒核函数通常仍然不足以完全消除外点的影响，因此 RMSE 仍然保持在较高水平。

接下来，通过计算当前模型下的重投影误差，对观测进行简单的阈值过滤（例如 5 像素），可以将明显不符合几何约束的观测点剔除。在本实验中，观测数量从 500 降低到 412，说明约 20% 的观测被过滤。这是因为随机生成的外点往往具有极大的投影误差，因此能够被可靠地识别并剔除。

最后，在清洗后的数据上再次执行 BA 优化，系统最终收敛到：Final RMSE ≈ 1.09 pixel。这一误差已经接近我们在数据生成阶段设定的像素噪声水平（σ≈1 pixel），说明优化结果已经基本恢复了真实的几何结构。

3. 问题

通过上述实验可以看到，尽管最终能够获得较好的优化结果，但在整个过程中仍然出现了一些值得注意的现象。例如：

外点比例仅为 10% ，但最终被剔除的观测却高达 20%
引入 Huber Loss 后，RMSE 反而上升

这些现象在实际 BA 系统中其实非常常见，其背后反映了鲁棒优化与外点处理的一些重要特点。下面分别进行分析。

3.1 剔除外点

在本代码实现中，外点的真实比例仅为 10%，但在外点过滤阶段，却有大约 20% 的观测被剔除。这种"过度剔除"的原因在于：外点不仅会自身产生巨大误差，还会拖累整个优化结果。

在没有鲁棒机制的情况下，BA 的优化目标是最小化：

\[\sum_i r_i^2 \]

由于外点的残差往往达到数百像素，平方后会被极度放大，因此优化过程会试图同时"兼顾"这些错误观测。结果是：

相机位姿被拉偏
三维点位置被扭曲
许多原本正确的观测也会产生较大的重投影误差

因此，在进行外点过滤时，实际上被删除的不仅仅是原始外点，还包括一些由于模型被污染而产生较大残差的"次级异常观测"。换句话说：外点会污染模型，而污染后的模型又会制造更多大残差观测。在本例的实现中，使用的是固定的外点阈值，很容易就会造成这些其实是内点的"次级异常观测"也被剔除。那么，如何如何避免过度剔除呢？

3.1.1 统计方法进行外点检测

由于使用统一使用固定阈值，可能会造成在高质量数据中可能误删大量正常观测，在噪声较大的数据中又可能保留大量外点。因此，许多视觉系统会采用基于 统计分布的自适应外点检测 方法，根据当前残差分布自动确定合理阈值。

1. Median Absolute Deviation

Median Absolute Deviation（MAD）是一种常见的鲁棒统计量，在视觉系统中被广泛用于外点检测。

首先计算所有观测的重投影误差：

\[r_1, r_2, ..., r_n \]

然后计算残差的 中位数：

\[m = \text{median}(r) \]

接下来计算每个残差与中位数之间的绝对偏差：

\[|r_i - m| \]

再对这些偏差取中位数：

\[MAD = \text{median}(|r_i - m|) \]

最后根据经验公式得到外点阈值：

\[threshold = k \cdot 1.4826 \cdot MAD \]

其中：

(1.4826) 是将 MAD 转换为高斯标准差的系数
(k) 通常取 2--3

于是可以得到外点判断规则：

\[|r_i - m| > threshold \Rightarrow \text{outlier} \]

MAD 方法的优势在于由于使用的是中位数 而不是均值，不会被少量极端外点严重影响。因此即使存在大量大残差，阈值仍然能够稳定反映 数据主体部分的统计特性。

2. χ² 检验

在束平差中，我们通常假设图像特征点的定位误差是由高斯噪声 引起的。也就是说，每个像素坐标的测量误差可以看作是从均值为 0、标准差为 $\sigma$ 的正态分布中随机采样得到的：

\[\text{noise} \sim N(0, \sigma^2) \]

对于一个二维的重投影残差 $\mathbf{r} = [r_x, r_y]^\top$，如果这个残差确实只由上述高斯噪声引起（即它是内点），那么它的归一化平方长度：

\[\frac{\|\mathbf{r}\|^2}{\sigma^2} = \frac{r_x^2 + r_y^2}{\sigma^2} \]

在统计上会服从自由度为 2 的卡方分布 （记作 $\chi^2(2)$）。这是概率论中的一个经典结论：两个独立的标准正态变量的平方和，就服从 $\chi^2(2)$。

利用这一性质，我们可以设定一个统计意义上的合理阈值 来判断某个观测是否"太离谱"而可能是外点。例如，在 95% 的置信水平下，$\chi^2(2)$ 分布的临界值约为 5.99。这意味着：如果一个观测确实是内点（仅受高斯噪声影响），那么有 95% 的概率，其 $\frac{\|\mathbf{r}\|^2}{\sigma^2}$ 会小于等于 5.99；只有约 5% 的情况会偶然超过这个值。因此，我们把超过该阈值的观测视为统计上不太可能来自高斯噪声，从而判定为外点：

\[\frac{\|\mathbf{r}\|^2}{\sigma^2} > 5.99 \quad \Rightarrow \quad \text{outlier} \]

进一步推论，如果已知特征点定位的噪声标准差为 $\sigma = 1$ 像素（这是一个常见的经验值），那么对应的重投影误差阈值就是：

\[\|\mathbf{r}\| > \sqrt{5.99} \approx 2.45 \text{ 像素} \]

换句话说，在 1 像素噪声水平下，超过 2.45 像素的重投影误差就有理由被怀疑是外点。与简单使用固定阈值（比如一律用 5 像素）相比，χ² 检验的优势在于：

显式考虑了噪声水平 $\sigma$ ，当标定更精确或图像质量更高（$\sigma$ 更小）时，阈值自动收紧；
基于概率模型，具有明确的统计意义，避免了"拍脑袋"设阈值的随意性。

正因为这些优点，χ² 检验被广泛应用于 SFM、SLAM 等系统中，作为 BA 前后外点过滤的重要依据。

3. 残差分布分析

在实际工程系统中，束平差优化后的重投影残差通常呈现出一种典型的混合分布形态：

主体部分集中在较小误差范围内（如 0--2 像素），近似服从高斯分布，对应于正确的 2D-3D 匹配；
少量观测 则散布在较大误差区域（如 >5 像素），形成明显的长尾（long tail），主要由误匹配、动态物体、遮挡或低纹理区域的不稳定特征引起。

这种"尖峰 + 长尾"的分布特征可以通过绘制残差直方图直观展现：

复制代码

残差 (像素) | 观测数量
------------|----------
0 -- 1       | ████████████████ (大量)
1 -- 2       | ██████████
2 -- 3       | ████
3 -- 5       | ██
> 5         | █ (稀疏但显著)

通过分析该分布，我们可以：

定性判断 BA 质量：若长尾过重（如 >10% 的观测残差 >5 像素），说明前端匹配质量较差，可能需要加强 RANSAC 或特征筛选；
辅助设定外点阈值 ：理想情况下，阈值应设在"主体分布"与"长尾"之间的谷底（valley），以最大化内点保留率并最小化外点污染；
验证噪声模型假设：若主体部分明显偏离高斯（如偏斜、多峰），可能暗示存在系统性误差（如未校正的畸变、时序不同步等）。

因此，在关键系统（如自动驾驶、测绘）中，可以在 BA 后自动绘制残差直方图或计算统计指标（如中位数、MAD、95% 分位数），作为重建质量的诊断工具。

3.1.2 空间均衡外点剔除

仅根据误差大小进行外点过滤，还可能带来另一个问题：观测的空间分布被破坏。

例如，在某些区域由于纹理较弱或匹配不稳定，观测误差可能整体偏大。如果直接按照误差阈值删除观测，这些区域的点可能会被全部剔除，例如原始观测分布：

text 复制代码

+--------------------+
|  ●  ●   ●   ●   ●  |
|  ●  ●   ●   ●   ●  |
|  ●  ●   ●   ●   ●  |
|  ●  ●   ●   ●   ●  |
+--------------------+

过滤后：

text 复制代码

+--------------------+
|  ●  ●              |
|  ●  ●              |
|                    |
|                    |
+--------------------+

此时观测点集中在图像的一小部分区域，可能会导致如下问题：

结构退化：例如只有图像左半部分有点，相机位姿约束变弱。
局部最优：优化会倾向于只拟合某一部分数据，例如左侧结构正确，右侧结构漂移。
深度不稳定：三角化需要视差和空间分布。

因此，许多视觉系统会在外点过滤中加入 空间均衡策略（Spatially Balanced Filtering）。

一种常见做法是将图像划分为多个网格区域，例如：

复制代码

+----+----+----+
| G1 | G2 | G3 |
+----+----+----+
| G4 | G5 | G6 |
+----+----+----+
| G7 | G8 | G9 |
+----+----+----+

然后在每个网格内部单独进行外点筛选，例如：按重投影误差进行排序，保留误差最小的前 K 个观测。伪代码如下：

cpp 复制代码

for each grid cell:
    collect observations in cell
    sort by reprojection error
    keep first K observations

这样就可以保证每个区域都保留一定数量的观测，从而使得整体空间分布更加均匀，避免结构信息完全丢失。

在实际视觉系统中，空间均衡策略被广泛使用。例如：

ORB-SLAM：通过图像网格限制每个区域的特征数量
视觉里程计系统：使用 feature bucketing 保证特征均匀分布
SFM 系统：在匹配或 BA 过滤阶段限制每个区域的观测数量

这些方法的核心思想就是外点剔除不仅要考虑误差大小，还需要关注观测的空间分布。保持均匀的观测分布通常比单纯增加观测数量更有利于 BA 的稳定收敛。

3.2 鲁棒核函数

在实验结果中还可以观察到一个看似反直觉的现象：

text 复制代码

BA without robust : RMSE ≈ 125
BA with Huber     : RMSE ≈ 253

也就是说，在引入 Huber Loss 后，整体 RMSE 反而略微增大。这一现象其实并不奇怪，其原因在于 RMSE 的统计方式与鲁棒优化的目标并不完全一致。标准 BA 优化的是：

\[\min \sum r_i^2 \]

而鲁棒 BA 优化的是：

\[\min \sum \rho(r_i^2) \]

其中 $\rho(\cdot)$ 为鲁棒核函数。当残差较小时：

\[\rho(r^2) \approx r^2 \]

当残差较大时：

\[\rho(r^2) \approx |r| \]

这意味着 大残差会被显著降权 。因此，在鲁棒 BA 中, 外点不会再强烈影响优化，优化主要关注大多数正常观测。但如果我们仍然使用包含所有外点的原始 RMSE 进行评估，那么被降权的外点依然存在，并且这些外点仍然具有很大的残差，因此 RMSE 可能看起来更大。换句话说：鲁棒优化的目标是恢复正确的几何结构，而不是最小化所有观测的 RMSE。

在视觉 BA 中，常见的鲁棒核函数包括：

Huber Loss ：Huber Loss是最常见、最稳定的鲁棒核函数，其形式为： $\\rho(r) = \\begin{cases} \\frac{1}{2}r\^2 \& \|r\| \\le \\delta \\ \\delta(\|r\| - \\frac{1}{2}\\delta) \& \|r\| \> \\delta \\end{cases}$ 。它的特点是在残差较小时，能保持 二次损失 ；而在残差较大时，就成为了 线性损失。因此既保持了最小二乘的效率，又具有一定的鲁棒性。
Cauchy Loss ：Cauchy Loss 的形式为：$\rho(r^2) = c^2 \log\left(1 + \frac{r^2}{c^2}\right)$。它的特点是大残差会被 更强烈压制 ，对极端外点更鲁棒，但也可能带来 收敛速度下降 的问题。

在实际的工程实践中，常见的经验是：先使用鲁棒核函数稳健 BA，然后结合过滤机制去除外点，最后在最终 BA 中关闭鲁棒核函数进行精修。并且这个过程可以多次迭代：

text 复制代码

RANSAC
 ↓
BA + robust
 ↓
filter
 ↓
BA
 ↓
filter
 ↓
BA

通过这种策略，即使在存在大量误匹配的情况下，系统仍然能够稳定地恢复准确的相机位姿和三维结构。

4. 挑战

前面的实践中讨论了外点鲁棒性这一核心问题，并通过 Huber Loss 与残差滤波展示了典型修复流程。然而，在真实的 SFM、SLAM 或三维重建系统中，束平差还面临一系列无法在简单合成数据中复现、但对系统稳定性与精度至关重要的工程挑战。

4.1 零空间自由度（Gauge Freedom）

束平差优化的是相机位姿与 3D 点的相对几何关系 ，而整个场景在 3D 空间中可进行任意刚体变换（旋转 + 平移）而不改变重投影误差。这意味着优化问题存在 6 个不可观测自由度（3 旋转 + 3 平移），导致 Hessian 矩阵天然奇异。

现象：若不固定任何参数，Ceres 可能报错 The Jacobian is rank deficient，或优化过程震荡不收敛。
解决方案 ：
- 固定第一帧相机 ：最常用方法，将第一个相机的位姿设为世界坐标系原点（即 SetParameterBlockConstant）；
- 固定一个 3D 点 + 相机朝向：适用于无全局参考的纯运动恢复；
- 施加软约束（Soft Prior）：在某些 SLAM 系统中，通过先验信息（如 IMU）提供弱约束，而非硬固定。
注意：在增量式 SFM 中，每次新增相机后都需确保当前子图有足够约束，避免局部零空间。

4.2 相机内参与畸变模型

在本文的案例实现中，假设相机内参 $\mathbf{K}$ 已知且图像无畸变。但真实相机往往存在：

标定误差 ：内参（$f_x, f_y, c_x, c_y$）不精确；
镜头畸变：径向/切向畸变导致针孔模型失效。
后果：即使所有匹配正确，重投影误差仍系统性偏大，BA 无法收敛到高精度。
解决方案：
1. 显式去畸变 ：在 BA 前将像素点反投影到无畸变平面（OpenCV 的 undistortPoints）；
2. 联合优化内参与畸变 ：将 $f_x, f_y, k_1, k_2, p_1, p_2$ 等作为额外变量加入 BA，并施加合理正则化（如内参变化不应过大）；
3. 使用更复杂相机模型：如 Kannala-Brandt（鱼眼）、双球模型（wide-angle）等。

4.3 大规模问题的可扩展性

当相机数 $N > 100$、3D 点数 $M > 10^4$ 时，即使使用 SPARSE_SCHUR，内存和计算时间仍可能成为瓶颈。

挑战：
- Schur 补矩阵 $\mathbf{B} - \mathbf{E}\mathbf{C}^{-1}\mathbf{E}^\top$ 虽为 $6N \times 6N$，但通常稠密，Cholesky 分解复杂度 $O(N^3)$；
- 内存消耗随 $N^2$ 增长，易超限。
解决方案 ：
- 滑动窗口 BA（Local BA） ：仅优化最近 $K$ 帧（如 K=10）及其共视 3D 点，其余固定；
- 关键帧机制：只保留信息量大的帧参与全局 BA；
- 迭代求解器 ：使用 ITERATIVE_SCHUR（预条件共轭梯度法），内存占用低，适合超大规模问题；
- 分布式 BA：如 Theia、OpenMVG 支持多线程/多机并行。
典型应用：ORB-SLAM 使用 Local BA 实现实时性；COLMAP 在全局 BA 中结合关键帧与稀疏求解器。

4.4 初始值敏感性与分层优化

BA 是非凸优化，对初始值敏感。若前端（PnP/三角化）误差过大（如 >50 像素），BA 可能陷入局部极小。可能的对策是：

分层优化（Coarse-to-Fine） ：
1. 先固定相机，仅优化 3D 点（快速收敛）；
2. 再联合优化相机与点；
阻尼因子控制 ：Ceres 的 LM 算法默认已处理，但可手动调整 initial_trust_region_radius；
多尺度 BA：先在低分辨率图像上优化，再迁移到高分辨率。

4.5 动态物体与非刚性场景

BA 假设场景静态。若存在移动物体（行人、车辆）、非刚性变形（人脸、旗帜），其对应点会表现为"伪外点"。

影响：即使匹配正确，残差仍很大，被误判为外点，或污染整体结构。
解决方案 ：
- 前端动态分割：使用光流、语义分割或几何一致性检测动态区域；
- 多模型 BA：为不同运动物体分配独立运动模型（如 SE(3) + 刚性片段）；
- 鲁棒核函数 + 时空一致性：结合时间连续性判断是否为真外点。
注意：这类问题通常需在 BA 之前解决，而非依赖 BA 自身鲁棒性。

4.6 系统集成：何时触发

BA 计算开销大，不能每帧都执行。可能的对策是：

局部 BA：新增关键帧后，优化该帧 + 共视关键帧 + 共视点；
全局 BA：闭环检测成功后触发，用于纠正累积漂移；
异步 BA：在后台线程运行，不影响主线程实时性。

4.7 深度退化（Degenerate Geometry）

在某些几何配置下，BA 的参数会变得 弱可观测甚至不可观测。常见的退化场景包括：

4.7.1 纯旋转运动

如果相机只发生旋转而没有平移。此时所有特征点的视差接近于零，三角化深度无法确定。结果导致3D 点深度发散、BA 难以稳定收敛、以及相机位姿估计不稳定。

解决方案就是检测视差（parallax），若视差过小，就暂停三角化，只优化旋转。这也是为什么很多 SLAM 系统要求 minimum parallax才会三角化。

4.7.2 近平面场景

如果大部分特征点都位于同一平面，例如：建筑墙面、地面、书页，此时系统更适合用 单应矩阵（Homography） 而不是完整的 3D 模型。因为在这种情况下可能出现深度估计不稳定和 3D 点漂移的问题。具体来说，可在初始化阶段通过单应矩阵（H）与基础矩阵（E）的模型选择机制（例如 Nister 的五点法 vs 四点法得分比较），仅在 E 模型显著优于 H 模型时建立 3D 结构------这正是 ORB-SLAM 初始化阶段的做法。

4.8 观测权重不一致（Heteroscedastic Noise）

在理想情况下，BA 假设所有观测具有相同噪声 $\\sigma_i = \\sigma$ 。但真实系统中，观测质量差异很大，例如：

观测来源	误差
中心区域特征	小
边缘畸变区域	大
远距离点	大
运动模糊	大

如果所有观测使用同一权重，低质量观测会污染优化结果。解决方法是可以引入 加权 BA（Weighted BA）：

\[\min \sum_i w_i r_i^2 \]

其中：

\[w_i = \frac{1}{\sigma_i^2} \]

权重可以根据特征匹配置信度、角点响应值（Harris / FAST score）、视差大小或者深度不确定性进行估计。一些系统甚至会使用：weight = cos(view_angle)来降低斜视角观测的权重。

4.9 参数尺度问题（Parameter Scaling）

BA 中不同变量的量纲差异很大：

参数	量级
rotation	~1
translation	~1--10
3D point	~1--100
focal length	~1000

如果直接优化[rotation, translation, point, focal]这样的值，Hessian 的条件数会变差，可能会造成 LM 步长异常、收敛速度慢以及数值不稳定的问题。解决方法如下：

参数归一化。例如point /= scene_scale，使场景尺寸接近 1--10。
使用合理的参数化。例如不直接使用 rotation 参数，而是用李代数或者四元数。
Solver 内部 scaling。例如 Ceres 内部也会进行Jacobian column scaling改善数值稳定性。

4.10 数据关联漂移（Data Association Drift）

在束平差中，每个观测都隐含着一个关键假设：图像中的 2D 特征点与同一个真实 3D 点正确对应 。然而在真实系统中，这种数据关联往往来自特征匹配或特征跟踪，而这些过程不可避免地会产生误匹配。例如，重复纹理、视角变化、遮挡、动态物体或描述子相似度不足，都可能导致特征在不同帧之间被错误关联。随着系统运行时间增加，这类错误匹配可能在特征轨迹（track）中逐渐累积并传播，形成所谓的数据关联漂移。与随机外点不同，这种误差往往具有持续性：一个 3D 点可能在某些帧中对应真实物体 A，而在另一些帧中被误匹配到物体 B，从而破坏 BA 所依赖的几何一致性，导致三维结构被拉扯、相机位姿偏移，甚至在局部区域形成错误但"自洽"的几何结构。

由于数据关联漂移往往不会产生极端大的残差，因此仅依赖鲁棒核函数通常难以彻底解决。工程系统通常通过多层机制抑制这一问题，例如：在前端进行更严格的几何验证（如极线约束或RANSAC筛选匹配），在中间层通过多视图一致性检测 和特征轨迹长度过滤 删除不稳定的观测（例如只保留被多帧观测的点），并在系统层面引入回环检测 或图结构一致性检查以纠正长期漂移。总体而言，束平差本身主要用于精化已经基本正确的几何结构，而可靠的数据关联管理才是保证 BA 能够稳定工作的关键前提。

4.11 总结

束平差虽是"精修"工具，但其工程落地远不止"调用 Ceres"那么简单。一个健壮的 BA 系统需要综合考虑：

数据质量（外点、噪声、分布）
模型完整性（内参、畸变、自由度）
计算效率（稀疏性、规模、求解器）
系统集成（触发时机、失败回滚、动态处理）

掌握 BA 的原理只是起点；真正的挑战在于如何在复杂、不完美的现实条件下，灵活组合上述要素，构建稳定可靠的系统。

最小二乘问题详解16：束平差工程实践总结

1. 引言

2. 实例

3. 问题

3.1 剔除外点

3.1.1 统计方法进行外点检测

1. Median Absolute Deviation

2. χ² 检验

3. 残差分布分析

3.1.2 空间均衡外点剔除

3.2 鲁棒核函数

4. 挑战

4.1 零空间自由度（Gauge Freedom）

4.2 相机内参与畸变模型

4.3 大规模问题的可扩展性

4.4 初始值敏感性与分层优化

4.5 动态物体与非刚性场景

4.6 系统集成：何时触发

4.7 深度退化（Degenerate Geometry）

4.7.1 纯旋转运动

4.7.2 近平面场景

4.8 观测权重不一致（Heteroscedastic Noise）

4.9 参数尺度问题（Parameter Scaling）

4.10 数据关联漂移（Data Association Drift）

4.11 总结