視覺慣性SLAM實作 - 原始程式剖析真正讀懂理論

前言

人工智慧技術按照資訊來源主要分為電腦視覺（視覺）、自然語言處理（文字）和語音辨識（語音）三大方向。其中，電腦視覺是需求最多、發展最快、應用最廣泛的領域。電腦視覺演算法通常分為以學習的方法為基礎和以幾何的方法為基礎，前者主要指利用深度學習來實現影像辨識、物體偵測、物體分割、視訊理解和影像生成等；後者主要指利用多視圖立體幾何來實現空間定位、三維重建和測距測繪等。
本書主要聚焦於以幾何方法寫基礎的電腦視覺核心技術—同時定位與地圖建構（Simultaneous Localization and Mapping，SLAM）。SLAM 技術最早應用於潛艇、太空車等軍用領域，之後逐漸進入民用領域。近幾年，學術界誕生了大量優秀的 SLAM 演算法框架，並且隨著三維感測器的高速發展和嵌入式裝置算力的快速提升，SLAM 技術開始大規模商業化應用，包括但不限於自主移動機器人、自動駕駛車輛、擴增實境、智慧穿戴裝置和智慧無人機等。
那麼什麼是 SLAM 呢？它是指移動智慧體從一個未知環境中的未知地點出發，在運動過程中透過自身感測器觀測周圍環境，根據環境定位自身的位置並進行增量式的地圖建構，從而達到同時定位和建構地圖的目的。
對於初學者來說，很難從晦澀的定義中看懂 SLAM 技術到底在幹什麼，也無法理解機器人在應用 SLAM 技術的過程中有什麼困難。其實，人類也能執行定位和地圖建構的任務，為方便讀者理解，這裡不妨拿人類的探索過程和機器人的視覺 SLAM 過程來進行類比。
假設我們接到一個任務，需要在不借助專業裝置的前提下到一個陌生的地方探索並簡單繪製當地的地圖。我們如何完成上述任務呢？
• 我們在陌生的起始點用雙眼（機器人上安裝的視覺感測器）觀察四周的環境，並記錄那些與眾不同的標識物（特徵點），從而確定自己的初始位置（地圖初始化）。然後我們一邊行走，一邊觀察環境，記錄當前位置（定位）並繪
製地圖（地圖建構）。
• 由於我們探索的是未知的環境，因此難免發生意外。比如我們不小心從山坡上摔下，爬起來後已經不知道自己具體的位置（追蹤遺失）。此時有兩種方法，一種方法是重新爬回山坡上，觀察周圍環境並和已經繪製的地圖進行對比，從而確定自己當前在已繪製地圖中的位置（重定位）；另一種方法是從山坡下當前地點出發，重新開始繪製新的地圖（重新初始化地圖）。
• 在探索的過程中還可能會遇到曾經去過的地方，這時我們需要非常謹慎地反覆觀察對比，確認這裡是否真的是已繪製地圖中我們曾經走過的某個地方（閉環檢測）。由於在繪製地圖的過程中有誤差（累計飄移），這時相同的地點在地圖上很可能無法形成一個閉環，而是一個缺口。一旦確認這是同一個地方，就需要整體調整已經繪製的地圖，以便把缺口平滑地銜接起來（閉環矯正）。
• 隨著探索的區域越來越大，我們的地圖也越來越完善，如果存在多個地圖，則可以根據地圖的重疊區域將它們合併為統一的地圖（地圖融合）。最終我們獲得了整個區域的地圖，完成任務。
上述括號內粗體部分是 SLAM 中的常用術語，讀者暫時不理解也不用擔心，我們會在本書中逐步消化吸收這些術語背後的原理和程式實作。或許有些讀者認為上述探索和繪圖過程對一個普通人來說不難做到，機器人毫無懸念地應該做得更好。但事實上並非如此。人類在成長的過程中不斷觀察和學習，更擅長辨識物體和場景，但在即時量化計算空間座標位置方面比較吃力。而機器人等智慧裝置更擅長精密計算，但在準確辨識物體和場景方面先天不足（影像深度學習技術正在改變這個現狀）。舉一個例子，相機在同一地點不同光源、不同角度、不同遠近等情況下拍攝的影像差別非常大，但人類根據經驗可以快速而準確地判斷這是同一地點。而由於相機拍攝的數位化影像在計算單元中儲存的僅僅是不同數字組成的矩陣，讓機器人去理解這是同一個場景，進而在不同影像的像素之間建立對應關係是比較困難的，這正是 SLAM 技術研究的一個困難。
目前 SLAM 相關書籍還比較少，《SLAM 視覺十四講：雙倍內容強化版 》《機器人學中的狀態估計》《電腦視覺中的多視圖幾何》是非常經典的圖書，它們涵蓋了該領域的核心基礎知識，公式推導嚴謹，有的還配有重要概念的程式實作。不過，筆者在和同行學習交流的過程中了解到，很多初學者在掌握了 SLAM基礎知識後，發現距離真正的專案實作還有較大差距，他們對如何開始自己的第一個 SLAM 專案實作仍然比較迷茫。本書則致力於解決這個問題，選取了最經典的視覺 SLAM 框架 ORB-SLAM2 和 2022 年綜合效果最好的視覺慣性 SLAM 框架 ORB-SLAM3，透過層層拆解、分析，引導讀者循序漸進地掌握自己的第一個SLAM 專案。

主要特點
• 帶領讀者從頭到尾學習一個完整的 SLAM 專案，從原理解析、程式解讀到專案技巧，一步一個腳印地完成。
• 本書從初學者的角度切入，部分章節以零基礎的小白和經驗豐富的師兄兩人對話的形式闡述。採用對話形式，一方面可以把初學者在學習過程中的很多基礎問題展現出來，幫助讀者在學習過程中不斷思考和提升，提高專案實作經驗；另一方面，對話這種口語化的表達方式能夠讓讀者在輕鬆的氣氛中快速理解理論知識。
• 每個重要的基礎知識都嘗試從 3 個角度去分析—「What （是什麼）」「Why（為什麼）」「How（怎麼做）」，讓讀者知其然也知其所以然。
• 豐富的圖示和類比。我們把大量複雜或難以理解的原理或流程繪製成具體化的影像，一圖勝千言，極大地降低了學習門檻。
• 開放原始程式碼搭配詳細的中文註釋。
雖然本書講解的是針對 ORB-SLAM 系列的原理及程式解析，但其中涉及的基礎知識同樣適用於其他同類演算法，學習方法和思路也值得參考。

組織方式
本書的內容主要分為三部分。
1. 第一部分：介紹 SLAM 的部分基礎知識，它們將在第二、三部分的原理或專案實作中使用。
• 第 1 章為 SLAM 導覽。你將了解 SLAM 的定義、應用場景和應用領域。
• 第 2 章為程式設計及編譯工具。你將了解 C++ 11 新特性和 CMake 工具，方便讀者看懂程式，提高專案實作能力。
• 第 3 章為 SLAM 中常用的數學基礎知識。你將了解齊次座標和三維空間中剛體旋轉的表達方式。
• 第 4 章為相機成像模型。你將了解針孔相機模型的背景、推導和相機扭曲模型。
• 第 5 章為對極幾何。你將了解對極幾何的基本概念，並從物理意義上理解推導的原理。
• 第 6 章為圖最佳化函式庫的使用。你將了解 g2o 函式庫的程式設計框架，以及如何自己用g2o 函式庫建構圖的頂點和邊。

2. 第二部分：介紹視覺 SLAM 框架 ORB-SLAM2 的原理和核心程式。
• 第 7 章為 ORB 特徵提取。你將了解 ORB 特徵點的建構及特徵點均勻化策略。
• 第 8 章為 ORB-SLAM2 中的特徵匹配。你將了解不同場景下使用的不同特徵匹配方法，包括單目初始化中的特徵匹配、透過詞袋進行特徵匹配、透過地圖點投影進行特徵匹配、透過 Sim(3) 變換進行相互投影匹配。
• 第 9 章為地圖點、主要畫面格、圖結構。你將了解這 3 個核心概念，它們貫穿在整個 SLAM 過程中。
• 第 10 章為 ORB-SLAM2 中的地圖初始化。你將了解地圖初始化的意義，以及單目模式和雙目模式的不同地圖初始化方法。
• 第 11 章為 ORB-SLAM2 中的追蹤執行緒。你將了解參考主要畫面格追蹤、恒速模型追蹤、重定位追蹤和局部地圖追蹤。
• 第 12 章為 ORB-SLAM2 中的局部地圖建構執行緒。你將了解如何處理新的主要畫面格、剔除不合格的地圖點、生成新的地圖點、檢查並融合當前主要畫面格與相鄰畫面格的地圖點及主要畫面格的剔除。
• 第 13 章為 ORB-SLAM2 中的閉環執行緒。你將了解閉環檢測的原因，如何尋找並驗證閉環候選主要畫面格，計算 Sim(3) 變換，閉環矯正。
• 第 14 章為 ORB-SLAM2 中的最佳化方法。你將了解追蹤執行緒僅最佳化位姿、局部地圖建構執行緒中局部地圖最佳化、閉環執行緒中的 Sim(3) 位姿最佳化、閉環時本質圖最佳化及全域最佳化。

3. 第三部分：介紹 ORB-SLAM2 的升級版—視覺慣性系統 ORB-SLAM3 的主要新增內容和程式。
• 第 15 章為 ORB-SLAM3 中的 IMU 預積分。你將了解視覺慣性緊耦合的意義、IMU 預積分原理及推導、IMU 預積分的程式實作。
• 第 16 章為 ORB-SLAM3 中的多地圖系統。你將了解多地圖的基本概念、多地圖系統的效果和作用、建立新地圖的方法和時機，以及地圖融合。
• 第 17 章為 ORB-SLAM3 中的追蹤執行緒。你將了解 ORB-SLAM3 中追蹤執行緒流程圖及追蹤執行緒的新變化。
• 第 18 章為 ORB-SLAM3 中的局部地圖建構執行緒。你將了解局部地圖建構執行緒的作用、局部地圖建構執行緒的流程及其中 IMU 的初始化。
• 第 19 章為 ORB-SLAM3 中的閉環及地圖融合執行緒。你將了解共同區域檢測、地圖融合的具體流程和程式實作。
• 第 20 章為視覺 SLAM 的現在與未來。你將了解視覺 SLAM 的發展歷程、視覺慣性 SLAM 框架對比及資料集，以及視覺 SLAM 的未來發展趨勢。

書附程式
本書涉及的程式來自 ORB-SLAM2 和 ORB-SLAM3 作者開放原始碼的程式，我們對該程式中的重點和困難部分進行了中文詳細註釋，並託管在 GitHub 上。
https://github.com/electech6/ORB_SLAM2_detailed_comments
https://github.com/electech6/ORB_SLAM3_detailed_comments
GitHub 上的註釋內容和程式會持續更新。如果本書程式和 GitHub 上的有出入，則以 GitHub 上的最新程式及註釋為準。

目標讀者
本書適合有一定 SLAM 基礎的大專院校學生、科學研究機構的研究人員和企業從業者閱讀，尤其適合希望深入研究視覺（慣性）SLAM 的演算法工程師參考。建議在學習本書前，讀者已經具備如下知識。
• 微積分、線性代數和機率論的基礎知識。對於大多數理工科背景的讀者來說，讀懂本書所需的數學知識已經足夠。
• C++ 程式設計基礎。SLAM 領域使用的程式設計語言主要是 C++，所以強烈建議讀者事先掌握 C++ 的程式設計基礎，推薦學習《C++ Primer》等入門圖書。本書第一部分也說明了部分 C++ 11 標準以供參考，它們在程式中出現時能夠讀懂即可。
• Linux 基礎。SLAM 的開發環境用 Linux 會非常方便，如果你不了解 Linux，市面上有許多入門圖書，只需掌握基礎的 Linux 操作指令即可開始學習。
• SLAM 基礎知識。本書第一部分並未詳細講解 SLAM 的完整理論，只是對第二、三部分涉及的基礎知識和專案實作技巧進行了介紹。推薦讀者在閱讀完《SLAM 視覺十四講：雙倍內容強化版 》之後再來看本書的內容，會比較容易理解。

風格說明
本書包含理論闡述、公式推導和程式註釋，內容較多，為了較好的閱讀體驗，我們做如下約定。
• 數學公式中純量使用斜體（如 a），向量和矩陣使用粗體斜體（如 a，A）。
• 程式中重要部份或疑難部分都用中文進行了註釋。程式中部分不重要的內容會進行省略，並用「……」加以標明。讀者可以去 GitHub 上下載完整的原始程式碼和註釋。
• 本書以「*」開頭的內容為選讀部分，對於初學者來說可能有一定難度，讀者可以根據需要選擇性地學習。

致謝與宣告
本書從開始策劃到完稿，經歷了 2 年時間，這期間我獲得了很多朋友的幫助。劉國慶在ORB-SLAM2 程式的註釋方面提供了幫助，劉宴誠在 ORB-SLAM3程式的註釋及預積分部分提供了幫助，荊黎明對 ORB-SLAM2 部分內容進行了驗證，在此表示感謝！
本書在寫作過程中獲得了不少朋友的幫助，也參考了部分學者的公開資料，包括但不限於高翔、楊東升、邱笑晨、吳博、吳昭燃、趙德銘、石楠、付堉家、丁瑞旭、馮凱、黃志明、王寰、單鵬輝、周佳峰、張俊傑等。在此向他們表示感謝！
感謝電子工業出版社的宋亞東編輯的大力支持！
本書在寫作過程中參考了很多前人的成果，包括論文、程式註釋、文件手冊等，在此表示感謝。
由於時間、精力有限，書中難免有不妥和疏漏之處，如讀者發現任何問題，歡迎與我聯繫（E-mail：learn_slam@126.com），我會即時修正。
最後，感謝我的妻子的理解和支持，感謝我的父母背後的默默付出！

程小六

前言

人工智慧技術按照資訊來源主要分為電腦視覺（視覺）、自然語言處理（文字）和語音辨識（語音）三大方向。其中，電腦視覺是需求最多、發展最快、應用最廣泛的領域。電腦視覺演算法通常分為以學習的方法為基礎和以幾何的方法為基礎，前者主要指利用深度學習來實現影像辨識、物體偵測、物體分割、視訊理解和影像生成等；後者主要指利用多視圖立體幾何來實現空間定位、三維重建和測距測繪等。
本書主要聚焦於以幾何方法寫基礎的電腦視覺核心技術—同時定位與地圖建構（Simultaneous Localization and Mapping，SLAM）。SLAM 技術最早...

第一部分　SLAM 基礎

第1 章　SLAM 導覽
1.1 什麼是 SLAM
1.2 SLAM 有什麼不可替代性
1.3 SLAM 的應用領域

第 2 章　程式設計及編譯工具
2.1 C++ 新特性
2.2 CMake 入門

第3章　SLAM 中常用的數學基礎知識
3.1 為什麼要用齊次座標
3.2 三維空間中剛體旋轉的幾種表達方式

第4 章　相機成像模型
4.1 針孔相機成像原理
4.2 針孔相機成像模型
4.3 相機扭曲模型

第5 章　對極幾何
5.1 對極幾何的基本概念
5.2 理解對極約束

第6 章　圖最佳化函式庫的使用
6.1 g2o 程式設計框架
6.2 建構 g2o 頂點
6.3 建構 g2o 邊

第二部分　ORB-SLAM2 理論與實作

第7 章　ORB 特徵提取
7.1 ORB 特徵點
7.2 ORB 特徵點均勻化策略

第8 章　ORB-SLAM2 中的特徵匹配
8.1 單目初始化中的特徵匹配
8.2 透過詞袋進行特徵匹配
8.3 透過地圖點投影進行特徵匹配
8.4 透過 Sim(3) 變換進行相互投影匹配

第9 章　地圖點、主要畫面格和圖結構
9.1 地圖點
9.2 主要畫面格
9.3 圖結構

第10 章　ORB-SLAM2 中的地圖初始化
10.1 為什麼需要初始化
10.2 單目模式地圖初始化
10.3 雙目模式地圖初始化

第11 章　ORB-SLAM2 中的追蹤執行緒
11.1 參考主要畫面格
11.2 恒速模型追蹤
11.3 重定位追蹤
11.4 局部地圖追蹤

第12 章　ORB-SLAM2 中的局部地圖建構執行緒
12.1 處理新的主要畫面格
12.2 剔除不合格的地圖點
12.3 生成新的地圖點
12.4 檢查並融合當前主要畫面格與相鄰畫面格
12.5 主要畫面格的剔除

第13 章　ORB-SLAM2 中的閉環執行緒
13.1 什麼是閉環檢測
13.2 尋找並驗證閉環候選主要畫面格
13.3 計算 Sim(3) 變換
13.4 閉環矯正
13.5 閉環全域 BA 最佳化

第14 章　ORB-SLAM2 中的最佳化方法
14.1 追蹤執行緒僅最佳化位姿
14.2 局部地圖建構執行緒中局部地圖最佳化
14.3 閉環執行緒中的 Sim(3) 位姿最佳化
14.4 閉環時本質圖最佳化
14.5 全域最佳化

第三部分　ORB-SLAM3 理論與實作

第15 章　ORB-SLAM3 中的IMU 預積分
15.1 視覺慣性緊耦合的意義
15.2 IMU 預積分原理及推導
15.3 IMU 預積分的程式實現

第16 章　ORB-SLAM3 中的多地圖系統
16.1 多地圖的基本概念
16.2 多地圖系統的效果和作用
16.3 建立新地圖的方法和時機
16.4 地圖融合概述

第17 章　ORB-SLAM3 中的追蹤執行緒
17.1 追蹤執行緒流程圖
17.2 追蹤執行緒的新變化

第18 章　ORB-SLAM3 中的局部地圖建構執行緒
18.1 局部地圖建構執行緒的作用
18.2 局部地圖建構執行緒的流程
18.3 IMU 的初始化

第19 章　ORB-SLAM3 中的閉環及地圖融合執行緒
19.1 檢測共同區域
19.2 地圖融合

第20 章　視覺 SLAM 的現在與未來
20.1 視覺 SLAM 的發展歷程
20.2 視覺慣性 SLAM 框架對比及資料集
20.3 未來發展趨勢
20.4 總結

第一部分　SLAM 基礎

第1 章　SLAM 導覽
1.1 什麼是 SLAM
1.2 SLAM 有什麼不可替代性
1.3 SLAM 的應用領域

第 2 章　程式設計及編譯工具
2.1 C++ 新特性
2.2 CMake 入門

第3章　SLAM 中常用的數學基礎知識
3.1 為什麼要用齊次座標
3.2 三維空間中剛體旋轉的幾種表達方式

第4 章　相機成像模型
4.1 針孔相機成像原理
4.2 針孔相機成像模型
4.3 相機扭曲模型

第5 章　對極幾何
5.1 對極幾何的基本概念
5.2 理解對極約束

第6 章　圖最佳化函式庫的使用
6.1 g2o 程式設計框架
6.2 建構 g2o 頂點
6.3 建構 g2...