1.旋轉矩陣

2.旋轉向量和歐拉角

3.四元數

1.旋轉矩陣

1.1點向量座標系

向量是空間中存在的有方向有長度的一樣東西，向量不等於座標，不同座標系下向量的座標表示也不盡相同。只有確定了一個座標系，我們才能說這個向量的座標是多少。

${\pmb a} =\left [ {\pmb e_{1}}, {\pmb e_{2}}, {\pmb e_{3}} \right ] \begin{bmatrix} a_{1}\\ a_{2}\\a_{3} \end{bmatrix} =a_{1}{\pmb e_{1}}+a_{2} {\pmb e_{2}}+a_{3}{\pmb e_{3}} \ \eqno(3.1)$

向量的外積運算

${\pmb a}\times {\pmb b}=\begin{bmatrix}{\pmb i}&{\pmb j}&{\pmb k}\\ a_{1}&a_{2}&a_{3}\\ b_{1}&b_{2}&b_{3}\end{bmatrix}=\begin{bmatrix}a_{2}b_{3}-a_{3}b_{2}\\ a_{3}b_{1}-a_{1}b_{3}\\a_{1}b_{2}-a_{2}b_{1}\end{bmatrix}=\begin{bmatrix}0&-a_{3}&a_{2}\\ a_{3}&0&-a_{1}\\ -a_{2}&a_{1}&0 \end{bmatrix} {\pmb b}\triangleq {\pmb a}^{\Lambda }{\pmb b} \ \eqno(3.2)$

其中， $^{\Lambda }$ 代表反對稱矩陣，即 ${\pmb a}=\begin{bmatrix}a_{1}\\ a_{2} \\ a_{3} \end{bmatrix} and \ \ {\pmb a}^{\Lambda }=\begin{bmatrix}0&-a_{3}&a_{2}\\ a_{3}&0&-a_{1}\\ -a_{2}&a_{1}&0 \end{bmatrix} \ \eqno(3.3)$

外積可以表示旋轉，利用右手法則，大拇指朝向就是旋轉向量的方向，事實也是 ${\pmb a}\times {\pmb b}$ 的方向。所以 ${\pmb a}$ 到 ${\pmb b}$ 的旋轉就可以由大拇指朝向的向量 ${\pmb \omega }$ 來表示，也就是說，在這個座標系下，旋轉可以用三個實數來描述（這三個實數就是 ${\pmb \omega }$ 的座標）

1.2座標系間的歐式變換

${\pmb a} =\left [ {\pmb e_{1}}, {\pmb e_{2}}, {\pmb e_{3}} \right ] \begin{bmatrix} a_{1}\\ a_{2}\\a_{3} \end{bmatrix} =\left [ {{\pmb e_{1}}}', {{\pmb e_{2}}}',{{\pmb e_{3}}}' \right ] \begin{bmatrix} {a_{1}}' \\ {a_{2}}' \\{a_{3}}'\end{bmatrix} \ \eqno(3.4)$

同一個向量在不同座標系下有不同的座標表示，但這都表示的是一個向量，這個向量是固有存在的。

現在將上式（3.4）左乘 $\begin{bmatrix} e_{1}^{T}\\ e_{2}^{T}\\ e_{3}^{T} \end{bmatrix}$ ,得到

式 (3.5) 中的 ${\pmb R}$ 爲旋轉矩陣，旋轉矩陣 ${\pmb R}$ 的特殊性質：

旋轉矩陣是行列式爲 1 的正交矩陣（旋轉矩陣的逆爲自身轉置，逆矩陣 $\pmb R^{-1}$ 代表一個相反的旋轉）

旋轉矩陣的集合稱之爲特殊正交羣

${\bf SO}(n)=\left \{\pmb R\in \mathbb{R}^{n\times n} }\Big| \pmb R \pmb R^{T}={ \emph I} &, det( {\pmb R})=1|\right \} \ \eqno(3.6)$

1.3 變換矩陣和齊次變換

之前的變換表達式 ${{\pmb a}}'={\pmb R\pmb a+\pmb t} \ \eqno(3.7)$ ,如果發生多次變換， ${{\pmb a}}''={\pmb R_{2}} \left ( {\pmb R_{1} \pmb a+\pmb t_{1}} \right ) +{\pmb t_{2}} \ \eqno(3.8)$ (2次)，這樣使得計算過於麻煩，所以引入一個自由度，利用齊次座標和變換矩陣重寫（3.7）

$\begin{bmatrix} {{\pmb a}}' \\ 1 \end{bmatrix} =\begin{bmatrix} {\pmb R} & {\pmb t}\\ 0^{T} &1\end{bmatrix} \begin{bmatrix} {\pmb a} \\ 1 \end{bmatrix} \triangleq {\emph T} \begin{bmatrix} {\pmb a} \\ 1 \end{bmatrix} \ \eqno(3.9)$ 式子中的 ${\emph T}$ 稱爲變換矩陣，這種矩陣又稱爲特殊歐式羣（Special Euclidean Group）:

${\bf SE}(3)=\left \{ {\pmb T}= \begin{bmatrix} {\pmb R} & {\pmb t}\\ 0^{T} &1\end{bmatrix} \in \mathbb{R}^{4\times 4} }\Big| {\pmb R} \in {\bf SO}(3)&, {\bf t} \in \mathbb{R}^{3} \right \} \ \eqno(3.10)$ ,同理，T的逆矩陣也代表一個相反的變換。

2.旋轉向量和歐拉角

2.1旋轉向量

${\bm R}$ 有九個量，但旋轉自由度是3，T有16個量，但歐式變換自由度是6。

於是我們尋找更簡潔的替代品，用一個旋轉軸和旋轉角來描述旋轉，旋轉軸爲 ${\pmb n}$ ,旋轉角爲 $\theta$ ，旋轉向量爲 ${\pmb n}\theta$

從旋轉向量到 ${\bm R}$ 旋轉矩陣，由一個羅德里格斯公式表明

${\pmb R}= \cos \theta {\bm I} +(1-\cos \theta ) {\pmb n\pmb n^{T}} +\sin \theta{\pmb n^{\Lambda} } \ \ \eqno(3.11)$

由上式可以推出 $tr \left ( {\pmb R} \right )= 1+2\cos \theta \ \ &, \theta=\arccos(( tr(\pmb R)-1)/2 ) \ \ \eqno(3.12)$

同時，由於轉軸 ${\bm n}$ 在旋轉後不發生改變，說明 ${\pmb R\pmb n}={\pmb R}$ ，所以 ${\pmb n}$ 是矩陣 ${\pmb R}$ 特徵值=1時，對應的特徵向量

2.2歐拉角

用三個分離的轉角來描述旋轉。歐拉角描述時。應該指定，旋轉的轉軸順序。

yaw偏航角（繞Z軸） pitch俯仰角（繞Y軸） roll翻滾角（繞X軸） ypr對應ZYX。

ZYX的萬向鎖發生在pitch爲 $\pm 90$ 度時。（好像是第二個旋轉軸的 $\pm 90$ 度旋轉時發生）

3.四元數。

3.1定義

歐拉角和旋轉向量都具有奇異性，需要不帶奇異性的三維向量描述方式。

四元數是一種擴展的複數。既是緊湊的，也沒有奇異性，缺點在於計算稍微複雜，表示不夠直觀。

一個四元數 ${\bm q}$ 擁有一個實部和三個虛部

${\pmb q}={\bm q_{0}}+{\bm q_{1}i}+{\bm q_{2}j}+{\bm q_{3}k} &. \ \ i,j,k$ 代表三個虛部，虛部滿足以下關係式
$\begin{Bmatrix} i^{2}=j^{2}=k^{2}=-1 \\ij=k,ji=-k\\jk=i,kj=-i\\ki=j,ik=-j \end{matrix} \ \eqno (3.13 )$

或者是用 $\pmb q= \begin{bmatrix} s & \pmb v \end{bmatrix}, s=q_{0} \in \mathbb{R}, \pmb v=\begin{bmatrix} q_{1} &q_{2} &q_{3} \end{bmatrix}^{T} \in \mathbb{R}^{3}$ 此時，s稱爲實部， $\pmb v$ 稱爲虛部。s=0虛四元數， $\pmb v=0$ 實四元數。

四元數是複數的擴展，和複數不同的是，複數中乘以一個i代表逆時針旋轉90度，而四元數裏還乘以一個i代表旋轉180度，而 $i^{2}=j^{2}=k^{2}=-1$ ，代表需要繞着軸繞兩圈才能回到原來的位置。

旋轉向量與四元數

假設某個旋轉繞 ${\pmb n}$ 軸， ${\pmb n}=\begin{bmatrix} n_{x},n_{y},n_{z} \end{bmatrix}^{T}$ 轉了 $\theta$ 度，那麼這個旋轉的四元數形式爲

$\pmb q =\begin{bmatrix} \cos \frac{\theta}{2}, \ n_{x}\sin \frac{\theta}, \ n_{y}\sin \frac{\theta}, \ n_{z}\sin \frac{\theta}{2}\end{bmatrix}^{T}$ (3.14)

由上也可以反算對應的旋轉軸和夾角

$\left\{\begin{matrix} \theta= a\arccos q_{0}\\ \begin{bmatrix} n_{x},n_{y},n_{z}\end{bmatrix}^{T}=\begin{bmatrix} q_{1},q_{2},q_{3}\end{bmatrix}^{T}/ \sin\frac{\theta}{2} \end{matrix}\right.$ (3.15)

直觀來看，四元數好像關於 $\theta$ 是轉了一半的感覺，對（3.14）式加上一個 $2\pi$ ，（從繞着 ${\pmb n}$ 轉了 $\theta$ 和 $\theta+2\pi$ ，是同一個旋轉）

我們會得到一個爲 $-\pmb q$ 的四元數，這說明，同一個旋轉可以由互爲相反數的一對四元數表示。同理， $\theta$ =0時，得到的是一個沒有任何旋轉的實四元數， $\pmb q =\begin{bmatrix} \pm1, 0,0,0\end{bmatrix}^{T}$

3.2運算

現有兩個四元數 $\pmb q_{a} ={\bm s_{a}}+{\bm x_{a}i}+{\bm y_{a}j}+{\bm z_{a}k} , \ \ \ \pmb q_{b} ={\bm s_{b}}+{\bm x_{b}i}+{\bm y_{b}j}+{\bm z_{b}k}$ , 或用向量表示 $\pmb q_{a}= \begin{bmatrix} s_{a} & \pmb v_{a} \end{bmatrix}, \pmb q_{b}= \begin{bmatrix} s_{b} & \pmb v_{b} \end{bmatrix},$

1）加減

實部實部相加，虛部虛部相加

2）乘法

乘法是將 $\pmb q_{a}$ 的每一項與 $\pmb q_{b}$ 的每一項相乘，最後相加

3）共軛

實部不變虛部爲相反數

4）模

$\begin{Vmatrix} \pmb q_{a}\end{Vmatrix}=\sqrt{s_{a}^{2}+{x_{a}^2}+{y_{a}^2}+{z_{a}^2}}$

5）逆

$\pmb q^{-1}=\pmb q^{*}/ \begin{Vmatrix}\pmb q \end{Vmatrix}$

6）數乘與點乘

3.3四元數表示旋轉

類似複數表達旋轉，複平面上有一點爲 $\pmb p=[a,b]$ ，旋轉爲 $\pmb q = [ \cos \theta ,\sin \theta]$ , 變換後的座標 $\pmb p^{'}= \pmb p * \pmb q = [a * \cos \theta, b*\sin \theta]^{T}$ ,

這裏的四元數也可以這樣近似理解， $\pmb p=[0,x,y,z]=[0, \pmb v]$ , 但是此時表示旋轉的四元數爲 $\pmb q = [ \cos \frac{\theta}{2} ,\sin \frac{\theta}{2}]$ (根據3.14式，繞着 ${\pmb n}$ 軸轉了 $\theta$ 角)

旋轉後的點 $\pmb p^{'}$ 可以表示爲這樣的乘積

$\pmb p^{'}= \pmb q \pmb p \pmb q ^{-1}$

3.4四元數到旋轉矩陣的變換。

$\bm{R} = \begin{matrix} {1 - 2q_2^2 - 2q_3^2}&{2{q_1}{q_2} - 2{q_0}{q_3}}&{2{q_1}{q_3} + 2{q_0}{q_2}}\\ {2{q_1}{q_2} + 2{q_0}{q_3}}&{1 - 2q_1^2 - 2q_3^2}&{2{q_2}{q_3} - 2{q_0}{q_1}}\\ {2{q_1}{q_3} - 2{q_0}{q_2}}&{2{q_2}{q_3} + 2{q_0}{q_1}}&{1 - 2q_1^2 - 2q_2^2} \end {matrix}$

（右手系以上公式爲準，下圖有誤，應是其轉置）

對於下面那個式子，只要第一個式子得出之後，求出q0，1，2，3其實非常簡單。

部分公式懶得打，截取自http://blog.csdn.net/youngpan1101/article/details/71086851

作業部分

習題1.

這裏我是用

$R=R_{x}*R_{y}*R_{z}, R^{T}=R_{z}^{T}*R_{y}^{T}R_{x}^{T}, \\RR^{T}=R_{x}*R_{y}*R_{z}*R_{z}^{T}*R_{y}^{T}R_{x}^{T}=I, with \ \ R_{z}R_{z}^{T}=R_{y}R_{y}^{T}=R_{x}R_{x}^{T}=I$

來證明的， $R_{x}*R_{y}*R_{z}$ 是分別只繞三個軸旋轉而得到的旋轉矩陣，

其中

這個推導很簡單，其實就是一個軸不動，所以把旋轉投影到另外兩個軸的平面即可,另外兩軸同理。此處略。

習題2

羅德里格斯公式

參考wiki百科 https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

或百度百科

https://baike.baidu.com/item/%E7%BD%97%E5%BE%B7%E9%87%8C%E6%A0%BC%E6%97%8B%E8%BD%AC%E5%85%AC%E5%BC%8F

其實思路很簡單，如果 $\bf v$ 向量與 $\pmb k$ 向量平行，繞着旋轉軸 $\pmb k$ 軸轉動 $\theta$ ，等於沒有進行旋轉。也就是說 $\bf v$ 向量中與 $\pmb k$ 軸平行的分量，其實沒有發生變化，真正旋轉的是與 $\pmb k$ 軸垂直的 $\bf v_{\perp }$ 分量。所以最終目標 $\pmb v_{rot}=\pmb v_{\perp }_{rot} + \pmb v_{\parallel }$

讓我們逐步分析

1）得到 $\pmb v_{\parallel }$

$\pmb k$ 爲單位向量，由投影可知，

$\begin{Vmatrix} \pmb v_{\parallel } \end{Vmatrix}=\begin{Vmatrix} \pmb v \end{Vmatrix} * \cos \alpha , where \ \alpha \ is \ the \ angle \ between \ \pmb k and \ \pmb v, \\ \cos \alpha =\frac{\pmb v \cdot \pmb k }{\begin{Vmatrix} \pmb v \end{Vmatrix} \begin{Vmatrix} \pmb k \end{Vmatrix} } , \ \ \ \begin{Vmatrix} \pmb v_{\parallel } \end{Vmatrix} = \pmb v \cdot \pmb k, \ \ \frac{\pmb v_{\parallel } }{\begin{Vmatrix} \pmb v_{\parallel } \end{Vmatrix} }= \pmb k \\ \pmb v_{\parallel }=(\pmb v \cdot \pmb k )\pmb k$

2）得到 $\pmb v_{\perp }$

$\pmb v_{\perp }=\pmb v- \pmb v_{\parallel }=\pmb v - (\pmb v \cdot \pmb k )\pmb k$

3)引入 $\pmb w$

(注意，最後的 $\pmb \omega=\pmb v_{\perp} \pmb h$ 只是爲了說明，其大小爲 $\pmb v_{\perp}$ ，座標是依據座標系的不同而不同的，這個式子是建立在以 $\pmb k$ 爲x軸， $\pmb v_{\perp}$ 爲y軸的右手系座標下的，其餘座標系可利用旋轉關係得到。)

$\pmb \omega = \pmb k \times \pmb v_{\perp}= \pmb k \times \left ( \pmb v- \left ( \pmb v \cdot \pmb k \right )\pmb k \right )= \pmb k \times \pmb v- \pmb k \times\left ( \pmb v \cdot \pmb k \right )\pmb k \right ), where \ \ \pmb k \times\pmb k =0 \\\pmb \omega =\pmb k \times \pmb v,\ \ \omega= \begin{bmatrix} \pmb i & \pmb j&\pmb h \\ 1 &0 &0\\ \pmb v_{parellel} & \pmb v_{\perp}&0 \end{bmatrix}=\pmb v_{\perp} \pmb h$

4)求解 $\pmb v_{\perp }_{rot}$ ，原理見圖

$\pmb v_{\perp}_{rot}=\pmb v_{\perp}*\cos \theta + \pmb \omega *\sin \theta$ 帶入， $\pmb v_{\perp}_{rot}=\cos \theta (\pmb v -(\pmb v\cdot \pmb k) \pmb k) + \sin \theta \pmb k \times \pmb v=\cos \theta \pmb v -\cos \theta (\pmb v\cdot \pmb k) \pmb k+ \sin \theta \pmb k \times \pmb v$

5)解得 $\pmb v_{rot}$

$\pmb v_{rot}= \pmb v_{\parallel }+\pmb v_{\perp}_{rot}=(\pmb v \cdot \pmb k ) \pmb k + \cos \theta (\pmb v -(\pmb v\cdot \pmb k) \pmb k) + \sin \theta \pmb k \times \pmb v \\ \pmb v_{rot}=\cos \theta \pmb v - (1-\cos \theta)(\pmb v\cdot \pmb k) \pmb k + \sin \theta \pmb k \times \pmb v$

6)解得 $\pmb R, \ \ \pmb v_{rot} = \pmb R \pmb v,$

設 $\pmb k = \begin {bmatrix} \pmb k _{1} \\ \pmb k _{2}\\ \pmb k _{3} \end{bmatrix}, \ \pmb v = \begin {bmatrix} \pmb v_{1} \\ \pmb v_{2}\\ \pmb v_{3} \end{bmatrix}$

$\pmb R= \pmb v_{rot} * \pmb v^{-1}= ( \cos \theta \pmb v - (1-\cos \theta)(\pmb v\cdot \pmb k) \pmb k + \sin \theta \pmb k \times \pmb v ) * \pmb v^{-1} \\\pmb R= \cos \theta \pmb I - (1-\cos \theta)\pmb k^{T}\pmb k + \sin \theta \pmb k^{\wedge } \\where (\pmb v\cdot \pmb k) \pmb k = \pmb k (\pmb k^{T}\pmb v) ,\ \ \ \pmb k \times \pmb v =\pmb k^{\wedge } \pmb v$

當然這裏 $\pmb v$ 並不一定可逆，但暫時也沒有找到其他直觀的簡易的得到 $\pmb R$ 的解法

總之，得到了羅德里格斯旋轉公式 $\pmb R= \cos \theta \pmb I - (1-\cos \theta)\pmb k^{T}\pmb k + \sin \theta \pmb k^{\wedge }$

習題3

這個也很簡單，利用四元數的乘法公式帶入即可，注意只需要關心實部，還有可以認爲是單位四元數，則四元數的逆等於共軛，對於更一般的情況只需要共軛除以一個模得到逆即可。

此處略

習題4

前文已經很細節了

習題5 習題6 習題7

都比較簡單不復述了

q1.normalize(); //可以實現歸一化

//可以實現從四元數到旋轉矩陣 
Tcw.prerotate(q1); 
Tcw.pretranslate(t2);

【SLAM十四講】第三講

1.旋轉矩陣

1.1點向量座標系

1.2座標系間的歐式變換

1.3 變換矩陣和齊次變換

2.旋轉向量和歐拉角

2.1旋轉向量

2.2歐拉角

3.四元數。

3.1定義

3.2運算

3.3四元數表示旋轉

3.4四元數到旋轉矩陣的變換。

作業部分

習題1.

習題2

習題3

習題4

AI 畫圖真刺激，手把手教你如何用 ComfyUI 來畫出刺激的圖

公司剛入職了一名 Java 中級開發，短短 4 行代碼居然湊齊了 3 個 bug！我哭了~~

公衆號5月C#/.NET熱文一覽

git 下載大陸鏡像地址

【SLAM十四講】第六講

【SLAM十四講】第三講

【論文系列】讀《SeqSLAM: Visual Route-Based Navigation for Sunny Summer Days and Stormy Winter Nights》

【SLAM十四講】第四講李羣與李代數部分證明（包括習題）

源碼實踐指南！Mastering opencv Ch3 Markerless AR--UBUNTU下 opencv+opengl 運行

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

【SLAM十四講】 第三講

1.旋轉矩陣

1.1點 向量 座標系

1.2座標系間的歐式變換

1.3 變換矩陣和齊次變換

2.旋轉向量和歐拉角

2.1旋轉向量

2.2歐拉角

3.四元數。

3.1定義

3.2運算

3.3四元數表示旋轉

3.4四元數到旋轉矩陣的變換。

作業部分

習題1.

習題2

習題3

習題4

【SLAM十四講】第三講

1.1點向量座標系