如何理解視覺SLAM視覺里程計VO：如何從圖片中還原計算出相機的三維位姿算法教程Python代碼實踐

視覺里程計是什麼？視覺里程計有什麼用？需要我們求解什麼？

視覺里程計（Visual Odometry,VO） 算法是什麼？視覺SLAM中的視覺里程計算法就是指的是通過某個算法根據前後兩張圖片求出當前時刻內相機位置和姿態相對上個時刻相機位置和姿態的變化的那種算法。這個就和出租車的里程計一樣記錄每個時刻的車輪速度及其滾動距離類似。只不過視覺里程計算法是求相機在各個時刻的位置姿態，所以才叫做視覺里程計算法。

視覺里程計有什麼用？我們如果將最開始那個時刻的相機位置當做世界座標系。現在我們使用視覺里程計算法計算出了相機各個時刻的位姿變化，這意味着我們得到了相機相對於世界座標的位置。這個有什麼用呢？有兩個用途：1. 定位，現在我們知道了相機相對於世界座標的位置，而相機是和機器人綁在一起的，這不就知道了機器人在哪了麼？ 2. 三維重建（建圖），現在很多相機是能夠測出當前時刻環境中各個物體離相機自己的距離（Lidar, RGB-D和雙目相機都能測距離），而視覺里程計算法是已經計算出相機自己相對世界座標的距離。這就可以計算出環境中各個物體相對世界座標的距離。這不就建好圖了麼？

前面已經從直觀上描述了視覺SLAM的核心思想——視覺里程計算法。有了視覺里程計算法我們就能知道相機在哪（相機與機器人/汽車綁定的所以也知道了機器人/汽車在哪）。有了視覺里程計算法我們就能對環境進行三維重建。前面提到了視覺里程計算法是一種能夠計算出相機在各個時刻相對於前一個時刻的位置和姿態的變化的算法。那麼問題來了位置和姿態的變化到底它在數學上是如何量化的？

我們知道任何剛體的運動包含兩個部分一個是旋轉一個是平移。旋轉的變化量對應的是旋轉矩陣$\bold R$，平移量對應的是一個向量$t$。所以視覺里程計算法就是想根據前後兩個時刻拍出的照片中的一些特徵點之間的約束關係求$R$和$t$。

舉個例子：現在某個自動駕駛的汽車上有個雷達。我們將雷達最開始開機的地方作爲世界座標系。現在是開機的下一個時刻，當前時刻用雷達對環境掃描，於是我們就獲得了一系列點雲。注意這些點雲的座標是相對於當前雷達的座標。爲了對環境建圖我需要計算出這些點雲相對於世界座標系下的座標，然後將他們保存這就建好了地圖。那麼這和視覺里程計算法有什麼聯繫呢？通過視覺里程計算法我們能計算出當前雷達相對於上個時刻的旋轉矩陣$R$和平移$t$。前面提到了要想建一個地圖那就得計算出掃描出的點相對於世界座標系下的座標，那麼如何根據這兩個數據計算出點雲中各個點相對於世界座標系下的座標呢？假設點雲中某個點的座標是$P$，那麼這個點相對於世界座標系下的座標是$RP+t$。我們將這個結果保存就得到了地圖。

我想現在你已經知道視覺里程計算法有多麼重要了。也知道視覺SLAM的基本思想了（其實SLAM並不是指某種算法而是一套處理數據的流程）。也知道視覺里程計算法就是用來求出前後兩個時刻相機位置和姿態的變化量。那麼問題來了如何計算當前時刻相對上個時刻的位置和姿態的變化量?

如何從前後兩張圖片中還原計算出相機的三維位姿變化量？

$$