對于“面向對象”的程序設計者來說，有本必備的書的叫做《設計模式-可復用面向對象軟件的基礎》。那么在slam的實現中有沒有這一種或者幾種可以總結歸納的設計模式呢？
我們在看一些Slam的書的時候，有些要么就是介紹很多狀態估計的理論，要么就是介紹多視圖幾何的各種視覺理論。那么我們在看了這些理論之后，仍然無法下手，那么一個典型的好的Slam的代碼的基本結構是什么樣的呢？一個slam代碼的基本結構是怎樣的？有哪些基本要素？
一個自然的想法就是從圖像中獲取想要的定位的信息。在機器人移動的過程中相機捕捉到一個圖像序列。在這個圖像序列中機器人看到周圍的景物是在按照一定的規律移動和變化的，那么景物移動的規律和機器人自身移動的規律有什么關系呢？這個關系可以根據空間變換來確定。
那么如何根據圖像序列的內容提取景物的移動規律呢？可以跟蹤圖像上景物的一些點，圖像很多點的統計規律可以表征景物的移動規律。這就是提取特征點進行跟蹤或者直接使用灰度點跟蹤的意義所在，根據我們使用的點的方式不容，可以看到兩種不同的slam前端方法——直接法和特征法。
那么如何實現這樣的跟蹤器呢？slam算法在讀到一張圖像之后，首先把當前圖像的點和前一幀圖像的點對應起來，那么需要多少個點對應呢？怎么實現點對應呢？基于特征的方法是提取圖像中的特征點，用于匹配不同圖像中相同的點，這是一種思路，常用的有SIFT ORB等。另外一個思路是由于機器人的運動相對于圖像的幀率較慢的時候，圖像的內容變化較小，因此在局部搜索可以找到上一幀圖像的點的位置，這就是直接法的思路。
我們這里引入一個Frame結構，為了實現上述的跟蹤器，第一步是讀取圖像，通過圖像的數據估計當前幀對應的位姿狀態。場景中相同的點在不同的視角中構成一個基礎矩陣的關系，一個圖像幀的圖像平面和另一幀的圖像的平面之間構成單應矩陣的關系。使用最小二乘法通過圖像點可以計算得到單應矩陣和基礎矩陣，使用矩陣奇異值分解（SVD）可將單應矩陣或者基礎矩陣分解恢復出位姿變化。這些內容的實現可以放到Frame結構，也可以像ORB那樣專門放到一個Initializer里邊。Frame需要和Map完成互動。除了需要計算當前幀的位姿，還要完成特征點的三維重建。有的方法使用了一個深度濾波器，如SVO，LSD中考慮了極線與深度的夾角，這樣會得到一個半稠密的地圖。因為稠密的地圖點想要試試完成一般是需要使用GPU來完成的。圖像的上的特征點和空間中的三維點如何對應呢？常用的方法是PnP。PnP是已知三維點計算相機位姿，三維重建是已知相機位姿計算三維點。
PnP的計算中需要知道相機的內參數，這里引入第二個結構—-相機模型Camera，他保存并管理相機的參數，進行一些圖像空間到世界空間的轉換，完成圖像點（關鍵特征點）的去畸變。
如何解決累積誤差問題？常用的方法是使用路標點和當前圖像點進行匹配估計相機的運動，這時候需要存儲這些路標點的描述子。另外一方面在LSD-SLAM, SVO等方法中，是怎樣存儲路標點的呢？這里引入一個結構叫做Mappoint。
如何評價一個路標點的好壞程度？如何保存并管理這些路標點呢？路標點的添加刪除等管理工作可以放入另外一個結構Map中進行。地圖中以何種形式保存這些路標呢，一種方法是路標放在圖像幀里，作為關鍵幀KeyFrame進行保存和提取。關鍵幀串起來成為一個地圖。這樣做的好處就是我們使用了關鍵幀繼承了幀的性質，便于以后特征點匹配。在前邊累計相機運動的時候，參考幀是前一幀圖像，這里的參考幀是關鍵幀的圖像。關鍵幀是相機運動過程中保留了重要信息的最小集合中的元素。理論上關鍵幀是沒有重疊的。后端優化的對象主要是關鍵幀。
那么我們換一個角度來看問題，從時間軸上，我們關心的有兩個狀態，一個當前的時刻，另外一個以前的時刻。對于當前的時刻我們要保持實時性。對于以前的時刻我們要通過他來提高精度。這就是為什么要分開來做前端和后端的問題。 事實上，優化并非僅僅存在于后端，優化存在于前端的運動估計中(基礎矩陣計算，于運動估計的計算)，后端中關鍵幀的優化（BA）。這些優化中形成矩陣結構H是稀疏的（對應圖網絡），可以顯式的利用圖優化表示。H的稀疏性為圖優化方法的SLAM提供了實時性的保證。這里可以引入一個結構Optimizater，用來完成上述內容中提到的各種優化工作。我們這里總結一下可以優化的步驟是那些呢？
? 三維點（點的深度）的優化（三角測量）
? 后端的關鍵幀中的位姿和路標點的BA（關鍵幀的邊緣化）
? 初始化過程的單應矩陣和基礎矩陣的優化
? PnP計算相機位姿
注意
有些VO的算法中由于后端很小，所以把后端的內容一起移到了前端，然后每一幀都做一次BA，這樣要求H矩陣（BA中的被優化結構）一定要保持稀疏性。
維護一個地圖的另外一個好處就是即使當前跟蹤的某一幀丟失，之后跟蹤上的幀仍然可以恢復出運動。這樣的前提是一個不斷更新的地圖，而且這個地圖是正確的。這樣的地圖可以是局部的也可以全局的。全局地圖的規模較大（更像SLAM，可以完成閉環），計算量比較大，因此適用于一些要求精確定位的地方，對于一些計算能力弱的場合適用于局部地圖（更像一個VO，不能做閉環）。在局部地圖中，考慮需要丟掉一些之前的幀，這個過程叫做marginalization(邊緣化)。
跟蹤器的狀態有幾種，1. 初始化，順利跟蹤，丟失。因此我們還需要一個初始化的過程，這個過程中有哪些內容？對第一/二幀圖像的匹配，單應和基礎矩陣的計算。這個其實在Frame結構中涉及過。那么可以構建一個Initializer，完成相應的工作。
到這里，slam只是完成了一部分，可以稱之為前端吧。我們在之后的內容接續介紹關于后端的設計方法和一些模塊的關系總結。
這里我們繼續完成slam的前后端的設計
首先我們給出slam程序中各模塊的主要組成部分。
我們先理清楚到底他們干了些什么，然后再說他們怎么實現的。其實吧，現代的CPU對多線程的代碼支持的越來越好，因為沒法提速，只能多個人（CPU核）干。所以在PTAM把Tracking 和 Maping 分開完之后，大家一下子就覺得這樣玩真的不錯，一窩蜂都轉成這種玩法。
所以按照國際慣例，我們還是把前后端分開處理，前端是一個跟蹤器，后端干的事還挺多的，什么建圖啊，BA，閉環啊，各種，看著都累。
跟蹤器（定位的過程）
跟蹤顧名思義就是上一幀里邊的點我們特別關心他們現在在哪里，然后呢，怎么知道他們去哪里呢？有兩種方法，一種使用局部范圍的直接搜索，另外只是使用特征描述子進行匹配。匹配正確的點用來計算他們的幾何關系，根據幾何關系可以恢復出相機的運動，進而重建出世界中的三維點。這樣到來新的一幀之后我們都可以提取到一些新的三維點。另外我們可以對應三維點和圖像中的點進行計算相機的姿態。這就是跟蹤器的主要目的。
以上內容是假設跟蹤器在順利跟蹤的時候要干的事，那么問題來了，如果跟蹤不順利或者這就是第一幀那怎么辦？
所以我們把跟蹤分為三種狀態，一種是第一幀，叫做初始狀態，其實在單目中第二幀也屬于這個狀態，因為第一幀什么也干不了，就只能提特征，第二幀來了才有匹配等等。
如果跟蹤失敗，那么這個狀態我們成為lost，然后對應的措施是relocalization。 我們看到的那篇posenet就是干這個的。
在跟蹤中初始化的時候，計算匹配點的集合關系使用的是的一般是單應矩陣和基礎矩陣。
在跟蹤時候匹配當前幀和路標點，一個圖像中的點，一個三維空間中的點，因此用的是PnP，優化Pose，路標點這里是假設為靜止的，只有pose被優化。
那么優化完了，我們可以得到測量數據了，但是為了完成下一次的跟蹤，我們還有一些工作要做。
1. update Local KeyFrame
2. update Local Points
為什么？我們先看他到底做了哪些事就是他的目的是什么。在更新KeyFrame中增加了路標點對當前關鍵幀的索引。我們是通過正匹配找到的這些路標點，然后還沒有整理好這些路標點和當前關鍵幀的所有關系，然后就沒法使用這個幀進行匹配了，因為我們靠一個參考關鍵幀找到他們的路標點。
初始化狀態
其實初始化的內容還是比較簡單，因為干的多了就容易干的久，干的久，實時性就沒了，所以只有一件事就是計算前兩者之間的相對位姿。一些方法只使用Homograph，有一些方法還要加上基礎矩陣（極線約束），然后就是雙視BA，有一些方法（ORB），指明了要做一個full BA（為了提高精度），為啥？ 因為開始的時候最好基礎打的好一些，有信心一些，不然以后的誤差太大，就玩不下去啦。
Lost state
地圖（BA優化）
主要有2個待優化的內容，一是軌跡中位姿，另外一個是圖中的路標點，不過很多時候路標經過幾次優化之后就會不動了，那么這個時候就會使用一個pose graph來降低優化的規模，在ORB中 Covisibility Graph建模這些位姿，就是用路標點個數表明位姿節點的權重的一個圖。
BA，這個東西一般是放在整個過程的最后，就是別人都折騰過（優化）一遍之后（緊著那些保證實時性的那些東西盡快的先搞出點東西來用著），把所有的東西放到一個優化器全部優化。這樣做的好處就是優化過后的各部分都會精度提到，便于以后放心的使用這些東西，剔除掉了不好的outliers，把網絡之間的關系理順。
由于跟蹤器和建圖的過程是分線程實現的，因此兩者之間的同步和協調這里也要做一下，主要配合tracker，當然tracker因為兼具實時性的重要任務，那么這些工作就丟給了mapping。
mapping其實也很忙，tracker一直再把關鍵幀丟過來，mapping根本就來不及處理，只好先 放到一個地方存著，有一個隊列deque。
功能 函數
計算關鍵幀特征點的BoW映射，將關鍵幀插入地圖 ProcessNewKeyFrame()
剔除ProcessNewKeyFrame函數中引入的不合格MapPoints MapPointCulling()
相機運動過程中與相鄰關鍵幀通過三角化恢復出一些MapPoints CreateNewMapPoints()
檢查并融合當前關鍵幀與相鄰幀（兩級相鄰）重復的MapPoints SearchInNeighbors()
已經處理完隊列中的最后的一個關鍵幀，并且閉環檢測沒有請求停止 LocalMapping
local BA Optimizer::LocalBundleAdjustment
檢測并剔除當前幀相鄰的關鍵幀中冗余的關鍵幀 KeyFrameCulling();
將當前幀加入到閉環檢測隊列中 InsertKeyFrame
相機
? 相機的內參數
? 相機的畸變參數
? 世界空間到圖像空間的變換
? 圖像空間到世界空間的逆轉換
? 圖像點（關鍵特征點）的去畸變
圖像幀結構
? 圖像的數據
? frame ID
? frame 位姿
? 圖像的特征點（關鍵點）提取，特征匹配
? 尺度因子（這個是特征里的對應了描述子的圖像的層）
? 獲取相機的光心
? 計算單應矩陣
? 計算基礎矩陣
? 單應矩陣分解相機運動
? 基礎矩陣分解相機運動
? 根據2幀圖像三角測量點的三維位置（三維重建）
? 計算詞袋
尺度的問題
特征法中，把尺度作為幀的狀態，和深度關聯，那么在DSO中由于殘差計算的時候已經考慮了尺度。ORB中同時具備了Frame和關鍵幀的結構，所以關鍵幀中包括了三個ID號，當前幀ID，關鍵幀ID，上一關鍵幀ID
Mappoint/路標點
? 三維點，在世界坐標系下的坐標
? 描述子，這個描述子相對于其他的描述子（表征同一個三維點）的漢明距離最小。
? 可見關鍵幀，觀測到該點的關鍵幀和該點點在該幀中的索引
? 該點的平均觀測方向
? 參考關鍵幀（這個的設置是在干什么？）
? 觀測距離，最小距離和最大距離根絕ORB的尺度不變形計算。
路標點和關鍵幀之間
- 表述該點在哪些關鍵幀可以看到
- 如何添加共視關系呢？維持一個std::map類型的hash，指定對于該路標點對于那些幀看到了，對應的是該幀里路標點的序號為多少。如果要刪除該點，那么就要刪除該hash。
路標點和描述子
一個路標點可多個描述子對應，在插入關鍵幀（建圖的時候），對于每個路標點可以找到一個最合適的描述子，所以會出現一個過程就是獲取所有的描述子，探后計算兩兩之間的距離，最好的描述子就是與其他距離最小的那個。描述子和每個關鍵幀對應。
路標點的觀測方向和觀測距離
觀測方向的計算是所有可見關鍵幀的方向（相對于路標點）的平均，觀測距離（深度）通過路標點減去參考關鍵幀的光心。
在路標點中特征法和直接法的區別
- 優化路標點
　關鍵幀
? 相機的位姿，從世界坐標系到相機坐標系的剛體變換
? 相機的內參數
? ORB 特征、描述子 以上這兩個和圖像幀結構是相同的。（特征法中的內容）
? vector<路標點>
? 刪除/增加路標點
? 特征點網格
? BOW
關鍵幀和Covisibility Graph
Covisibility Graph的實現使用map”keyfram kf, int idx”的一種結構，前者指定了有共視關系的關鍵幀，后者指定了兩者之間的共視關系的強弱（共視點的個數）。refer
更新具有共視關系的關鍵幀之間的鏈接的步驟
1. 首先獲得該關鍵幀的所有MapPoint點，統計觀測到這些3d點的每個關鍵與其它所有關鍵幀之間的共視程度
對每一個找到的關鍵幀，建立一條邊，邊的權重是該關鍵幀與當前關鍵幀公共3d點的個數。
2. 并且該權重必須大于一個閾值，如果沒有超過該閾值的權重，那么就只保留權重最大的邊（與其它關鍵幀的共視程度比較高）
3. 對這些連接按照權重從大到小進行排序，以方便將來的處理
更新完covisibility圖之后，如果沒有初始化過，則初始化為連接權重最大的邊（與其它關鍵幀共視程度最高的那個關鍵幀），類似于最大生成樹
我們看到大量重復的并且要保存下來數據的有2種，一種是MapPoint，一種是KeyFrame，路標點之間假設是獨立的，路標點和位姿（關鍵幀）之間具備聯系，位姿和位姿之間是關聯的。
另外一個問題SLAM中的Data Association表示的是什么，這里最開始的認識就是路標點之間的數據關聯，只有但那些點關聯之后，我們才會直到，哪些點構成一個物體，或者一個目標，如果目標是移動的，我們也得到一個啟發式的處理動態場景的方法。
地圖
? vector<路標點>
? vector<關鍵幀>
? 對關鍵幀進行優化（BA）
? 刪除/增加路標點
? 增加/刪除關鍵幀
跟蹤器（定位的過程）
在跟蹤中初始化的時候使用的是單應矩陣和基礎矩陣，在跟蹤時候匹配當前幀和路標點，一個圖像中的點，一個三維空間中的點，因此用的是PnP，優化Pose，路標點這里是假設為靜止的，只有pose被優化。
地圖（BA優化）
主要有2個待優化的內容，一是軌跡中位姿，另外一個是圖中的路標點，不過很多時候路標進過幾次優化之后就會不動了，那么這個時候就會使用一個pose graph來降低優化的規模，在ORB中 Covisibility Graph建模這些位姿，就是用路標點個數表明位姿節點的權重的一個圖。
BA，這個東西一般是放在整個過程的最后，就是別人都折騰過（優化）一遍之后（緊著那些保證實時性的那些東西盡快的先搞出點東西來用著），把所有的東西放到一個優化器全部優化。這樣做的好處就是優化過后的各部分都會精度提到，便于以后放心的使用這些東西，踢掉了不好的outliers，把網絡之后的關系理順。

總結

以上是生活随笔為你收集整理的Slam设计模式的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。