1. 從“增量”到“先驗(yàn)”為什么你的SFM優(yōu)化總差一口氣搞三維重建的朋友對增量式SFMStructure from Motion一定不陌生。從開源庫COLMAP到商業(yè)軟件RealityCapture這套“先選兩幀初始化再一幀一幀往里加”的流程幾乎成了標(biāo)準(zhǔn)操作。但不知道你有沒有遇到過這種情況模型重建到一半突然發(fā)現(xiàn)某個(gè)局部區(qū)域扭曲了或者整個(gè)場景的尺度飄忽不定甚至因?yàn)槟硯讕瑘D像質(zhì)量不佳導(dǎo)致后續(xù)的增量優(yōu)化直接崩掉前功盡棄。問題出在哪很多時(shí)候根源在于我們只關(guān)注了“增量”卻忽略了“約束”。傳統(tǒng)的增量式SFM其核心優(yōu)化目標(biāo)是最小化重投影誤差——也就是讓三維點(diǎn)投影回二維圖像上的位置與它實(shí)際被檢測到的像素位置盡可能接近。這個(gè)目標(biāo)本身沒問題但它是一個(gè)純粹的“數(shù)據(jù)驅(qū)動(dòng)”過程。當(dāng)數(shù)據(jù)圖像特征本身存在噪聲、遮擋、重復(fù)紋理或者匹配歧義時(shí)優(yōu)化器就會(huì)像無頭蒼蠅一樣在解空間里亂撞最終收斂到一個(gè)看似“誤差最小”、實(shí)則物理上不合理的局部最優(yōu)解。這時(shí)候“帶先驗(yàn)約束”的優(yōu)化就成了那根“定海神針”。這里的“先驗(yàn)”指的是我們在開始優(yōu)化之前就已經(jīng)知道或者可以合理假設(shè)的、關(guān)于場景或相機(jī)的一些知識。它不是從當(dāng)前圖像數(shù)據(jù)中“學(xué)”出來的而是我們“告訴”優(yōu)化器的額外信息。比如尺度先驗(yàn)?zāi)阒缊鼍爸心硞€(gè)物體的實(shí)際尺寸比如一張A4紙是21cm x 29.7cm。平面先驗(yàn)?zāi)阒缊鼍爸杏写笃钠矫鎱^(qū)域比如地面、墻面。運(yùn)動(dòng)先驗(yàn)?zāi)阒老鄼C(jī)是手持勻速運(yùn)動(dòng)或者被固定在平穩(wěn)的滑軌上。重力方向先驗(yàn)通過手機(jī)IMU或簡單的假設(shè)你知道重力向量的方向。把這些先驗(yàn)知識作為“軟約束”或“硬約束”加入到最小二乘優(yōu)化框架中相當(dāng)于給優(yōu)化問題增加了額外的“錨點(diǎn)”。優(yōu)化器在最小化重投影誤差的同時(shí)還必須兼顧這些先驗(yàn)條件從而將解引導(dǎo)向一個(gè)更符合物理世界規(guī)律、更穩(wěn)定的區(qū)域。所以“帶先驗(yàn)約束的增量式SFM優(yōu)化”解決的正是傳統(tǒng)增量SFM的頑疾在數(shù)據(jù)質(zhì)量不佳或場景本身存在歧義時(shí)提升重建結(jié)果的魯棒性、精度和物理合理性。它讓SFM從一個(gè)純粹依賴視覺數(shù)據(jù)的“盲人摸象”過程變成了一個(gè)能綜合利用多種信息來源的“心中有圖”的智能過程。接下來我們就深入這個(gè)最小二乘問題的核心看看這些先驗(yàn)是如何被數(shù)學(xué)化并優(yōu)雅地嵌入到我們熟悉的BABundle Adjustment框架中的。2. 數(shù)學(xué)基石最小二乘框架下的先驗(yàn)約束如何表達(dá)要理解帶先驗(yàn)的優(yōu)化必須先回到最小二乘問題的本質(zhì)。對于一個(gè)標(biāo)準(zhǔn)的視覺BA問題我們的目標(biāo)函數(shù)通常是這樣的$$ 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$ 中點(diǎn) $j$ 的觀測像素坐標(biāo)$\pi$ 是相機(jī)投影函數(shù)$\mathbf{P}_i$ 是相機(jī)位姿旋轉(zhuǎn)和平移$\mathbf{X}j$ 是三維點(diǎn)坐標(biāo)$\rho$ 是魯棒核函數(shù)如Huber$\Sigma{ij}$ 是觀測協(xié)方差。優(yōu)化就是調(diào)整所有的 $\mathbf{P}_i$ 和 $\mathbf{X}_j$讓總的重投影誤差 $E$ 最小。現(xiàn)在我們引入先驗(yàn)約束。假設(shè)我們有一個(gè)關(guān)于優(yōu)化變量 $\mathbf{\theta}$可能包含部分或全部位姿、三維點(diǎn)的先驗(yàn)知識其數(shù)學(xué)形式通常是一個(gè)約束方程或一個(gè)懲罰項(xiàng)。1. 作為懲罰項(xiàng)軟約束這是最常用、最靈活的方式。我們把先驗(yàn)知識表示為一個(gè)誤差項(xiàng) $e_{prior}$加到總的目標(biāo)函數(shù)中 $$ E_{total} E_{reproj} \lambda E_{prior} \sum \rho(|e_{reproj}|^2) \lambda \sum |e_{prior}|^2 $$ 其中 $\lambda$ 是一個(gè)權(quán)重系數(shù)用于平衡重投影誤差和先驗(yàn)約束的強(qiáng)度。$\lambda$ 越大先驗(yàn)約束越“硬”越小則越“軟”。這種方式允許約束被輕微違反對噪聲更魯棒。2. 作為等式/不等式約束硬約束在某些情況下我們可以將先驗(yàn)表示為嚴(yán)格的等式 $h(\theta) 0$ 或不等式 $g(\theta) \leq 0$。這構(gòu)成了一個(gè)帶約束的非線性最小二乘問題通常用拉格朗日乘子法或序列二次規(guī)劃SQP來求解。在增量SFM中硬約束用得相對較少因?yàn)閳D像數(shù)據(jù)本身有噪聲強(qiáng)加絕對嚴(yán)格的約束可能適得其反。關(guān)鍵點(diǎn)在于 $e_{prior}$ 的具體形式。它必須是一個(gè)關(guān)于優(yōu)化變量 $\theta$ 的可微函數(shù)這樣才能利用高斯-牛頓、列文伯格-馬夸爾特LM等算法進(jìn)行求解。下面我們看幾個(gè)典型先驗(yàn)的數(shù)學(xué)建模。2.1 尺度與絕對尺寸先驗(yàn)假設(shè)我們通過某種方式如已知尺寸的標(biāo)定物、傳感器融合知道了場景中兩個(gè)三維點(diǎn) $\mathbf{X}a$ 和 $\mathbf{X}b$ 之間的真實(shí)距離 $d{true}$。那么尺度先驗(yàn)誤差可以定義為 $$ e{scale} |\mathbf{X}a - \mathbf{X}b| - d{true} $$ 將這個(gè)誤差的平方 $|e{scale}|^2$ 作為 $E_{prior}$ 的一項(xiàng)。在優(yōu)化過程中這一項(xiàng)會(huì)“拉拽”或“推擠”點(diǎn) $\mathbf{X}_a$ 和 $\mathbf{X}b$使它們之間的距離向 $d{true}$ 靠近從而校正整個(gè)場景的尺度。實(shí)操注意這里使用的是歐氏距離其關(guān)于點(diǎn)坐標(biāo)的雅可比矩陣推導(dǎo)需要一點(diǎn)鏈?zhǔn)椒▌t但形式規(guī)整。在實(shí)現(xiàn)時(shí)通常我們不會(huì)對場景中所有點(diǎn)對都加約束而是選擇置信度最高、最穩(wěn)定的少數(shù)幾對比如地面上的兩個(gè)明顯角點(diǎn)以避免過度約束。2.2 平面先驗(yàn)假設(shè)我們通過點(diǎn)云分割或交互式標(biāo)注知道有一組點(diǎn) ${\mathbf{X}_k}$ 應(yīng)該位于同一個(gè)平面上。該平面可以用法向量 $\mathbf{n}$單位向量和到原點(diǎn)的距離 $d$ 表示為 $\mathbf{n}^T \mathbf{X} d 0$。 那么對于這組點(diǎn)中的每一個(gè)點(diǎn) $\mathbf{X}k$其到平面的距離誤差為 $$ e{plane,k} \mathbf{n}^T \mathbf{X}k d $$ 平面先驗(yàn)誤差項(xiàng)就是所有 $e{plane,k}^2$ 的和。這里有個(gè)技巧$\mathbf{n}$ 和 $d$ 本身也可以作為優(yōu)化變量如果我們對平面參數(shù)也有一個(gè)粗略的估計(jì)例如從RANSAC擬合得到我們可以將 $(\mathbf{n}, d)$ 也加入優(yōu)化狀態(tài)向量并為其添加一個(gè)溫和的先驗(yàn)讓它們不要偏離初始值太遠(yuǎn)或者固定 $\mathbf{n}$ 只優(yōu)化 $d$如果法線方向很確信比如垂直向上的地面。個(gè)人心得平面約束對于室內(nèi)場景重建是“神器”。它能有效壓制墻面、地板、天花板上的點(diǎn)云噪聲讓平面區(qū)域變得非常平整。在優(yōu)化時(shí)建議給平面約束一個(gè)較大的初始權(quán)重 $\lambda$在優(yōu)化幾輪后適當(dāng)降低讓數(shù)據(jù)重投影誤差和先驗(yàn)之間達(dá)到更好的平衡。2.3 運(yùn)動(dòng)平滑性先驗(yàn)軌跡先驗(yàn)在車載或手持連續(xù)拍攝序列中相機(jī)的運(yùn)動(dòng)在短時(shí)間內(nèi)通常是平滑的。我們可以用連續(xù)幀間的位姿變化來建模這種平滑性。例如用一個(gè)簡單的加速度懲罰 假設(shè)相機(jī)位姿用 $\mathbf{T}i [\mathbf{R}i | \mathbf{t}i]$ 表示我們可以定義相鄰幀間的“加速度”誤差。但更常見的是直接約束連續(xù)三幀的位姿變化一致性。 令 $\Delta \mathbf{T}{i,i-1} \mathbf{T}{i-1}^{-1} \mathbf{T}i$ 表示幀 $i-1$ 到 $i$ 的相對變換。運(yùn)動(dòng)平滑先驗(yàn)可以要求相鄰的相對變換盡可能相似 $$ e{motion,i} \text{Log}(\Delta \mathbf{T}{i,i-1} \cdot \Delta \mathbf{T}_{i-1,i-2}^{-1}) $$ 這里 $\text{Log}()$ 是將李群元素映射到其李代數(shù)的操作對于SO(3)就是旋轉(zhuǎn)向量對于SE(3)是6維twist。這個(gè)誤差衡量了運(yùn)動(dòng)速度的變化。將其平方和作為懲罰項(xiàng)可以抑制軌跡的突變使重建的相機(jī)路徑更平滑。注意運(yùn)動(dòng)先驗(yàn)要慎用尤其是在相機(jī)運(yùn)動(dòng)本身就有劇烈變化如快速轉(zhuǎn)向、急停的場景。通常只在高幀率視頻或確信運(yùn)動(dòng)平滑時(shí)啟用并且權(quán)重不宜過大。2.4 重力方向先驗(yàn)對于手機(jī)或帶IMU的相機(jī)我們可以獲得重力向量在相機(jī)坐標(biāo)系下的方向 $\mathbf{g}_c$。在世界坐標(biāo)系中我們通常定義重力方向?yàn)?$-Z$ 軸即 $(0,0,-1)$。那么對于第 $i$ 幀相機(jī)其旋轉(zhuǎn)矩陣 $\mathbf{R}_i$ 應(yīng)該滿足$\mathbf{R}_i \cdot (0,0,-1)^T \approx \mathbf{g}c$在歸一化尺度下。 因此重力先驗(yàn)誤差可以定義為 $$ e{gravity,i} \mathbf{R}_i \cdot \begin{bmatrix} 0 \ 0 \ -1 \end{bmatrix} - \mathbf{g}_c $$ 優(yōu)化 $\mathbf{R}_i$ 時(shí)這一項(xiàng)會(huì)迫使相機(jī)的Z軸與感知到的重力方向?qū)R。這對于糾正SFM中常見的“傾斜”或“翻滾”誤差特別有效能立刻讓場景“立”起來。實(shí)現(xiàn)細(xì)節(jié)$\mathbf{g}_c$ 通常來自IMU但需要和視覺幀進(jìn)行時(shí)間同步和坐標(biāo)系對齊標(biāo)定。即使沒有IMU如果你能人工指定圖像中兩條垂直的鉛垂線也可以計(jì)算出 $\mathbf{g}_c$。這個(gè)先驗(yàn)的權(quán)重可以設(shè)得比較高因?yàn)橹亓Ψ较蛟诖蠖鄶?shù)場景下是一個(gè)極其可靠的絕對參考。3. 增量式融合何時(shí)、何地、如何添加先驗(yàn)約束有了先驗(yàn)的數(shù)學(xué)形式下一個(gè)核心問題就是在增量式SFM的哪個(gè)環(huán)節(jié)把這些約束加進(jìn)去加給哪些變量這直接決定了算法的效率和效果。一個(gè)典型的增量式SFM流程包括特征提取與匹配、兩視圖幾何初始化、三角化新點(diǎn)、圖像注冊PnP、局部BA、全局BA可選。先驗(yàn)約束的引入主要發(fā)生在圖像注冊PnP和Bundle AdjustmentBA這兩個(gè)優(yōu)化步驟中。3.1 在圖像注冊PnP階段引入先驗(yàn)當(dāng)我們要將一張新圖像注冊到現(xiàn)有模型時(shí)需要求解該圖像的相機(jī)位姿。標(biāo)準(zhǔn)的PnP只使用2D-3D點(diǎn)對應(yīng)關(guān)系。如果此時(shí)我們有關(guān)于這張新圖像的先驗(yàn)比如它的重力方向、或者它與前一幀的運(yùn)動(dòng)平滑性約束我們可以構(gòu)建一個(gè)帶先驗(yàn)的PnP問題。目標(biāo)函數(shù)變?yōu)?$$ \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}$ 是待求的新相機(jī)位姿$\mathbf{P}{prev}$ 是上一幀的位姿。為什么要在PnP階段加提升注冊魯棒性當(dāng)新圖像與模型的特征匹配數(shù)較少少于10個(gè)或分布不佳時(shí)純視覺PnP可能失敗或產(chǎn)生較大誤差。重力先驗(yàn)提供了一個(gè)極強(qiáng)的旋轉(zhuǎn)約束運(yùn)動(dòng)先驗(yàn)提供了一個(gè)較好的初始位姿預(yù)測能極大地增加注冊成功的概率。為后續(xù)BA提供更好的初始值一個(gè)用先驗(yàn)約束“修正”過的位姿初始值比一個(gè)可能漂移的純視覺PnP結(jié)果要好得多這使得后續(xù)的BA優(yōu)化更容易收斂到全局最優(yōu)。實(shí)操策略重力先驗(yàn)只要設(shè)備提供重力矢量就在每幀PnP時(shí)加入。權(quán)重 $\lambda_1$ 可以設(shè)高因?yàn)檫@是絕對信息。運(yùn)動(dòng)先驗(yàn)在視頻序列中對于連續(xù)幀的注冊非常有用?？梢詫?$\lambda_2$ 設(shè)置為一個(gè)與幀間隔相關(guān)的函數(shù)間隔越短權(quán)重越大。尺度先驗(yàn)在PnP階段一般用不上因?yàn)镻nP求解的是相對位姿尺度信息蘊(yùn)含在三維點(diǎn)中。3.2 在局部/全局BA階段引入先驗(yàn)這是先驗(yàn)約束發(fā)揮核心作用的主戰(zhàn)場。增量SFM中每注冊若干幀后通常會(huì)對一個(gè)局部窗口內(nèi)的相機(jī)和點(diǎn)進(jìn)行BA局部BA偶爾也會(huì)進(jìn)行全局BA。此時(shí)優(yōu)化變量包括窗口內(nèi)所有相機(jī)位姿 ${\mathbf{P}_i}$ 和其觀測到的三維點(diǎn) ${\mathbf{X}_j}$。我們可以加入所有已知的先驗(yàn)約束尺度先驗(yàn)作用于選定的三維點(diǎn)對。平面先驗(yàn)作用于被標(biāo)記為屬于同一平面的三維點(diǎn)集可能還有平面參數(shù)本身。重力先驗(yàn)作用于所有提供IMU數(shù)據(jù)的相機(jī)位姿。運(yùn)動(dòng)先驗(yàn)作用于連續(xù)幀的相機(jī)位姿之間。目標(biāo)函數(shù) $$ \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}$ 表示可能的平面參數(shù)變量。增量融合的關(guān)鍵技巧動(dòng)態(tài)權(quán)重調(diào)整先驗(yàn)的權(quán)重 $\lambda$ 不是一成不變的。在BA開始時(shí)可以給予先驗(yàn)較高的權(quán)重將優(yōu)化快速拉到一個(gè)合理的區(qū)域。在迭代幾次后逐漸降低先驗(yàn)權(quán)重如每次迭代乘以0.5讓重投影誤差項(xiàng)主導(dǎo)最終的精細(xì)優(yōu)化。這類似于“模擬退火”的思想。部分變量約束不必對所有變量都加約束。例如只對最近幾幀加運(yùn)動(dòng)平滑約束只對地面點(diǎn)加平面約束。這減少了計(jì)算量也避免了過約束。先驗(yàn)的協(xié)方差更精細(xì)的做法是為每個(gè)先驗(yàn)誤差 $e_{prior}$ 賦予一個(gè)協(xié)方差矩陣 $\Sigma_{prior}$將 $|e_{prior}|^2_{\Sigma_{prior}^{-1}}$ 作為懲罰項(xiàng)。這能反映不同先驗(yàn)信息的置信度。例如IMU提供的重力方向置信度高協(xié)方差小而人工估計(jì)的平面置信度可能低一些協(xié)方差大。3.3 實(shí)現(xiàn)層面的融合架構(gòu)在代碼層面如何優(yōu)雅地將先驗(yàn)約束集成到現(xiàn)有的BA框架中如g2o、Ceres Solver以Ceres Solver為例其核心是定義“代價(jià)函數(shù)”CostFunction。對于重投影誤差我們使用AutoDiffCostFunction或SizedCostFunction。對于先驗(yàn)誤差我們需要自定義新的代價(jià)函數(shù)類。例如實(shí)現(xiàn)一個(gè)尺度先驗(yàn)代價(jià)函數(shù)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]; // 計(jì)算當(dāng)前距離 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可自動(dòng)求導(dǎo)但手動(dòng)提供有時(shí)更高效 if (jacobians ! nullptr) { if (jacobians[0] ! nullptr) { // 對點(diǎn)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) { // 對點(diǎn)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_; };然后在構(gòu)建BA問題時(shí)像添加重投影誤差塊一樣添加這個(gè)先驗(yàn)誤差塊ceres::Problem problem; // ... 添加重投影誤差塊 ... // 添加尺度先驗(yàn) ceres::CostFunction* scale_cost_function new ScalePriorCostFunction(known_distance); problem.AddResidualBlock(scale_cost_function, new ceres::HuberLoss(1.0), // 也可以加核函數(shù) point_a_ptr, // 三維點(diǎn)a的數(shù)據(jù)指針 point_b_ptr); // 三維點(diǎn)b的數(shù)據(jù)指針對于重力、平面等先驗(yàn)依葫蘆畫瓢定義對應(yīng)的代價(jià)函數(shù)即可。關(guān)鍵在于正確計(jì)算殘差和雅可比矩陣。架構(gòu)設(shè)計(jì)心得建議設(shè)計(jì)一個(gè)PriorFactor的基類或接口然后派生出ScalePriorFactor、GravityPriorFactor等。在SFM的Reconstruction類中維護(hù)一個(gè)std::vectorstd::shared_ptrPriorFactor列表。在執(zhí)行BA之前遍歷這個(gè)列表將所有的先驗(yàn)因子以正確的權(quán)重添加到優(yōu)化問題中。這樣的設(shè)計(jì)清晰、可擴(kuò)展便于開關(guān)不同的先驗(yàn)?zāi)K。4. 實(shí)戰(zhàn)一個(gè)完整的帶重力與平面先驗(yàn)的增量SFM實(shí)現(xiàn)案例理論說了這么多我們來勾勒一個(gè)具體的實(shí)現(xiàn)案例看看代碼和流程如何組織。假設(shè)我們有一個(gè)手機(jī)拍攝的室內(nèi)場景視頻手機(jī)提供了重力矢量并且我們想利用地面和墻面的平面先驗(yàn)。4.1 系統(tǒng)流程與模塊設(shè)計(jì)整個(gè)系統(tǒng)可以分為以下幾個(gè)模塊視覺前端特征提取SIFT/SuperPoint、特征匹配SuperGlue/暴力匹配、兩視圖幾何驗(yàn)證對極幾何、單應(yīng)矩陣。IMU數(shù)據(jù)處理模塊讀取手機(jī)傳感器數(shù)據(jù)進(jìn)行時(shí)間戳對齊將重力矢量從設(shè)備坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系需要事先標(biāo)定外參并平滑濾波。平面檢測模塊在初始化后的稀疏點(diǎn)云上運(yùn)行RANSAC平面檢測例如使用PCL庫。檢測出主要的平面地面、墻面并為屬于這些平面的三維點(diǎn)打上標(biāo)簽。同時(shí)記錄平面的初始參數(shù)法向量 $\mathbf{n}_0$, 距離 $d_0$。帶先驗(yàn)的增量SFM核心初始化選擇兩幀計(jì)算基礎(chǔ)矩陣三角化初始點(diǎn)云。利用IMU數(shù)據(jù)將初始坐標(biāo)系的重力方向?qū)R到世界坐標(biāo)系的-Z軸。圖像注冊對于新幀先進(jìn)行標(biāo)準(zhǔn)的特征匹配和PnP。在構(gòu)建PnP優(yōu)化問題時(shí)加入重力先驗(yàn)誤差項(xiàng)使用該幀對應(yīng)的IMU重力數(shù)據(jù)。使用Ceres求解這個(gè)帶先驗(yàn)的PnP得到更魯棒的初始位姿。三角化用新注冊的相機(jī)和已有相機(jī)三角化新的三維點(diǎn)。局部BA維護(hù)一個(gè)滑動(dòng)窗口例如最近10幀。當(dāng)窗口更新時(shí)執(zhí)行局部BA。優(yōu)化變量包括窗口內(nèi)所有相機(jī)位姿、被觀測到的所有三維點(diǎn)以及檢測到的平面參數(shù)。為所有被標(biāo)記為“地面”或“墻面”的點(diǎn)添加平面先驗(yàn)誤差項(xiàng)誤差項(xiàng)關(guān)聯(lián)該點(diǎn)和其所屬的平面參數(shù)。為所有有IMU數(shù)據(jù)的相機(jī)位姿添加重力先驗(yàn)誤差項(xiàng)。如果場景中有已知尺寸的物體如門框?yàn)槠鋬蓚€(gè)角點(diǎn)添加尺度先驗(yàn)。為連續(xù)幀的相機(jī)位姿添加輕量的運(yùn)動(dòng)平滑先驗(yàn)。全局BA可選在重建結(jié)束或關(guān)鍵幀達(dá)到一定數(shù)量后可以觸發(fā)一次全局BA此時(shí)所有先驗(yàn)依然有效。4.2 核心代碼片段詳解平面先驗(yàn)因子的實(shí)現(xiàn)簡化版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輸入?yún)?shù)維度點(diǎn)(3)平面參數(shù)(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]; // 三維點(diǎn) [x, y, z] const double* plane_ptr parameters[1]; // 平面參數(shù) [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]; // 殘差點(diǎn)到平面的有符號距離 residuals[0] n.dot(point) d; // 雅可比計(jì)算手動(dòng)推導(dǎo) if (jacobians) { if (jacobians[0]) { // 對點(diǎn)的雅可比 Eigen::MapEigen::Matrixdouble, 1, 3, Eigen::RowMajor J_point(jacobians[0]); J_point n.transpose(); // 殘差對點(diǎn)坐標(biāo)的導(dǎo)數(shù)是法向量n^T } if (jacobians[1]) { // 對平面參數(shù)的雅可比 Eigen::MapEigen::Matrixdouble, 1, 4, Eigen::RowMajor J_plane(jacobians[1]); J_plane.head3() point.transpose(); // 對法向量n的導(dǎo)數(shù)是點(diǎn)坐標(biāo)^T J_plane[3] 1.0; // 對d的導(dǎo)數(shù)是1 } } return true; } private: Eigen::Vector3d point_init_; Eigen::Vector4d plane_coeff_init_; };注意這里平面參數(shù) $\mathbf{n}$ 是一個(gè)4維向量但 $\mathbf{n}$ 理論上應(yīng)該是單位向量。直接優(yōu)化4個(gè)參數(shù)會(huì)導(dǎo)致自由度冗余。更嚴(yán)謹(jǐn)?shù)淖龇ㄊ鞘褂们蛎孀鴺?biāo)或四元數(shù)等過參數(shù)化表示來優(yōu)化3維的法向量并固定 $d$ 的尺度或者使用流形優(yōu)化Manifold Optimization。Ceres Solver提供了EigenQuaternionManifold等工具對于法向量這種單位約束可以使用SphereManifold。在局部BA中添加先驗(yàn)的代碼邏輯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. 添加重投影誤差塊 (標(biāo)準(zhǔn)操作) for (auto frame : frames) { for (auto observation : frame-observations) { // ... 構(gòu)建重投影誤差添加到problem ... } } // 2. 添加平面先驗(yàn)誤差塊 for (auto plane : planes) { for (auto point : plane-attached_points) { // 確保點(diǎn)也在本次BA的優(yōu)化變量列表中 if (std::find(points.begin(), points.end(), point) ! points.end()) { ceres::CostFunction* plane_cost new PlanePriorCostFunction(point-position, plane-coeffs); // 使用CauchyLoss降低平面約束的權(quán)重避免過約束 problem.AddResidualBlock(plane_cost, new ceres::CauchyLoss(0.5), // 核函數(shù)參數(shù)控制權(quán)重 point-position.data(), plane-coeffs.data()); } } // 可選為平面參數(shù)本身添加一個(gè)弱先驗(yàn)防止其偏離初始值太多 problem.AddParameterBlock(plane-coeffs.data(), 4); // 使用LocalParameterization約束法向量為單位長度 problem.SetManifold(plane-coeffs.data(), new ceres::SphereManifold3()); } // 3. 添加重力先驗(yàn)誤差塊 for (auto frame : frames) { if (frame-has_gravity) { ceres::CostFunction* gravity_cost new GravityPriorCostFunction(frame-gravity_vector); // 重力先驗(yàn)權(quán)重可以大一些使用較小的核函數(shù)參數(shù)或不用核函數(shù) problem.AddResidualBlock(gravity_cost, nullptr, // 不使用核函數(shù)強(qiáng)約束 frame-rotation.data()); // 只作用于旋轉(zhuǎn)部分 } } // 4. 配置并運(yùn)行求解器 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 參數(shù)調(diào)優(yōu)與效果評估權(quán)重參數(shù) $\lambda$ 的調(diào)優(yōu)重力先驗(yàn)權(quán)重應(yīng)最大。因?yàn)檫@是來自物理傳感器的絕對信息非?？煽?。在Ceres中可以通過不使用核函數(shù)nullptr或使用很小的核函數(shù)參數(shù)來實(shí)現(xiàn)強(qiáng)約束。平面先驗(yàn)權(quán)重中等。需要平衡“讓點(diǎn)落在平面上”和“滿足重投影誤差”。建議從較小的權(quán)重開始如CauchyLoss with scale1.0觀察平面區(qū)域的點(diǎn)云平整度與整體重投影誤差的變化。如果平面變得非常平整但整體誤差飆升說明權(quán)重太大如果平面點(diǎn)依然很散亂說明權(quán)重太小。運(yùn)動(dòng)先驗(yàn)權(quán)重最小。僅在視頻序列非常平滑時(shí)使用主要用于抑制高頻抖動(dòng)?？梢詮囊粋€(gè)很小的值開始如HuberLoss with scale0.1并隨著幀間隔增大而衰減。尺度先驗(yàn)權(quán)重取決于你對已知尺寸的置信度。如果尺寸測量非常精確如標(biāo)定板可以用強(qiáng)約束如果只是粗略估計(jì)如“這扇門大概2米高”則用弱約束。效果評估指標(biāo)重投影誤差最終BA后的平均重投影誤差不應(yīng)比不加先驗(yàn)時(shí)顯著增大例如增加不超過10%。如果增大了很多說明先驗(yàn)可能與視覺數(shù)據(jù)沖突權(quán)重可能過高。先驗(yàn)誤差檢查各個(gè)先驗(yàn)項(xiàng)自身的殘差。例如重力先驗(yàn)誤差旋轉(zhuǎn)后的Z軸與重力向量的夾角應(yīng)接近0平面先驗(yàn)誤差點(diǎn)到平面距離的RMS值應(yīng)遠(yuǎn)小于點(diǎn)云的平均噪聲水平。幾何一致性平面平整度計(jì)算屬于同一平面的所有點(diǎn)到擬合平面的距離標(biāo)準(zhǔn)差這個(gè)值應(yīng)明顯小于不加先驗(yàn)的情況。尺度準(zhǔn)確性測量場景中多個(gè)已知尺寸的物體計(jì)算其重建尺寸與真實(shí)尺寸的相對誤差。軌跡平滑性可視化相機(jī)軌跡觀察是否消除了不合理的突變和抖動(dòng)。主觀視覺質(zhì)量這是最重要的指標(biāo)。在MeshLab或CloudCompare中查看生成的點(diǎn)云和網(wǎng)格檢查墻面是否直立、地面是否水平、物體形狀是否合理與原始圖像進(jìn)行比對。常見陷阱與調(diào)試先驗(yàn)沖突例如重力先驗(yàn)要求場景豎直但某個(gè)平面先驗(yàn)如傾斜的屋頂定義的法向量與重力方向不垂直。這會(huì)導(dǎo)致優(yōu)化無法收斂或結(jié)果扭曲。解決方案是仔細(xì)檢查先驗(yàn)信息的正確性或者為沖突的先驗(yàn)設(shè)置更合理的權(quán)重讓更可靠的那個(gè)贏。過約束加入了太多或太強(qiáng)的先驗(yàn)導(dǎo)致優(yōu)化問題僵化重投影誤差降不下來。表現(xiàn)為優(yōu)化很快收斂但重投影誤差依然很高。需要降低先驗(yàn)權(quán)重或減少先驗(yàn)的數(shù)量。參數(shù)化問題如前述平面法向量的單位約束。如果處理不當(dāng)優(yōu)化會(huì)不穩(wěn)定。務(wù)必使用正確的流形Manifold或局部參數(shù)化LocalParameterization。初始值太差先驗(yàn)優(yōu)化依然是一個(gè)非線性優(yōu)化好的初始值至關(guān)重要。確保PnP階段提供的帶先驗(yàn)的初始位姿是合理的平面檢測的初始參數(shù)也是合理的。通過這樣一個(gè)完整的案例我們可以看到將先驗(yàn)約束融入增量式SFM并非簡單的“加法”而是一個(gè)需要仔細(xì)設(shè)計(jì)誤差項(xiàng)、權(quán)重策略、優(yōu)化架構(gòu)的系統(tǒng)工程。但當(dāng)這一切就緒后其帶來的重建質(zhì)量提升是肉眼可見的尤其對于那些“困難”場景先驗(yàn)信息往往是讓重建工作從“失敗”走向“可用”的關(guān)鍵一步。