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

1. 引言

在上一篇文章《最小二乘问题详解15：束平差原理与基础实现》中，我们从几何模型出发，系统推导了 束平差（Bundle Adjustment, BA） 的数学形式，并基于 Ceres Solver 实现了一个基础但完整的 BA 系统。在理想合成数据下，该系统能够将初始存在偏差的相机位姿与 3D 点高效优化至亚像素级精度，充分验证了 BA 作为“全局一致性精修”工具的强大能力。
然而，理论上的优雅并不总能直接转化为工程上的稳定。在真实的 BA 应用场景（ SFM / SLAM / 稀疏重建）中不会那么理想，往往会遇到各种问题。其中最为常见的就是由于误匹配造成的外点（Outliers）污染的问题。
2. 实例

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

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

在生成观测数据时，我们首先按照真实几何关系计算每个 3D 点在相机中的投影位置，并叠加 高斯像素噪声。随后按照设定的比例随机注入 错误匹配（外点），即直接生成随机像素坐标来替代真实观测值，从而模拟真实特征匹配中不可避免的误匹配问题。具体代码实现如下：
[code]#include #include #include #include #include #include #include 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   bool operator()(const T* const q, const T* const t, const T* const point,                  T* residuals) const {    T p[3];    ceres:uaternionRotatePoint(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(        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:uaterniond 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& cams, const vector& pts,                   const vector& 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& cams, vector& pts,             const vector& obs, double fx, double fy, double cx,             double cy, bool robust) {  ceres:roblem problem;  ceres::Manifold* q_manifold = new ceres:uaternionManifold();  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:ossFunction* 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