最小二乘问题详解16：束平差工程实践总结
1. 引言在上一篇文章《最小二乘问题详解15：束平差原理与基础实现》中，我们从几何模型出发，系统推导了 束平差（Bundle Adjustment, BA） 的数学形式，并基于 Ceres Solver 实现了一个基础但完整的 BA 系统。在理想合成数据下，该系统能够将初始存在偏差的相机位姿与 3D 点高效优化至亚像素级精度，充分验证了 BA 作为“全局一致性精修”工具的强大能力。
然而，理论上的优雅并不总能直接转化为工程上的稳定。在真实的 BA 应用场景（ SFM / SLAM / 稀疏重建）中不会那么理想，往往会遇到各种问题。其中最为常见的就是由于误匹配造成的外点（Outliers）污染的问题。
2. 实例
为了更直观地说明外点对 BA 优化的影响，以及如何在工程实践中提升 BA 的稳健性，这里通过一个完整的实例进行演示。在该示例中，我们构造一个简单但具有代表性的 多视图重建场景。具体设置如下：
[*]相机数量：5 个
[*]空间点数量：100 个
[*]相机模型：理想 pinhole 相机
[*]观测噪声：像素级高斯噪声
[*]外点比例：10%
在生成观测数据时，我们首先按照真实几何关系计算每个 3D 点在相机中的投影位置，并叠加 高斯像素噪声。随后按照设定的比例随机注入 错误匹配（外点），即直接生成随机像素坐标来替代真实观测值，从而模拟真实特征匹配中不可避免的误匹配问题。具体代码实现如下：
#include #include #include #include #include #include #include using namespace std;struct Camera {double q;double t;};struct Point3D {double xyz;};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; ceres::QuaternionRotatePoint(q, point, p); p += t; p += t; p += t; T xp = p / p; T yp = p / p; T u = T(fx_) * xp + T(cx_); T v = T(fy_) * yp + T(cy_); residuals = u - T(x_); residuals = 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::Quaterniond q(c.q, c.q, c.q, c.q);Eigen::Vector3d t(c.t, c.t, c.t);Eigen::Vector3d P(p.xyz, p.xyz, p.xyz);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, pts, 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::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.q, cams.t, pts.xyz);}problem.SetParameterBlockConstant(cams.q);problem.SetParameterBlockConstant(cams.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
页:
[1]