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

首先來4個基礎邏輯：

SLAM本質是數學問題，是一個科學家與工程師可以控制的數學問題，本質不是玄學，實現需要大量的數學知識與工具，需要極強的代碼功底與硬軟件開發能力。

無論對SLAM系統如何分割，建圖仍是位姿估計的副產品。

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

無論任何SLAM系統，精度無論多高，本質都是求解非線性優化的最優解，勢必存在誤差，且隨時長與探索距離遞增。

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

01

主流SLAM相關工作分類

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

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

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

(2)線掃/固態激光SLAM

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

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

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

效果如上圖所示

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

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

02

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

(1) 三相性VIO系統研究與試驗發展：

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

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

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

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

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

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

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

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

稀疏的MAPPING

UAV完成作業及重定位拍攝。

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

(2) 激光雷達SLAM系統簡述：

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

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

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

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

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

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

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

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

DSO的靈魂是：光度不變假設

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

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

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

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

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

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

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

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

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

審核編輯 ：李倩