1. 从“自由网”说起为什么增量式SFM需要它如果你做过三维重建尤其是基于图像序列的增量式运动恢复结构那你一定遇到过这个经典困境随着新图像不断加入整个场景的稀疏点云和相机姿态会开始“漂移”。这种漂移不是某个点的误差而是整个重建的坐标系在缓慢地扭曲、缩放甚至旋转就像一艘没有锚的船在海上随波逐流。我们辛辛苦苦跑出来的重建结果可能因为一个微小的累积误差导致最后生成的模型和真实世界对不上或者相邻的模型块无法无缝拼接。这个问题的根源在于增量式SFMStructure from Motion本质上是一个“从无到有”的过程。我们通常从两张图像开始通过特征匹配和五点法或八点法计算出初始的相机相对姿态和一组三维点。然后我们不断地“注册”新图像找到新图像与已有三维点的对应关系通过PnPPerspective-n-Point求解新相机的姿态再通过三角化生成新的三维点。这个过程就像搭积木每一块新积木都依赖于之前已经搭好的部分。然而这里有一个关键假设我们求解每一块“积木”时都认为之前的部分是绝对正确的。但事实上之前的求解本身就带有误差。这些误差会随着增量过程不断累积和传播。更重要的是在整个过程中我们缺乏一个全局的、固定的参考系。初始的两张图像定义了一个任意的局部坐标系后续所有计算都基于这个“浮动”的坐标系。没有外部的绝对约束比如已知的GPS坐标、已知尺寸的标定物这个坐标系本身就可以发生任意的刚体变换旋转、平移、缩放而不影响图像投影的重投影误差。这就是所谓的“尺度模糊性”和“坐标系自由度”。“自由网平差”要解决的正是这个“浮动”坐标系的问题。它不是引入外部先验信息去“锚定”坐标系而是承认并处理这种自由度。其核心思想是在优化过程中允许整个网络所有相机和三维点作为一个整体进行平移、旋转和缩放但同时通过施加一组最小约束来消除这些自由度带来的数值不稳定问题从而得到一个在“最小二乘”意义下最优的、内部一致的重建结果。这个结果虽然仍然缺少绝对的尺度和朝向但其内部几何关系是最准确的为后续的绝对定向、模型融合或纹理映射提供了可靠的基础。所以当我们谈论“无先验约束下的增量式SFM自由网平差”时我们讨论的是一套在只有图像数据本身的情况下如何让增量式重建结果在数学上更严谨、更稳定、内部一致性更高的核心技术。它不是重建流程的替代而是其精度保障和理论完备性的关键一环。2. 拆解最小二乘SFM优化的数学心脏要理解自由网平差必须先透彻理解驱动整个SFM的引擎非线性最小二乘优化。在SFM中我们最终要优化的目标函数几乎总是重投影误差的平方和。假设我们有m个相机姿态n个三维点。对于第i个相机观察到的第j个三维点其在图像上的观测像素坐标为u_ij。我们用相机模型比如常用的针孔模型加径向畸变将三维点X_j投影到第i个相机的图像平面上得到预测的像素坐标π(R_i, t_i, X_j)其中R_i和t_i是第i个相机的旋转和平移。那么重投影误差就是观测值与预测值之差e_ij u_ij - π(R_i, t_i, X_j)。SFM的Bundle AdjustmentBA问题就是寻找所有相机参数{R_i, t_i}和所有三维点{X_j}使得所有观测到的重投影误差的平方和最小min Σ_ij || e_ij ||^2这是一个典型的非线性最小二乘问题因为投影函数π是非线性的。我们通常使用迭代优化算法来解决它最主流的就是列文伯格-马夸尔特Levenberg-Marquardt, LM算法。LM算法可以看作是高斯-牛顿法和最速下降法的结合通过引入一个阻尼因子来动态调整步长在远离最优解时像最速下降法一样稳定在接近最优解时像高斯-牛顿法一样快速收敛。LM算法的核心是求解一个线性方程组正规方程(J^T J λ I) δ -J^T e这里J是整个问题关于所有优化变量所有相机参数和三维点坐标的雅可比矩阵e是所有残差向量堆叠而成δ是我们要求解的增量更新λ是阻尼因子。这个雅可比矩阵J的规模非常庞大但它的结构具有鲜明的特点它是稀疏的。因为一个三维点通常只被少数几个相机看到一个相机也只看到一部分三维点。所以J可以按相机块和点块进行分块。对应地信息矩阵H J^T J也是一个稀疏的块矩阵。这种稀疏性是BA能够处理成千上万个相机和数百万个点的关键。然而这个信息矩阵H有一个致命的问题它是奇异的不可逆的。原因就是我们开篇提到的自由度问题。整个系统有7个不可观的自由度在相似变换下不变整体平移3个自由度整体旋转3个自由度整体尺度1个自由度这意味着如果我们对所有的相机和点同时施加一个相同的旋转、平移或缩放重投影误差不会发生任何改变。从数学上讲这导致信息矩阵H的零空间维度为7。在求解H δ -b忽略阻尼项简化表示时由于H奇异解δ要么不存在要么不唯一。直接求解会导致数值计算不稳定迭代更新会朝着无穷大的方向发散优化失败。这就是为什么我们不能直接对原始的BA问题进行优化而必须引入某种形式的约束来“钉住”这个浮动的网络消除奇异性。自由网平差就是施加约束的一种优雅方式。3. 自由网平差的约束之道消除奇异性而不引入偏差既然问题的根源是7个自由度的奇异性那么最直观的想法就是施加7个约束把这艘“船”固定住。但如何施加约束却大有讲究。错误或过强的约束会扭曲优化结果引入人为的偏差。自由网平差的核心在于施加最小约束即恰好能消除7个自由度奇异性所需的最少、最弱的约束。这些约束不应该改变问题在可观测方向上的最优解只应该消除不可观测方向上的不确定性。常用的方法有以下几种我们需要深入理解其原理和实现细节3.1 固定参数法简单粗暴但需谨慎这是最直接的方法在优化过程中固定某几个特定的参数不动。例如固定第一个相机的旋转矩阵为单位矩阵平移向量为零向量。这消除了6个自由度3旋转3平移。固定一个三维点的深度或者固定某两个三维点之间的距离为1。这消除了第7个尺度自由度。实现方式在构建雅可比矩阵J和信息矩阵H时直接将与这些固定参数对应的行和列移除。等价于在LM算法的增量方程中将这些参数的增量δ强制设为0。为什么需要谨慎选择依赖性结果依赖于你选择固定哪个相机、哪个点。如果恰好选中的那个相机姿态本身估计误差很大或者选中的那个三维点是个外点那么以这个“错误”的基准去约束整个网络会把误差传播给所有其他变量。破坏对称性这种方法人为地赋予了一个相机或点特殊的“基准”地位这在数学上并不优雅也可能在后续处理中带来不便。在实际的增量式SFM中固定初始两个相机是一种常见做法因为它简单且为重建提供了一个初始的基准坐标系。但在进行全局BA自由网平差时我们通常希望采用更“公平”、对结果影响更小的方法。3.2 伪逆法数学严谨的通用解从线性代数的角度看对于奇异系统H δ -b虽然经典逆不存在但我们可以求其摩尔-彭罗斯伪逆H⁺。伪逆给出的解δ -H⁺ b是所有可能解中范数最小的那个最小范数解。数学原理奇异值分解SVD是求解伪逆的稳定方法。对H进行SVDH U Σ V^T其中Σ是对角矩阵包含奇异值。由于H的秩亏为7那么Σ中会有7个零或接近零由于数值误差的奇异值。伪逆H⁺ V Σ⁺ U^T其中Σ⁺是将Σ中非零奇异值取倒数零奇异值保持不变或设为一个极小的阈值。这个最小范数解在数学上是良好的但它有一个潜在的缺点它给出的更新量δ可能并不是我们“期望”的方向。在BA迭代中我们更关心的是降低重投影误差而不是更新量的范数最小。不过在LM算法中由于阻尼项λI的存在即使H奇异(H λI)也可能是可逆的此时LM算法实际上隐式地给出了一个正则化解。实操注意对于大规模的BA问题对完整的H矩阵进行SVD计算代价极高通常不可行。因此伪逆法更多是一种理论指导在实际大规模优化中我们采用下面这种等价但更高效的方法。3.3 添加先验约束法软化固定更鲁棒这是一种更灵活、更鲁棒的方法可以看作是固定参数法的“软化”版本。我们不强行将某些参数固定为常数而是为它们添加一个很弱的先验约束。具体做法是在原有的BA代价函数后面增加一个惩罚项min Σ_ij || e_ij ||^2 Σ_k w_k || c_k - c_k0 ||^2其中c_k是我们要约束的参数例如第一个相机的平移向量c_k0是我们希望它靠近的值例如零向量w_k是一个很小的权重例如1e-6。为什么有效这个额外的惩罚项相当于在信息矩阵H的对角线对应位置加上了一个很小的值w_k。这直接解决了H的奇异性问题因为现在(H diag(...))是正定的。同时由于权重w_k非常小只要优化本身有明确的趋势这个弱约束几乎不会影响最终结果的方向它仅仅起到了“稳定器”的作用防止优化在不可观的方向上乱跑。如何选择约束参数通常我们选择7个参数来对应7个自由度。一个稳健的策略是约束所有相机平移的质心为零。这需要3个约束对应整体平移。具体来说添加惩罚项w * || (Σ_i t_i) / m ||^2其中m是相机数量。这比固定单个相机的平移更“公平”。约束所有相机旋转的“平均旋转”为单位矩阵。这需要3个约束对应整体旋转。实现上更复杂一些可以通过约束所有相机旋转矩阵的李代数之和为零来近似。约束所有三维点质心到原点的平均距离为1。这需要1个约束对应整体尺度。例如添加惩罚项w * || (Σ_j ||X_j||) / n - 1 ||^2。这种方法在流行的BA库如ceres-solver, g2o中很容易实现只需在构建残差块时额外添加这些先验残差块即可。它是工程实践中最常用、最推荐的方法。3.4 舒尔补消元与边缘化规模缩减与自由度的关系在大型BA中为了加速求解我们经常使用舒尔补技巧来消去三维点参数只求解相机参数。这是因为点的数量远多于相机且每个点只被少数相机看到消去点参数可以极大减小方程规模。过程如下将参数分为相机块δ_c和点块δ_p将正规方程写为分块形式[ H_cc H_cp ] [ δ_c ] [ b_c ] [ H_pc H_pp ] [ δ_p ] [ b_p ]其中H_pp是块对角矩阵很容易求逆。通过高斯消元舒尔补消去δ_p得到关于相机参数δ_c的约化方程(H_cc - H_cp * H_pp^{-1} * H_pc) δ_c b_c - H_cp * H_pp^{-1} * b_p令S H_cc - H_cp * H_pp^{-1} * H_pc这个S矩阵称为舒尔补矩阵。求解约化方程得到δ_c再回代得到δ_p。这里的关键是约化后的舒尔补矩阵S继承了原信息矩阵H的奇异性。也就是说即使消去了点参数相机参数系统仍然有7个自由度的奇异性。因此自由网平差的约束必须施加在相机参数空间或者施加在消元后系统的层面上。通常我们在构建最终的S矩阵并求解δ_c之前采用“添加先验约束法”来处理S的奇异性。注意在增量式SFM中我们经常使用“滑动窗口”BA来平衡精度和效率。当窗口滑动有旧的相机和点被边缘化出去时这个过程会将它们的约束信息以先验的形式保留在剩下的参数中。此时自由网平差的约束需要施加在当前活跃的优化窗口上同时要考虑边缘化带来的先验信息可能已经部分约束了某些自由度需要仔细分析当前系统的零空间。4. 融入增量流程何时、何地、如何执行平差理解了自由网平差的原理接下来就要把它嵌入到增量式SFM的流水线中。这不是一个一次性步骤而是一个需要精心设计触发时机和策略的持续过程。4.1 局部BA与全局BA的分工一个健壮的增量式SFM系统通常包含两种粒度的BA局部BALocal BA在注册新图像后立即执行。它只优化一个滑动窗口内的相机例如最新注册的10个相机以及它们观测到的所有三维点。窗口外的相机和点保持不变。局部BA速度快能及时纠正最新引入的误差防止误差过快累积。在局部BA中由于窗口外的参数固定它们实际上为窗口内的优化提供了一个“临时”的基准因此通常不需要显式的自由网约束或者只需要很弱的约束来防止窗口内的奇异性。全局BAGlobal BA在重建进行到一定阶段例如每注册50张图像后或者重建完成时执行。它优化所有的相机和三维点。这是计算量最大的一步也是自由网平差最主要的应用场景。只有在这里我们才面对完整的、具有7自由度奇异性的系统。4.2 增量式SFM中的平差流程设计一个结合了自由网平差的增量式SFM流程可以如下设计初始化选择两张图像计算初始相对姿态和三维点。此时固定第一个相机姿态旋转为单位阵平移为零将第二个相机和三维点作为优化变量进行一次小规模的BA。这建立了初始的浮动坐标系。图像注册循环 a.新图像姿态估计通过2D-3D匹配用EPnP等鲁棒方法估计新相机姿态。 b.三角化新点用新相机和已有相机三角化生成新的三维点。 c.几何验证对新的匹配和三维点进行外点滤除如RANSAC。 d.局部BA将新相机和受影响的点以及滑动窗口内的相机加入优化。此时窗口外的相机和点固定作为基准。如果窗口内参数开始出现奇异性迹象如信息矩阵条件数过大可为窗口内所有相机平移的质心添加一个微弱的先验约束。 e.点云与相机筛选滤除重投影误差过大的点和不稳定的相机。全局BA触发周期性触发每注册N张图像后如N100。关键帧触发当累计的旋转或平移变化超过阈值时。闭环检测触发当检测到闭环当前图像与很久以前的图像匹配成功时这是执行全局BA的最佳时机因为闭环提供了强烈的全局约束。执行全局自由网平差 a.构建全局BA问题将所有相机姿态{R_i, t_i}和三维点{X_j}设为优化变量。 b.添加最小约束采用“添加先验约束法”。 - 约束1所有相机平移的质心为零。残差 weight * (sum(t_i) / m)。 - 约束2所有相机旋转的李代数均值为零。残差 weight * (sum(log(R_i)) / m)。这里log是从旋转矩阵到李代数的映射。 - 约束3所有三维点到原点的平均距离为1。残差 weight * ((sum(||X_j||) / n) - 1)。 c.配置求解器使用LM算法设置合理的迭代次数和收敛阈值。由于是全局优化初始的阻尼因子λ可以设大一些。 d.执行优化调用后端优化库如Ceres, g2o进行迭代求解。 e.后处理检查优化后各相机和点的重投影误差是否均匀下降。如果某些误差异常高可能需要将其标记为外点并移除然后重新优化。闭环融合如果全局BA是由闭环触发的在BA之后需要将闭环边的约束正式加入到图中并可能进行一次额外的BA来吸收闭环信息。4.3 工程实现要点与避坑指南在实际编码中以下几个细节决定了自由网平差的成败约束权重的选择 权重w的选择是一门艺术。太大会扭曲结果太小则约束不足。一个经验法则是让先验残差项的量级远小于典型的重投影误差项。例如如果重投影误差在像素坐标下是几个像素即残差范数在个位数那么可以将先验权重设为1e-6到1e-8量级。最好的方式是进行敏感性测试在同一个数据集上用不同的权重跑几次BA观察相机轨迹和点云的整体形状特别是尺度是否发生显著变化。如果没有说明权重是合适的。参数化与流形 三维空间的旋转不属于欧几里得空间不能直接用3x3矩阵的9个参数去优化会破坏正交性。必须使用流形上的优化。常用的参数化有四元数 局部参数化用单位四元数表示旋转在优化时对其使用一个三维的切空间增量李代数进行更新。角轴/李代数直接用三维向量表示旋转旋转向量。 在Ceres Solver中可以使用EigenQuaternionParameterization或LocalParameterization来实现。在g2o中有专门的VertexSE3Expmap等节点类型。错误地参数化旋转是BA不收敛或结果错误的常见原因。处理大规模问题的策略 当相机和点数量达到上万甚至百万时直接求解S δ_c b可能内存不足。此时需要使用稀疏求解器如SuiteSparse的CHOLMOD或Eigen的稀疏Cholesky分解。使用迭代法如共轭梯度法CG特别是预处理共轭梯度法PCG。BA的舒尔补矩阵S通常是病态的需要好的预处理器如块对角预处理器。采用更高级的优化库如Ceres Solver它内部自动处理稀疏性、提供多种求解器选项DENSE_SCHUR, SPARSE_SCHUR, ITERATIVE_SCHUR并支持流形优化极大降低了实现难度。诊断与调试监控信息矩阵的条件数在优化前后计算S矩阵的条件数。自由网平差应该能显著降低条件数从无穷大或极大值变为一个有限值但不会改变问题本身的性质。可视化更新量在LM算法的每次迭代中观察相机平移增量δ_t和点坐标增量δ_X的范数。一个健康的优化过程这些增量范数应该随着迭代单调下降阻尼LM可能震荡下降。如果出现某个参数增量异常巨大可能是约束不足或数值不稳定。检查零空间对于最终优化后的信息矩阵S_final可以计算其最小的7个特征值。在理想情况下它们应该非常接近于零由于数值计算和弱约束会是一个很小的正值。如果它们显著大于零说明约束可能过强了。5. 从理论到实践一个简化的代码示例与结果分析为了更具体地说明我们用一个高度简化的二维模拟例子并使用Ceres Solver来演示自由网平差的实现。这个例子包含5个相机和10个三维点相机观测带有高斯噪声。#include ceres/ceres.h #include Eigen/Dense #include vector #include iostream // 1. 定义重投影误差代价函数针孔模型忽略内参和畸变简化到2D struct ReprojectionError { ReprojectionError(double observed_x, double observed_y) : observed_x(observed_x), observed_y(observed_y) {} template typename T bool operator()(const T* const camera_rotation, // 用角度表示旋转 const T* const camera_translation, // [tx, ty] const T* const point, // [x, y] T* residuals) const { // 旋转点: R * point (2D旋转) T cos_r cos(camera_rotation[0]); T sin_r sin(camera_rotation[0]); T rotated_x cos_r * point[0] - sin_r * point[1]; T rotated_y sin_r * point[0] cos_r * point[1]; // 平移并投影假设焦距1光心在(0,0) T predicted_x rotated_x camera_translation[0]; T predicted_y rotated_y camera_translation[1]; residuals[0] predicted_x - T(observed_x); residuals[1] predicted_y - T(observed_y); return true; } double observed_x, observed_y; }; // 2. 定义先验约束代价函数 // 约束1所有相机平移的质心为零 struct TranslationCenterPrior { TranslationCenterPrior(double weight) : weight(weight) {} template typename T bool operator()(const T* const translation_i, T* residual) const { // 这个残差会在所有相机上求和。为了“公平”我们约束每个相机的平移向量本身。 // 更精确的做法是构建一个约束所有平移之和的残差这里为简化约束每个平移接近零。 // 注意这实际上施加了过强的约束3m个约束但权重很小时效果类似于约束质心。 residual[0] T(weight) * translation_i[0]; residual[1] T(weight) * translation_i[1]; return true; } double weight; }; // 约束2所有相机旋转角度均值为零简化版2D下旋转只有一个角度 struct RotationMeanPrior { RotationMeanPrior(double weight) : weight(weight) {} template typename T bool operator()(const T* const rotation_i, T* residual) const { residual[0] T(weight) * rotation_i[0]; return true; } double weight; }; // 约束3所有三维点到原点的平均距离为1 struct ScalePrior { ScalePrior(double weight, int total_points) : weight(weight), inv_n(1.0/total_points) {} template typename T bool operator()(const T* const point_j, T* residual) const { // 这个残差计算该点到原点的距离并减去1。需要在所有点上求和。 // 为了在Ceres中实现对所有点的平均约束一种方法是为每个点创建一个残差块 // 计算 (||point_j|| - 1)然后通过权重来控制其对总代价的影响。 // 另一种更准确的方法是构建一个单独的残差块接收所有点计算平均距离。 // 这里采用第一种简化方法为每个点添加一个弱先验。 T norm sqrt(point_j[0]*point_j[0] point_j[1]*point_j[1]); residual[0] T(weight) * (norm - T(1.0)); return true; } double weight; double inv_n; }; int main() { // 生成模拟数据略 std::vectordouble camera_rotations(5, 0.0); std::vectordouble camera_translations(5*2, 0.0); std::vectordouble points(10*2, 0.0); std::vectorstd::pairint, int observations; // (camera_idx, point_idx) // ... 此处填充模拟数据为相机和点赋予真值并添加观测和噪声 ... // 3. 构建Ceres优化问题 ceres::Problem problem; // 添加重投影误差残差块 for (const auto obs : observations) { int cam_id obs.first; int pt_id obs.second; ceres::CostFunction* cost_function new ceres::AutoDiffCostFunctionReprojectionError, 2, 1, 2, 2( new ReprojectionError(observed_x, observed_y)); // observed_x,y需从数据中读取 problem.AddResidualBlock(cost_function, nullptr, // 使用默认的平方损失 camera_rotations[cam_id], camera_translations[cam_id*2], points[pt_id*2]); } // 4. 添加自由网平差的最小约束关键步骤 double weak_weight 1e-6; // 约束相机平移质心简化为每个相机平移添加弱先验使其靠近当前值 // 注意更好的做法是添加一个约束所有平移之和的残差块。这里为演示采用简化版。 for (int i 0; i 5; i) { ceres::CostFunction* trans_prior_cost new ceres::AutoDiffCostFunctionTranslationCenterPrior, 2, 2( new TranslationCenterPrior(weak_weight)); problem.AddResidualBlock(trans_prior_cost, nullptr, camera_translations[i*2]); } // 约束相机旋转均值简化为每个相机旋转添加弱先验 for (int i 0; i 5; i) { ceres::CostFunction* rot_prior_cost new ceres::AutoDiffCostFunctionRotationMeanPrior, 1, 1( new RotationMeanPrior(weak_weight)); problem.AddResidualBlock(rot_prior_cost, nullptr, camera_rotations[i]); } // 约束尺度为每个三维点添加弱先验使其到原点距离接近1 for (int j 0; j 10; j) { ceres::CostFunction* scale_prior_cost new ceres::AutoDiffCostFunctionScalePrior, 1, 2( new ScalePrior(weak_weight, 10)); problem.AddResidualBlock(scale_prior_cost, nullptr, points[j*2]); } // 5. 配置并运行求解器 ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_SCHUR; // 小规模问题用DENSE options.minimizer_progress_to_stdout true; options.max_num_iterations 50; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.FullReport() \n; // 6. 分析结果 // 计算并比较添加约束前后的重投影误差总和、相机轨迹的质心、点云的平均尺度 double total_reproj_error 0; // ... 计算代码 ... Eigen::Vector2d trans_center(0,0); for (int i0; i5; i) trans_center Eigen::MapEigen::Vector2d(camera_translations[i*2]); trans_center / 5.0; double avg_scale 0; for (int j0; j10; j) avg_scale Eigen::MapEigen::Vector2d(points[j*2]).norm(); avg_scale / 10.0; std::cout After optimization with free-network adjustment:\n; std::cout Total reprojection error: total_reproj_error \n; std::cout Center of camera translations: trans_center.transpose() (should be near 0)\n; std::cout Average point distance from origin: avg_scale (should be near 1)\n; return 0; }结果分析要点 在没有添加先验约束的情况下直接对带有噪声的模拟数据进行BA求解器可能会报告失败矩阵奇异或者得到一组解但这组解的整体平移、旋转和尺度是任意的。每次运行结果可能都不一样。 添加了弱先验约束后优化应该能成功收敛。你会观察到最终的重投影误差会降低到一个稳定值。所有相机平移的质心非常接近(0,0)。所有三维点到原点的平均距离非常接近1。如果你有真值数据在模拟中你知道你可以计算优化后的结构与真值结构之间的相似变换误差即先求一个最佳拟合的相似变换然后计算对齐后的误差。这个误差应该很小它衡量的是重建的内部几何精度。而整体的平移、旋转和尺度差异则被自由网平差“吸收”掉了这正是我们想要的——我们得到了一个内部一致的最优结构尽管它位于一个任意的坐标系中。这个简单的例子揭示了自由网平差的本质它不追求与某个绝对坐标系的吻合而是追求观测数据内部几何关系的最优拟合。它为后续的绝对定向如果需要提供了一个坚实、无扭曲的基础。在实际的视觉SLAM或三维重建系统中这套机制被封装在优化后端内部开发者通过配置参数来启用和调整约束但其背后的数学原理和工程考量正是我们作为从业者需要深刻理解的。