简介光束法区域网平差是摄影测量中的核心优化技术这份VC代码资源专门用于实现光束法平差与外方位元素解算适合摄影测量、计算机视觉方向的学生与开发者深入学习。压缩包共32个文件以8个.h头文件与7个.cpp源文件为主附带5个txt数据文件及若干工程配置文件代码结构清晰便于在VC环境中直接打开编译。其中txt文档包含原始观测数据与解算结果可对照程序运行输出帮助理解最小二乘平差、旋转矩阵构建及外方位元素迭代求解流程。整个rar包仅55KB轻量实用。已有1320人学习下载配合工程源码与数据说明读者可逐步跟踪从像点坐标到外方位元素的计算全过程掌握Bundle Adjustment算法在摄影测量中的实际应用。工程内注释较为完整适合作为课堂配套代码或自学入门范例便于二次开发与算法验证。 光束法区域网平差这名字听着就唬人我当年第一次在课本上看到共线方程线性化的时候也是头皮发麻。后来真刀真枪用VC写空三程序踩了无数坑才把这套东西跑顺。现在回头看这个领域最难的从来不是公式本身而是怎么把数学表达式变成能稳定迭代、能处理几万张像片、还能快速收敛的工程代码。这篇就专门讲光束法区域网平差的VC实现从共线方程展开到法方程组装最后聊收敛控制和粗差剔除适合正在写平差程序、或者打算从零搭一套空三管线的同学参考。1. 光束法平差不是拿来就写的先搞懂它解决什么问题很多人在GitHub上搜到一堆光束法区域网平差的代码下载下来编译一堆错或者跑通了却不知道结果对不对根本原因是不清楚这套算法到底在优化什么。一句话概括光束法平差本质上是把相机内参、每张影像的外方位元素位置和姿态、以及每个加密点的三维坐标放在同一个最小二乘系统里联合求解让所有像点观测值的重投影误差总和最小。物理意义也很好理解。一条光线从地面点X出发穿过镜头中心S打在像平面上形成一个像点x。理论上这三者严格共线但实际测量中像点坐标有噪声所以光线并不严格共线。光束法就是调整X、S、以及影像姿态角让重新投影出来的像点尽量贴近实际量测的像点。注意这里的关键词是光束——每一对地面点和像点构成一条光束整个区域网有成百上千条这样的光束大家一起参与平差所以叫光束法区域网平差。需要提醒的是如果你只是用现成的商业空三软件比如Inpho、ContextCapture、Pix4D这类的完全不需要自己写这个算法。真正需要手写代码的往往是这几种情况一是做新型传感器模型研究比如线阵推扫相机、鱼眼镜头、甚至非量测相机的标定商业软件不支持你的内参模型二是做大规模离线处理的地面端工具需要把平差逻辑嵌入到自有数据管线里三是教学科研需要把平差过程拆开看清楚每一步发生了什么。我当初属于第二种要在自研的三维重建工具链里嵌一套控制点平差模块VC是团队的老技术栈所以选了C17加Eigen3的组合。关于开发环境多说一句。网上搜到的老代码很多是VC6或VS2008时代的用的是自定义Matrix类加afx.h放到VS2022里根本编译不过。我现在的建议是用Visual Studio 2022的C17标准Eigen3做矩阵运算OpenCV只用来做影像读写、特征提取、像点坐标观测值准备平差核心不要依赖OpenCV的结构因为OpenCV的Mat在频繁动态reshape时并不方便。2. 共线方程线性化最容易写错的雅可比矩阵部分2.1 共线方程的代码形式共线方程的标准形式是这样x - x0 -f * (r11*(X-Xs) r21*(Y-Ys) r31*(Z-Zs)) / (r13*(X-Xs) r23*(Y-Ys) r33*(Z-Zs)) y - y0 -f * (r12*(X-Xs) r22*(Y-Ys) r32*(Z-Zs)) / (r13*(X-Xs) r23*(Y-Ys) r33*(Z-Zs))注意这里旋转矩阵R的写法各教材不一有的用R的元素是列向量还是行向量转置来转置去很容易搞混。我的经验是固定采用一个惯例R的第一行是相机坐标系到物方坐标系的旋转矩阵的第一列也就是R的元素与角元素的关系在一次推导里定死然后写一个单元测试验证——把物方点先转到相机坐标系再投影到像平面结果必须和直接带入共线方程一致。实际写代码的时候我更推荐把它拆分两步// Camera space coordinate Vec3d Pcam R * (X - S); // R is rotation from world to camera double x -f * Pcam.x / Pcam.z; double y -f * Pcam.y / Pcam.z;这样可读性好很多而且后面求雅可比的时候链式法则也清晰。2.2 线性化与误差方程的展开共线方程是关于未知数外方位元素6个、地面点坐标3个的非线性函数所以要用泰勒展开线性化。把当前估计值带入后得到近似影像坐标实测像点坐标减去计算值得到残差向量l然后对每个未知数求偏导组成误差方程v A * delta_t B * delta_x - l其中delta_t是每张像片6个外方位元素的改正数delta_x是每个地面点3个坐标的改正数。A是2x6的雅可比矩阵对相机位姿求导B是2x3的雅可比矩阵对地面点求导。我见过很多人在这一步被坑对着教科书公式抄偏导数结果符号错了、或者漏了分母的链式。其实不用死记用数值扰动求导来验证解析推导的结果就行。// Analytical Jacobian for exif rotation angles (phi, omega, kappa) Matrix2x6 computePoseJacobian(const Vec3d Pc, const Vec3d X, const Matrix3d dR_dphi, ...)具体的解析表达式往推导容易绕晕。我建议这样写先写一个函数输入外方位元素、地面点坐标、相机内参输出投影坐标然后把雅可比矩阵的所有项用解析式展开。但展开前先做数值检查——用有限差分法算一遍数值导数和解析导数对比全部对齐后再集成进平差循环。这个习惯救了我太多次了。2.3 旋转矩阵与角元素求导的坑姿态角的定义顺序直接决定雅可比长什么样。摄影测量里传统用phi、omega、kappa也就是先绕X轴再绕Y轴再绕Z轴的组合矩阵是三个基本旋转矩阵的乘积。但是如果你用Tait-Bryan角的Yaw-Pitch-Roll或Roll-Pitch-Yaw求导结果完全不同。更麻烦的是在某些姿态比如俯仰角接近90度时欧拉角会退化导致求导奇异。我的建议是平差内部用旋转矩阵或四元数做状态量只在输入输出时转成欧拉角给人看。如果一定要用欧拉角做未知数就限定转角范围并在迭代中检查增量是否引起退化一旦发现要人为压制步长。我自己最后用的是旋转向量axis-angle前三个参数是旋转向量配合罗德里格斯公式转成矩阵雅可比用数值扰动配合解析组合稳定性和可维护性都很好。3. 区域网结构与法方程组装稀疏是所有性能优化的起点3.1 数据组织方式写平差程序第一步是把观测数据用合适的结构装起来数据结构设计和算法本身同样重要。我常用的设计是这样struct Image { int id; std::string name; Vec6d pose; // 6 DOF pose or rotation vector, depending on parameterization double focal; Vec2d principal; // principal point }; struct TiePoint { int id; Vec3d X; // estimated 3D coordinates std::vectorObservation obs; // observations in multiple images }; struct Observation { int imageId; Vec2d xy; // measured image point };每张像片有一个位姿参数索引每个地面点有一个坐标参数索引。关键是建立参数索引表把所有待求参数排成一个大向量每个地面点和每张像片都知道自己的参数在向量中的位置。这一步在组装法方程时至关重要我最初图省事用线性搜索找索引结果1万张像片、200万个观测时程序跑得和蜗牛一样。后来改成预先构建哈希表或者直接给每个对象存一个整数索引速度提升非常明显。3.2 法方程的分块结构把所有误差方程的系数矩阵组合起来得到法方程[ A^T P A A^T P B ] [ delta_t ] [ A^T P l ] [ B^T P A B^T P B ] [ delta_x ] [ B^T P l ]这里P是观测值权阵通常取对角阵。直接把这个大矩阵整个存下来做Cholesky分解是灾难——假设有1万张像片、50万个加密点、每张像片1000个观测点未知数数量大约是6万150万156万法方程矩阵的稠密版本存储根本不可能。但法方程矩阵是分块稀疏的每张像片只和它观测到的加密点相关每个加密点只出现在拍到它的那几张像片中。于是就有了经典的两类解法改化法方程先消去地面点先把加密点坐标消元解出相机位姿未知数6m个m是像片数然后再回代求加密点坐标。这是老一代摄影测量程序的经典路线因为早期的计算机内存极小法方程只能存几十阶的小矩阵。整体稀疏Cholesky分解直接用稀疏矩阵存储法方程然后做稀疏Cholesky分解求解。现代的做法适合处理超大区域网。我当时先写的是改化法方程因为逻辑清楚逐点消元的思路和手工计算流程完全一致容易调试。后来扩展到几万张影像时又加了稀疏直接求解器。3.3 逐点消元法的VC实现要点逐点消元的核心流程是遍历每个加密点把该点涉及的所有观测误差方程组成一个小的法方程子块然后用Schur补把这个地面点的未知数消掉把贡献累加到全局改化法方程上同时保留相关信息用于回代。伪代码如下for each tiePoint p: // collect all obs of p // for each obs: compute A_i (2x6), B_i (2x3), l_i (2x1) // build block normal equations for this point: // N_tt sum A_i^T P A_i (6x6 per obs, accumulated) // N_tx sum A_i^T P B_i (6x3) // N_xx sum B_i^T P B_i (3x3) // b_t sum A_i^T P l_i // b_x sum B_i^T P l_i // Schur complement: // N_tt_total - N_tx * inv(N_xx) * N_tx^T // b_t_total - N_tx * inv(N_xx) * b_xN_xx是个3x3的矩阵求逆非常便宜可以用Eigen的inverse()或者手动算伴随矩阵。要注意的是有的地面点只被少数几张像片拍到N_xx可能病态这时要加一个微小的正则项比如1e-6的单位阵或者直接把投影点坐标固定掉。全局改化法方程的大小是6m x 6mm是像片数。这个矩阵虽然是稠密的但对几千张像片来说约几万阶内存还能接受用带主元的Cholesky分解或LDLT分解即可。到几万张像片时改化法方程也会变大但仍然比原始法方程小两个数量级在很多无人机小场景下已经够用了。3.4 稀疏存储的建议如果你要走整体稀疏求解器Eigen自带的SparseMatrixdouble配SimplicialLDLT是个很实用的组合。组装时用triplets收集法方程的非零元素然后一次性构建稀疏矩阵再求分解。这个方案对中等规模几万张像片、几十万加密点都很稳缺点是Cholesky分解对矩阵的填充顺序敏感可能需要用AMDOrdering或自带的嵌套剖分算法做重排序。Eigen里这部分是内置的直接用就行。关于数值类型我只提醒一点平差过程中内参焦距、外方位线元素和地面点坐标的量级可能是几毫米到几千米法方程矩阵的条件数会非常差。这时候用float绝对不行必须double。有条件的话可以做一次简单的对角缩放即把每个未知数的法方程行/列除以该对角元的平方根能明显改善迭代收敛速度。4. 迭代与控制收敛判定、初值、粗差剔除的一整套实战经验4.1 初值从哪来光束法区域网平差是迭代算法初值给不好直接发散或者在局部极小值里出不来。初值获取的通常路线是先对每张像片做单像空间后方交会用控制点或相对定向结果求出初始外方位元素然后用前方交会算加密点坐标再用这些初值做光束法整体平差提升精度。没有控制点的场景就先做相对定向把连续的像片链拼接起来构建出一个初始模型坐标再做相似变换或绝对定向放到物方坐标系。这一步很容易被忽略但它的质量直接决定后面迭代能不能收敛。我自己写过一个检查初值质量的小技巧把初始外方位元素带入共线方程对所有观测计算投影误差的均方根若均方根大于几十个像素说明初值不行需要回头检查相对定向和连接点匹配的精度而不是硬着头皮平差。4.2 收敛判定的几个阈值迭代收敛的判断一般看三类指标未知数改正数所有外方位元素和加密点坐标的改正数L2范数或最大绝对值小于阈值。比如位置改正小于1e-4米、角元素改正小于1e-6弧度。单位权中误差变化每次迭代后计算单位权中误差sigma0 sqrt(V^T P V / r)当连续两次迭代sigma0的相对变化小于0.1%时认为收敛。像点残差分布重投影残差均值接近0、方差达到预设精度比如对于1/10像素级别的匹配点均方根残差小于0.3像素。实践中最常用的是第一条加第二条组合。但要注意改正数小于阈值也可能意味着迭代陷入平坦区域而没收敛所以还要看残差是否真的下降。我习惯边迭代边打印RMS肉眼扫一眼曲线就知道是不是正常下降。4.3 迭代发散的常见原因和应对发散的原因无非几种初值差太远、有粗差观测值、个别点的法方程病态。我踩得最多的是第三种尤其是一些纹理稀疏的区域匹配点几乎共面N_xx接近奇异。这时候解出来的改正数会巨大直接把整个平差带飞。应对办法有这几招阻尼法Levenberg-Marquardt思想在法方程对角元上加上一个小的阻尼项N N lambda * diag(N)lambda从1e-3开始每次迭代若残差上升就增大lambda若下降就减小lambda。这一招能救回大多数发散情况。删点或降权如果某观测的残差超过3倍中误差下轮迭代直接把它剔除或者权设为零。如果某个点参与观测的像片数太少比如少于3张从平差系统里剔除加密点。固定病态参数把法方程的小特征值对应的方向固定住不要更新等后续有更多观测再释放。4.4 粗差剔除与选权迭代区域网平差的实际数据里像点匹配错误是永远的噩梦。哪怕误匹配率只有1%也可能让平差结果偏掉几个像素。经典的剔除策略是data snooping迭代结束后算出每个观测的残差用残差和单位权中误差的比值做统计检验把超限的观测剔除后重新平差如此反复。更实用的是选权迭代法就是用一个权函数根据残差动态调整每项的权相当于自动给粗差降权。常用的Huber权函数double huberWeight(double r, double sigma) { double c 1.345; double absr std::abs(r); if (absr c * sigma) { return 1.0; } else { return (c * sigma) / absr; } }每次迭代求解后计算每个观测的标准化残差用Huber或IGG权函数更新每个观测的权然后继续迭代。这个方法不需要显式删点程序逻辑简单且非常稳健对付平面匹配中的离群点效果显著。5. 实测效果与调参思路一个无人机三像片小场景的验证理论说再多不跑数据都是空的。我建议第一次写光束法平差的人先构造一个最小的验证场景三张无人机影像每张有足够重叠区域的重叠度几十个连接点外加四个控制点。这个规模可以手工检查每一个中间结果是否合理。我当时用一组模拟数据验证相机焦距2000像素、像幅6000x4000、航高100米外方位元素给定一个真实值从这些参数生成像点坐标再添加0.1像素的高斯噪声。平差程序收敛后把解算的外方位元素与真值对比位置误差应该在厘米级角度误差在0.001度以内。这一步通过后我才开始接真实数据。调参的时候注意这几个关键项观测残差单位是像素还是毫米统一有的代码里焦距用米导致单位混乱像点坐标要做主点偏移和畸变改正如果只做相对定向主点可以用大概值但做绝对定向和控制点拟合时就有影响权的设置——控制点坐标的权要用坐标精度换算成等效像点精度别直接给个1。另外迭代上限设置也要合理我一般设置最多20次迭代若超过仍然不收敛就把阻尼系数放大重新迭代还不行就打印出最大的残差项检查是否有匹配错误。迭代次数太多还可能是初值问题不要光调阻尼参数回头整理初值更有效。四元数方式则在更新姿态时可以一直保持归一化用quadratic solver也平滑很多。如果做分布式的平差需要每个子块之间交换参数估计四元数的切空间表示也比较直接。所以我现在的实现默认用旋转向量配罗德里格斯公式避免欧拉角奇异性又不至于像四元数那样维护起来费心。平差这个环节在整个三维重建管线里往往是最后一块拼图前面特征匹配、相对定向做得再好到这里一步出错精度就全毁了。我个人的体会是不要迷信某一种参数化方式多用模拟数据验证你的雅可比矩阵和更新策略把这块地基打扎实之后后面接大规模数据反而没那么难。最后再多说一句网上流传的各种VC光束法平差代码能直接跑通的是少数很多都是教材代码的誊写或者旧项目残片。你把这篇文章里的核心逻辑吃透完全可以自己动手写一个比它们可靠的版本。遇到bug时先把问题拆成雅可比对不对、法方程组装对不对、求解器对不对三块单独验证比在整套代码里大海捞针高效得多。真到了几万张像片的规模你还能把稀疏求解器、选权迭代这些优化项一个个加上去这套东西就用一辈子了。本文还有配套的精品资源点击获取