1. 从“增量”到“先验”为什么你的SFM优化总差一口气搞三维重建的朋友对增量式SFMStructure from Motion一定不陌生。从开源库COLMAP到商业软件RealityCapture这套“先选两帧初始化再一帧一帧往里加”的流程几乎成了标准操作。但不知道你有没有遇到过这种情况模型重建到一半突然发现某个局部区域扭曲了或者整个场景的尺度飘忽不定甚至因为某几帧图像质量不佳导致后续的增量优化直接崩掉前功尽弃。问题出在哪很多时候根源在于我们只关注了“增量”却忽略了“约束”。传统的增量式SFM其核心优化目标是最小化重投影误差——也就是让三维点投影回二维图像上的位置与它实际被检测到的像素位置尽可能接近。这个目标本身没问题但它是一个纯粹的“数据驱动”过程。当数据图像特征本身存在噪声、遮挡、重复纹理或者匹配歧义时优化器就会像无头苍蝇一样在解空间里乱撞最终收敛到一个看似“误差最小”、实则物理上不合理的局部最优解。这时候“带先验约束”的优化就成了那根“定海神针”。这里的“先验”指的是我们在开始优化之前就已经知道或者可以合理假设的、关于场景或相机的一些知识。它不是从当前图像数据中“学”出来的而是我们“告诉”优化器的额外信息。比如尺度先验你知道场景中某个物体的实际尺寸比如一张A4纸是21cm x 29.7cm。平面先验你知道场景中有大片的平面区域比如地面、墙面。运动先验你知道相机是手持匀速运动或者被固定在平稳的滑轨上。重力方向先验通过手机IMU或简单的假设你知道重力向量的方向。把这些先验知识作为“软约束”或“硬约束”加入到最小二乘优化框架中相当于给优化问题增加了额外的“锚点”。优化器在最小化重投影误差的同时还必须兼顾这些先验条件从而将解引导向一个更符合物理世界规律、更稳定的区域。所以“带先验约束的增量式SFM优化”解决的正是传统增量SFM的顽疾在数据质量不佳或场景本身存在歧义时提升重建结果的鲁棒性、精度和物理合理性。它让SFM从一个纯粹依赖视觉数据的“盲人摸象”过程变成了一个能综合利用多种信息来源的“心中有图”的智能过程。接下来我们就深入这个最小二乘问题的核心看看这些先验是如何被数学化并优雅地嵌入到我们熟悉的BABundle Adjustment框架中的。2. 数学基石最小二乘框架下的先验约束如何表达要理解带先验的优化必须先回到最小二乘问题的本质。对于一个标准的视觉BA问题我们的目标函数通常是这样的$$ E \sum_{i} \sum_{j} \rho \left( | \mathbf{z}_{ij} - \pi(\mathbf{P}i, \mathbf{X}j) |^2{\Sigma{ij}^{-1}} \right) $$这里$\mathbf{z}_{ij}$ 是图像 $i$ 中点 $j$ 的观测像素坐标$\pi$ 是相机投影函数$\mathbf{P}_i$ 是相机位姿旋转和平移$\mathbf{X}j$ 是三维点坐标$\rho$ 是鲁棒核函数如Huber$\Sigma{ij}$ 是观测协方差。优化就是调整所有的 $\mathbf{P}_i$ 和 $\mathbf{X}_j$让总的重投影误差 $E$ 最小。现在我们引入先验约束。假设我们有一个关于优化变量 $\mathbf{\theta}$可能包含部分或全部位姿、三维点的先验知识其数学形式通常是一个约束方程或一个惩罚项。1. 作为惩罚项软约束这是最常用、最灵活的方式。我们把先验知识表示为一个误差项 $e_{prior}$加到总的目标函数中 $$ E_{total} E_{reproj} \lambda E_{prior} \sum \rho(|e_{reproj}|^2) \lambda \sum |e_{prior}|^2 $$ 其中 $\lambda$ 是一个权重系数用于平衡重投影误差和先验约束的强度。$\lambda$ 越大先验约束越“硬”越小则越“软”。这种方式允许约束被轻微违反对噪声更鲁棒。2. 作为等式/不等式约束硬约束在某些情况下我们可以将先验表示为严格的等式 $h(\theta) 0$ 或不等式 $g(\theta) \leq 0$。这构成了一个带约束的非线性最小二乘问题通常用拉格朗日乘子法或序列二次规划SQP来求解。在增量SFM中硬约束用得相对较少因为图像数据本身有噪声强加绝对严格的约束可能适得其反。关键点在于 $e_{prior}$ 的具体形式。它必须是一个关于优化变量 $\theta$ 的可微函数这样才能利用高斯-牛顿、列文伯格-马夸尔特LM等算法进行求解。下面我们看几个典型先验的数学建模。2.1 尺度与绝对尺寸先验假设我们通过某种方式如已知尺寸的标定物、传感器融合知道了场景中两个三维点 $\mathbf{X}a$ 和 $\mathbf{X}b$ 之间的真实距离 $d{true}$。那么尺度先验误差可以定义为 $$ e{scale} |\mathbf{X}a - \mathbf{X}b| - d{true} $$ 将这个误差的平方 $|e{scale}|^2$ 作为 $E_{prior}$ 的一项。在优化过程中这一项会“拉拽”或“推挤”点 $\mathbf{X}_a$ 和 $\mathbf{X}b$使它们之间的距离向 $d{true}$ 靠近从而校正整个场景的尺度。实操注意这里使用的是欧氏距离其关于点坐标的雅可比矩阵推导需要一点链式法则但形式规整。在实现时通常我们不会对场景中所有点对都加约束而是选择置信度最高、最稳定的少数几对比如地面上的两个明显角点以避免过度约束。2.2 平面先验假设我们通过点云分割或交互式标注知道有一组点 ${\mathbf{X}_k}$ 应该位于同一个平面上。该平面可以用法向量 $\mathbf{n}$单位向量和到原点的距离 $d$ 表示为 $\mathbf{n}^T \mathbf{X} d 0$。 那么对于这组点中的每一个点 $\mathbf{X}k$其到平面的距离误差为 $$ e{plane,k} \mathbf{n}^T \mathbf{X}k d $$ 平面先验误差项就是所有 $e{plane,k}^2$ 的和。这里有个技巧$\mathbf{n}$ 和 $d$ 本身也可以作为优化变量如果我们对平面参数也有一个粗略的估计例如从RANSAC拟合得到我们可以将 $(\mathbf{n}, d)$ 也加入优化状态向量并为其添加一个温和的先验让它们不要偏离初始值太远或者固定 $\mathbf{n}$ 只优化 $d$如果法线方向很确信比如垂直向上的地面。个人心得平面约束对于室内场景重建是“神器”。它能有效压制墙面、地板、天花板上的点云噪声让平面区域变得非常平整。在优化时建议给平面约束一个较大的初始权重 $\lambda$在优化几轮后适当降低让数据重投影误差和先验之间达到更好的平衡。2.3 运动平滑性先验轨迹先验在车载或手持连续拍摄序列中相机的运动在短时间内通常是平滑的。我们可以用连续帧间的位姿变化来建模这种平滑性。例如用一个简单的加速度惩罚 假设相机位姿用 $\mathbf{T}i [\mathbf{R}i | \mathbf{t}i]$ 表示我们可以定义相邻帧间的“加速度”误差。但更常见的是直接约束连续三帧的位姿变化一致性。 令 $\Delta \mathbf{T}{i,i-1} \mathbf{T}{i-1}^{-1} \mathbf{T}i$ 表示帧 $i-1$ 到 $i$ 的相对变换。运动平滑先验可以要求相邻的相对变换尽可能相似 $$ e{motion,i} \text{Log}(\Delta \mathbf{T}{i,i-1} \cdot \Delta \mathbf{T}_{i-1,i-2}^{-1}) $$ 这里 $\text{Log}()$ 是将李群元素映射到其李代数的操作对于SO(3)就是旋转向量对于SE(3)是6维twist。这个误差衡量了运动速度的变化。将其平方和作为惩罚项可以抑制轨迹的突变使重建的相机路径更平滑。注意运动先验要慎用尤其是在相机运动本身就有剧烈变化如快速转向、急停的场景。通常只在高帧率视频或确信运动平滑时启用并且权重不宜过大。2.4 重力方向先验对于手机或带IMU的相机我们可以获得重力向量在相机坐标系下的方向 $\mathbf{g}_c$。在世界坐标系中我们通常定义重力方向为 $-Z$ 轴即 $(0,0,-1)$。那么对于第 $i$ 帧相机其旋转矩阵 $\mathbf{R}_i$ 应该满足$\mathbf{R}_i \cdot (0,0,-1)^T \approx \mathbf{g}c$在归一化尺度下。 因此重力先验误差可以定义为 $$ e{gravity,i} \mathbf{R}_i \cdot \begin{bmatrix} 0 \ 0 \ -1 \end{bmatrix} - \mathbf{g}_c $$ 优化 $\mathbf{R}_i$ 时这一项会迫使相机的Z轴与感知到的重力方向对齐。这对于纠正SFM中常见的“倾斜”或“翻滚”误差特别有效能立刻让场景“立”起来。实现细节$\mathbf{g}_c$ 通常来自IMU但需要和视觉帧进行时间同步和坐标系对齐标定。即使没有IMU如果你能人工指定图像中两条垂直的铅垂线也可以计算出 $\mathbf{g}_c$。这个先验的权重可以设得比较高因为重力方向在大多数场景下是一个极其可靠的绝对参考。3. 增量式融合何时、何地、如何添加先验约束有了先验的数学形式下一个核心问题就是在增量式SFM的哪个环节把这些约束加进去加给哪些变量这直接决定了算法的效率和效果。一个典型的增量式SFM流程包括特征提取与匹配、两视图几何初始化、三角化新点、图像注册PnP、局部BA、全局BA可选。先验约束的引入主要发生在图像注册PnP和Bundle AdjustmentBA这两个优化步骤中。3.1 在图像注册PnP阶段引入先验当我们要将一张新图像注册到现有模型时需要求解该图像的相机位姿。标准的PnP只使用2D-3D点对应关系。如果此时我们有关于这张新图像的先验比如它的重力方向、或者它与前一帧的运动平滑性约束我们可以构建一个带先验的PnP问题。目标函数变为 $$ \min_{\mathbf{P}} \left( \sum_{j} | \mathbf{z}{j} - \pi(\mathbf{P}, \mathbf{X}j) |^2 \lambda_1 | e{gravity}(\mathbf{P}) |^2 \lambda_2 | e{motion}(\mathbf{P}, \mathbf{P}{prev}) |^2 \right) $$ 这里 $\mathbf{P}$ 是待求的新相机位姿$\mathbf{P}{prev}$ 是上一帧的位姿。为什么要在PnP阶段加提升注册鲁棒性当新图像与模型的特征匹配数较少少于10个或分布不佳时纯视觉PnP可能失败或产生较大误差。重力先验提供了一个极强的旋转约束运动先验提供了一个较好的初始位姿预测能极大地增加注册成功的概率。为后续BA提供更好的初始值一个用先验约束“修正”过的位姿初始值比一个可能漂移的纯视觉PnP结果要好得多这使得后续的BA优化更容易收敛到全局最优。实操策略重力先验只要设备提供重力矢量就在每帧PnP时加入。权重 $\lambda_1$ 可以设高因为这是绝对信息。运动先验在视频序列中对于连续帧的注册非常有用。可以将 $\lambda_2$ 设置为一个与帧间隔相关的函数间隔越短权重越大。尺度先验在PnP阶段一般用不上因为PnP求解的是相对位姿尺度信息蕴含在三维点中。3.2 在局部/全局BA阶段引入先验这是先验约束发挥核心作用的主战场。增量SFM中每注册若干帧后通常会对一个局部窗口内的相机和点进行BA局部BA偶尔也会进行全局BA。此时优化变量包括窗口内所有相机位姿 ${\mathbf{P}_i}$ 和其观测到的三维点 ${\mathbf{X}_j}$。我们可以加入所有已知的先验约束尺度先验作用于选定的三维点对。平面先验作用于被标记为属于同一平面的三维点集可能还有平面参数本身。重力先验作用于所有提供IMU数据的相机位姿。运动先验作用于连续帧的相机位姿之间。目标函数 $$ \min_{{\mathbf{P}i}, {\mathbf{X}j}, {\Pi_k}} \left( E{reproj} \lambda{scale}E_{scale} \lambda_{plane}E_{plane} \lambda_{gravity}E_{gravity} \lambda_{motion}E_{motion} \right) $$ 其中 ${\Pi_k}$ 表示可能的平面参数变量。增量融合的关键技巧动态权重调整先验的权重 $\lambda$ 不是一成不变的。在BA开始时可以给予先验较高的权重将优化快速拉到一个合理的区域。在迭代几次后逐渐降低先验权重如每次迭代乘以0.5让重投影误差项主导最终的精细优化。这类似于“模拟退火”的思想。部分变量约束不必对所有变量都加约束。例如只对最近几帧加运动平滑约束只对地面点加平面约束。这减少了计算量也避免了过约束。先验的协方差更精细的做法是为每个先验误差 $e_{prior}$ 赋予一个协方差矩阵 $\Sigma_{prior}$将 $|e_{prior}|^2_{\Sigma_{prior}^{-1}}$ 作为惩罚项。这能反映不同先验信息的置信度。例如IMU提供的重力方向置信度高协方差小而人工估计的平面置信度可能低一些协方差大。3.3 实现层面的融合架构在代码层面如何优雅地将先验约束集成到现有的BA框架中如g2o、Ceres Solver以Ceres Solver为例其核心是定义“代价函数”CostFunction。对于重投影误差我们使用AutoDiffCostFunction或SizedCostFunction。对于先验误差我们需要自定义新的代价函数类。例如实现一个尺度先验代价函数class ScalePriorCostFunction : public ceres::SizedCostFunction1, 3, 3 { public: ScalePriorCostFunction(double measured_distance) : measured_distance_(measured_distance) {} virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { const double* X_a parameters[0]; const double* X_b parameters[1]; // 计算当前距离 double dx X_b[0] - X_a[0]; double dy X_b[1] - X_a[1]; double dz X_b[2] - X_a[2]; double current_distance std::sqrt(dx*dx dy*dy dz*dz); // 残差 residuals[0] current_distance - measured_distance_; // 提供雅可比矩阵可选Ceres可自动求导但手动提供有时更高效 if (jacobians ! nullptr) { if (jacobians[0] ! nullptr) { // 对点X_a的雅可比 jacobians[0][0] -dx / current_distance; // d(res)/d(X_a_x) jacobians[0][1] -dy / current_distance; // d(res)/d(X_a_y) jacobians[0][2] -dz / current_distance; // d(res)/d(X_a_z) } if (jacobians[1] ! nullptr) { // 对点X_b的雅可比 jacobians[1][0] dx / current_distance; // d(res)/d(X_b_x) jacobians[1][1] dy / current_distance; // d(res)/d(X_b_y) jacobians[1][2] dz / current_distance; // d(res)/d(X_b_z) } } return true; } private: double measured_distance_; };然后在构建BA问题时像添加重投影误差块一样添加这个先验误差块ceres::Problem problem; // ... 添加重投影误差块 ... // 添加尺度先验 ceres::CostFunction* scale_cost_function new ScalePriorCostFunction(known_distance); problem.AddResidualBlock(scale_cost_function, new ceres::HuberLoss(1.0), // 也可以加核函数 point_a_ptr, // 三维点a的数据指针 point_b_ptr); // 三维点b的数据指针对于重力、平面等先验依葫芦画瓢定义对应的代价函数即可。关键在于正确计算残差和雅可比矩阵。架构设计心得建议设计一个PriorFactor的基类或接口然后派生出ScalePriorFactor、GravityPriorFactor等。在SFM的Reconstruction类中维护一个std::vectorstd::shared_ptrPriorFactor列表。在执行BA之前遍历这个列表将所有的先验因子以正确的权重添加到优化问题中。这样的设计清晰、可扩展便于开关不同的先验模块。4. 实战一个完整的带重力与平面先验的增量SFM实现案例理论说了这么多我们来勾勒一个具体的实现案例看看代码和流程如何组织。假设我们有一个手机拍摄的室内场景视频手机提供了重力矢量并且我们想利用地面和墙面的平面先验。4.1 系统流程与模块设计整个系统可以分为以下几个模块视觉前端特征提取SIFT/SuperPoint、特征匹配SuperGlue/暴力匹配、两视图几何验证对极几何、单应矩阵。IMU数据处理模块读取手机传感器数据进行时间戳对齐将重力矢量从设备坐标系转换到相机坐标系需要事先标定外参并平滑滤波。平面检测模块在初始化后的稀疏点云上运行RANSAC平面检测例如使用PCL库。检测出主要的平面地面、墙面并为属于这些平面的三维点打上标签。同时记录平面的初始参数法向量 $\mathbf{n}_0$, 距离 $d_0$。带先验的增量SFM核心初始化选择两帧计算基础矩阵三角化初始点云。利用IMU数据将初始坐标系的重力方向对齐到世界坐标系的-Z轴。图像注册对于新帧先进行标准的特征匹配和PnP。在构建PnP优化问题时加入重力先验误差项使用该帧对应的IMU重力数据。使用Ceres求解这个带先验的PnP得到更鲁棒的初始位姿。三角化用新注册的相机和已有相机三角化新的三维点。局部BA维护一个滑动窗口例如最近10帧。当窗口更新时执行局部BA。优化变量包括窗口内所有相机位姿、被观测到的所有三维点以及检测到的平面参数。为所有被标记为“地面”或“墙面”的点添加平面先验误差项误差项关联该点和其所属的平面参数。为所有有IMU数据的相机位姿添加重力先验误差项。如果场景中有已知尺寸的物体如门框为其两个角点添加尺度先验。为连续帧的相机位姿添加轻量的运动平滑先验。全局BA可选在重建结束或关键帧达到一定数量后可以触发一次全局BA此时所有先验依然有效。4.2 核心代码片段详解平面先验因子的实现简化版class PlanePriorCostFunction : public ceres::CostFunction { public: PlanePriorCostFunction(const Eigen::Vector3d point_initial, const Eigen::Vector4d plane_coeff_initial) // 平面方程: n·x d 0, coeff [n; d] : point_init_(point_initial), plane_coeff_init_(plane_coeff_initial) { // 声明残差维度为1输入参数维度点(3)平面参数(4) mutable_parameter_block_sizes()-push_back(3); mutable_parameter_block_sizes()-push_back(4); set_num_residuals(1); } virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { const double* point_ptr parameters[0]; // 三维点 [x, y, z] const double* plane_ptr parameters[1]; // 平面参数 [nx, ny, nz, d] Eigen::Mapconst Eigen::Vector3d point(point_ptr); Eigen::Mapconst Eigen::Vector4d plane_coeff(plane_ptr); Eigen::Vector3d n plane_coeff.head3(); double d plane_coeff[3]; // 残差点到平面的有符号距离 residuals[0] n.dot(point) d; // 雅可比计算手动推导 if (jacobians) { if (jacobians[0]) { // 对点的雅可比 Eigen::MapEigen::Matrixdouble, 1, 3, Eigen::RowMajor J_point(jacobians[0]); J_point n.transpose(); // 残差对点坐标的导数是法向量n^T } if (jacobians[1]) { // 对平面参数的雅可比 Eigen::MapEigen::Matrixdouble, 1, 4, Eigen::RowMajor J_plane(jacobians[1]); J_plane.head3() point.transpose(); // 对法向量n的导数是点坐标^T J_plane[3] 1.0; // 对d的导数是1 } } return true; } private: Eigen::Vector3d point_init_; Eigen::Vector4d plane_coeff_init_; };注意这里平面参数 $\mathbf{n}$ 是一个4维向量但 $\mathbf{n}$ 理论上应该是单位向量。直接优化4个参数会导致自由度冗余。更严谨的做法是使用球面坐标或四元数等过参数化表示来优化3维的法向量并固定 $d$ 的尺度或者使用流形优化Manifold Optimization。Ceres Solver提供了EigenQuaternionManifold等工具对于法向量这种单位约束可以使用SphereManifold。在局部BA中添加先验的代码逻辑void LocalBundleAdjustment(const std::vectorFramePtr frames, const std::vectorPoint3DPtr points, const std::vectorPlanePtr planes) { ceres::Problem problem; ceres::LossFunction* loss_function new ceres::HuberLoss(1.0); // 1. 添加重投影误差块 (标准操作) for (auto frame : frames) { for (auto observation : frame-observations) { // ... 构建重投影误差添加到problem ... } } // 2. 添加平面先验误差块 for (auto plane : planes) { for (auto point : plane-attached_points) { // 确保点也在本次BA的优化变量列表中 if (std::find(points.begin(), points.end(), point) ! points.end()) { ceres::CostFunction* plane_cost new PlanePriorCostFunction(point-position, plane-coeffs); // 使用CauchyLoss降低平面约束的权重避免过约束 problem.AddResidualBlock(plane_cost, new ceres::CauchyLoss(0.5), // 核函数参数控制权重 point-position.data(), plane-coeffs.data()); } } // 可选为平面参数本身添加一个弱先验防止其偏离初始值太多 problem.AddParameterBlock(plane-coeffs.data(), 4); // 使用LocalParameterization约束法向量为单位长度 problem.SetManifold(plane-coeffs.data(), new ceres::SphereManifold3()); } // 3. 添加重力先验误差块 for (auto frame : frames) { if (frame-has_gravity) { ceres::CostFunction* gravity_cost new GravityPriorCostFunction(frame-gravity_vector); // 重力先验权重可以大一些使用较小的核函数参数或不用核函数 problem.AddResidualBlock(gravity_cost, nullptr, // 不使用核函数强约束 frame-rotation.data()); // 只作用于旋转部分 } } // 4. 配置并运行求解器 ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_SCHUR; options.minimizer_progress_to_stdout true; options.max_num_iterations 50; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.BriefReport() \n; }4.3 参数调优与效果评估权重参数 $\lambda$ 的调优重力先验权重应最大。因为这是来自物理传感器的绝对信息非常可靠。在Ceres中可以通过不使用核函数nullptr或使用很小的核函数参数来实现强约束。平面先验权重中等。需要平衡“让点落在平面上”和“满足重投影误差”。建议从较小的权重开始如CauchyLoss with scale1.0观察平面区域的点云平整度与整体重投影误差的变化。如果平面变得非常平整但整体误差飙升说明权重太大如果平面点依然很散乱说明权重太小。运动先验权重最小。仅在视频序列非常平滑时使用主要用于抑制高频抖动。可以从一个很小的值开始如HuberLoss with scale0.1并随着帧间隔增大而衰减。尺度先验权重取决于你对已知尺寸的置信度。如果尺寸测量非常精确如标定板可以用强约束如果只是粗略估计如“这扇门大概2米高”则用弱约束。效果评估指标重投影误差最终BA后的平均重投影误差不应比不加先验时显著增大例如增加不超过10%。如果增大了很多说明先验可能与视觉数据冲突权重可能过高。先验误差检查各个先验项自身的残差。例如重力先验误差旋转后的Z轴与重力向量的夹角应接近0平面先验误差点到平面距离的RMS值应远小于点云的平均噪声水平。几何一致性平面平整度计算属于同一平面的所有点到拟合平面的距离标准差这个值应明显小于不加先验的情况。尺度准确性测量场景中多个已知尺寸的物体计算其重建尺寸与真实尺寸的相对误差。轨迹平滑性可视化相机轨迹观察是否消除了不合理的突变和抖动。主观视觉质量这是最重要的指标。在MeshLab或CloudCompare中查看生成的点云和网格检查墙面是否直立、地面是否水平、物体形状是否合理与原始图像进行比对。常见陷阱与调试先验冲突例如重力先验要求场景竖直但某个平面先验如倾斜的屋顶定义的法向量与重力方向不垂直。这会导致优化无法收敛或结果扭曲。解决方案是仔细检查先验信息的正确性或者为冲突的先验设置更合理的权重让更可靠的那个赢。过约束加入了太多或太强的先验导致优化问题僵化重投影误差降不下来。表现为优化很快收敛但重投影误差依然很高。需要降低先验权重或减少先验的数量。参数化问题如前述平面法向量的单位约束。如果处理不当优化会不稳定。务必使用正确的流形Manifold或局部参数化LocalParameterization。初始值太差先验优化依然是一个非线性优化好的初始值至关重要。确保PnP阶段提供的带先验的初始位姿是合理的平面检测的初始参数也是合理的。通过这样一个完整的案例我们可以看到将先验约束融入增量式SFM并非简单的“加法”而是一个需要仔细设计误差项、权重策略、优化架构的系统工程。但当这一切就绪后其带来的重建质量提升是肉眼可见的尤其对于那些“困难”场景先验信息往往是让重建工作从“失败”走向“可用”的关键一步。 先验约束优化：提升增量式SFM鲁棒性与精度的关键技术
1. 从“增量”到“先验”为什么你的SFM优化总差一口气搞三维重建的朋友对增量式SFMStructure from Motion一定不陌生。从开源库COLMAP到商业软件RealityCapture这套“先选两帧初始化再一帧一帧往里加”的流程几乎成了标准操作。但不知道你有没有遇到过这种情况模型重建到一半突然发现某个局部区域扭曲了或者整个场景的尺度飘忽不定甚至因为某几帧图像质量不佳导致后续的增量优化直接崩掉前功尽弃。问题出在哪很多时候根源在于我们只关注了“增量”却忽略了“约束”。传统的增量式SFM其核心优化目标是最小化重投影误差——也就是让三维点投影回二维图像上的位置与它实际被检测到的像素位置尽可能接近。这个目标本身没问题但它是一个纯粹的“数据驱动”过程。当数据图像特征本身存在噪声、遮挡、重复纹理或者匹配歧义时优化器就会像无头苍蝇一样在解空间里乱撞最终收敛到一个看似“误差最小”、实则物理上不合理的局部最优解。这时候“带先验约束”的优化就成了那根“定海神针”。这里的“先验”指的是我们在开始优化之前就已经知道或者可以合理假设的、关于场景或相机的一些知识。它不是从当前图像数据中“学”出来的而是我们“告诉”优化器的额外信息。比如尺度先验你知道场景中某个物体的实际尺寸比如一张A4纸是21cm x 29.7cm。平面先验你知道场景中有大片的平面区域比如地面、墙面。运动先验你知道相机是手持匀速运动或者被固定在平稳的滑轨上。重力方向先验通过手机IMU或简单的假设你知道重力向量的方向。把这些先验知识作为“软约束”或“硬约束”加入到最小二乘优化框架中相当于给优化问题增加了额外的“锚点”。优化器在最小化重投影误差的同时还必须兼顾这些先验条件从而将解引导向一个更符合物理世界规律、更稳定的区域。所以“带先验约束的增量式SFM优化”解决的正是传统增量SFM的顽疾在数据质量不佳或场景本身存在歧义时提升重建结果的鲁棒性、精度和物理合理性。它让SFM从一个纯粹依赖视觉数据的“盲人摸象”过程变成了一个能综合利用多种信息来源的“心中有图”的智能过程。接下来我们就深入这个最小二乘问题的核心看看这些先验是如何被数学化并优雅地嵌入到我们熟悉的BABundle Adjustment框架中的。2. 数学基石最小二乘框架下的先验约束如何表达要理解带先验的优化必须先回到最小二乘问题的本质。对于一个标准的视觉BA问题我们的目标函数通常是这样的$$ E \sum_{i} \sum_{j} \rho \left( | \mathbf{z}_{ij} - \pi(\mathbf{P}i, \mathbf{X}j) |^2{\Sigma{ij}^{-1}} \right) $$这里$\mathbf{z}_{ij}$ 是图像 $i$ 中点 $j$ 的观测像素坐标$\pi$ 是相机投影函数$\mathbf{P}_i$ 是相机位姿旋转和平移$\mathbf{X}j$ 是三维点坐标$\rho$ 是鲁棒核函数如Huber$\Sigma{ij}$ 是观测协方差。优化就是调整所有的 $\mathbf{P}_i$ 和 $\mathbf{X}_j$让总的重投影误差 $E$ 最小。现在我们引入先验约束。假设我们有一个关于优化变量 $\mathbf{\theta}$可能包含部分或全部位姿、三维点的先验知识其数学形式通常是一个约束方程或一个惩罚项。1. 作为惩罚项软约束这是最常用、最灵活的方式。我们把先验知识表示为一个误差项 $e_{prior}$加到总的目标函数中 $$ E_{total} E_{reproj} \lambda E_{prior} \sum \rho(|e_{reproj}|^2) \lambda \sum |e_{prior}|^2 $$ 其中 $\lambda$ 是一个权重系数用于平衡重投影误差和先验约束的强度。$\lambda$ 越大先验约束越“硬”越小则越“软”。这种方式允许约束被轻微违反对噪声更鲁棒。2. 作为等式/不等式约束硬约束在某些情况下我们可以将先验表示为严格的等式 $h(\theta) 0$ 或不等式 $g(\theta) \leq 0$。这构成了一个带约束的非线性最小二乘问题通常用拉格朗日乘子法或序列二次规划SQP来求解。在增量SFM中硬约束用得相对较少因为图像数据本身有噪声强加绝对严格的约束可能适得其反。关键点在于 $e_{prior}$ 的具体形式。它必须是一个关于优化变量 $\theta$ 的可微函数这样才能利用高斯-牛顿、列文伯格-马夸尔特LM等算法进行求解。下面我们看几个典型先验的数学建模。2.1 尺度与绝对尺寸先验假设我们通过某种方式如已知尺寸的标定物、传感器融合知道了场景中两个三维点 $\mathbf{X}a$ 和 $\mathbf{X}b$ 之间的真实距离 $d{true}$。那么尺度先验误差可以定义为 $$ e{scale} |\mathbf{X}a - \mathbf{X}b| - d{true} $$ 将这个误差的平方 $|e{scale}|^2$ 作为 $E_{prior}$ 的一项。在优化过程中这一项会“拉拽”或“推挤”点 $\mathbf{X}_a$ 和 $\mathbf{X}b$使它们之间的距离向 $d{true}$ 靠近从而校正整个场景的尺度。实操注意这里使用的是欧氏距离其关于点坐标的雅可比矩阵推导需要一点链式法则但形式规整。在实现时通常我们不会对场景中所有点对都加约束而是选择置信度最高、最稳定的少数几对比如地面上的两个明显角点以避免过度约束。2.2 平面先验假设我们通过点云分割或交互式标注知道有一组点 ${\mathbf{X}_k}$ 应该位于同一个平面上。该平面可以用法向量 $\mathbf{n}$单位向量和到原点的距离 $d$ 表示为 $\mathbf{n}^T \mathbf{X} d 0$。 那么对于这组点中的每一个点 $\mathbf{X}k$其到平面的距离误差为 $$ e{plane,k} \mathbf{n}^T \mathbf{X}k d $$ 平面先验误差项就是所有 $e{plane,k}^2$ 的和。这里有个技巧$\mathbf{n}$ 和 $d$ 本身也可以作为优化变量如果我们对平面参数也有一个粗略的估计例如从RANSAC拟合得到我们可以将 $(\mathbf{n}, d)$ 也加入优化状态向量并为其添加一个温和的先验让它们不要偏离初始值太远或者固定 $\mathbf{n}$ 只优化 $d$如果法线方向很确信比如垂直向上的地面。个人心得平面约束对于室内场景重建是“神器”。它能有效压制墙面、地板、天花板上的点云噪声让平面区域变得非常平整。在优化时建议给平面约束一个较大的初始权重 $\lambda$在优化几轮后适当降低让数据重投影误差和先验之间达到更好的平衡。2.3 运动平滑性先验轨迹先验在车载或手持连续拍摄序列中相机的运动在短时间内通常是平滑的。我们可以用连续帧间的位姿变化来建模这种平滑性。例如用一个简单的加速度惩罚 假设相机位姿用 $\mathbf{T}i [\mathbf{R}i | \mathbf{t}i]$ 表示我们可以定义相邻帧间的“加速度”误差。但更常见的是直接约束连续三帧的位姿变化一致性。 令 $\Delta \mathbf{T}{i,i-1} \mathbf{T}{i-1}^{-1} \mathbf{T}i$ 表示帧 $i-1$ 到 $i$ 的相对变换。运动平滑先验可以要求相邻的相对变换尽可能相似 $$ e{motion,i} \text{Log}(\Delta \mathbf{T}{i,i-1} \cdot \Delta \mathbf{T}_{i-1,i-2}^{-1}) $$ 这里 $\text{Log}()$ 是将李群元素映射到其李代数的操作对于SO(3)就是旋转向量对于SE(3)是6维twist。这个误差衡量了运动速度的变化。将其平方和作为惩罚项可以抑制轨迹的突变使重建的相机路径更平滑。注意运动先验要慎用尤其是在相机运动本身就有剧烈变化如快速转向、急停的场景。通常只在高帧率视频或确信运动平滑时启用并且权重不宜过大。2.4 重力方向先验对于手机或带IMU的相机我们可以获得重力向量在相机坐标系下的方向 $\mathbf{g}_c$。在世界坐标系中我们通常定义重力方向为 $-Z$ 轴即 $(0,0,-1)$。那么对于第 $i$ 帧相机其旋转矩阵 $\mathbf{R}_i$ 应该满足$\mathbf{R}_i \cdot (0,0,-1)^T \approx \mathbf{g}c$在归一化尺度下。 因此重力先验误差可以定义为 $$ e{gravity,i} \mathbf{R}_i \cdot \begin{bmatrix} 0 \ 0 \ -1 \end{bmatrix} - \mathbf{g}_c $$ 优化 $\mathbf{R}_i$ 时这一项会迫使相机的Z轴与感知到的重力方向对齐。这对于纠正SFM中常见的“倾斜”或“翻滚”误差特别有效能立刻让场景“立”起来。实现细节$\mathbf{g}_c$ 通常来自IMU但需要和视觉帧进行时间同步和坐标系对齐标定。即使没有IMU如果你能人工指定图像中两条垂直的铅垂线也可以计算出 $\mathbf{g}_c$。这个先验的权重可以设得比较高因为重力方向在大多数场景下是一个极其可靠的绝对参考。3. 增量式融合何时、何地、如何添加先验约束有了先验的数学形式下一个核心问题就是在增量式SFM的哪个环节把这些约束加进去加给哪些变量这直接决定了算法的效率和效果。一个典型的增量式SFM流程包括特征提取与匹配、两视图几何初始化、三角化新点、图像注册PnP、局部BA、全局BA可选。先验约束的引入主要发生在图像注册PnP和Bundle AdjustmentBA这两个优化步骤中。3.1 在图像注册PnP阶段引入先验当我们要将一张新图像注册到现有模型时需要求解该图像的相机位姿。标准的PnP只使用2D-3D点对应关系。如果此时我们有关于这张新图像的先验比如它的重力方向、或者它与前一帧的运动平滑性约束我们可以构建一个带先验的PnP问题。目标函数变为 $$ \min_{\mathbf{P}} \left( \sum_{j} | \mathbf{z}{j} - \pi(\mathbf{P}, \mathbf{X}j) |^2 \lambda_1 | e{gravity}(\mathbf{P}) |^2 \lambda_2 | e{motion}(\mathbf{P}, \mathbf{P}{prev}) |^2 \right) $$ 这里 $\mathbf{P}$ 是待求的新相机位姿$\mathbf{P}{prev}$ 是上一帧的位姿。为什么要在PnP阶段加提升注册鲁棒性当新图像与模型的特征匹配数较少少于10个或分布不佳时纯视觉PnP可能失败或产生较大误差。重力先验提供了一个极强的旋转约束运动先验提供了一个较好的初始位姿预测能极大地增加注册成功的概率。为后续BA提供更好的初始值一个用先验约束“修正”过的位姿初始值比一个可能漂移的纯视觉PnP结果要好得多这使得后续的BA优化更容易收敛到全局最优。实操策略重力先验只要设备提供重力矢量就在每帧PnP时加入。权重 $\lambda_1$ 可以设高因为这是绝对信息。运动先验在视频序列中对于连续帧的注册非常有用。可以将 $\lambda_2$ 设置为一个与帧间隔相关的函数间隔越短权重越大。尺度先验在PnP阶段一般用不上因为PnP求解的是相对位姿尺度信息蕴含在三维点中。3.2 在局部/全局BA阶段引入先验这是先验约束发挥核心作用的主战场。增量SFM中每注册若干帧后通常会对一个局部窗口内的相机和点进行BA局部BA偶尔也会进行全局BA。此时优化变量包括窗口内所有相机位姿 ${\mathbf{P}_i}$ 和其观测到的三维点 ${\mathbf{X}_j}$。我们可以加入所有已知的先验约束尺度先验作用于选定的三维点对。平面先验作用于被标记为属于同一平面的三维点集可能还有平面参数本身。重力先验作用于所有提供IMU数据的相机位姿。运动先验作用于连续帧的相机位姿之间。目标函数 $$ \min_{{\mathbf{P}i}, {\mathbf{X}j}, {\Pi_k}} \left( E{reproj} \lambda{scale}E_{scale} \lambda_{plane}E_{plane} \lambda_{gravity}E_{gravity} \lambda_{motion}E_{motion} \right) $$ 其中 ${\Pi_k}$ 表示可能的平面参数变量。增量融合的关键技巧动态权重调整先验的权重 $\lambda$ 不是一成不变的。在BA开始时可以给予先验较高的权重将优化快速拉到一个合理的区域。在迭代几次后逐渐降低先验权重如每次迭代乘以0.5让重投影误差项主导最终的精细优化。这类似于“模拟退火”的思想。部分变量约束不必对所有变量都加约束。例如只对最近几帧加运动平滑约束只对地面点加平面约束。这减少了计算量也避免了过约束。先验的协方差更精细的做法是为每个先验误差 $e_{prior}$ 赋予一个协方差矩阵 $\Sigma_{prior}$将 $|e_{prior}|^2_{\Sigma_{prior}^{-1}}$ 作为惩罚项。这能反映不同先验信息的置信度。例如IMU提供的重力方向置信度高协方差小而人工估计的平面置信度可能低一些协方差大。3.3 实现层面的融合架构在代码层面如何优雅地将先验约束集成到现有的BA框架中如g2o、Ceres Solver以Ceres Solver为例其核心是定义“代价函数”CostFunction。对于重投影误差我们使用AutoDiffCostFunction或SizedCostFunction。对于先验误差我们需要自定义新的代价函数类。例如实现一个尺度先验代价函数class ScalePriorCostFunction : public ceres::SizedCostFunction1, 3, 3 { public: ScalePriorCostFunction(double measured_distance) : measured_distance_(measured_distance) {} virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { const double* X_a parameters[0]; const double* X_b parameters[1]; // 计算当前距离 double dx X_b[0] - X_a[0]; double dy X_b[1] - X_a[1]; double dz X_b[2] - X_a[2]; double current_distance std::sqrt(dx*dx dy*dy dz*dz); // 残差 residuals[0] current_distance - measured_distance_; // 提供雅可比矩阵可选Ceres可自动求导但手动提供有时更高效 if (jacobians ! nullptr) { if (jacobians[0] ! nullptr) { // 对点X_a的雅可比 jacobians[0][0] -dx / current_distance; // d(res)/d(X_a_x) jacobians[0][1] -dy / current_distance; // d(res)/d(X_a_y) jacobians[0][2] -dz / current_distance; // d(res)/d(X_a_z) } if (jacobians[1] ! nullptr) { // 对点X_b的雅可比 jacobians[1][0] dx / current_distance; // d(res)/d(X_b_x) jacobians[1][1] dy / current_distance; // d(res)/d(X_b_y) jacobians[1][2] dz / current_distance; // d(res)/d(X_b_z) } } return true; } private: double measured_distance_; };然后在构建BA问题时像添加重投影误差块一样添加这个先验误差块ceres::Problem problem; // ... 添加重投影误差块 ... // 添加尺度先验 ceres::CostFunction* scale_cost_function new ScalePriorCostFunction(known_distance); problem.AddResidualBlock(scale_cost_function, new ceres::HuberLoss(1.0), // 也可以加核函数 point_a_ptr, // 三维点a的数据指针 point_b_ptr); // 三维点b的数据指针对于重力、平面等先验依葫芦画瓢定义对应的代价函数即可。关键在于正确计算残差和雅可比矩阵。架构设计心得建议设计一个PriorFactor的基类或接口然后派生出ScalePriorFactor、GravityPriorFactor等。在SFM的Reconstruction类中维护一个std::vectorstd::shared_ptrPriorFactor列表。在执行BA之前遍历这个列表将所有的先验因子以正确的权重添加到优化问题中。这样的设计清晰、可扩展便于开关不同的先验模块。4. 实战一个完整的带重力与平面先验的增量SFM实现案例理论说了这么多我们来勾勒一个具体的实现案例看看代码和流程如何组织。假设我们有一个手机拍摄的室内场景视频手机提供了重力矢量并且我们想利用地面和墙面的平面先验。4.1 系统流程与模块设计整个系统可以分为以下几个模块视觉前端特征提取SIFT/SuperPoint、特征匹配SuperGlue/暴力匹配、两视图几何验证对极几何、单应矩阵。IMU数据处理模块读取手机传感器数据进行时间戳对齐将重力矢量从设备坐标系转换到相机坐标系需要事先标定外参并平滑滤波。平面检测模块在初始化后的稀疏点云上运行RANSAC平面检测例如使用PCL库。检测出主要的平面地面、墙面并为属于这些平面的三维点打上标签。同时记录平面的初始参数法向量 $\mathbf{n}_0$, 距离 $d_0$。带先验的增量SFM核心初始化选择两帧计算基础矩阵三角化初始点云。利用IMU数据将初始坐标系的重力方向对齐到世界坐标系的-Z轴。图像注册对于新帧先进行标准的特征匹配和PnP。在构建PnP优化问题时加入重力先验误差项使用该帧对应的IMU重力数据。使用Ceres求解这个带先验的PnP得到更鲁棒的初始位姿。三角化用新注册的相机和已有相机三角化新的三维点。局部BA维护一个滑动窗口例如最近10帧。当窗口更新时执行局部BA。优化变量包括窗口内所有相机位姿、被观测到的所有三维点以及检测到的平面参数。为所有被标记为“地面”或“墙面”的点添加平面先验误差项误差项关联该点和其所属的平面参数。为所有有IMU数据的相机位姿添加重力先验误差项。如果场景中有已知尺寸的物体如门框为其两个角点添加尺度先验。为连续帧的相机位姿添加轻量的运动平滑先验。全局BA可选在重建结束或关键帧达到一定数量后可以触发一次全局BA此时所有先验依然有效。4.2 核心代码片段详解平面先验因子的实现简化版class PlanePriorCostFunction : public ceres::CostFunction { public: PlanePriorCostFunction(const Eigen::Vector3d point_initial, const Eigen::Vector4d plane_coeff_initial) // 平面方程: n·x d 0, coeff [n; d] : point_init_(point_initial), plane_coeff_init_(plane_coeff_initial) { // 声明残差维度为1输入参数维度点(3)平面参数(4) mutable_parameter_block_sizes()-push_back(3); mutable_parameter_block_sizes()-push_back(4); set_num_residuals(1); } virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { const double* point_ptr parameters[0]; // 三维点 [x, y, z] const double* plane_ptr parameters[1]; // 平面参数 [nx, ny, nz, d] Eigen::Mapconst Eigen::Vector3d point(point_ptr); Eigen::Mapconst Eigen::Vector4d plane_coeff(plane_ptr); Eigen::Vector3d n plane_coeff.head3(); double d plane_coeff[3]; // 残差点到平面的有符号距离 residuals[0] n.dot(point) d; // 雅可比计算手动推导 if (jacobians) { if (jacobians[0]) { // 对点的雅可比 Eigen::MapEigen::Matrixdouble, 1, 3, Eigen::RowMajor J_point(jacobians[0]); J_point n.transpose(); // 残差对点坐标的导数是法向量n^T } if (jacobians[1]) { // 对平面参数的雅可比 Eigen::MapEigen::Matrixdouble, 1, 4, Eigen::RowMajor J_plane(jacobians[1]); J_plane.head3() point.transpose(); // 对法向量n的导数是点坐标^T J_plane[3] 1.0; // 对d的导数是1 } } return true; } private: Eigen::Vector3d point_init_; Eigen::Vector4d plane_coeff_init_; };注意这里平面参数 $\mathbf{n}$ 是一个4维向量但 $\mathbf{n}$ 理论上应该是单位向量。直接优化4个参数会导致自由度冗余。更严谨的做法是使用球面坐标或四元数等过参数化表示来优化3维的法向量并固定 $d$ 的尺度或者使用流形优化Manifold Optimization。Ceres Solver提供了EigenQuaternionManifold等工具对于法向量这种单位约束可以使用SphereManifold。在局部BA中添加先验的代码逻辑void LocalBundleAdjustment(const std::vectorFramePtr frames, const std::vectorPoint3DPtr points, const std::vectorPlanePtr planes) { ceres::Problem problem; ceres::LossFunction* loss_function new ceres::HuberLoss(1.0); // 1. 添加重投影误差块 (标准操作) for (auto frame : frames) { for (auto observation : frame-observations) { // ... 构建重投影误差添加到problem ... } } // 2. 添加平面先验误差块 for (auto plane : planes) { for (auto point : plane-attached_points) { // 确保点也在本次BA的优化变量列表中 if (std::find(points.begin(), points.end(), point) ! points.end()) { ceres::CostFunction* plane_cost new PlanePriorCostFunction(point-position, plane-coeffs); // 使用CauchyLoss降低平面约束的权重避免过约束 problem.AddResidualBlock(plane_cost, new ceres::CauchyLoss(0.5), // 核函数参数控制权重 point-position.data(), plane-coeffs.data()); } } // 可选为平面参数本身添加一个弱先验防止其偏离初始值太多 problem.AddParameterBlock(plane-coeffs.data(), 4); // 使用LocalParameterization约束法向量为单位长度 problem.SetManifold(plane-coeffs.data(), new ceres::SphereManifold3()); } // 3. 添加重力先验误差块 for (auto frame : frames) { if (frame-has_gravity) { ceres::CostFunction* gravity_cost new GravityPriorCostFunction(frame-gravity_vector); // 重力先验权重可以大一些使用较小的核函数参数或不用核函数 problem.AddResidualBlock(gravity_cost, nullptr, // 不使用核函数强约束 frame-rotation.data()); // 只作用于旋转部分 } } // 4. 配置并运行求解器 ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_SCHUR; options.minimizer_progress_to_stdout true; options.max_num_iterations 50; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.BriefReport() \n; }4.3 参数调优与效果评估权重参数 $\lambda$ 的调优重力先验权重应最大。因为这是来自物理传感器的绝对信息非常可靠。在Ceres中可以通过不使用核函数nullptr或使用很小的核函数参数来实现强约束。平面先验权重中等。需要平衡“让点落在平面上”和“满足重投影误差”。建议从较小的权重开始如CauchyLoss with scale1.0观察平面区域的点云平整度与整体重投影误差的变化。如果平面变得非常平整但整体误差飙升说明权重太大如果平面点依然很散乱说明权重太小。运动先验权重最小。仅在视频序列非常平滑时使用主要用于抑制高频抖动。可以从一个很小的值开始如HuberLoss with scale0.1并随着帧间隔增大而衰减。尺度先验权重取决于你对已知尺寸的置信度。如果尺寸测量非常精确如标定板可以用强约束如果只是粗略估计如“这扇门大概2米高”则用弱约束。效果评估指标重投影误差最终BA后的平均重投影误差不应比不加先验时显著增大例如增加不超过10%。如果增大了很多说明先验可能与视觉数据冲突权重可能过高。先验误差检查各个先验项自身的残差。例如重力先验误差旋转后的Z轴与重力向量的夹角应接近0平面先验误差点到平面距离的RMS值应远小于点云的平均噪声水平。几何一致性平面平整度计算属于同一平面的所有点到拟合平面的距离标准差这个值应明显小于不加先验的情况。尺度准确性测量场景中多个已知尺寸的物体计算其重建尺寸与真实尺寸的相对误差。轨迹平滑性可视化相机轨迹观察是否消除了不合理的突变和抖动。主观视觉质量这是最重要的指标。在MeshLab或CloudCompare中查看生成的点云和网格检查墙面是否直立、地面是否水平、物体形状是否合理与原始图像进行比对。常见陷阱与调试先验冲突例如重力先验要求场景竖直但某个平面先验如倾斜的屋顶定义的法向量与重力方向不垂直。这会导致优化无法收敛或结果扭曲。解决方案是仔细检查先验信息的正确性或者为冲突的先验设置更合理的权重让更可靠的那个赢。过约束加入了太多或太强的先验导致优化问题僵化重投影误差降不下来。表现为优化很快收敛但重投影误差依然很高。需要降低先验权重或减少先验的数量。参数化问题如前述平面法向量的单位约束。如果处理不当优化会不稳定。务必使用正确的流形Manifold或局部参数化LocalParameterization。初始值太差先验优化依然是一个非线性优化好的初始值至关重要。确保PnP阶段提供的带先验的初始位姿是合理的平面检测的初始参数也是合理的。通过这样一个完整的案例我们可以看到将先验约束融入增量式SFM并非简单的“加法”而是一个需要仔细设计误差项、权重策略、优化架构的系统工程。但当这一切就绪后其带来的重建质量提升是肉眼可见的尤其对于那些“困难”场景先验信息往往是让重建工作从“失败”走向“可用”的关键一步。