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并非簡單的“加法”而是一個需要仔細設計誤差項、權重策略、優化架構的系統工程。但當這一切就緒后其帶來的重建質量提升是肉眼可見的尤其對于那些“困難”場景先驗信息往往是讓重建工作從“失敗”走向“可用”的關鍵一步。