首页/资讯中心/详情

C++实现摄影测量光束法平差：从共线方程到三维重建

C++实现摄影测量光束法平差：从共线方程到三维重建
📅 发布时间：2026/7/24 16:36:00

1. 项目概述：从像素到三维世界的桥梁

摄影测量，听起来是个挺专业的词，但它的核心思想其实很直观：如何从几张二维的照片里，还原出三维世界的模样。这就像我们人眼，通过左右眼看到的略有差异的图像，大脑就能自动构建出立体感和距离感。摄影测量就是让计算机学会这个本事。而“内外方位元素”，就是实现这个转换过程的“密码本”和“定位器”。内方位元素告诉你相机本身的“性格”——它的焦距、像主点坐标，这些参数决定了光线如何穿过镜头在传感器上成像，是相机内部的固有属性。外方位元素则描述了相机在拍摄那一刻的“姿态”和“位置”——它在世界坐标系下的X, Y, Z坐标，以及它绕三个轴旋转的角度，这决定了相机是从哪个视角看世界的。

这个项目，就是用C++这把“手术刀”，来精准地求解这套“密码本”。为什么是C++？因为在处理海量的像点坐标数据、进行复杂的矩阵运算（比如解算成千上万个方程组成的超定方程组）时，对计算效率和内存控制的极致要求，让C++成了不二之选。它没有高级语言那些“甜蜜的负担”，能让你直接操作内存，精细地控制每一个计算步骤，这对于追求毫米级甚至更高精度的摄影测量解算来说至关重要。你可能会在无人机测绘、文物数字化重建、工业检测甚至电影特效制作中看到它的身影。无论你是测绘工程、计算机视觉的学生，还是对三维重建感兴趣的开发者，理解并实现这套核心算法，都能让你真正打通从二维影像到三维模型的任督二脉。

2. 核心原理与数学模型拆解

2.1 共线条件方程：一切的起点

摄影测量最基础的数学模型就是共线条件方程。它描述了一个完美的几何关系：物方点（真实世界的点A）、投影中心（相机镜头中心S）和相应的像点（照片上的点a）这三者必须严格位于同一条直线上。这个方程是连接二维像点和三维物方点的桥梁。

其数学表达式如下：

x - x0 = -f * [a1*(X - Xs) + b1*(Y - Ys) + c1*(Z - Zs)] / [a3*(X - Xs) + b3*(Y - Ys) + c3*(Z - Zs)] y - y0 = -f * [a2*(X - Xs) + b2*(Y - Ys) + c2*(Z - Zs)] / [a3*(X - Xs) + b3*(Y - Ys) + c3*(Z - Zs)]

这里：

  • (x, y)是像点在像平面坐标系（以像主点为原点）下的坐标。
  • (x0, y0, f)就是内方位元素。(x0, y0)是像主点坐标，理论上应是图像中心，但镜头畸变和传感器安装会使它偏移；f是相机焦距。
  • (Xs, Ys, Zs)是外方位元素中的三个线元素，即投影中心在世界坐标系下的坐标。
  • (a1, b1, c1; a2, b2, c2; a3, b3, c3)是一个3x3的旋转矩阵R，由外方位元素中的三个角元素（通常用φ, ω, κ表示）计算得来。这个矩阵描述了相机坐标系相对于世界坐标系的旋转姿态。
  • (X, Y, Z)是物方点在世界坐标系下的坐标。

注意：这个方程是非线性的，因为未知数（外方位元素和物方点坐标）出现在分母和三角函数（旋转矩阵中）里。直接求解非常困难，因此我们必须将其线性化。

2.2 线性化与误差方程：迭代逼近的基石

为了求解，我们采用泰勒公式将共线条件方程在未知数的近似值处展开，忽略二次及以上高阶项，得到线性化的误差方程。这是整个解算过程的核心步骤。

对于每一个像点，我们可以列出两个误差方程（对应x和y方向）：

vx = (∂F/∂Xs)*dXs + (∂F/∂Ys)*dYs + (∂F/∂Zs)*dZs + (∂F/∂φ)*dφ + (∂F/∂ω)*dω + (∂F/∂κ)*dκ + (∂F/∂X)*dX + (∂F/∂Y)*dY + (∂F/∂Z)*dZ - (x_观测 - x_计算) vy = (∂G/∂Xs)*dXs + ... (类似地，包含所有偏导数项) - (y_观测 - y_计算)

其中：

  • vx, vy是像点坐标观测值的改正数（残差）。
  • dXs, dYs, dZs, dφ, dω, dκ是外方位元素近似值的改正数（我们要求解的量）。
  • dX, dY, dZ是物方点坐标近似值的改正数（在空间后方交会中，物方点坐标已知，此项为0；在光束法平差中，此项也需要求解）。
  • (x_观测 - x_计算)是观测值减去用近似值计算得到的像点坐标，称为常数项。

偏导数的计算是这里的重头戏，它们有具体的解析表达式，涉及到对旋转矩阵的求导。推导过程稍显繁琐，但结果是固定的公式。在编程时，我们需要精确地实现这些偏导数的计算。

2.3 平差模型：后方交会与光束法

根据已知条件的不同，我们有两种主要的平差模型：

  1. 空间后方交会：已知至少三个物方控制点（其X,Y,Z坐标已知）及其在像片上的像点坐标，求解单张像片的六个外方位元素。此时，每个控制点提供两个误差方程，未知数只有6个外方位元素改正数。例如，有4个控制点，就能列出8个方程，解算6个未知数，形成超定方程组，通过最小二乘求解。
  2. 光束法区域网平差：这是最通用、最严密的方法。同时求解所有像片的外方位元素和所有待求物方点的坐标。它把所有像点（包括控制点和待求点）的观测值都纳入同一个庞大的误差方程系统中，进行整体平差。这是本项目的高级目标，其数学模型是后方交会的扩展，未知数规模巨大（像片数6 + 物方点数3），方程数也巨大（像点数*2）。

3. C++实现的关键技术与工程架构

3.1 核心数据结构设计

良好的数据结构是高效程序的基础。我们需要设计类来封装核心概念。

点类 (Point3D, Point2D)：

class Point3D { public: double X, Y, Z; int id; // 点号 bool isControlPoint; // 是否是控制点 // ... 构造函数、运算符重载等 }; class Point2D { public: double x, y; int point3D_id; // 对应的三维点ID int image_id; // 所属像片ID // ... 构造函数 };

像片类 (Image)：

class Image { public: int id; // 内方位元素 double x0, y0, f; // 外方位元素 (近似值及平差后的值) double Xs, Ys, Zs; double phi, omega, kappa; // 三个旋转角 // 旋转矩阵R (由角元素计算得到，需频繁使用，故存储) Eigen::Matrix3d R; // 该像片上观测到的所有二维点 std::vector<Point2D> observations; // 方法：计算旋转矩阵、计算像点坐标、计算偏导数等 void calcRotationMatrix(); Eigen::Vector2d projectPoint(const Point3D& p) const; // ... };

平差系统类 (BundleAdjustment)：

class BundleAdjustment { private: std::vector<Image> images_; std::vector<Point3D> points3D_; std::map<std::pair<int, int>, Point2D> observations_; // 键: (image_id, point3D_id) public: void addImage(const Image& img); void addPoint3D(const Point3D& p); void addObservation(int img_id, int pt3d_id, const Point2D& obs); // 核心平差函数 bool solve(bool useSparse = true, int maxIterations = 20, double threshold = 1e-6); // 结果输出 void reportStatistics() const; };

3.2 矩阵运算库的选择：Eigen

摄影测量平差最终归结为求解大型线性方程组A * x = b。其中A是设计矩阵（偏导数矩阵），x是未知数改正数向量，b是常数项向量。强烈推荐使用Eigen库。它是一个纯头文件的C++模板库，无需编译安装，直接包含即可。它提供了媲美MATLAB的线性代数API，并且对稀疏矩阵（光束法平差中矩阵A绝大多数元素为0）有出色的支持。

例如，构建和求解法方程：

#include <Eigen/Dense> #include <Eigen/Sparse> // 假设我们已构建了稠密矩阵A和向量b Eigen::MatrixXd A = ...; // 设计矩阵 Eigen::VectorXd b = ...; // 常数项向量 // 解法方程 A^T * A * x = A^T * b (最小二乘) Eigen::VectorXd x = (A.transpose() * A).ldlt().solve(A.transpose() * b); // 对于稀疏矩阵（光束法常用） typedef Eigen::SparseMatrix<double> SparseMatrix; typedef Eigen::Triplet<double> T; std::vector<T> tripletList; // 用于高效构建稀疏矩阵 SparseMatrix A_sparse(rows, cols); A_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); // 使用稀疏求解器，如SimplicialLDLT或Conjugate Gradient Eigen::SimplicialLDLT<SparseMatrix> solver; solver.compute(A_sparse.transpose() * A_sparse); if(solver.info() != Eigen::Success) { /* 分解失败 */ } Eigen::VectorXd x = solver.solve(A_sparse.transpose() * b);

3.3 迭代求解与收敛判断

由于问题是非线性的，求解必须迭代进行：

  1. 给定外方位元素和物方点坐标的初始近似值。对于外方位元素，可能来自POS记录或粗略估计；对于物方点，可能来自前方交会或粗略值。
  2. 用当前近似值，根据共线方程计算每个像点的“理论坐标”(x_calc, y_calc)。
  3. 计算观测值与理论值的差值，构建误差方程，列出A * dx = b。
  4. 求解线性方程组，得到未知数改正数dx。
  5. 用改正数更新近似值：X_new = X_old + dx。
  6. 判断是否收敛。收敛条件通常有两个：
    • 改正数阈值：所有改正数dx的绝对值最大值小于某个阈值（如1e-6）。
    • 残差变化：本次迭代的单位权中误差（所有像点残差平方和除以自由度再开方）与上次迭代的变化小于阈值。
  7. 若不收敛，则用更新后的值作为新的近似值，回到第2步。

单位权中误差计算公式：

sigma0 = sqrt( (sum(vx^2 + vy^2)) / (2 * n_observations - n_unknowns) )

其中n_observations是像点观测值总数，n_unknowns是未知数总数。这个值是衡量平差整体精度的关键指标。

4. 分步实现与代码剖析

4.1 第一步：实现空间后方交会（单像解析）

这是入门的关键一步，能帮你理清整个流程。

核心步骤：

  1. 数据准备：读取一张像片的内方位元素(x0, y0, f)，以及至少3个（最好4-6个）控制点的物方坐标(X, Y, Z)和对应的像点坐标(x, y)。
  2. 确定外方位元素初始值：这是一个难点。如果完全没有初始值，可以采用“直接线性变换（DLT）”的简化模型先求一个粗略解，或者如果控制点分布良好，可以尝试用共面条件等方法估算。实践中，常假设Xs, Ys近似为控制点坐标均值，Zs用航高近似，角元素初始设为小量或0。
  3. 迭代求解循环：
    bool spaceResection(const std::vector<ControlPoint>& ctrlPts, double x0, double y0, double f, double& Xs, double& Ys, double& Zs, double& phi, double& omega, double& kappa) { const double convThreshold = 1e-6; const int maxIter = 20; Eigen::Vector6d corrections; // 存储6个外方位元素改正数 for (int iter = 0; iter < maxIter; ++iter) { // 1. 由当前角元素计算旋转矩阵R Eigen::Matrix3d R = calcRotationMatrix(phi, omega, kappa); // 2. 构建设计矩阵A和常数项矩阵L int numPts = ctrlPts.size(); Eigen::MatrixXd A(2 * numPts, 6); Eigen::VectorXd L(2 * numPts); for (int i = 0; i < numPts; ++i) { const auto& pt = ctrlPts[i]; // 计算当前物方点在像片上的理论坐标 (x_calc, y_calc) Eigen::Vector3d vec(pt.X - Xs, pt.Y - Ys, pt.Z - Zs); Eigen::Vector3d vec_cam = R * vec; // 转到像空间坐标系 double x_calc = -f * vec_cam.x() / vec_cam.z() + x0; double y_calc = -f * vec_cam.y() / vec_cam.z() + y0; // 计算6个偏导数 (具体公式需实现) Eigen::RowVector6d row_dx = calcPartialDerivatives(pt, R, Xs, Ys, Zs, f, true); // for x Eigen::RowVector6d row_dy = calcPartialDerivatives(pt, R, Xs, Ys, Zs, f, false); // for y A.row(2*i) = row_dx; A.row(2*i+1) = row_dy; // 常数项 = 观测值 - 计算值 L(2*i) = pt.x_obs - x_calc; L(2*i+1) = pt.y_obs - y_calc; } // 3. 解法方程 A^T * A * dx = A^T * L corrections = (A.transpose() * A).ldlt().solve(A.transpose() * L); // 4. 更新外方位元素 Xs += corrections[0]; Ys += corrections[1]; Zs += corrections[2]; phi += corrections[3]; omega += corrections[4]; kappa += corrections[5]; // 5. 检查收敛 if (corrections.cwiseAbs().maxCoeff() < convThreshold) { std::cout << "空间后方交会收敛于第 " << iter+1 << " 次迭代。" << std::endl; return true; } } std::cerr << "警告：空间后方交会未在最大迭代次数内收敛。" << std::endl; return false; }

4.2 第二步：扩展至多像前方交会

在获得所有像片的外方位元素后，对于非控制点，我们可以利用它在多张像片上的像点，通过前方交会确定其三维坐标。这本质上是解一个由多张像片共线方程构成的超定方程组，未知数是该点的(X, Y, Z)。实现上与后方交会类似，但设计矩阵A的每一行对应一张像片对该点的两个偏导数(∂F/∂X, ∂F/∂Y, ∂F/∂Z)等。

4.3 第三步：实现完整的光束法区域网平差

这是最终的挑战。你需要构建一个庞大的稀疏线性系统。

核心流程：

  1. 未知数排序：将所有待求参数（所有像片的6个外方位元素 + 所有待求物方点的3个坐标）排列成一个长向量X。记住每个参数在向量中的索引位置至关重要。
  2. 构建稀疏设计矩阵A：
    • 遍历每一个像点观测值。
    • 对于该观测值，它关联一个像片i和一个物方点j。
    • 它贡献两行到矩阵A：一行对应x方程，一行对应y方程。
    • 在这两行中，只有与该像片对应的6个未知数（外方位元素）和与该物方点对应的3个未知数（坐标）的位置上有非零值（即偏导数值），其他位置均为0。
    • 使用Eigen::Triplet列表来逐个添加这些非零元是最有效的方式。
  3. 构建常数项向量L：同样，每个观测值贡献两个常数项(x_obs - x_calc), (y_obs - y_calc)。
  4. 求解与迭代：使用Eigen的稀疏求解器解法方程A^T * A * dX = A^T * L。由于矩阵巨大，直接求逆不可能，必须使用迭代法（如共轭梯度法CG）或直接法中的稀疏Cholesky分解（如LDLT）。
  5. 更新与收敛判断：用解得的dX更新所有未知参数，重复迭代直至收敛。

实操心得：在构建稀疏矩阵时，预先为tripletList预留足够空间（reserve(观测值数量 * 2 * (6+3))）能显著提升性能。另外，光束法平差对初始值非常敏感，糟糕的初始值会导致迭代发散。通常先用后方交会（对控制点像片）和前方交会（对连接点）得到一个相对较好的初始网，再送入光束法进行整体优化。

5. 性能优化与工程实践要点

5.1 稀疏矩阵求解策略

对于成百上千张像片、数十万个点的项目，法方程矩阵N = A^T * A的维度可能达到数十万，但它是高度稀疏且具有特定块状结构的。选择合适的求解器是关键：

  • Eigen::SimplicialLDLT：对于正定对称矩阵，这是一种非常高效且稳定的直接分解法。适用于中小型问题或能放入内存的大型稀疏问题。
  • Eigen::ConjugateGradient或Eigen::BiCGSTAB：迭代法。对于超大规模问题，当直接法因内存不足而失效时，迭代法是唯一选择。但需要配置合适的预处理器（如不完全Cholesky分解）来加速收敛。
  • 使用专用库：对于工业级应用，可以考虑SuiteSparse（其CHOLMOD模块非常强大）或Intel MKL中的PARDISO求解器。它们比Eigen的稀疏求解器更加强大和高效，但集成稍复杂。

5.2 内存管理与数据组织

  • 避免拷贝大矩阵：尽量使用const引用或Eigen::Map来传递数据。
  • 清晰的数据生命周期：将原始观测数据、平差过程中的临时变量、以及最终结果分开管理。使用智能指针（std::unique_ptr）管理动态数组。
  • 利用观测值索引：建立从像片ID到其观测点列表、从物方点ID到其被哪些像片观测的索引，可以快速访问数据，避免线性搜索。

5.3 鲁棒性增强：粗差检测与剔除

实际数据中难免有误匹配或粗差。必须在平差流程中加入鲁棒性机制：

  • 验后残差分析：每次平差迭代后，计算每个像点观测值的标准化残差。如果某个观测值的残差绝对值远大于中误差（例如，大于3倍中误差），则标记为可疑粗差。
  • 权函数迭代（IGGIII方案）：不给可疑粗差直接赋零权（剔除），而是根据其残差大小，动态降低其权重。这比简单剔除更稳健，可以避免误删正确观测值。
    double robustWeight(double residual, double sigma0) { double k0 = 1.5, k1 = 3.0; // 常用阈值 double u = std::abs(residual) / sigma0; if (u <= k0) return 1.0; else if (u <= k1) return k0 / u; else return 0.0; // 或一个极小的值 }
    在下次迭代构建法方程时，将每个观测值的权重乘以其鲁棒权函数值。

5.4 开发与调试技巧

  1. 从小数据开始：先用2-3张像片、几个控制点和连接点的微型数据集调试，确保算法逻辑正确。
  2. 与成熟软件对比：用你的程序解算一个简单案例，将结果与商业或开源摄影测量软件（如OpenMVG, Colmap, Metashape）的结果进行对比。检查外方位元素和物方点坐标的差异量级。
  3. 可视化中间结果：将每次迭代后的物方点云和相机位置用简单的OpenGL或matplotlib（通过文件交互）画出来，直观观察解算过程的收敛情况。
  4. 单元测试：为旋转矩阵计算、偏导数计算、坐标投影等核心函数编写单元测试，使用已知的几何关系验证其正确性。

6. 常见问题排查与实战心得

6.1 迭代不收敛或发散

这是最常见的问题。

  • 症状：改正数dx越迭代越大，或者单位权中误差sigma0不降反升。
  • 排查：
    1. 检查偏导数：这是首要怀疑对象。用一个非常小的扰动（如1e-6）数值计算偏导数（(F(x+dx)-F(x))/dx），与你解析推导的公式计算结果对比。这是定位错误最有效的方法。
    2. 检查初始值：外方位元素初始值太差。尝试用更可靠的方法获取初始值，或者先用少量控制点解算一个粗略结果作为更多像片的初始值。
    3. 检查控制点配置：控制点不能近似共面或分布过于集中，否则会导致法方程病态。确保控制点在像片上的构像良好，分布均匀。
    4. 检查数据：仔细核对控制点物方坐标和像点坐标的对应关系，单位是否一致（米 vs 毫米？像素 vs 毫米？）。一个错误的数据点就可能导致整个解算崩溃。

6.2 解算精度不佳

  • 症状：平差后，单位权中误差sigma0仍然很大（例如，大于2个像素），或者检查点（未参与平差的控制点）的残差很大。
  • 排查：
    1. 内方位元素不准：如果使用的是相机检校得到的(x0, y0, f)，确保其准确。对于非量测相机，镜头畸变（特别是径向畸变）的影响巨大。必须在平差前对像点坐标进行畸变改正，或者将畸变参数（如k1, k2, p1, p2）也作为附加参数加入平差模型中（自检校光束法平差）。
    2. 观测值权重不合理：默认所有观测值等权。如果某些像点位于影像边缘（畸变大）或匹配质量不高，应适当降低其权重。
    3. 系统误差：可能存在未模型化的系统误差，如大气折光、地球曲率（对于大范围航测）等。对于高精度应用，需要在模型中考虑。

6.3 程序运行缓慢或内存溢出

  • 症状：处理稍大数据集时，程序卡顿或崩溃。
  • 排查与优化：
    1. 使用稀疏矩阵：确认你的光束法实现确实使用了Eigen::SparseMatrix，并且正确构建了Triplet列表。稠密矩阵会瞬间耗尽内存。
    2. 选择合适的求解器：对于超大规模问题，尝试迭代求解器（如CG）并配合预处理器。
    3. 减少拷贝：分析代码热点，使用性能分析工具（如gprof、Valgrind）。确保在循环中没有不必要的临时对象创建和拷贝。
    4. 分块处理：对于特大规模区域网，可以考虑“分块平差”策略，先对子区域平差，再将结果作为整体平差的初始值。

6.4 实战心得碎碎念

  • 坐标系是万恶之源：务必清晰定义并贯穿使用同一套坐标系。摄影测量中常用的是“右前上”坐标系：像空间坐标系（x右，y下，z前？不，应是x右，y下，z前？注意！）和物方坐标系。旋转角(φ, ω, κ)的定义顺序（是绕ZYX旋转还是绕YZX？）也必须与你的旋转矩阵计算公式严格对应。一个符号错误就能让你调试一整天。
  • 归一化的力量：在构建法方程前，将像点坐标(x, y)减去像主点(x0, y0)并除以焦距f，转换为归一化坐标。这能显著改善数值稳定性，因为数值量级都在1附近。
  • 日志输出很重要：在迭代过程中，详细输出每次迭代的单位权中误差、最大改正数、以及某些关键点坐标的变化。这是你监控平差进程的眼睛。
  • 从DLT开始理解：如果直接理解共线方程线性化有困难，不妨先实现一个直接线性变换（DLT）算法。DLT忽略旋转矩阵的正交性，用一组简单的线性参数直接建立像点与物方点的关系。虽然精度不高且不能直接得到外方位角元素，但它能帮你无初值地获得一个粗略解，对于理解整个“从二维到三维”的映射过程非常有帮助。

实现一个完整的摄影测量平差程序是一个系统工程，它融合了数学建模、数值计算和软件工程。当你第一次看到散乱的点云通过自己的程序被优化成一个严密的、符合几何约束的三维模型时，那种成就感是无与伦比的。这个过程会强迫你深入理解每一个参数、每一个方程的意义。记住，耐心调试和细致验证是通往成功的唯一路径。先从后方交会这个小目标开始，把它做精做透，再逐步扩展到光束法，你会发现自己对三维视觉的理解已经上了一个全新的台阶。