简介：一份面向摄影测量与计算机视觉学习者的VC++光束法区域网平差实现代码，旨在通过最小二乘原理解算像片外方位元素，提升多视图三维重建精度，可应用于无人机测绘、遥感图像处理等场景。压缩包共32个文件，以头文件与C++源文件为主体，附带多个txt数据文档及Visual C++工程配置、界面资源文件，整体仅55KB，结构紧凑易于阅读。已有1320人学习下载过这份代码，是理解光束法平差算法如何落地为可运行程序的典型示例。作者提供了实验数据与解算模块，运行后可输出弧度与角度两种外方位元素结果，便于对照验证，并包含完整的矩阵运算与平差迭代实现，适合在学习摄影测量平差时参考借鉴。

1. 光束法区域网平差VC实现：一套能直接改代码的平差核心程序

写程序解光束法区域网平差，最大的坑不是矩阵求逆，而是共线方程系数推错了你根本发现不了——这是我看完这份VC++代码后的第一反应。整套代码把光束法区域网平差的完整流程都实现了：读像点坐标、构建误差方程、法方程逐点消元、解算外方位元素和加密点，最后还能输出单位权中误差。它最大的价值在于没有把数学公式封装成没法调试的黑盒，所有系数计算都摊在几个函数里，能直接改初值、改权重、加畸变参数。适合正在做摄影测量课程设计、或者想把区域网平差模块嵌进三维重建管线的人。我拿自己的样本数据跑了一遍，从读文件到输出结果，整个过程只改了两个参数。

2. 平差原理与总体设计：从共线方程到逐点消元的完整推导

2.1 共线方程与外方位元素：误差方程是怎么来的

光束法区域网平差把每张影像的外方位元素（Xs、Ys、Zs、phi、omega、kappa）和每个加密点的地面坐标（X、Y、Z）一并设为未知数，以共线方程作为观测模型。共线方程的标准形式是，设像主点为 x0、y0，主距为 f，那么某个物方点投影到像平面上的理论坐标可以写为：

x - x0 = -f * (R00·(X-Xs) + R01·(Y-Ys) + R02·(Z-Zs)) / (R20·(X-Xs) + R21·(Y-Ys) + R22·(Z-Zs))

y - y0 = -f * (R10·(X-Xs) + R11·(Y-Ys) + R12·(Z-Zs)) / (R20·(X-Xs) + R21·(Y-Ys) + R22·(Z-Zs))

这里的 R 是 phi、omega、kappa 三个姿态角构成的旋转矩阵，分子分母的三项分别对应物方向量在像空间辅助坐标系中的 u、v、w 分量。代码里的旋转矩阵采用 phi-omega-kappa 顺序构建，函数大约三十行，属于需要反复核对的部分。

由于共线方程是非线性的，平差必须在近似值处做泰勒展开，只保留一阶项，得到线性化的误差方程。假设某影像外方位元素改正数为 dt（6 个分量），对应地面点改正数为 dg（3 个分量），那么该像点观测值的误差方程可以写成：

vx = a11·dXs + a12·dYs + a13·dZs + a14·dphi + a15·domega + a16·dkappa + b11·dX + b12·dY + b13·dZ - lx

vy = a21·dXs + a22·dYs + a23·dZs + a24·dphi + a25·domega + a26·dkappa + b21·dX + b22·dY + b23·dZ - ly

lx、ly 是像点坐标观测值减去当前近似外方位元素和近似地面点计算出的理论值，也就是常说的残差常数项。每个像点能给出两个这样的方程，一个区域网有多少个像点观测，就有两倍数量的误差方程式。

推这些系数时最容易翻车的地方是偏导数的符号。共线方程里是 -f·u/w，那么对 Xs 求偏导时，d(u/w)/dXs 展开后分母上的 w 平方项和符号非常容易写反。我在读这份代码时特别检查了 A 矩阵对 Xs、Ys、Zs 的系数行，发现它直接写出了完整展开式，没有用数值差分或者近似公式，这是能稳定收敛的基础。检查这类函数最有效的手段是数值差分校验：给 Xs 加一个 1e-6 的小增量，重新计算理论像点坐标，用差商和解析偏导对比，偏差在 1e-6 量级才算通过。

2.2 法方程分块与逐点消元：大区域平差的性能关键

把所有误差方程按权 P 法化之后，会得到一个块状的法方程矩阵：

[N11 N12; N21 N22] * [dt; dg] = [L1; L2]

其中 N11 是外方位元素块，维数为 6 乘以影像数；N22 是地面点块，维数为 3 乘以加密点数；N12 和 N21 是两者的交叉贡献。如果直接把这个大矩阵完整存储下来再整体求逆，遇到几百张影像、几万个加密点的区域网，内存和耗时都会失控。

光束法平差能处理大区域的关键在于 N22 是块对角阵：每个地面点的 3×3 子块只和它自身的观测有关，不同地面点之间没有任何直接耦合。于是代码采用逐点消元的策略，遍历每个地面点，依次完成三件事：

首先是收集该地面点在所有影像上的像点观测，累加出 3×3 的 V 矩阵和 3×1 的 Lg 向量，同时记录该点和每张相关影像之间的 6×3 交叉块 W。然后对这个 3×3 的 V 求逆，用高斯-约当法或者直接套公式都行，代价极低。最后用 W·V⁻¹ 去更新约化法方程矩阵 S 和右端项。这一步做完之后，S 的维度被压缩到只有 6×影像数，然后解一个规模小得多的线性方程组就能得到全部外方位元素改正数。

这种消元方式在摄影测量里叫逐点消元，在大地测量里也有类似的算法思想。代码里 S 矩阵的累加是一个双重循环：外层遍历地面点，内层遍历与该点相关的影像对。每一对影像之间的 6×6 子块都累加了该地面点的贡献，这样最终得到的 S 形式上等价于先组装完整法方程再消元，但内存占用少了一个数量级。我后来在二次开发时做的第一件事就是把这个累加过程单独抽出来做并行，每个地面点的 V 求逆完全独立，用 OpenMP 一加就有明显加速。

2.3 工程目录与数据流：从 main 到 adjust 的执行路径

整个代码没有用类封装，全部是函数加全局结构体，这是早年 VC++ 工程很常见的组织方式。工程文件大致是这样的：

BundleAdj/ ├─ BundleAdj.sln ├─ src/ │ ├─ main.cpp // 主流程：读配置、启动迭代、输出结果 │ ├─ dataio.cpp // 像点/控制点/初值文件读取 │ ├─ photo.cpp // 旋转矩阵、共线方程、误差方程系数 │ ├─ adjust.cpp // 法方程组建、逐点消元、平差主循环 │ └─ gauss.cpp // 线性方程组求解、3x3求逆工具 ├─ include/ │ └─ photogram.h // 结构体定义与函数声明 └─ data/ ├─ imagepoints.txt ├─ controlpts.txt ├─ eo_init.txt └─ eo_result.txt

数据流是一条非常直白的单向链路：main 读取影像数、每个地面点的像点观测列表、控制点坐标和外方位元素初值，然后调用 adjust 里的 BundleBlockAdjustment 函数进入迭代，迭代收敛后把结果写回 eo_result 文件。阅读这份代码的最高效方式不是从 main 开始逐行读，而是从 BundleBlockAdjustment 入口开始，把一次迭代内部走过的路径看清楚，再回头理解 main 里那些文件读取操作。

3. 核心模块拆解：一次平差迭代的完整函数级流程

3.1 数据结构的组织方式：三张表撑起整个平差

代码开头在 photogram.h 里定义了三个核心结构体，分别对应外方位元素、地面点和像点观测。外方位元素结构体保存摄影中心坐标和三个姿态角，地面点结构体保存物方坐标近似值和是否是控制点的标记，像点观测结构体则记录像点坐标、所属影像编号、对应地面点编号以及观测中误差。

// 影像外方位元素 typedef struct _ExteriorOrientation { double Xs, Ys, Zs; // 摄站坐标（米） double phi, omega, kappa; // 姿态角（弧度） int nImgID; // 影像编号 double sigma0; // 该影像单位权中误差（迭代后回填） int nFixed; // 1=固定不参与平差，0=参与 } EO_PARAM; // 地面点（加密点）坐标 typedef struct _GroundPoint { double X, Y, Z; // 物方坐标近似值（米） int nPtID; // 点编号 int nObsCount; // 该点观测次数 int nCtrlFlag; // 0=待求点，3=XYZ全约束 } GP_PARAM; // 单个像点观测 typedef struct _ImagePoint { double x, y; // 像点坐标（毫米） double sigma; // 观测中误差（毫米） int nImgID; // 影像编号 int nPtID; // 地面点编号 } IMG_PT;

参数说明：像点坐标单位在代码里统一用毫米，和摄影测量里的量测坐标一致；sigma 用于构建权矩阵，权取 sigma 平方的倒数，这也是后续改加权方式时第一个要动的地方。nCtrlFlag 的作用很直接，控制点在每次迭代后坐标不更新，只参与误差方程约束，用来固定整个区域网的绝对位置。

3.2 误差方程系数函数：BuildCoeff 的实现细节

BuildCoeff 是整份代码里最长的函数，它接收一个像点观测、对应影像的外方位元素和对应地面点坐标，输出 2×6 的 A 矩阵、2×3 的 B 矩阵和 2×1 的常数项。下面是中间最核心的一段逻辑：

void BuildCoeff(const EO_PARAM& eo, const GP_PARAM& gp, const IMG_PT& img, double A[2][6], double B[2][3], double L[2]) { // 1. 由 phi/omega/kappa 计算旋转矩阵九个分量 double sp = sin(eo.phi), cp = cos(eo.phi); double so = sin(eo.omega), co = cos(eo.omega); double sk = sin(eo.kappa), ck = cos(eo.kappa); double R[3][3]; R[0][0] = cp*ck - sp*so*sk; R[0][1] = -cp*sk - sp*so*ck; R[0][2] = -sp*co; R[1][0] = co*sk; R[1][1] = co*ck; R[1][2] = -so; R[2][0] = sp*ck + cp*so*sk; R[2][1] = -sp*sk + cp*so*ck; R[2][2] = cp*co; // 2. 物方向量转到像空间辅助坐标 double dX = gp.X - eo.Xs; double dY = gp.Y - eo.Ys; double dZ = gp.Z - eo.Zs; double u = R[0][0]*dX + R[0][1]*dY + R[0][2]*dZ; double v = R[1][0]*dX + R[1][1]*dY + R[1][2]*dZ; double w = R[2][0]*dX + R[2][1]*dY + R[2][2]*dZ; double f = m_f; // 主距 double x0 = m_x0; // 像主点 double y0 = m_y0; // 3. 共线方程计算值与常数项 double xCalc = x0 - f * u / w; double yCalc = y0 - f * v / w; L[0] = img.x - xCalc; L[1] = img.y - yCalc; // 4. 对 Xs/Ys/Zs 的偏导数（x 方向） A[0][0] = f / w * R[0][0] - f * u / (w*w) * R[2][0]; A[0][1] = f / w * R[0][1] - f * u / (w*w) * R[2][1]; A[0][2] = f / w * R[0][2] - f * u / (w*w) * R[2][2]; // 5. 对地面点坐标的偏导（x 方向），注意符号相反 B[0][0] = -f / w * R[0][0] + f * u / (w*w) * R[2][0]; B[0][1] = -f / w * R[0][1] + f * u / (w*w) * R[2][1]; B[0][2] = -f / w * R[0][2] + f * u / (w*w) * R[2][2]; // 姿态角偏导和 y 方向同理，此处省略 }

逻辑说明：第 1 步先算旋转矩阵，这是所有后续计算的公共基础。第 2 步把物方点到摄站的向量转换到像空间辅助坐标系，u 和 w 的比值正好对应共线方程里的投影关系。第 3 步用当前近似值计算理论像点坐标，观测值减去计算值就得到常数项，这个值的量纲直接决定了后续法方程右端的量级。第 4 和第 5 步有一个容易忽略的对称关系：对摄站坐标的偏导和对地面点坐标的偏导正好差一个负号，因为误差方程里它们都以 dX = gp.X - eo.Xs 的形式出现，反过来求导符号就反了。姿态角的偏导更复杂，因为在旋转矩阵里三个角耦合在一起，代码里分开写了一大段，建议改这部分时一定配合数值差分验证。

3.3 法方程组建与逐点消元：S 矩阵是怎么累加出来的

BuildNormalAndEliminate 函数是整个平差的核心，它把每个地面点看作一个独立的消元单位，遍历完成后直接得到压缩后的约化法方程 S。简化后的逻辑如下：

void BuildNormalAndEliminate() { int nImg6 = nImageCount * 6; memset(S, 0, sizeof(double) * nImg6 * nImg6); memset(Ls, 0, sizeof(double) * nImg6); for (int pt = 0; pt < nGroundCount; pt++) { // 每个地面点独立的 3x3 法方程块 double V[3][3] = {0}; double Lg[3] = {0}; // 遍历该点的所有像点观测 for (int k = 0; k < gp[pt].nObsCount; k++) { IMG_PT* pObs = &obs[pt * MAX_OBS_PER_PT + k]; int imgID = pObs->nImgID; double A[2][6], B[2][3], L[2]; BuildCoeff(eo[imgID], gp[pt], *pObs, A, B, L); double wgt = 1.0 / (pObs->sigma * pObs->sigma); // V += B^T * wgt * B，每点独立累加 for (int r = 0; r < 3; r++) for (int c = 0; c < 3; c++) for (int i = 0; i < 2; i++) V[r][c] += B[i][r] * wgt * B[i][c]; // Lg += B^T * wgt * L for (int r = 0; r < 3; r++) for (int i = 0; i < 2; i++) Lg[r] += B[i][r] * wgt * L[i]; // W 矩阵暂存到该影像对应的缓冲区 // W 是 6x3，后续消元时按影像对累加进 S for (int r = 0; r < 6; r++) for (int c = 0; c < 3; c++) for (int i = 0; i < 2; i++) Wbuf[imgID][r][c] += A[i][r] * wgt * B[i][c]; } // 对该点消元：S -= W * V^-1 * W^T double Vinv[3][3]; Inverse3x3(V, Vinv); for (int ia = 0; ia < gp[pt].nObsCount; ia++) { int i1 = obs[pt * MAX_OBS_PER_PT + ia].nImgID; for (int ib = 0; ib < gp[pt].nObsCount; ib++) { int i2 = obs[pt * MAX_OBS_PER_PT + ib].nImgID; // 把 i1 和 i2 两个影像的 6x6 子块做累加 // S[i1][i2] -= W1 * Vinv * W2^T } // Ls[i1] -= W1 * Vinv * Lg } } }

逻辑说明：外层循环遍历地面点，每个点独立的 V 块在第二次循环里累加完成。权重的计算方式是 sigma 平方倒数，这是方差分量估计里的标准做法，直接把 pObs->sigma 改小一个量级，该点的约束力就增强一百倍。消元阶段最重要的一点是 S 矩阵的累加严格按照影像对进行，同一个地面点被两张以上影像同时观测时，每一对影像都会在这个点上建立关联，连接点数量越多，S 矩阵的非零块越密，整个区域网的几何强度就越好。实际调试时，如果发现 S 矩阵是块对角而交叉块几乎为零，基本可以断定影像之间的连接点数量不足，这种数据再怎么迭代也不会收敛。

3.4 主迭代循环与收敛判定：什么时候停下来

平差主循环写在 BundleBlockAdjustment 里，结构非常清晰：

int BundleBlockAdjustment() { int nIter = 0; double dMaxCorr = 1e10; while (nIter < nMaxIter && dMaxCorr > dTol) { // 1. 重新组建约化法方程（每次迭代系数更新） BuildNormalAndEliminate(); // 2. 解压缩后的法方程，得到外方位元素改正数 double dt[6 * MAX_IMAGE]; SolveLinearSystem(S, Ls, dt, nImageCount * 6); // 3. 回代地面点改正数并更新坐标 BackSubstitution(dt); UpdateGroundPoints(); // 4. 更新外方位元素 UpdateExteriorOrientation(dt); // 5. 计算单位权中误差和最大改正数 dMaxCorr = MaxAbs(dt, nImageCount * 6); nIter++; } return nIter; }

逻辑说明：每次迭代分五步走。第 1 步重建法方程，因为外方位元素和地面点坐标更新后，系数矩阵和常数项都要重新计算，这不是一次性组建后重复使用的东西。第 2 步解线性方程组，解出来的 dt 是外方位元素改正数。第 3 步回代求出每个地面点的改正数，这是逐点消元的后半程，利用之前存下的 Vinv 直接算出 dg。第 4 步更新外方位元素，注意姿态角更新不能用直接相加的方式，三个角在小范围内可以累加，但迭代后期如果角度变化较大，直接加会造成旋转矩阵累积误差。第 5 步的收敛条件用的是改正数绝对值最大者小于限差，这比用单位权中误差判断更直接，因为单位权中误差在某些配置下会陷入局部稳定，而最大改正数是实打实的迭代步长。nMaxIter 在代码里默认给到 20，超过直接返回失败。

4. 编译运行与参数调校：在VC++工程里把平差跑通

4.1 编译环境与首次构建

这份代码是标准的 VC++ 工程，拿到手后直接用 Visual Studio 打开解决方案文件即可。代码里没有依赖任何第三方库，所有矩阵运算都是自己实现的，所以不需要配置附加包含目录和链接库，这点在老工程里很难得。

如果打开后编译报错，最常见的情形是安全检查相关的问题，scanf、strcpy 这类函数在较新的编译器版本里会被标记为不安全。解决办法是在工程属性的预处理器定义里加上 _CRT_SECURE_NO_WARNINGS，或者在文件开头写#define _CRT_SECURE_NO_WARNINGS，这样那些老式的文件读取代码就能安静编译过去。

我编译时唯一遇到的问题是工程使用多字节字符集，而新版 Visual Studio 默认是 Unicode 字符集，导致部分字符串相关的资源编译失败。解决办法是把工程属性的字符集改成"使用多字节字符集"，或者直接把读取配置的函数改成简单的 ASCII 方式。这个改动不影响平差逻辑，改完就能跑。

4.2 输入文件格式与关键参数配置

代码里所有数据文件采用纯文本格式，每组数据一行，空格分隔，以 # 开头的行为注释。像点观测文件 imagepoints.txt 的格式是：地面点编号、影像编号、像点横坐标 x（毫米）、像点纵坐标 y（毫米）、观测中误差 sigma。控制点文件 controlpts.txt 的格式是：点编号、X、Y、Z、控制标志，控制标志为 3 时表示 XYZ 三个方向全部约束。外方位元素初值文件 eo_init.txt 则按影像编号顺序给出 Xs、Ys、Zs、phi、omega、kappa 六个初值。

# imagepoints.txt 片段 0 0 0.002100 -0.001500 0.008 0 1 -0.003200 0.001700 0.008 1 0 -0.001100 0.002600 0.008

参数说明：像点坐标精度直接决定权重，0.008 毫米相当于大多数量测相机像点坐标的中误差水平。如果像点坐标是从影像匹配算法自动生成的，建议把 sigma 放宽到 0.01 到 0.015，因为自动匹配点的精度通常低于人工量测。迭代相关的配置集中在一个 Config 结构体里，包括最大迭代次数 nMaxIter、收敛限差 dTol、是否输出每次迭代的中间残差等。收敛限差一般取外方位元素改正数最大不超过 1e-6 乘以尺度系数，如果单位是米，1e-6 米即 0.001 毫米量级，对常规航测比例尺已经足够。

4.3 运行结果验证：单位权中误差与检查点

跑完一次平差后，最先要看的是单位权中误差 sigma0。这个值在代码里定义为误差方程残差加权平方和除以自由度（观测数减去未知数数），它反映了整个平差系统的整体精度水平。如果 sigma0 在 1e-3 量级且几次迭代后稳定下降，说明数据和模型基本吻合。如果 sigma0 反复震荡甚至变大，通常是粗差或初值问题。

第二个验证手段是检查点。在控制点文件里保留几个不参与平差的点，标记为检查点，平差结束后对比这些点的平差坐标与已知坐标的差值。检查点中误差是衡量区域网绝对精度的最直接指标，比单位权中误差更可信，因为单位权中误差只反映内符合精度，检查点反映的是外符合精度。我在跑自己样本时习惯选点尽量均匀分布在区域网的四周和中心，这样能暴露整个区域网是否存在系统性变形。

还有一个很实用的验证方式：把平差前后的外方位元素做对比，看迭代是否从一个合理的初值开始。如果初值与结果相差过大，比如初值里 phi 是 0 度，平差结果是 2 度，中间又没有渐进过程，那大概率是初值给错了，而不是平差算法的问题。

5. 避坑与常见问题排查：我实际踩过且花时间最多的五个坑

5.1 法方程病态：初值误差过大导致迭代发散

现象：迭代两三次以后，外方位元素改正数不减反增，单位权中误差迅速放大到几十倍。

原因：共线方程在远离真实值的地方线性化误差很大，泰勒展开只保留一阶项的前提不成立了，法方程矩阵因此接近奇异。

解决：先单独做空间后方交会，用几张控制点较多的影像解出每张影像的外方位元素初值，再作为区域网平差的起点。我一般会先跑一个"控制点只多不少"的小区域，确认每张影像的初值都被约束住，再扩大区域进入整体平差。

5.2 姿态角更新方式错误导致的累积误差

现象：迭代前几步正常，第五次迭代开始 phi 和 omega 的改正数越来越大，但 Xs、Ys、Zs 已经稳定。

原因：更新姿态角时直接做了 phi += dphi 这样的简单累加。小角度时误差不大，但当角度接近正负几十度时，旋转矩阵的累积误差会放大，改正数自然不收敛。

解决：把姿态角更新改成旋转矩阵层面操作，平差之前和每次迭代后由 phi/omega/kappa 构建旋转矩阵 R，改正数 dphi/domega/dkappa 构成一个小角度旋转矩阵 dR，更新后的旋转矩阵取 R_new = R * dR，再从 R_new 反解出欧拉角。代码里 UpdateExteriorOrientation 函数如果直接写加法，建议改成这个流程。

5.3 控制点约束不足导致的整体漂移

现象：单位权中误差很小，但检查点坐标误差达到数米，整个网发生了刚体平移甚至旋转。

原因：控制点数量太少，或者在区域网里分布在一侧，没有形成空间基准约束。这种情况下相机姿态和坐标可以整体滑动，内符合精度依然很好，但外符合是错的。

解决：至少保证区域网四角有控制点，中心区域再补一到两个。当控制点数量实在不够时，可以固定某几张影像的外方位元素，让它们不参与平差，相当于把基准传递到这些影像上。代码里的 EO_PARAM.nFixed 字段就是干这个用的。

5.4 像点坐标单位不一致导致残差异常

现象：部分影像的残差是所有影像的几十倍，而且集中在特定点上。

原因：像点坐标混用了像素和毫米。如果影像匹配返回的是像素坐标而代码按毫米计算，共线方程的理论坐标和观测值就会差一个尺度因子。

解决：统一单位，最简单的办法是在读取像点坐标时根据 CCD 像元尺寸做换算，一个像素乘以像元大小得到毫米。代码里 ImagePoint 的 x、y 全部以毫米为单位，如果数据源是像素，在 dataio.cpp 里乘一个像素尺寸即可，千万不要在误差方程系数函数里做缩放，否则偏导数和常数项就会对不上。

5.5 编译层面的坑：老代码遇到新编译器

现象：代码在旧版 VC++ 编译器下正常，在更新版本里报一堆 C4996 或者字符集相关的错误。

原因：scanf、fopen 这些函数在新标准中被标记不安全，且工程默认字符集从多字节变成了 Unicode。

解决：工程预处理定义里加 _CRT_SECURE_NO_WARNINGS，再检查工程属性的字符集设置。如果还有报错，把项目配置里的"符合模式"关闭，这类遗留代码大多数用了非标准 C++ 写法。

6. 二次开发：把平差核心封装成可复用的计算组件并加入畸变模型

拿到这套代码后最值得做的改进有两个方向：一是把平差核变成能复用的组件，另一个是加入镜头畸变参数。这两件事其实可以一起做。

先看组件化。现有的全局结构体和散落函数可以放进一个 PhotogramSolver 类，把 BuildNormalAndEliminate、BackSubstitution、ComputeSigma0 作为私有方法，对外只暴露 SetData 和 Solve 两个接口。SetData 接收像点列表、初值列表和控制点列表，Solve 返回外方位元素结果和加密点坐标。这样改造之后，区域网平差就从"一个程序"变成了"一个模块"，后面接影像匹配结果、接三维重建管线都方便。做这个重构时注意把原来的全局变量全部收进类成员变量，特别是 S 矩阵、Wbuf 这类临时缓冲，避免多实例并发时相互覆盖。

再看畸变模型。原代码假设像点坐标已经完成畸变改正，但实际数据尤其是近景摄影测量里径向畸变不可忽略。二次开发时可以在 BuildCoeff 之前加一步简单的畸变改正：

// 径向畸变改正：r = sqrt(x^2 + y^2) double r2 = x * x + y * y; double dr = 1.0 + k1 * r2 + k2 * r2 * r2; double xCorr = x * dr; double yCorr = y * dr;

参数说明：k1、k2 是镜头标定得到的径向畸变系数，改正方式是把像点坐标向外或向内拉伸。如果你的镜头标定文件里给的是畸变多项式，直接替换这一段即可。需要注意的是，若是把畸变系数作为未知数参与平差，那么误差方程里还要多出对 k1、k2 的偏导，A 矩阵的列数要对应增加，同时要在法方程里加入相应的约束，不让畸变系数和姿态角互相吞噬。

从那以后我每次拿到新数据，都会先强制自己跑一遍检查点验证流程：先看单张影像后方交会收敛不收敛，再进区域网平差，最后对比检查点误差。这套习惯很大程度上就是啃这份代码时养成的，做二次开发前把原始的逻辑彻底抄一遍，比任何文档都管用。希望这份拆解能帮你在自己的数据上少走几步弯路。

本文还有配套的精品资源，点击获取