視覺SLAM的數學基礎 第一篇 3D空間的位置表示

視覺SLAM中的數學基礎 第一篇 3D空間的位置表示

前言

　　轉眼間一個學期又將過去，距離我上次寫《一塊兒作RGBD SLAM》已經半年之久。《一塊兒作》系列反響很不錯，主要因爲它爲讀者提供了一個能夠一步步編碼、運行的SLAM程序，爲讀者理解SLAM實現的細節做了詳細的介紹。可是我也有不少對它不滿意的地方。做爲面向實現的介紹，它的代碼不夠穩定可靠，例如，甚至沒有對匹配丟失的狀況進行處理，於是只能用於教學。另外一方面，對SLAM研究者來講，我只是介紹了編碼方面如何調用一些常見的庫函數，而沒有對這些函數進行深刻的，原理上的講解。這就致使了讀者只瞭解了函數的接口，而無法根據數學原理進行創新。歸根到底，研究機器人相關問題，一是要有紮實的數學基礎，二是要有強大的動手編程能力，這對大多數剛入門的研究者來講，極具挑戰性。我也但願，經過閱讀個人博客，你能走進SLAM研究的門檻，有朝一日本身也寫出優秀的程序和論文。

　　有鑑於此，我準備寫一寫SLAM相關的數學知識，包括代數、幾何、機率、運籌等等。對於重要的算法例如ICP，EKF，細緻討論它的原理，並給出它的實現（原生的代碼或在某個庫的實現）。因爲它們的原理較複雜，我會從最基本的東西開始講起。可是我畢竟不是在寫數學書，我不會像數學書那樣寫成``定義——定理——推論」的結構。咱們不會糾纏於一些定理的嚴格證實，相反的，咱們只在必要的狀況下加以說明，告訴讀者這些數學公式在SLAM中有何應用，如何應用。

　　因爲博客編輯器的限制，咱們以斜體字$x$表示變量，以粗正體$\mathbf{A}$表示矢量和字母，以黑板粗體$\mathbb{R}$表示空間。希臘字母沒有粗體因此保持原樣。向量默認爲列向量。其他和普通的數學書一致。 

　　小蘿蔔：師兄，這麼嚴肅不是你的風格啊！

　　師兄：啊，數學嘛……

---

 

剛體運動

　　本篇咱們討論一個很基礎的問題：如何描述機器人的位姿。這也是研究SLAM的第一個問題。注意這裏「位姿」的用語包含了位置和姿態。描述位置是很簡單的。若是機器人在平面內運動，那麼用兩個座標來描述它的位置：$$ \mathbf{x} = \left[ x, y \right]^T.$$ 相應的，若是它在三維空間中，咱們就用三個空間座標來表示：$$ \mathbf{x} = \left[ x, y, z \right].$$

　　姿態的表達比點稍爲複雜。2D的姿態能夠只用一個旋轉角$\theta$表達。3D姿態的表達方式則有多種。常見的如歐拉角、四元數、旋轉矩陣等。咱們將在後文詳細介紹。有了位置和姿態，咱們就能夠描述一個座標系。進一步，還能描述座標系間的變換關係。常見的問題如：機器人視野中某個點，對世界座標系的（或地圖的）哪一個點？這時，就須要先獲得該點針對機器人座標系座標值，再根據機器人位姿轉換到世界座標系中。 

齊次座標系

　　在位姿轉換中，一般採用射影空間的齊次座標表示。齊次座標是什麼呢？記$n$維射影空間爲$\mathbb{P}^n$，其中一個空間點的座標爲普通的3D座標加一個齊次份量：$$ \mathbf{x}=\left[ x_1, \ldots, x_n, w \right]^T. $$ 例如，在2維和3維射影空間中的點，分別表示爲：

\[\begin{equation}
\mathbf{x}_{2D} = \left[x, y, w \right]^T \quad \mathbf{x}_{3D} = \left[ x, y, z, w \right]^T
\end{equation}\]

　　小蘿蔔：既然一個空間點只有3個座標，爲啥非要用四個數表示呢？

　　師兄：嗯，四個數表示點，說明點和座標確定不是一一對應的。沒錯，在齊次座標中，某個點$\mathbf{x}$的每一個份量同乘一個非零常數$k$後，仍然表示的是同一個點。所以，一個點的具體座標值不是惟一的。如$\left[1,1,1,1\right]^T$和$\left[2,2,2,2\right]^T$是同一個點。但在$w \neq 0$時，咱們能夠對每一個座標除以最後一項$w$，強制最後一項爲1，從而獲得一個點惟一的座標表示：

\[\begin{equation}
\mathbf{x}_{3D} = \left[ x/w, y/, z/w, 1 \right]^T
\end{equation}\]

　　這時，忽略掉最後一項，這個點的座標和歐氏空間就是同樣的。那麼，爲要用齊次座標呢？緣由有如下幾條。

• 齊次座標下點和直線（高維空間裏爲超平面）可以使用一樣的表達。

　　例如，3D空間$\mathbb{R}^3$中，一個平面$\pi$可由一個方程定義：
\[\begin{equation}
\pi : ax + by + cz + d = 0
\end{equation}\]

　　則該平面$\pi$能夠用$\mathbb{P}^3$中的座標$\mathbf{\pi} = \left[ a,b,c,d \right]^T$來描述。這樣，點位於平面上（2D對應點位於直線上）的事情能夠簡潔地表示爲：
\[\begin{equation}
\label{eq:pointOnPlane}
\mathbf{\pi}^T \mathbf{x} = 0.
\end{equation}\]

　　把點和超平面採用一樣的表示，這種作法一個很是直接的好處，是射影幾何裏的「對偶原理」。該原理是說，任何有關「點」與「平面」的命題，均可以交換「點」與「平面」的概念，獲得一個對偶的命題。對偶命題和原命題是同樣的。經過「對偶原理」，射影幾何的數學家就能夠偷懶，只須要證一半定理，由於對偶命題和原命題有一樣的涵義。例如，咱們證實了$\mathbb{P}^2$中某條件下三點共線，那麼替換概念後的三線共點則天然成立。

　　小蘿蔔：數學家真是好懶啊！

• 齊次座標能囊括無窮遠點與無窮遠超平面。

　　最後一項座標爲零的點稱爲無窮遠點，它們在$\mathbb{P}^n$中真實存在，且可以很方便地參與正常的代數運算。根據式\ref{eq:pointOnPlane}，易見全部無窮遠點都在一個平面$\mathbf{\pi}_\infty = \left[ 0, 0, 0, 1\right]^T$上，該平面記做無窮遠平面（2D對應無窮遠直線）。
　　$\mathbb{P}^2$中的無窮遠直線較容易理解。它就像是地平線，與全部直線相交於位於它之上的無窮遠點。並且，在射影變換中（例如照相），很容易在照片中看到地平線並算出它的方程。這說明2D射影變換會把無窮遠線變成一般的直線。

• 齊次座標能夠方便地將平移與旋轉放在一個矩陣中。

　　師兄：這應該是最明顯的好處啦！你們都愛用齊次座標，包括我。有關座標系怎麼用齊次座標進行變換，後文會詳細解釋。如今咱們能表達點了，還剩下一個姿態。因爲2D與3D差異較大，咱們分而述之。

---

 2D姿態的描述

　　2D空間中，物體的位姿可用兩個平移量$t_x, t_y$加一個旋轉角$\theta$表示，以下圖所示。此時，設機器人座標系$O'-X'-Y'$下某點的座標爲$\left[ x_r, y_r\right]^T$，對應在世界座標系$O-X-Y$下爲$\left[x_w, y_w\right]^T$，那麼由直觀推得：

\[\begin{equation}
\left\{ \begin{array}{l}
{x_w} = {x_r}\cos \theta - {y_r}\sin \theta + {t_x}\\
{y_w} = {x_r}\sin \theta + {y_r}\cos \theta + {t_y}
\end{array} \right.
\end{equation}\]

　　讀者能夠自行嘗試推導一下，在虛線處創建一箇中間座標系便可。若將該式寫成矩陣形式，則有：

\[\begin{equation}
\label{eq:2dTransRT}
\mathbf{x}_w = \mathbf{R} \mathbf{x}_r + \mathbf{t}
\end{equation}\]

　　其中
$$\mathbf{R} = \left[ {\begin{array}{*{20}{c}}
{\cos \theta }& - \sin \theta \\
{\sin \theta }&{\cos \theta }
\end{array}} \right], \quad \mathbf{t} = \left[ t_x, t_y\right]^T$$
$\mathbf{R}$稱爲旋轉矩陣，$\mathbf{t}$爲平移矢量。注意到$\mathbf{R}$是一個正交矩陣，且只有一個自由度。加上平移矢量後，一共有三個自由度。

　　此定義下的旋轉陣必是正交陣。而正交陣並不是全是旋轉矩陣。事實上，行列式爲$+1$的正交陣纔是旋轉矩陣，行列式爲$-1$的正交陣是鏡像後的旋轉矩陣。

　　式\ref{eq:2dTransRT}中，$\mathbf{x}_w$和$\mathbf{x}_r$還不是線性關係。下面咱們用齊次座標表示它們，即：
$$ \mathbf{x}_w = \left[ x_w, y_w, 1\right]^T, \mathbf{x}_r = \left[ x_r, y_r, 1\right]^T $$
則有：

\[\begin{equation}
{\mathbf{x}_w} = \left[ {\begin{array}{*{20}{c}}
\mathbf{R}_{2 \times 2} & \mathbf{t}_{2 \times 1 }\\
\mathbf{0}^T_{1 \times 2} & I_{1 \times 1}
\end{array}} \right]{\mathbf{x}_r}
\end{equation}\]

　　爲便於理解，咱們在矩陣下方標出了它的維數。能夠看使用齊次座標標知足了線性關係，記做：
\[\begin{equation}
\mathbf{x}_w = \mathbf{T}_{w,r} \mathbf{x}_r
\end{equation}\]
　　其中$\mathbf{T}_{w,r}$表示從世界座標系到機器人座標系的變換矩陣。咱們也能夠輕鬆地寫出反向的變換矩陣：

\[\begin{equation}
\mathbf{x}_r = \mathbf{T}_{r,w} \mathbf{x}_w = \mathbf{T}^{-1}_{w,r} \mathbf{x}_w = \left[ {\begin{array}{*{20}{cc}}
{{\mathbf{R}^{ - 1}}}&{{-\mathbf{R}^{ - 1}} \mathbf{t}}\\
{{0^T}} & 1
\end{array}} \right] \mathbf{x}_w
\end{equation}\]

　　既然如此，咱們就可用$\mathbf{T}$表示機器人的位姿，那麼機器人在時刻$t$的位姿就能夠記做$\mathbf{T}_t$。固然，從存儲上來說，存儲$\mathbf{T}$是不經濟的。在2D運動中，它有九個變量，但實際自由度只有三個。因此咱們能夠只存儲位移矢量$\mathbf{t}$與旋轉角$\theta$，而在須要計算的時候再構建出$\mathbf{T}$。稱2D歐幾里得變換，它對矩陣乘法構成羣(羣是一個集合加一種運算，且運算在該集合上知足封閉性、結合律、有單位元和逆元。)，該羣記做$SE(2)$。相應的，二維旋轉構成二維旋轉羣(或稱特殊正交羣)$SO(2)$。有關它們進一步的性質，咱們會在之後的李羣、李代數中提到。

---

 3D變換

　　3D的旋轉能夠由旋轉矩陣、歐拉角、四元數等若干種方式描述，它們也統稱爲三維旋轉羣$SO(3)$。而3D的變換即旋轉加上位移，是爲$SE(3)$。爲了和2D變換統一塊兒見，咱們首先介紹旋轉矩陣表示法。

旋轉矩陣描述

　　旋轉矩陣是一種$3\times 3$的正交矩陣，它對變換的描述十分相似於2D情形。參照上一節的數學符號，咱們有：
\[\begin{equation}
{\mathbf{x}_w} = \left[
{\begin{array}{*{20}{c}}
\mathbf{R}_{3 \times 3} & \mathbf{t}_{3 \times 1 }\\
\mathbf{0}^T_{1 \times 3} & 1_{1 \times 1}
\end{array}} \right]{\mathbf{x}_r}
\end{equation}\]

　　這裏$\mathbf{R}$爲3D的旋轉矩陣，一樣的，$\mathbf{t}$爲3D的平移矢量。

　　因爲3D旋轉均可以歸結成按照某個單位向量$\mathbf{n}$進行大小爲$\theta$的旋轉。因此，已知某個旋轉時，能夠推導出對應的旋轉矩陣。該過程由羅德里格斯公式代表，因爲過程比較複雜，咱們在此不做贅述，只給出轉換的結果：
\[\begin{equation}
\mathbf{R}(\mathbf{n}, \theta) = \left[ {\begin{array}{*{20}{c}}
{n_x^2\left( {1 - c\theta } \right) + c\theta }&{{n_x}{n_y}\left( {1 - c\theta } \right) + {n_z}s\theta }&{{n_x}{n_z}\left( {1 - c\theta } \right) - {n_y}s\theta }\\
{{n_x}{n_y}\left( {1 - c\theta } \right) - {n_z}s\theta }&{n_y^2\left( {1 - c\theta } \right) + c\theta }&{{n_y}{n_z}\left( {1 - c\theta } \right) + {n_x}s\theta }\\
{{n_x}{n_z}\left( {1 - c\theta } \right) + {n_y}s\theta }&{{n_y}{n_z}\left( {1 - c\theta } \right) - {n_x}s\theta }&{n_z^2\left( {1 - c\theta } \right) + c\theta }
\end{array}} \right]
\end{equation}\]

　　這裏$\mathbf{n} = \left[ n_x, n_y, n_z\right]^T, c\theta=\cos \theta, s\theta=\sin \theta$。公式雖然較爲複雜，但實際寫成程序後，只需知道旋轉方向和角度後便可完成計算。另外一件有趣的事是，若是用$$\mathbf{n}^{\wedge} = \left[ {\begin{array}{*{20}{c}}
0&{ - {n_z}}&{{n_y}}\\
{{n_z}}&0&{ - {n_x}}\\
{ - {n_y}}&{{n_x}}&0
\end{array}} \right] $$表示與$\mathbf{n}$對應的一個反對稱矩陣，那麼有：

\[\begin{equation}
\mathbf{R} (\mathbf{n}, \theta ) = \cos \theta \mathbf{I} + (1-\cos \theta ) \mathbf{n} \mathbf{n}^T + \sin \theta \mathbf{n}^{\wedge} = \exp ( \theta \mathbf{n}^{\wedge} )
\end{equation}\]

　　最後那個指數，讀者若不理解，能夠暫時無論它，這將在以後的李代數中會講到。根據此式，咱們也能夠從任意給定的旋轉矩陣，求出對應的轉軸與轉角。關於轉角$\theta$，咱們對上式兩邊求矩陣的跡，可得：

\[\begin{equation}
\begin{array}{lll}
tr\left( \mathbf{R} \right) &=& \cos \theta tr\left( \mathbf{I} \right) + \left( {1 - \cos \theta } \right)tr\left( { \mathbf{n} {\mathbf{n}^T}} \right) + \sin \theta tr({\mathbf{n}^ \wedge })\\
&=& 3\cos \theta + (1 - \cos \theta )\\
&=& 1 + 2\cos \theta
\end{array}
\end{equation}\]

　　所以：
\[\begin{equation}
\theta = \arccos ( \frac{1}{2} [ tr(\mathbf{R}) - 1 ] )
\end{equation}\]

　　關於轉軸$\mathbf{n}$，因爲旋轉軸上的向量在旋轉後不發生改變，說明$$\mathbf{R} \mathbf{n} = \mathbf{n}. $$

　　所以，只要求此方程的解向量便可。這也說明$\mathbf{n}$是$\mathbf{R}$特徵值爲$1$的一個特徵向量。

　　總之，讀者應當明白在3D時，旋轉和平移仍可用轉移矩陣$\mathbf{T}$來描述，其結構也與2D相似。而$\mathbf{T}_{4\times 4}$構成了三維歐氏變換羣$SE(3)$。注意到$\mathbf{T}$雖然有16個變量，但真正的自由度只有6個，其中3個旋轉，3個位移。

　　旋轉矩陣描述是一種比較適合數學推導及計算機實現的方式，但它不符合人類的思惟方式。當你看到一個$3 \times 3$的矩陣時，很難想象物體實際上發生了怎樣的旋轉。反之，給定一個旋轉方式，人也很難直接寫出矩陣的數值。因此，爲了便於人類理解，人們還使用了其餘方法來表示三維旋轉。

歐拉角

　　歐拉角是一種廣爲使用的姿態描述方式，以直觀見長。在最經常使用的歐拉角表達方式中，咱們把旋轉分解成沿三個軸轉動的量：滾轉角－俯仰角－偏航角(roll-pitch-yaw)。它的好處是十分的直觀，且只有三個參數描述。缺點是會碰到著名的萬向鎖問題：在俯仰爲$\pm 90 ^\circ $時，表達某個姿態的形式不惟一。此外，它也不易於插值和迭代。

　　咱們並不會詳細介紹歐拉角，由於它在SLAM中用處也頗有限。咱們既不會在數學推導中使用它，也不會在程序中用歐拉角表示機器人的姿態。不過，在您想驗證本身算法輸出的姿態是否有錯時，轉換成歐拉角能讓你快速辨認結果是否有錯。

 四元數

　　四元數原理和各類運算將在下一篇博客中提到。

---

本篇小結

　　本篇博客介紹了2D和3D空間中剛體運動的表示方法，以旋轉矩陣爲主。下一篇咱們將介紹四元數表示法，而後演示如何用Eigen3對這些矩陣進行操做。敬請期待。

　　若是你以爲個人博客有幫助，能夠進行幾塊錢的小額贊助，幫助我把博客寫得更好。