作者：智林

引言

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

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

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

空間變換及其參數化

我們已經知道點云配準就是計算空間變換關系，也就是位置和姿態。那么，用什么方法表示位置和姿態呢？每個表示方法又有什么優點和缺點呢？實際運用中我們如何選擇呢？帶著這些有趣的問題，開始接下來的學習。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

那么，從 到 的變換為

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

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

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

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

旋轉向量/角軸/軸角

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

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

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

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

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

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

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

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

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

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

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

歐拉角

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

四元數

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

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

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

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

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

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

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

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

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

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

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

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

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

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

總結

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

參考資料

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

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

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

編輯：黃飛