VSLAM與VIO的3D建圖，重定位與世界觀綜述

本文重點描述VSLAM與VIO的3D建圖，重定位，回環(huán)與世界觀，從小伙伴們最關(guān)心的工程和商用搞錢的角度進(jìn)行詳細(xì)分析，并從技術(shù)和實現(xiàn)部分詳細(xì)描述各種類型SLAM在這塊的差異。

首先來4個基礎(chǔ)邏輯：

• SLAM本質(zhì)是數(shù)學(xué)問題，是一個科學(xué)家與工程師可以控制的數(shù)學(xué)問題，本質(zhì)不是玄學(xué)，實現(xiàn)需要大量的數(shù)學(xué)知識與工具，需要極強的代碼功底與硬軟件開發(fā)能力。

• 無論對SLAM系統(tǒng)如何分割，建圖仍是位姿估計的副產(chǎn)品。

• 當(dāng)下SLAM主流落地就兩類：二維三維導(dǎo)航建圖與具體操作(如抓取)

• 無論任何SLAM系統(tǒng)，精度無論多高，本質(zhì)都是求解非線性優(yōu)化的最優(yōu)解，勢必存在誤差，且隨時長與探索距離遞增。

三相性：開銷/魯棒性/精度已經(jīng)反復(fù)提過了，這個部分我們也實現(xiàn)了，本文的重點在于描述VIO/VSLAM的世界觀。

01

 主流SLAM相關(guān)工作分類 

首先，稀疏點云不是世界觀，即使稀疏點云有完整而正確的深度，也是難以被直觀理解的機器語言，這塊在學(xué)界和業(yè)界都得到了反復(fù)的驗證，目前先對所有SLAM系統(tǒng)做一個基本的分類，(二維單點激光SLAM與矢量重定位不放在里面，是原點和古早的系統(tǒng)類別)：

(1) 以VINS,ORB3為代表的間接法(或特征點法)構(gòu)建的系統(tǒng)，或LSD-SLAM與SD-VIS這一類并不直接的直接法

如上圖所示，從非常直觀的感受就能感覺到，這種稀疏的點云，即使構(gòu)建了完整和正確的深度，也是不容易被直觀理解的機器語言，相對的商業(yè)價值貌似較低。其實這種認(rèn)知是不完善的，后續(xù)會進(jìn)行詳述。

(2)線掃/固態(tài)激光SLAM

線掃或者固態(tài)激光生成的點云相對稠密，距離和范圍較大，而且可以建立世界觀相對本端的誤差較小的深度，也是最容易理解的，廣泛應(yīng)用于汽車與自動駕駛行業(yè)，還有一定的缺陷是沒有更加稠密和豐富的紋理(類似真實世界)。

這一類激光SLAM大多數(shù)由多種(4-5種)傳感器的融合卡爾曼濾波構(gòu)成：IEKF或MSCKF，然后對點云進(jìn)行如ICP類型的暴力匹配，目前并行化的工作也做得相當(dāng)好了，從業(yè)人員很多。問題在于當(dāng)下最主流的L4級自動駕駛本身是一個非常卷的領(lǐng)域，并慢慢開始走向了2個路徑：1是本端基礎(chǔ)感知+AI目標(biāo)分類結(jié)合HD高精地圖(HD地圖目前也是一個很卷的東西(類似以前網(wǎng)吧接AI公司的單做標(biāo)注)，2是具有極強感知能力的本地多傳感器融合融態(tài)結(jié)合簡單的地圖，但是路徑2雖然厲害但是當(dāng)前無論是芯片各種U上的算力還是算法本身，都離真正的商用有距離(同時路徑1和2都有非常麻煩的法律法規(guī)問題)。接下來幾年這塊正在走向L2變?yōu)橹鳈C廠的系統(tǒng)支撐方，也許只有等到未來的某一天某篇真正劃時代的論文之后，才能迎來真正的大批量落地。

(3)緊耦合同步優(yōu)化直接法-DSO系，注意此處的緊耦合并不意指不同傳感器間的緊耦合，任何信息，包括同類傳感器間的信息同樣是可以被耦合的，整個處理流和pipeline也是可以被緊耦合的。

效果如上圖所示

這塊學(xué)術(shù)上帶頭的是我很喜歡和尊敬的Daniel Cremer教授(TUM)，他的思維是用純視覺完成整個世界觀的構(gòu)建，甚至在未來大面積的去替換雷達(dá)。

這條路徑的基礎(chǔ)特點是用視覺構(gòu)建了半稀疏的世界觀，而且隨著算力的提升，點云的密度可以進(jìn)一步的增加以形成實際的半稠密世界觀，是一條非常棒的路徑。但是這條路徑問題也非常多，最核心的一點是三相性中的開銷度，其次是視覺世界觀相當(dāng)于雷達(dá)的深度信息仍有一些明顯的缺陷，再次是和IMU等運動類傳感器緊耦合難以實施，后續(xù)進(jìn)行詳述。

02

相關(guān)工作與評估 (不懂可跳)：  

(1) 三相性VIO系統(tǒng)研究與試驗發(fā)展：

2021年至2022年中， 紫川組主要針對(1)完成了三相性相關(guān)工作，實現(xiàn)了viobot完整的軟硬件閉環(huán)，實現(xiàn)了獨立編解碼與OSD疊點，開發(fā)了獨立的UI與三維空間規(guī)劃底，通過CPU+GPU+DSP完成了整個系統(tǒng)前端/后端與回環(huán)的并行化(其實回環(huán)并沒有真正并行化只是做了SIMD)，完成了完整SDK，并針對在各種真實環(huán)境中的極端情況設(shè)計了對應(yīng)的ZUPT策略，輸出了VIO/VINS工程問題定位的思路與流程。以上各項工作在前文中都已經(jīng)詳述。最近一些收尾工作是雙目初始化的升級和環(huán)點SDK。2022年中至2023年6月重點完成VIO系統(tǒng)的世界觀，主要難點在數(shù)據(jù)結(jié)構(gòu)，每天不是整WARP原語就是各種亂七八糟的CELL和樹結(jié)構(gòu)。在之前這一系列工作中，首先感謝港科VINS組，尤其是基于四元數(shù)的預(yù)積分的編寫工作，后續(xù)大家的一些工作可以用GTSAM，但是用這個去處理預(yù)積分其實在工程側(cè)是不友好的，還是重了。

針對(1)，目前其實可用性是很高的，這個和大家對于稀疏點云的通常直觀理解還是會有一些差異。相關(guān)工作先重點描述這個：詞袋看起來貌似是一門玄學(xué)，其實它存儲了優(yōu)化之后在空間中的相機位置與姿態(tài)，以及它在當(dāng)時所對應(yīng)的點的x,y坐標(biāo)與深度信息(但是數(shù)量很少很稀疏比如200個，多了算不過來)。最終你存的那張圖其實是沒啥用的，形成的數(shù)據(jù)結(jié)構(gòu)是一整個用二進(jìn)制表述的關(guān)鍵字和描述符，同時詞袋字典是用很基本的k-d樹形式訓(xùn)練與存儲，所以它其實一點都不玄學(xué)，而是一門真正的顯學(xué)。在機器語言中系統(tǒng)是很容易完成這個工作流的，同時對準(zhǔn)確率及召回率來說，所有的VSLAM系統(tǒng)和VIO系統(tǒng)在商用時一定要有更高的準(zhǔn)確率！這塊照做即可。

首先你把這玩意當(dāng)成三維空間，紅色代表地面真實GT（也不要糾結(jié)哪來的，你當(dāng)我外感信標(biāo)或者RTK大概標(biāo)的），藍(lán)色的各種不確定小橢球你就可以當(dāng)成詞袋回環(huán)每一次定位的范圍了。綠色的線代表重復(fù)作業(yè)軌跡，在這個二次和多次機器人作業(yè)的過程中，所復(fù)用的位置姿態(tài)，即藍(lán)色不確定橢球給予的引導(dǎo)。同時這是一個標(biāo)準(zhǔn)3D-2D的PNP過程，進(jìn)入回算之后給出的位姿也是相對世界坐標(biāo)系原點的位姿。

缺點是雖然詞袋重定位及回環(huán)是一門顯學(xué)，但是畢竟它是一個二進(jìn)制的綜合描述，也來源于二維圖像中的特征點與描述子(如ORB)，它給人整體的感覺肯定是遠(yuǎn)遠(yuǎn)不如三維點云的，而且對作業(yè)重定位觀測方向也會有要求。也不像真正的二維重定位如Catagrapher這類系統(tǒng)中開發(fā)的二維邊界矢量重定位清晰。但是實際它是好用的，即使環(huán)境中出現(xiàn)了一些小的移動目標(biāo)物，也不會對整個詞袋的特性造成太大的破壞，仍然可以友好的重定位與回環(huán)。但當(dāng)然當(dāng)所取用的環(huán)點中如果有30-40%以上都改變了，肯定也是難以回環(huán)的(這種難以回環(huán)和重定位反而是正確的結(jié)果)

回到摘要中的基礎(chǔ)邏輯4，任何SLAM的優(yōu)化，勢必產(chǎn)生誤差，VIO只是提供了更好的魯棒性也是目前世界上性價比最好的傳感器組合(前提是解決三相性，否則算力平臺成本驚人)，而這種誤差，必須通過重定位手段解決，才能更新更準(zhǔn)確的位姿和世界觀(建圖)

實現(xiàn)的效果就是類似TANGO的這個簡單清晰的小視頻了【Visual-inertial teach and repeat powered by Google Tango】

 https://www.bilibili.com/video/BV1tP4y1d7AX/share_source=copy_web&vd_source=3cba44a8cb771560d536cc3085033dfe，但是要注意，無論如何，(1)這種系統(tǒng)的世界觀都是相當(dāng)糟糕的！

TANGO完成位姿計算并記錄工作點(重定位環(huán)點)

稀疏的MAPPING

UAV完成作業(yè)及重定位拍攝。

其實(1)還有一個很大的問題，雖然它是一套很好的導(dǎo)航與重定位系統(tǒng)，但是它仍然難以用來實現(xiàn)SLAM基礎(chǔ)邏輯3中的具體操作行為，比如抓取一個水杯，打開一扇門等，因為首先它的世界觀是稀疏的。當(dāng)然到這里有同學(xué)會問了，那我再加上一個面陣i-tof相機呢？其實我想說這個也是難以實現(xiàn)的，因為目前最好用的VIO(如我們組的系統(tǒng)，哈哈)其實誤差大約也是在0.5-0.8%，而很多具體操作要求的精度是cm級別的。實現(xiàn)這種耦合的最好設(shè)計是：先使用VIO導(dǎo)航定位到附近，如數(shù)十cm內(nèi)，再啟動另外一套工作邏輯(如AI的類識別結(jié)合i-TOF相機)結(jié)合機械臂完成接下來的工作。目前導(dǎo)航+操作類的大量行為還是更多的依賴外感信標(biāo)。

(2) 激光雷達(dá)SLAM系統(tǒng)簡述：

激光雷達(dá)最大的優(yōu)點就是有完全可控的深度，用非常弱智的方式算回來，線束越多，建圖越清晰。

真的是簡述，幾個問題，首先雷達(dá)太貴了。

其次雷達(dá)在非常近距離的具體操作實施上一樣是有問題的，誤差并不小，特別近和特別遠(yuǎn)都有問題。

最后一個問題是先算位姿再進(jìn)行ICP等暴力匹配的方式太浪費開銷，無論怎么優(yōu)化數(shù)據(jù)結(jié)構(gòu)，八叉樹/K-D/BBST，結(jié)果都類似，除非降幀降頻或者大量降低最后模型的精度(比如八叉樹最后搞成一坨坨的)，那這不就走回頭路了嗎。。。當(dāng)然現(xiàn)在也有很多更好的方式提高雷達(dá)的工作效率和開銷比，但是很多都需要基于GPU和NPU等矩陣乘法器，極大的提升了門檻，幾乎完全就是給車載平臺弄的，仍然不利于行業(yè)的進(jìn)步和業(yè)務(wù)的普及！

(3) 緊耦合連貫優(yōu)化的直接法/ DSO-直接稀疏里程計

這個部分后續(xù)我準(zhǔn)備專門針對性的寫個純技術(shù)的課題，春節(jié)后應(yīng)該還要發(fā)布DM-VIO的全解和pipeline。因此這篇文章還是寫粗一些。但是要注意，這個部分即使寫粗，對于一些基礎(chǔ)稍微弱一些的朋友，還是非常吃力的。

其實首先我一直對Daniel老師這個命名感覺到奇怪。。。為啥叫這個，其實明明挺稠密的或者說可以做到很稠密，嘿嘿。

TUM的這套方法和路徑是目前能實現(xiàn)視覺世界觀的主流方法，但是有一系列非常麻煩的先決條件，首先就是對光度標(biāo)定的要求非常高，另外對卷簾快門非常不友好(雖然已經(jīng)提供了優(yōu)化方法)，另外也沒有過多考慮自動增益等現(xiàn)代相機帶來的影響，但是瑕不掩瑜，這套方法在我的心目中，還是非常重要的，雖然它真的非常地難。。。

DSO的靈魂是：光度不變假設(shè)

他的學(xué)生Jacob Engel在2016-2017年就整個完成了DSO這套系統(tǒng)并開源，寫得。。。怎么說呢，一言難盡，其實就是寫得非常好，但是對普通工程師過于不友好，導(dǎo)致了后續(xù)這個方向一直沒有得到太多的發(fā)展，但是本身也是因為它的這一整套理論實施落地的難度。。。

DSO詳解高翔博士在知乎做過，另外東北大學(xué)的高手龔亦群同學(xué)做了整個代碼的詳解，我們在他這個海量工作的基礎(chǔ)上重新梳理了一下因子圖。所以我在這里就不多貼式子了，講核心重點！

DSO是基于優(yōu)化的，H矩陣形態(tài)很好理解，每個位姿8維，在6個自由度上增加了2個關(guān)鍵的光度參數(shù)a,b。最核心的3個殘差同時包含了：幾何/光度與圖像梯度，對應(yīng)3個Jacobian。

這3個殘差的優(yōu)化就是絕大部分同學(xué)難以上手的核心原因，另一個核心原因是其極其麻煩的初始化。整個DSO絕對不是大家簡單認(rèn)為的順序操作：比如先前端提取，然后跟蹤，然后匹配，然后RANSAC，然后BA或者卡爾曼優(yōu)化這類，而是整個工作流都在不斷地重復(fù)交叉優(yōu)化這3種殘差同時不斷地更新整個Hession矩陣的過程。

FRAME/POINT與Residual與對應(yīng)的能量EF全部互持指針，整個工作流耦合度極高，但注意因果關(guān)系！正是這種非常精美的模型，才能實現(xiàn)最終DSO所實現(xiàn)的真正相對稠密的視覺世界觀。

DSO系統(tǒng)解耦的難度極大，最好的方法就是重寫，另外后續(xù)地平線的工作組和涂大神也手寫了2個雙目的立體S-DSO, 問題仍然有不少但是已經(jīng)收斂了很多。后人也在不斷地推進(jìn)相關(guān)工作。

DSO系最大的問題就是如果要建立更好的視覺世界觀，其開銷會幾何級數(shù)增長，同時因為運算涉及到太多的交叉優(yōu)化與迭代(矩陣的處理和常規(guī)差不多)，很難ASIC化。同時因為耦合度很高，運動傳感器也很難并入，如VI-DSO和DM-VIO并入了IMU，代碼也是很難的。同時DM-VIO雖然運行效果很好，但是開銷更高了(直接2個BA+延遲邊緣化在跑，視覺緊耦，運動傳感器接近松耦，雖然也不是很松)。

視覺世界觀如果真正建立，我們的重定位便不再依賴詞袋這類東西，同時回環(huán)也會變得沒那么重點，我們將有更多方法匹配慣性系與世界系，因為我們得到的參照數(shù)據(jù)將變得更加直觀。

但是事情也不要太理想化，視覺世界觀畢竟還是來自于雙眼，和雷達(dá)世界觀比仍然有不少的缺陷：如大樹在陽光下的影子，一面墻它只是個矩形但到底是否可以通過呢？這一系列的問題。