做視覺(jué)SLAM的各位應(yīng)該都有過(guò)這種體驗(yàn)明明前端跟蹤沒(méi)毛病后端優(yōu)化看起來(lái)也對(duì)但地圖點(diǎn)就是飄、姿態(tài)就是抖。行內(nèi)人都知道問(wèn)題大概率出在滑窗里的SFM環(huán)節(jié)——相機(jī)位姿沒(méi)鎖準(zhǔn)3D點(diǎn)自然白扯。這篇接著系列往下聊專門(mén)說(shuō)說(shuō)Vins-Fusion處理滑窗內(nèi)SFM的關(guān)鍵動(dòng)作從兩兩相對(duì)位姿估計(jì)到三角化再到位姿和3D點(diǎn)的聯(lián)合優(yōu)化同時(shí)把我調(diào)試過(guò)程中踩過(guò)的坑一并曬出來(lái)。這套東西適合誰(shuí)看正準(zhǔn)備接手VINS系代碼、被初始化弄得頭大的人或者已經(jīng)在跑自己數(shù)據(jù)但是效果不穩(wěn)、想搞明白“到底該查前端還是后端”的人。我把原理邏輯和實(shí)操經(jīng)驗(yàn)揉在一起講盡量讓沒(méi)有系統(tǒng)學(xué)過(guò)視覺(jué)幾何的讀者也能跟上。1. SFM在Vins-Fusion滑窗里到底扮演什么角色1.1 滑窗不是靜態(tài)快照而是動(dòng)態(tài)測(cè)量系統(tǒng)很多新手看Vins-Fusion的代碼第一反應(yīng)是找“建圖模塊”結(jié)果翻半天發(fā)現(xiàn)根本沒(méi)什么顯式的重建流程只有一堆位姿、狀態(tài)量、殘差。實(shí)際上整條pipeline的核心就是“邊估位姿邊估點(diǎn)”只不過(guò)它把這個(gè)問(wèn)題限制在一個(gè)很小的滑窗里反復(fù)做?；耙话憔S持10到13幀關(guān)鍵幀不同版本和配置略有差異窗口每往前滑一步就丟掉最老的幀引入新幀。這個(gè)窗口本質(zhì)上是一個(gè)動(dòng)態(tài)的局部測(cè)量系統(tǒng)新關(guān)鍵幀帶來(lái)新的觀測(cè)老關(guān)鍵幀要被邊緣化中間的所有幀和特征點(diǎn)需要持續(xù)更新。SFM就是在這個(gè)窗口里完成的“視覺(jué)結(jié)構(gòu)恢復(fù)”——它既不是離線跑一次就完事也不是純前端的跟蹤而是整個(gè)狀態(tài)估計(jì)器的初始化器和校正器。我習(xí)慣打一個(gè)比方全局BA是修一棟大樓滑窗SFM是每次只修你所在的那一層但是每修一層都會(huì)把承重墻澆好等修到頂樓時(shí)整棟樓不會(huì)塌。這個(gè)“每一層的承重墻”就是滑窗里被反復(fù)驗(yàn)證過(guò)的相機(jī)位姿和3D點(diǎn)。1.2 為什么必須用滑窗而不是全局BA理論上SFM最“干凈”的做法是把所有幀和所有點(diǎn)一次性拿來(lái)做光束平差得到全局最優(yōu)解。但現(xiàn)實(shí)里這根本跑不動(dòng)——移動(dòng)機(jī)器人連續(xù)跑一個(gè)小時(shí)可能積累上萬(wàn)幀、幾十萬(wàn)個(gè)特征點(diǎn)全局BA的計(jì)算量爆炸。更麻煩的是環(huán)境是會(huì)變的光照會(huì)變、動(dòng)態(tài)物體會(huì)進(jìn)出視野你辛辛苦苦把所有歷史幀鎖在一起一旦某段軌跡漂了后期花多大力氣也拉不回來(lái)。所以Vins-Fusion采用滑窗策略本質(zhì)上是在精度和實(shí)時(shí)性之間找一個(gè)工程上可接受的平衡點(diǎn)。滑窗的尺寸決定了局部結(jié)構(gòu)的記憶長(zhǎng)度窗口太小位姿之間約束太少視覺(jué)退化和尺度漂移很容易爆發(fā)窗口太大后端優(yōu)化的實(shí)時(shí)性就保不住尤其還要和IMU做緊耦合的時(shí)候。我實(shí)測(cè)下來(lái)10到13幀是一個(gè)很穩(wěn)定的區(qū)間既能讓視差足夠覆蓋同一個(gè)特征點(diǎn)的多次觀測(cè)又不至于讓后端優(yōu)化單次迭代耗時(shí)超過(guò)幾十毫秒。另一個(gè)經(jīng)常被忽略的理由是“不確定性管理”。全局BA總假設(shè)所有誤差是高斯、獨(dú)立的但實(shí)際跑數(shù)據(jù)時(shí)早期幀的誤差結(jié)構(gòu)早就不準(zhǔn)了硬拉進(jìn)去參與優(yōu)化反而會(huì)污染后面的估計(jì)?；暗囊饬x在于老幀的占用信息被壓縮成先驗(yàn)項(xiàng)而不是讓原始觀測(cè)“陰魂不散”地一直干擾新估計(jì)。1.3 數(shù)據(jù)流動(dòng)從圖像到位姿再到3D點(diǎn)的管線在滑窗SFM里數(shù)據(jù)流向是嚴(yán)格分層的。第一步前端拿到圖像序列通過(guò)特征提取和光流跟蹤得到每一幀上特征點(diǎn)的2D像素坐標(biāo)并且把不同幀之間同一物理點(diǎn)的對(duì)應(yīng)關(guān)系維護(hù)起來(lái)。第二步利用這些2D對(duì)應(yīng)關(guān)系估計(jì)相鄰關(guān)鍵幀之間的相對(duì)位姿。第三步有了位姿用多視圖三角化恢復(fù)特征點(diǎn)的3D坐標(biāo)。第四步把相機(jī)位姿和3D特征點(diǎn)都放進(jìn)同一個(gè)優(yōu)化目標(biāo)里做局部BA讓兩者互相糾正。注意第四步不是說(shuō)“第三步做完再優(yōu)化一次”而是在滑窗內(nèi)反復(fù)進(jìn)行的。我把SFM和BA理解成一個(gè)連續(xù)交互的過(guò)程SFM提供初始幾何BA把它打磨結(jié)實(shí)BA打磨完的位姿又會(huì)讓下一輪SFM三角化更準(zhǔn)確。這個(gè)循環(huán)才叫“聯(lián)合估計(jì)”很多剛起步的人把SFM當(dāng)作離線預(yù)處理概念上就跑偏了。2. 相機(jī)位姿估計(jì)從匹配點(diǎn)到相對(duì)運(yùn)動(dòng)2.1 特征跟蹤產(chǎn)出的“測(cè)量值”長(zhǎng)什么樣進(jìn)入位姿估計(jì)前得先搞清楚輸入到底是什么。Vins-Fusion前端的特征點(diǎn)庫(kù)里每個(gè)特征點(diǎn)記錄了一串觀測(cè)在哪些幀出現(xiàn)、每次出現(xiàn)時(shí)的像素坐標(biāo)、歸一化坐標(biāo)、特征描述子或者光流跟蹤時(shí)的patch信息。我們要用的是歸一化坐標(biāo)也就是把像素坐標(biāo)經(jīng)過(guò)內(nèi)參變換到相機(jī)坐標(biāo)系下的一個(gè)方向向量。為什么用歸一化坐標(biāo)因?yàn)楸举|(zhì)矩陣和單應(yīng)矩陣模型都建立在對(duì)極約束的幾何關(guān)系上本身不關(guān)心內(nèi)參提前歸一化可以讓問(wèn)題更干凈。實(shí)際代碼里這一步通常是先將像素坐標(biāo)減掉主點(diǎn)再除以焦距得到camera frame下的坐標(biāo)即x(u-cx)/fx。這里有個(gè)常見(jiàn)坑如果你手里的內(nèi)參是標(biāo)定好的歸一化只按原圖尺寸做一次即可。如果用了圖像縮放必須把內(nèi)參同步縮放否則后面所有的閾值判斷都會(huì)失效。跟蹤質(zhì)量直接決定位姿估計(jì)的上限。我通常要求一個(gè)關(guān)鍵幀被至少跟蹤到15到20個(gè)特征點(diǎn)光流成功且通過(guò)雙向一致性檢查少于這個(gè)數(shù)量我會(huì)直接拒絕把該幀設(shè)為關(guān)鍵幀繼續(xù)等待新幀積累視差。這看起來(lái)保守實(shí)際上能省掉大量RANSAC拉鋸時(shí)間。2.2 本質(zhì)矩陣與單應(yīng)矩陣的選擇邏輯兩幀之間的相對(duì)位姿估計(jì)核心問(wèn)題是從2D對(duì)應(yīng)關(guān)系中恢復(fù)R和t。根據(jù)場(chǎng)景不同同一個(gè)問(wèn)題可以用兩種模型本質(zhì)矩陣E適合大部分普通三維場(chǎng)景自由度為5最少5個(gè)點(diǎn)對(duì)可解即五點(diǎn)法。E t^× R分解后直接得到相對(duì)旋轉(zhuǎn)和平移方向。單應(yīng)矩陣H當(dāng)場(chǎng)景近似平面或者相機(jī)發(fā)生純旋轉(zhuǎn)時(shí)本質(zhì)矩陣模型會(huì)退化此時(shí)用H二維單應(yīng)更穩(wěn)定。H的自由度為8最少4個(gè)點(diǎn)對(duì)。VINs-Fusion的初始化模塊里并不會(huì)徹底放棄任何一個(gè)模型而是把H和E都估出來(lái)用統(tǒng)一的評(píng)分機(jī)制比較RANSAC內(nèi)點(diǎn)數(shù)和重投影殘差誰(shuí)更優(yōu)就選誰(shuí)。這個(gè)“雙模型競(jìng)爭(zhēng)”的做法我認(rèn)為極其重要。如果只跑E模型你在走廊墻面前跑直線特征點(diǎn)幾乎都落在一個(gè)平面上本質(zhì)矩陣會(huì)非常病態(tài)初始化常常炸掉如果只跑H模型遇到真正的非平面場(chǎng)景單應(yīng)矩陣擬合出的模型同樣不靠譜。實(shí)操中的經(jīng)驗(yàn)是模型選擇不要只看內(nèi)點(diǎn)數(shù)還要看殘差的分布。如果H模型內(nèi)點(diǎn)殘差普遍很大而E模型內(nèi)點(diǎn)殘差小而集中哪怕H內(nèi)點(diǎn)數(shù)更高也優(yōu)先選擇E。因?yàn)槠矫鎴?chǎng)景下H的內(nèi)點(diǎn)可能很多但那些內(nèi)點(diǎn)的幾何約束本身就很弱用于位姿解算反而會(huì)把噪聲放大。2.3 RANSAC內(nèi)點(diǎn)閾值到底怎么給RANSAC閾值的選擇經(jīng)常被隨便填實(shí)際上它直接關(guān)系到位姿的精度和魯棒性。閾值過(guò)小真實(shí)匹配都被當(dāng)成外點(diǎn)丟棄內(nèi)點(diǎn)率難看閾值過(guò)大大量錯(cuò)匹配混進(jìn)來(lái)E矩陣被帶偏。在歸一化平面坐標(biāo)的情況下RANSAC的距離度量通常是對(duì)極線距離。我把閾值設(shè)在0.002到0.01之間歸一化單位對(duì)應(yīng)像素大約0.2到0.8個(gè)像素在640x480的圖像上歸一化1.0大約對(duì)應(yīng)幾百像素所以這個(gè)區(qū)間很敏感。具體值需要根據(jù)你圖像噪聲和標(biāo)定精度來(lái)定。平時(shí)跑車載數(shù)據(jù)圖像噪聲大一點(diǎn)我會(huì)放到0.01如果是實(shí)驗(yàn)室高分辨率靜物場(chǎng)景0.002就能跑得很好。還有一點(diǎn)容易被忽略RANSAC的迭代次數(shù)和置信度要配合。一般設(shè)置信度0.99初始內(nèi)外點(diǎn)比例未知時(shí)迭代次數(shù)從幾十到幾百動(dòng)態(tài)決定。我見(jiàn)過(guò)有人為了“省時(shí)間”把最大迭代次數(shù)鎖死在50結(jié)果在內(nèi)點(diǎn)率低的場(chǎng)景里解出來(lái)的模型完全是噪聲擬合。RANSAC本身不貴貴的是你對(duì)它過(guò)度信任。2.4 分解出R和t之后不能直接用于建圖從E分解出R和t是個(gè)數(shù)學(xué)上很優(yōu)雅、工程上很麻煩的過(guò)程。E矩陣本質(zhì)上是3x3矩陣但它必須滿足兩個(gè)奇異值相等、第三個(gè)為0的約束。實(shí)際計(jì)算出來(lái)的E由于噪聲不會(huì)完美滿足這個(gè)結(jié)構(gòu)所以通常先做一次SVD然后把奇異值強(qiáng)制設(shè)為(1, 1, 0)附近再重新合成。分解時(shí)R存在兩個(gè)候選解加上t的方向正負(fù)一共4種組合。正確的組合需要靠三角化后的“正深度檢驗(yàn)”來(lái)挑選把特征點(diǎn)三角化看它是否同時(shí)在兩個(gè)相機(jī)的正前方深度大于零一致的那個(gè)組合才被認(rèn)為是正確相對(duì)位姿。這一步是失敗高發(fā)區(qū)。我見(jiàn)過(guò)很多次初始化崩潰不是因?yàn)镋分解算錯(cuò)了而是因?yàn)樯疃葯z驗(yàn)只做了單幀檢查或者只檢查了一小部分點(diǎn)。正確做法是對(duì)所有被模型覆蓋的內(nèi)點(diǎn)做三角化統(tǒng)計(jì)深度大于零的比例同時(shí)檢查某幾個(gè)點(diǎn)的重投影誤差綜合投票。只檢查少數(shù)幾個(gè)點(diǎn)一旦這幾點(diǎn)恰好是外點(diǎn)你就選了一個(gè)完全錯(cuò)誤的位姿組合后面整個(gè)滑窗都救不回來(lái)。3. 特征點(diǎn)3D位置估計(jì)三角化的工程實(shí)現(xiàn)3.1 三角化本質(zhì)上是解超定方程有了相機(jī)位姿下一步是把特征點(diǎn)從2D映射成3D。這個(gè)動(dòng)作看起來(lái)像是“兩束射線求交點(diǎn)”但實(shí)際上因?yàn)橛^測(cè)噪聲兩條射線在空間里根本不交而是錯(cuò)開(kāi)的。所以三角化本質(zhì)是在解一個(gè)最小二乘問(wèn)題找一個(gè)3D點(diǎn)讓它在所有觀測(cè)相機(jī)上的重投影誤差盡可能小。工程上最通用的是線性三角化DLT一類的方法每個(gè)觀測(cè)都提供兩個(gè)方程x坐標(biāo)和y坐標(biāo)各一個(gè)把X的三個(gè)未知數(shù)或齊次形式的四個(gè)未知數(shù)代入最后得到一個(gè)超定線性方程組AX 0。解法是SVD分解取最小奇異值對(duì)應(yīng)的右奇異向量歸一化后得到3D坐標(biāo)。這里我要特別提醒一個(gè)理論之外的問(wèn)題輸入點(diǎn)位必須先做歸一化。直接在像素坐標(biāo)上做DLT數(shù)值條件會(huì)很差誤差大的離譜。正確的做法是先在每幅圖像上做坐標(biāo)歸一化這不同于前面提到的相機(jī)內(nèi)參歸一化將所有2D坐標(biāo)縮放和中心化解完再把結(jié)果變換回世界坐標(biāo)。這一步在很多簡(jiǎn)化的教學(xué)代碼里被省掉我教你一個(gè)判斷方法如果三角化出的點(diǎn)深度經(jīng)常是負(fù)的或者重投影誤差不穩(wěn)定優(yōu)先檢查有沒(méi)有做歸一化。3.2 微小視差會(huì)讓解病態(tài)三角化的精度對(duì)“視差角”極其敏感。視差角就是相機(jī)在觀測(cè)同一個(gè)點(diǎn)時(shí)兩條光線的夾角。夾角太小意味著兩幀相機(jī)位置幾乎沒(méi)變化3D點(diǎn)的深度就從噪聲中徹底不可觀了。我在實(shí)際數(shù)據(jù)里驗(yàn)證過(guò)對(duì)一個(gè)距離10米的點(diǎn)如果基線只有0.1米視差角不到0.6度三角化出的深度方差會(huì)大到一個(gè)離譜的程度——看起來(lái)重投影誤差很小但3D點(diǎn)位置完全不可信。所以滑窗SFM里一定要加視差檢驗(yàn)要么算兩個(gè)相機(jī)光心到特征點(diǎn)的光線夾角要么更直觀地在圖像上數(shù)特征點(diǎn)位移像素?cái)?shù)。VINS這類系統(tǒng)一般會(huì)判斷“足夠的視差才把當(dāng)前幀設(shè)為關(guān)鍵幀”目的就是為了保證后續(xù)三角化和優(yōu)化的幾何條件不至于病態(tài)。我在自己的參數(shù)表里設(shè)了一個(gè)最小視差角閾值大約0.5度到1度具體取決于特征點(diǎn)噪聲水平。噪聲越大閾值越要往高調(diào)。如果你發(fā)現(xiàn)三角化出的點(diǎn)跟著噪聲抖動(dòng)先把視差閾值往上拉比你去調(diào)RANSAC閾值更管用。另一個(gè)經(jīng)驗(yàn)是不要在只有2幀觀測(cè)時(shí)就把3D點(diǎn)鎖死。滑窗里一個(gè)特征點(diǎn)至少要被3到5幀連續(xù)觀測(cè)到才值得把它納入優(yōu)化狀態(tài)。觀測(cè)次數(shù)太少三角化的不確定度沒(méi)有充分約束加入BA只會(huì)增加計(jì)算量不會(huì)帶來(lái)精度收益。3.3 給3D點(diǎn)做體檢重投影、深度、共線三角化完畢不能直接“上戶口”。我習(xí)慣對(duì)每個(gè)3D點(diǎn)做三項(xiàng)體檢至少兩項(xiàng)通過(guò)才留用重投影誤差把3D點(diǎn)重新投影到每一幀圖像上計(jì)算與原始觀測(cè)的像素距離。一般要求小于2像素。這個(gè)指標(biāo)很直觀但只能反映“是否與觀測(cè)一致”不能說(shuō)明“是否真的準(zhǔn)”。深度正負(fù)檢查點(diǎn)在各個(gè)相機(jī)坐標(biāo)系下的深度是否都大于零。這個(gè)必須嚴(yán)格通過(guò)允許少數(shù)異常點(diǎn)被剔除。共線檢查檢查3D點(diǎn)到相機(jī)光心的連線方向與2D觀測(cè)方向的一致性尤其在多幀觀測(cè)時(shí)如果觀測(cè)之間的方向變化很亂說(shuō)明跟蹤大概率已經(jīng)漂了。還有一個(gè)工程細(xì)節(jié)當(dāng)一個(gè)3D點(diǎn)在后續(xù)滑窗中不斷被新觀測(cè)更新時(shí)我建議定期用滑窗內(nèi)所有觀測(cè)重新做一次帶約束的三角化而不是只保留第一次三角化結(jié)果。因?yàn)槲蛔嗽贐A中會(huì)更新初始三角化的點(diǎn)不再嚴(yán)格滿足新位姿下的幾何關(guān)系定期“換一次底”能顯著減少累計(jì)誤差。4. 滑窗內(nèi)聯(lián)合優(yōu)化SFM和BA的銜接4.1 怎么把兩兩位姿拼成一致軌跡SFM輸出的本質(zhì)是相對(duì)位姿和3D點(diǎn)但要真正發(fā)揮價(jià)值必須把散落的相對(duì)位姿統(tǒng)一到滑窗的一個(gè)坐標(biāo)系下。這個(gè)統(tǒng)一過(guò)程通常以滑窗里較老的一幀為參考系通過(guò)位姿乘法把所有幀的絕對(duì)位姿算出來(lái)。第一次初始化時(shí)可能用第一幀作為世界坐標(biāo)系原點(diǎn)后續(xù)滑窗滑動(dòng)時(shí)需要保證新老坐標(biāo)系之間的一致性。這一步的坑在于你從E分解出的t是單位尺度它沒(méi)有任何物理尺度信息。在純視覺(jué)滑窗里尺度本來(lái)就是自由漂移的。Vins-Fusion之所以引入IMU預(yù)積分就是要把重力方向和尺度拉回真實(shí)的物理量綱。如果你的場(chǎng)景里只有純視覺(jué)沒(méi)有IMU那你必須接受sfm結(jié)果在無(wú)尺度空間內(nèi)更新不能直接拿去當(dāng)真實(shí)軌跡。拼接軌跡時(shí)我強(qiáng)烈建議用李群SE(3)上做插值和平均而不是把旋轉(zhuǎn)矩陣拆成歐拉角來(lái)融合。歐拉角在接近90度時(shí)會(huì)奇異性爆表哪怕你數(shù)據(jù)里根本轉(zhuǎn)不到那個(gè)角度數(shù)值上也會(huì)出現(xiàn)奇怪的跳動(dòng)。代碼里直接用四元數(shù)或旋轉(zhuǎn)矩陣的log/exp映射至少能保證軌跡的連續(xù)性。4.2 滑窗BA為什么要走稀疏求解到位姿和3D點(diǎn)都有了初值接下來(lái)就是局部BA的活。BA的數(shù)學(xué)模型并不復(fù)雜最小化所有殘差的平方和其中殘差是3D點(diǎn)在某一幀的重投影誤差。復(fù)雜的是怎么快速求解。假設(shè)滑窗里有11幀、200個(gè)特征點(diǎn)那么未知量就是11個(gè)位姿每個(gè)6自由度加尺度信息和200個(gè)3D點(diǎn)每個(gè)3自由度。這個(gè)規(guī)?？粗淮蟮苯訉?duì)Cartesian坐標(biāo)求導(dǎo)并填充Hessian矩陣矩陣會(huì)非常龐大且稠密。幸好BA的Jacobian天然帶有稀疏結(jié)構(gòu)某個(gè)殘差只連接一個(gè)3D點(diǎn)和一幀位姿于是整個(gè)Hessian矩陣呈塊狀稀疏布局絕大部分元素是零。利用這個(gè)稀疏性標(biāo)準(zhǔn)做法是先對(duì)特征點(diǎn)塊做Schur消元也就是先在數(shù)學(xué)上把3D點(diǎn)的量“消”掉只剩下位姿相關(guān)的量最后組成一個(gè)維度小很多的位姿增量線性系統(tǒng)。我見(jiàn)過(guò)有人為了省事直接調(diào)用通用非線性優(yōu)化庫(kù)卻不開(kāi)稀疏求解器結(jié)果每輪迭代耗時(shí)幾十毫秒滑窗變成幻燈片。正確姿勢(shì)一定是用精求解器開(kāi)啟SCHUR_STRATEGY和稀疏線性代數(shù)把邊緣化掉的路標(biāo)點(diǎn)塊提前消元速度可以提升一個(gè)數(shù)量級(jí)。這里再提一個(gè)能提升穩(wěn)定性的技巧在BA迭代前把圖像坐標(biāo)投影誤差乘以一個(gè)huber核函數(shù)魯棒核從而抑制大誤差項(xiàng)對(duì)解的牽引。Vins-Fusion的殘差實(shí)現(xiàn)里也引入了這類魯棒化處理我的經(jīng)驗(yàn)是誤匹配率在1%3%時(shí)加了huber核能明顯改善優(yōu)化收斂性。如果你發(fā)現(xiàn)BA結(jié)果在一次誤匹配下就大跳八成是核函數(shù)沒(méi)加或者閾值設(shè)置太寬松。4.3 邊緣化把老幀的貢獻(xiàn)折算成先驗(yàn)滑窗每前進(jìn)一步最老的關(guān)鍵幀就要被移出窗口。直接把它丟掉會(huì)損失掉它與其他幀之間的約束信息系統(tǒng)會(huì)在某一刻“忘掉”過(guò)去導(dǎo)致漂移。所以這里要用邊緣化Marginalization把被移除幀的所有約束信息壓縮成一個(gè)先驗(yàn)概率項(xiàng)附加到后續(xù)的優(yōu)化問(wèn)題中。邊緣化的數(shù)學(xué)工具是Schur補(bǔ)。把要?jiǎng)h除的變量從信息矩陣中消掉剩下的信息矩陣加上測(cè)量貢獻(xiàn)就形成新的先驗(yàn)。我聽(tīng)得懂但真正寫(xiě)代碼時(shí)比較容易忘記的是邊緣化的前提是線性化點(diǎn)固定。一旦先驗(yàn)項(xiàng)在你當(dāng)前的雅可比線性化點(diǎn)生成之后不要再用新雅可比反復(fù)線性化它否則信息會(huì)反復(fù)放大導(dǎo)致所謂的“信息矩陣重復(fù)注入”系統(tǒng)會(huì)假性樂(lè)觀——協(xié)方差變小、誤差卻不減。這個(gè)坑我在調(diào)參時(shí)撞過(guò)一次因?yàn)樵谶吘壔鬀](méi)有凍結(jié)線性化點(diǎn)程序的RMSE看似越來(lái)越小但實(shí)際軌跡和真值越拉越遠(yuǎn)。排查了半天最后發(fā)現(xiàn)是先驗(yàn)項(xiàng)被重復(fù)線性化了幾次信息矩陣?yán)锏闹祽{空大了好幾倍。修復(fù)之后系統(tǒng)才恢復(fù)正常。所以你看邊緣化這一環(huán)節(jié)正確性比代碼性能更重要。5. 常見(jiàn)問(wèn)題與排查記錄5.1 初始化總是失敗的常見(jiàn)原因如果你發(fā)現(xiàn)Vins-Fusion在第一次初始化時(shí)反復(fù)失敗先別懷疑調(diào)參多數(shù)是數(shù)據(jù)本身不夠格。我看過(guò)很多數(shù)據(jù)集特征是夠多但相機(jī)幾乎在原地轉(zhuǎn)圈純旋轉(zhuǎn)基礎(chǔ)上平移量很小這時(shí)候本質(zhì)矩陣模型退化。你可以打印出H和E模型各自的內(nèi)點(diǎn)數(shù)來(lái)確認(rèn)如果H模型內(nèi)點(diǎn)數(shù)遠(yuǎn)超E模型且場(chǎng)景看起來(lái)有大塊平面那基本就是低視差或旋轉(zhuǎn)退化問(wèn)題。解決方法是等相機(jī)平移量積累起來(lái)再觸發(fā)初始化。你可以手動(dòng)控制機(jī)器人在初始化階段做一個(gè)帶明顯平移的八字繞行而不是原地轉(zhuǎn)圈。另外初始化時(shí)不要用模糊幀運(yùn)動(dòng)模糊會(huì)讓光流跟蹤的像素位置整體偏移RANSAC再怎么聰明也救不回系統(tǒng)性偏差。有個(gè)更隱蔽的原因內(nèi)參沒(méi)對(duì)準(zhǔn)輸出圖像的尺寸。我遇到過(guò)一次圖像被resize到一半分辨率標(biāo)定內(nèi)參卻沒(méi)跟著縮放出新主點(diǎn)和焦距初始化階段特征點(diǎn)歸一化之后全部偏了結(jié)果H/E模型內(nèi)點(diǎn)數(shù)慘不忍睹。這個(gè)問(wèn)題只靠肉眼看不出但我建議你在代碼里打印幾個(gè)點(diǎn)的歸一化坐標(biāo)跟真實(shí)物理方向?qū)σ粚?duì)能快速定位是不是內(nèi)參坐標(biāo)系搞錯(cuò)了。5.2 3D點(diǎn)在兩次三角化后跳變同一個(gè)特征點(diǎn)前幾幀三角化出來(lái)的3D坐標(biāo)在一個(gè)位置滑窗滾過(guò)幾幀之后再做一次三角化3D點(diǎn)位置突然跳了幾米。這是非常典型的“三角化時(shí)視差角不夠”擲出來(lái)的病態(tài)解。我第一次遇到時(shí)以為是前端跟蹤崩了但其實(shí)前端很穩(wěn)問(wèn)題就是當(dāng)幀的基線和深度比太低3D點(diǎn)深度方向上的不確定性高達(dá)數(shù)米重投影誤差卻很小看起來(lái)很正常。排查方法是把“該特征點(diǎn)所有觀測(cè)幀”標(biāo)出來(lái)逐幀打印出三角化時(shí)的視差角。視差角小于閾值時(shí)哪怕重投影誤差再小也不要更新這個(gè)3D點(diǎn)。另一個(gè)經(jīng)驗(yàn)是使用多幀觀測(cè)做三角化比只用兩幀要穩(wěn)得多。多出來(lái)的幀可以讓最小二乘殘差在深度方向上形成更好的約束。如果你只有一個(gè)特征庫(kù)但不記得它被多少幀觀測(cè)過(guò)可以統(tǒng)計(jì)一下“每個(gè)點(diǎn)在滑窗內(nèi)的觀測(cè)總數(shù)”低于3幀的一律不用它做定位。5.3 先驗(yàn)一致性破壞后的漂移特征邊緣化先驗(yàn)出問(wèn)題的時(shí)候系統(tǒng)的癥狀很有迷惑性姿態(tài)看起來(lái)很平滑但軌跡會(huì)隨時(shí)間緩慢往一個(gè)方向偏越拉越遠(yuǎn)。這種漂移和前端特征跟蹤錯(cuò)誤的“突變性抖動(dòng)”不一樣是慢性病。你檢查IMU看看沒(méi)異常檢查3D點(diǎn)重投影誤差也不大但全世界點(diǎn)云和軌跡就像被一股無(wú)形的力緩慢拖拽。大概率出在先驗(yàn)項(xiàng)的線性化點(diǎn)被重復(fù)更新或者先驗(yàn)項(xiàng)殘差的雅可比計(jì)算中使用的新位姿和生成先驗(yàn)時(shí)不一致。我自己的排錯(cuò)流程是把滑窗邊緣化前后的信息矩陣導(dǎo)出看對(duì)角元素是否異常增大如果增大超過(guò)一個(gè)數(shù)量級(jí)就基本坐實(shí)了重復(fù)注入。再檢查先驗(yàn)殘差模塊中“固定線性化點(diǎn)”那個(gè)開(kāi)關(guān)有沒(méi)有在代碼里丟失。這種問(wèn)題在純視覺(jué)定位場(chǎng)景里更明顯因?yàn)镮MU還能提供一部分參考多少能掩蓋先驗(yàn)錯(cuò)誤。一旦你的部署環(huán)境里IMU噪聲很大視覺(jué)邊緣化先驗(yàn)的問(wèn)題就會(huì)被放大你就更難懷疑到先驗(yàn)頭上了。5.4 參數(shù)調(diào)優(yōu)的心得控制不住的掉幀和漂移往往不是某一個(gè)參數(shù)的問(wèn)題而是參數(shù)之間不匹配。我的經(jīng)驗(yàn)是按下圖這個(gè)順序來(lái)調(diào)不要跳著調(diào)先確認(rèn)滑窗大小1013不被拍腦袋改再確認(rèn)關(guān)鍵幀的最小視差閾值0.5度到1度只做微調(diào)接著看RANSAC閾值和最大迭代次數(shù)歸一化閾值0.0020.01最后才動(dòng)特征點(diǎn)跟蹤窗口尺寸和金字塔層數(shù)。亂調(diào)參數(shù)的后果是你換了閾值A(chǔ)B的問(wèn)題被暴露你換B又讓A的問(wèn)題回來(lái)陷入死循環(huán)。另外一個(gè)心得是寫(xiě)一個(gè)小的“診斷日志”功能每隔幾幀輸出滑窗內(nèi)平均重投影誤差、內(nèi)點(diǎn)數(shù)、最老幀到當(dāng)前幀的平移量。調(diào)試時(shí)開(kāi)著它跑一小段就能快速判斷系統(tǒng)狀態(tài)。我每接手一個(gè)新場(chǎng)景數(shù)據(jù)第一件事就是開(kāi)著診斷日志走一遍看看初始化和滑窗內(nèi)的殘差變化趨勢(shì)比盯著一堆軌跡圖發(fā)呆有效太多。滑窗SFM這部分做到位最大的感受就是后端的BA優(yōu)化變“聰明”了它不再會(huì)因?yàn)橐淮握`匹配而翻車也不會(huì)因?yàn)閹讕牡鸵暡疃a(chǎn)出飄移點(diǎn)。前期的特征跟蹤、位姿估計(jì)、三角化這些看似基礎(chǔ)的操作直接決定了全系統(tǒng)最終能跑多穩(wěn)這不是公式推導(dǎo)能帶給你的經(jīng)驗(yàn)而是反復(fù)調(diào)數(shù)據(jù)積累出來(lái)的肌肉記憶。我個(gè)人到現(xiàn)在還保留著一個(gè)習(xí)慣每次改動(dòng)三角化模塊的參數(shù)我都會(huì)用一組帶真值的仿真數(shù)據(jù)做回歸測(cè)試看看深度誤差隨視差角變化的曲線。這能最快暴露你把閾值調(diào)過(guò)頭了還是調(diào)不到位了。以后哪位朋友要是被困在“初始化失敗”“點(diǎn)云漂移”“軌跡慢性漂”這三個(gè)坑里希望這篇文章能幫你繞開(kāi)大部分雷區(qū)。