作者：智林

引言

點云配準(點云配準初識)的目的是計算不同視角下若干組具有共視關系點云的空間變換關系(位置和姿態(tài))，從而將這些點云統(tǒng)一到一個坐標系下，進而得到被觀測物體的整體信息。那么，我們?nèi)绾斡嬎氵@種變換關系呢？或者說其對應的數(shù)學問題是怎么樣的呢？

其實，點云配準本質(zhì)就是一個優(yōu)化問題，從幾何關系來說就是使具有共視關系點云的空間距離足夠小，接近于零；從空間分布關系來說，就是最大化共視關系的點落在所在柵格的概率分布。因此，對于一個點云配準這個技術問題，我們需要定義優(yōu)化函數(shù)，運用優(yōu)化方法去求解所定義的優(yōu)化問題。所需要用到的數(shù)學知識包括，空間變換的表示，矩陣分析，優(yōu)化方法(常見的高斯-牛頓法，LM算法)，導數(shù)求解等等。

接下來的內(nèi)容就是分享其中的一部分數(shù)學基礎知識，其中優(yōu)化方法和導數(shù)求解建議大家先去看看高博的《視覺slam十四講》，這里可能就不展開了，因為一旦展開就太雜了，遲遲無法進入點云配準算法的講解(要講的東西實在太多太多)，所以大家自己先去學習一下吧！后面如果有機會，我在對這部分內(nèi)容詳細講解。其實，這兩部分是自動駕駛算法中的關鍵基礎知識，大家必須都要會，不然無論是看論文還是看代碼都是一臉茫然，快去自己看書學習，不懂的可以加入交流群一起討論交流。下面，我們就著重講解空間變換關系的表示及其參數(shù)化以及剛性配準的基本數(shù)學問題表達與求解，和對應的c++代碼。

空間變換及其參數(shù)化

我們已經(jīng)知道點云配準就是計算空間變換關系，也就是位置和姿態(tài)。那么，用什么方法表示位置和姿態(tài)呢？每個表示方法又有什么優(yōu)點和缺點呢？實際運用中我們?nèi)绾芜x擇呢？帶著這些有趣的問題，開始接下來的學習。

向量在三維空間中的表示方法

我們以后再沒有特別說明的情況下，討論的都是三維空間，所討論的點云配準也是三維點云配準。那么，我們首先要知道如何表示三維空間中的點，以及兩點連線構成的向量。可能，這里大家會有疑問，我知道怎么表示點不就行了嗎，為什么還要表示向量呢？原因是，點云配準需要求解的空間變換關系除了位置之外，還有姿態(tài)。點的信息只能刻畫位置，兩點連接所形成的向量就可以表示姿態(tài)，并且向量本身也可以表示位置，所以我們的核心實則是研究向量，其既包括位置信息又包括姿態(tài)信息。

在三維空間中，無論是研究點還是由點構成的向量，我們都必須要指定坐標系，只有在某個指定的坐標系下才能去研究點的表示以及向量的表示。

線性代數(shù)的知識告訴我們，只需要找到三維空間的一組基，就可以利用這組基表示三維空間下的任意向量。簡單回顧一下基的定義：

在線性代數(shù)中，一個向量空間 的基（basis）是指滿足以下兩個性質(zhì)的向量集合:

生成性質(zhì) (Spanning Property)：基中的向量能夠通過線性組合生成向量空間 中的任意向量。換句話說，對于任意 中的向量 ，存在基中的向量 和標量 使得

線性無關性質(zhì)（Linear Independence Property）：基中的向量之間不存在非零的線性組合使得結果為零向量。換句話說，如果 ，其中 是標量，且不全為零，則基中的向量線性相關。

假設我們已經(jīng)找到了三維空間的一組基 , 那么, 任意一個向量 在這組基下可以表示為:

這里 稱為 在此基下的坐標 。由此可見，坐標的具體取值，一是和向量本身有關,，二是和坐標系 (基) 的選取有關。通常情況下，坐標系由 3 個正交的坐標軸組成，也就是我們常說的笛卡爾坐標系的x，y，z軸，所選擇的基就是

在這里，為了方便后續(xù)對向量的討論，這里引出一個向量外積的概念，這個概念很簡單，但是在slam領域非常重要和實用。先簡單回顧一下高中學到的向量內(nèi)積是啥。對于任意的，內(nèi)積的定義為：

其中， 指向量 的夾角。雖然內(nèi)積的概念從高中就已經(jīng)知道了，但是那時候畢竟是為了應付高考，現(xiàn)在我們可以想一想這個有啥實際的工程或者學術價值。這里，我就拋磚引玉一下，內(nèi)積在實際工程中，可以計算其數(shù)值與零比較大小，判斷兩個向量的夾角是否超過90度，進而判斷移動的智能體是否發(fā)生空間位置的變化(大家好好琢磨，其中的一個向量是點的法向量，另外一個向量是當前位置與點連線構成的向量)。話不多時，趕緊來聊聊新的知識，向量的外積，向量外積的定義為：

外積的一個重要性質(zhì)是：外積的結果是一個向量，并且垂直于這兩個向量，其大小為。這里，有一個重要的符號引入，即，其作用就是將一個向量映射為一個矩陣，并且這個矩陣是一一映射（單射），這個矩陣是反對稱的，稱為反對稱矩陣。其定義為：對于任意向量，有：

同理，對于任意一個反對稱矩陣，我們有：。

坐標系間的歐式變換與旋轉矩陣

根據(jù)上面的分析，我們知道我們研究的最底層東西是向量，這個向量只有在定義了坐標系之后才有意義。那現(xiàn)在我們在回過頭想一想點云配準的核心是什么？點云配準是計算兩個具有共視關系點云的變換關系，這里的具有共視關系區(qū)域的每個點大家可以理解成向量，這些點(向量)是在不同時刻不同坐標系下定義的，因此我們所要求得坐標變換關系也就是坐標系的變換關系。

通過上面的分析，我想大家應該可以比較徹底的弄明白為啥書上講到點云配準，或者slam，一定要首先講解坐標系的歐式變換，只有理解了，才會覺得書上的內(nèi)容不是那么突兀和跳躍，而是非常合理的安排，哈哈。那下面我們就開始介紹坐標系的歐式變換，進而直接引出旋轉矩陣。

兩個坐標系之間的運動由一個旋轉加上一個平移組成,這種運動稱為剛體運動。由于剛體運動保持了向量的長度和夾角，相當于把一個剛體原封不動地進行移動或旋轉，不改變它自身的形狀，因此它是一種歐式變換(Euclidean Transform)。這里大家可能有這樣的疑問，夾角為啥沒有變化，我坐標系都發(fā)生旋轉了，向量的夾角怎么還沒有變化呢？沒有這樣的疑問最好，有這樣的疑問也沒事，因為只是絕大多數(shù)書上沒有怎么解釋這個夾角是什么意思。這里的夾角通常指的是兩個向量之間的角度。在不同坐標系中，兩個向量之間的夾角是不變的。這意味著，如果你有兩個向量 和，它們的夾角在剛體運動時會保持不變。無論剛體怎么運動，這兩個向量之間的夾角都不會發(fā)生變化。

這里借用高博書上的一個例子，剛好我自己最近也弄到了一臺mate60pro昆侖玻璃加持，摔不壞，但是我沒有嘗試過，哈哈哈。想象你把手機拋到空中、在它落地掉碎之前，只可能有空間位置和姿態(tài)的不同，而它自己的長度、各個面的角度等性質(zhì)不會有任何變化。手機并不會像橡皮那樣一會兒被擠扁，一會兒被拉長。此時，我們說手機坐標系到世界坐標之間，相差了一個歐氏變換。

有了這些基礎之上，我們終于可以引出旋轉矩陣了。假設某個單位正交基經(jīng)過了一次旋轉變成(,那么對于向量 而言，它在兩個坐標系下 (同一個點參考系不同) 的坐標分別為和。由于 本身沒有改變，因此：

對上面的式子做一個小小的變形，兩邊同時左乘，左邊直接變成了單位矩陣，因此有：

我們將中間的矩陣記為，其描述了同一個向量在旋轉前后(姿態(tài)發(fā)生變化前后)的坐標坐標變化關系，這里并沒有表達出位置的關系，因此稱為旋轉矩陣(Rotation Matrix)。

旋轉矩陣有一個非常重要的性質(zhì)：。

這里的證明大家可以看書，比較簡單，但是這個性質(zhì)非常非常重要，后面我們慢慢回味。旋轉矩陣的概念可以推廣到多維，將維旋轉矩陣的集合定義為：

式中 SO(n)表示特殊正交群，旋轉矩陣即描述了兩個坐標的姿態(tài)變換關系：

如果考慮平移，則可以更新表示為。因此兩個坐標系的剛體運動可以由和進行描述，但是該變換不是一個線性變換。假設我們進行了兩次坐標變換：，這里簡單說明一下， 是指“把坐標系 2 的向量變換到坐標系 1” 中，從右往左讀，這里借鑒了高博的十四講的做法，當然你也可以按照自己的習慣反過來都一樣。

那么，從 到 的變換為

這樣的形式在變換多次之后會顯得很不簡約。因此，引人齊次坐標和變換矩陣，重寫上式：

將原始的三維向最末尾增加1個維度，取值為1，該四維向量被稱為齊次坐標，而矩陣寫作，稱為變換矩陣(Transform Matrix)，這種類型的矩陣又被稱為特殊歐式群：

式中，為 3×3 的矩陣， 的逆表示進行反向的變換：

有了變換矩陣這個工具之后，我們就可以對上面連續(xù)進行兩次變換的疊加有一個簡單直觀的表示，用 表示 的齊次坐標。那么依靠齊次坐標和變換矩陣，兩次變換的疊加就可以有很好的形式：

旋轉向量/角軸/軸角

旋轉矩陣描述了物體運動的3自由度旋轉，在此基礎上引入了變換矩陣，描述了6自由度的三維剛體運動，看似已經(jīng)足夠了，我們已經(jīng)找到了點云配準所需計算的變換運動的表達形式。是的，的確是可以了，在計算的時候一般也是輸出和，然后將點云投影到統(tǒng)一的坐標系下，最終得到點云。

但是，通過矩陣表示方式存在兩個明顯的不足，分別是：

旋轉矩陣有9個量，但是一次選準只有3個自由度；變換矩陣有16個量，但是一次歐式變換只有6個自由度。因此，這種表示方式存在冗余，占用的內(nèi)存多。由此，是不是可以找到一種緊湊的表達形式呢？

上述分析中提到旋轉矩陣有一個重要的性質(zhì)：旋轉矩陣是行列式為1的正交矩陣。因此在后續(xù)估計或者優(yōu)化一個旋轉矩陣時，我們必要要保持這一性質(zhì)，這樣會讓求解變得麻煩困難。

首先，可以肯定的說，存在一種緊湊的方式來描述平移和旋轉，用一個三維向量表達旋轉，用一個六維的向量表達變換。事實上，任意旋轉都可以用一個旋轉軸和一個旋轉角來刻畫。于是，我們可以使用一個向量，其方向與旋轉軸一致，而長度等于旋轉角。這種問量稱為**旋轉向量(或軸角/角軸，Axis-Angle)**，只需一個三維向量即可描述旋轉。同樣，對于變換矩陣，我們使用一個旋轉向量和一個平移向量即可表達一次變換。這時的變量維數(shù)正好是六維。具體表達形式為：

式中， 與旋轉軸方向一致的單位向量， 為角度。

乍一看，這種表示方式好極了，表示6自由度的變換恰到好處非常緊湊。但是，旋轉向量也存在缺點：

使用軸角表示方式無法表達兩次連續(xù)的旋轉，由于兩次旋轉的旋轉軸會不一樣，因此旋轉角度不能直接相加。

當旋轉角度為 0°或者 180°時，羅德里格斯公式失效，此時的旋轉矩陣為 0。可以理解為這種情況下旋轉角具有無數(shù)多種情況。由于旋轉矩陣到軸角表示法的映射關系是一對多的關系，因此存在多種軸角組合對應一種旋轉矩陣的情況。

這里簡單解釋一下，因為后面我們單獨會講各種變換關系之間的轉化，其中羅德里格斯公式也會詳細介紹，這里直接給出：

羅德里格斯公式描述的是旋轉向量到旋轉矩陣的轉換關系，當或者時，。

歐拉角

通過上述分析知道，旋轉矩陣和旋轉向量都可以表示旋轉，但各自又都有缺點，不是很完美。除了已經(jīng)談到自身固有的缺點之后，它們兩還有一個非常大的bug，那就是不直觀。給你一個旋轉矩陣或者旋轉向量在你面前，比較難去想像這個旋轉究竟是什么樣的，或者比較簡單的旋轉向量還可以，但是一旦旋轉軸復雜一些，就懵了。那下面就介紹一個非常直觀的表示旋轉的一種方式------歐拉角。

歐拉角使用了3個分離的轉角，把一個旋轉分解成 3 次繞不同軸的旋轉。對于我們來說，很容易理解繞單個軸旋轉的過程。但是，這里也帶來一定的問題，由于分解方式有許多種，所以歐拉角也存在著眾多不同的、易于混淆的定義方法。例如，先繞 軸，再繞 軸，最后繞 軸旋轉，就得到了一個 軸的旋轉，同理也可以得到，，甚至可以定義，等等。

這種定義方式上的不確定性帶來了很多實際當中的困難，但是在某一個特定的領域，歐拉角通常有統(tǒng)一的定義方式。在航天航空領域，歐拉角便有一種約定俗成的定義方式，便是用“偏航- 俯仰-滾轉"(yaw-pitch-roll)3個角度來描述一個旋轉。它等價于 軸的旋轉，因此就以 為例。假設一個剛體的前方( 朝向我們的方向)為 軸，右側為軸，上方為軸，如下圖所示。那么， 轉角相當于把任意旋轉分解成以下 3 個軸上的轉角：

繞物體的 軸旋轉，得到偏航角 yaw。

繞旋轉之后的軸旋轉，得到俯仰角 pitcho

繞旋轉之后的 軸旋轉，得到滾轉角 roll。

此時，可以使用 這樣一個三維的向量描述任意旋轉，并且這個非常直觀。所以在跨行業(yè)或者跨模塊協(xié)作的時候，一定要問清楚對方是哪一種歐拉角。

歐拉角一般可以定義為靜態(tài)和動態(tài)兩種形式。在介紹靜態(tài)和動態(tài)兩種形式之前，首先定義繞各個旋轉軸轉相應角度時所應對的選準矩陣。

當繞軸旋轉 角度時對應的旋轉矩陣為：

當繞軸旋轉 角度時對應的旋轉矩陣為：

當繞軸旋轉 角度時對應的旋轉矩陣為：

這三個矩陣怎么得到的，大家可以去看我b站視頻，因為這里畫圖太麻煩了，實則非常簡單，大家直接百度也可以搜到。那下面就來介紹靜態(tài)和動態(tài)歐拉角。

所謂靜態(tài)的歐拉角指繞世界坐標系三個固定軸的旋轉，旋轉過程中世界坐標系的三個軸不發(fā)生變化，也稱為外旋旋轉方式。如圖 2-2 所示，按照外旋方式，X-Y- 旋轉順序 (指先繞固定軸X 旋轉 α, 再繞固定軸 Y 旋轉 , 最后繞固定軸 旋轉 ), 這種形式的歐拉角也稱為 RPY 角，α、β和γ對應航空領域的 Roll、Pitch 和 Yaw。最終可得旋轉矩陣

而動態(tài)歐拉角是指繞物體坐標系三個軸的旋轉，由于物體旋轉過程中坐標軸隨著旋轉變換運動，也稱為內(nèi)旋旋轉方式。如圖 2-3 所示，此時若按照 的順序依次旋轉 、 和 α 的角度，這種形式的歐拉角也稱為 歐拉角，則最終可得旋轉矩陣

這里要注意：內(nèi)旋時繞自身坐標系旋轉，右乘坐標向量，坐標系(基底)在變換， 變換；外旋時繞固定坐標系旋轉，左乘坐標向量，坐標(向量) 在變換，行變換。這種情況下如果使用 RPY 角進行描述，即繞物體的 軸旋轉 , 得到偏航角 Yaw, 繞旋轉之后的 Y軸旋轉,得到俯仰角 Pitch, 繞旋轉之后的 軸旋轉 , 得到滾轉角 Roll。因此 Yaw、Pitch 和Roll 分別對應 、 和

通過上述分析，可以看出來歐拉角非常直觀，便于立即。但是這個方法同樣也存在缺點，即著名的萬向鎖(或萬向節(jié)死鎖)(Gimbal Lock)問題：在俯仰角為 時， 第一次旋轉與第三次旋轉將使用同一個軸，使得系統(tǒng)丟失了一個自由度(由3次旋轉變成了2次旋轉)。這被稱為奇異性問題，在其他形式的歐拉角中也同樣存在。理論上可以證明，只要想用3 個實數(shù)來表達三維旋轉，都會不可避免地碰到奇異性問題。由于這種原理，歐拉角不適用于插值和迭代，往往只用于人機交互中。我們也很少在 SLAM 程序中直接使用歐拉角表達姿態(tài)，同樣不會在濾波或優(yōu)化中使用歐拉角表達旋轉(因為它具有奇異性)。不過，若你想驗證自己的算法是否有錯，轉換成歐拉角能夠幫你快速分辨結果是否正確。

四元數(shù)

四元數(shù)最早于 1843 年由哈密頓（William Rowan Hamilton ) 發(fā)明，作為復數(shù) ( Complex Numbers) 的擴展。直到1985 年才由 Shoemake 把四元數(shù)引入到計算機圖形學中。四元數(shù)在一 些方面優(yōu)于歐拉角、軸角和旋轉矩陣，比如解決萬向鎖問題。由于僅需存儲4 個浮點數(shù)，相比矩陣更加輕量，使得四元數(shù)解決求逆、串聯(lián)（多個變換的疊加變換）等操作，相比矩陣更加高效。總結一下，四元數(shù)具有一下兩個良好的性質(zhì)：一是其表示旋轉是緊湊的；二是其不存在奇異性。

這里應用高博十四講中的做法引出單位四元數(shù)可以表示三維旋轉。例如，當我們想要將復平面的向量旋轉 角時，可以給這個復向量乘以 e。這是極坐標表示的復數(shù)，它也可以寫成普通的形式，只要使用歐拉公式即可：

這正是一個單位長度的復數(shù)。所以，在二維情況下，旋轉可以由單位復數(shù)來描述。類似地，我們會看到，三維旋轉可以由單位四元數(shù)來描述。

一個四元數(shù)由一個實部和三個虛部構成，表達形式如下：

其中，,,為四元數(shù)的三個虛部。這三個虛部滿足以下關系式：

為了方便表示四元數(shù)，通常用一個標量和一個向量來表達四元數(shù)：

這里， 稱為四元數(shù)的實部，而 稱為它的虛部。如果一個四元數(shù)的虛部為 0, 則稱為實四元數(shù)；反之，若它的實部為0, 則稱為虛四元數(shù)。

四元數(shù)也可以進行各種運算，例如：加法。乘法、共軛，求逆，數(shù)乘等等，這些也都是我們工程或者學習中經(jīng)常使用。這里直接給出其結論：

對四元數(shù)有了初步的了解之后，下面我們來考慮四元數(shù)如何表示旋轉？

可以用四元數(shù)表達對一個點的旋轉。假設有一個空間三維點 。以及一個由單位四元數(shù) 指定的旋轉。三維點 經(jīng)過旋轉之后變?yōu)?。如果使用矩陣描述，那么有。而如果用四元數(shù)描述旋轉，它們的關系又如何表達呢？

首先，把三維空間點用一個虛四元數(shù)來描述：

相當于把四元數(shù)的 3 個虛部與空間中的 3 個軸相對應。那么，旋轉后的點 可表示為這樣的乘積：

這里的乘法均為四元數(shù)乘法，結果也是四元數(shù)。最后把的虛部取出，即得旋轉之后點的坐標。這是為什么呢？因為上述公式的計算結果的實部為 0, 故為純虛四元數(shù)。

關于四元數(shù)更多的知識，以及上述我所提到的知識的深入講解，大家可以參考論文《Quaternion kinematics for the error-state Kalman filter》。

總結

這一小節(jié)我們主要介紹了如何表示三自由度的旋轉，講解了四種表示方式，分別為：旋轉矩陣，旋轉向量，歐拉角和四元數(shù)，它們各有其優(yōu)勢和不足，大家可以自己思考一下，回顧一下自己是否掌握了。現(xiàn)在我們已經(jīng)解決了一個非常棘手的問題，就是用什么工具來表示點云配準中的變換關系。但是，我們有面臨一個苦惱，這么多方法我怎么選擇呀？或者說我計算出其中一個能不能得到另外三個？答案是肯定的，它們之間可以相互轉換，那一節(jié)我們將探究空間變換的不同表示方法之間的互相轉換。

參考資料

[1] 高翔等，視覺slam十四講[M].

[2] 郭浩，點云配準從入門到精通[M].

[3] Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.

編輯：黃飛