視覺里程計VO的搭建基本都會有以下幾個問題：怎麼管理地圖點，如何處理誤匹配，如何選擇關鍵幀。。。由簡到繁進行。
1. 視覺里程計分單目，雙目，RGB-D三大類。單目視覺相對複雜，RGB-D最簡單，沒有初始化，也沒有尺度問題。
2. 編寫小規模的庫時，往往建立一些文件夾，把源代碼，頭文件，文檔，測試數據，配置文件，日誌等分類存放。當一個庫內容很多時，往往還會把代碼分解各個獨立的小模塊，便於測試。OpenCV或g2o的組織方式就可以參考借鑑。小型程序揉在一起也沒什麼。我建立如下文件夾來組織代碼文件：
3. 構造基本的數據結構
a. 幀：幀是相機採集到的圖像單位。它主要包含一個圖像（RGB-D情形下是一對圖像）。此外，還有特徵點，位姿，內參等信息。視覺slam中談論關鍵幀，由於相機採集的數據很多，存儲所有的數據顯然不實際，通常選取我們認爲重要的幀保存起來，並認爲相機軌跡可以用這些關鍵幀來描述。然而關鍵幀的選擇方法基本基於工程經驗，很少有理論上的指導。
b. 路標：路標點即圖像中的特徵點。在相機運動後，可以估計它們的3D位置。通常，會把路標點放在一個地圖當中，並講新來的幀與地圖中的路標點進行匹配，估計相機位姿。
c.幀的位姿與路標的位置估計相當於一個局部的SLAM問題，基本還需一些工具，讓程序寫起來更流暢。引出配置文件：程序中使用各種各樣的參數，比如相機的內參，特徵點的數量，匹配時選擇的比例，等等。框架往往就是這樣產生的，外部定義配置文件，程序運行時讀取該配置文件中的參數值。修改也只需修改配置文件，而無需在程序中一個個修改。
d. 座標變換：程序需要經常在座標系間進行座標變換，例如：世界座標到相機座標，相機座標到歸一化相機座標，歸一化相機座標到像素座標，等等。定義一個類把這些操作都放到一個將更方便。
4. 基本思想，該程序偏向算法而非軟件工程，故不討論複雜的類繼承關係，接口，模板等，而更關注與算法的正確實現，以及是否便於拓展。VO開始寫5個類：Frame爲幀，Camera爲相機模型，MapPoint爲特徵點/路標點，Map管理特徵點，Config提供配置參數。
這個是簡化的UML圖。
5. 幾個類的功能
a. Camera類存儲相機的內參和外參，並完成相機座標系，像素座標系和世界座標系之間的座標變換。
b. Frame類，它作爲基本數據單元，在許多地方會用到，目前提供基本的數據存儲和接口。ID，時間戳，位姿，相機，圖像這幾個是一個幀中最重要的信息。幾個重要的方法: 創建Frame，尋找給定點對應的深度，獲取相機光心，判斷某個點是否在視野內。
c. MapPoint類表示路標點。講估計它的世界座標，並會拿當前幀提取到的特徵點與地圖中的路標點匹配，來估計相機的運動，還需要存儲它對應的描述子。額外：記錄一個點被觀測到的次數和被匹配的次數。
d. Map類管理所有的路標點，並負責添加新路標，刪除不好的路標等工作。VO的匹配過稱只要與Map打交道即可。Map類中存儲了各個關鍵幀和路標點，即需要隨機訪問，又需要隨時插入和刪除，因此使用散列（Hash）來進行存儲。
e. Config類負責參數文件的讀取，並在程序的任意地方都可以隨時提供參數的值。故可以把Config寫成單例模式（Singleton），它只有一個全局對象，當我們設置參數文件時，創建該對象並讀取參數文件，隨後就可以在任意地方訪問參數值，最後在程序結束時自動銷燬。