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

首先來(lái)4個(gè)基礎(chǔ)邏輯：

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

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

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

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

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

01

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

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

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

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

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

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

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

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

效果如上圖所示

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

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

02

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

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

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

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

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

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

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

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

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

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

稀疏的MAPPING

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

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

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

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

真的是簡(jiǎn)述，幾個(gè)問(wèn)題，首先雷達(dá)太貴了。

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

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

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

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

其實(shí)首先我一直對(duì)Daniel老師這個(gè)命名感覺(jué)到奇怪。。。為啥叫這個(gè)，其實(shí)明明挺稠密的或者說(shuō)可以做到很稠密，嘿嘿。

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

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

他的學(xué)生Jacob Engel在2016-2017年就整個(gè)完成了DSO這套系統(tǒng)并開(kāi)源，寫(xiě)得。。。怎么說(shuō)呢，一言難盡，其實(shí)就是寫(xiě)得非常好，但是對(duì)普通工程師過(guò)于不友好，導(dǎo)致了后續(xù)這個(gè)方向一直沒(méi)有得到太多的發(fā)展，但是本身也是因?yàn)樗倪@一整套理論實(shí)施落地的難度。。。

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

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

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

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

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

DSO系最大的問(wèn)題就是如果要建立更好的視覺(jué)世界觀，其開(kāi)銷(xiāo)會(huì)幾何級(jí)數(shù)增長(zhǎng)，同時(shí)因?yàn)檫\(yùn)算涉及到太多的交叉優(yōu)化與迭代(矩陣的處理和常規(guī)差不多)，很難ASIC化。同時(shí)因?yàn)轳詈隙群芨撸\(yùn)動(dòng)傳感器也很難并入，如VI-DSO和DM-VIO并入了IMU，代碼也是很難的。同時(shí)DM-VIO雖然運(yùn)行效果很好，但是開(kāi)銷(xiāo)更高了(直接2個(gè)BA+延遲邊緣化在跑，視覺(jué)緊耦，運(yùn)動(dòng)傳感器接近松耦，雖然也不是很松)。

視覺(jué)世界觀如果真正建立，我們的重定位便不再依賴(lài)詞袋這類(lèi)東西，同時(shí)回環(huán)也會(huì)變得沒(méi)那么重點(diǎn)，我們將有更多方法匹配慣性系與世界系，因?yàn)槲覀兊玫降膮⒄諗?shù)據(jù)將變得更加直觀。

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

審核編輯 ：李倩