【旋转矩阵和旋转向量】

首先我们来回顾一下点，向量和坐标系；
点：空间中的基本元素，没有长度，没有体积。
向量：两点连起来，可以看成从某点指向另一点箭头，误将坐标混淆，向量是空间一个东西，如a。
坐标：只有当我们指定这个三维空间中的某个坐标系时,才可以谈论该向量在此坐标系下的坐标。
假设我们在空间中找到一组基 ,那么任意向量a在这组基下就有一个坐标：
a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 \boldsymbol{a}=\left[\boldsymbol{e}_{1},\boldsymbol{e}_{2},\boldsymbol{e}_{3}\right]\left[\begin{array}{l}a_{1} \\a_{2} \\a_{3}\end{array}\right]=a_{1} e_{1}+a_{2} e_{2}+a_{3} e_{3} a=[e1,e2,e3]⎣ ⎡a1a2a3⎦ ⎤=a1e1+a2e2+a3e3

这里 ( a 1 , a 2 , a 3 ) T \left(a_{1}, a_{2}, a_{3}\right)^{\mathrm{T}} (a1,a2,a3)T称为a在此基下的坐标。
向量的运算可以用坐标的运算来表达

内积： a ⋅ b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣ ∣ b ∣ cos ⁡ ⟨ a , b ⟩ \boldsymbol{a}\cdot\boldsymbol{b}=\boldsymbol{a}^{\mathrm{T}} \boldsymbol{b}=\sum_{i=1}^{3} a_{i} b_{i}=|\boldsymbol{a}||\boldsymbol{b}| \cos \langle\boldsymbol{a}, \boldsymbol{b}\rangle a⋅b=aTb=∑i=13aibi=∣a∣∣b∣cos⟨a,b⟩

外积： a × b = ∥ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ∥ = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b = def a ∧ b . \boldsymbol{a} \times \boldsymbol{b}=\left\|\begin{array}{ccc} \boldsymbol{e}_{1} & \boldsymbol{e}_{2} & \boldsymbol{e}_{3} \\ a_{1} & a_{2} & a_{3} \\b_{1} & b_{2} & b_{3}\end{array}\right\|=\left[\begin{array}{l} 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{array}\right]=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0\end{array}\right] \boldsymbol{b} \stackrel{\text { def }}{=} \boldsymbol{a}^{\wedge} \boldsymbol{b} . a×b=∥ ∥e1a1b1e2a2b2e3a3b3∥ ∥=⎣ ⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦ ⎤=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤b= def a∧b.

外积结果是个向量，方向垂直于前两个向量，对于外积运算，引入 ∧ \wedge ∧符号，把a写成一个矩阵，这样把外积 a × b a \times b a×b写成了矩阵与向量的乘法 a ∧ b a^{\wedge} b a∧b，把他变成了线性运算。

a的反对称矩阵： a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] \boldsymbol{a}^{\wedge}=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\a_{3} & 0 & -a_{1} \\-a_{2} & a_{1} & 0\end{array}\right] a∧=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤

在机器人学中，我们将不动的惯性坐标系称为世界坐标系（ x W , y W , z W x_{\mathrm{W}}, y_{\mathrm{W}}, z_{\mathrm{W}} xW,yW,zW ），同时在我们的相机中有一个移动的坐标系可定义为（ x C , y C , z C x_{\mathrm{C}}, y_{\mathrm{C}}, z_{\mathrm{C}} xC,yC,zC ）。若相机视野中某个向量p，在相机坐标系下为 P c P_{\mathrm{c}} Pc,而在世界坐标系下看，他的坐标为 P w P_{\mathrm{w}} Pw，这时我们就需要一个矩阵来表示两坐标系之间的变换。

对于坐标变换也称（欧式变换）由旋转和平移组成。
对于旋转：设某个单位正交基 ( e 1 , e 2 , e 3 ) \left(e_{1}, e_{2}, e_{3}\right) (e1,e2,e3)经一次旋转后变为 ( e 1 ′ , e 2 ′ , e 3 ′ ) \left(e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right) (e1′,e2′,e3′)。那么对于同一a向量在前后两个坐标系下的坐标为 [ a 1 , a 2 , a 3 ] T 和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T \left[a_{1}, a_{2}, a_{3}\right]^{\mathrm{T}} \text { 和 }\left[a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime}\right]^{\mathrm{T}} [a1,a2,a3]T 和 [a1′,a2′,a3′]T。又由于向量本身没有随坐标旋转而改变，所以根据作标定义可得

[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l}a_{1} \\a_{2}\\a_{3}\end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime},\boldsymbol{e}_{2}^{\prime},\boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c}a_{1}^{\prime} \\a_{2}^{\prime} \\a_{3}^{\prime}\end{array}\right] [e1,e2,e3]⎣ ⎡a1a2a3⎦ ⎤=[e1′,e2′,e3′]⎣ ⎡a1′a2′a3′⎦ ⎤

此时我们给上式同时左乘 [ e 1 T e 2 T e 3 T ] , \left[\begin{array}{l}\boldsymbol{e}_{1}^{\mathrm{T}} \\\boldsymbol{e}_{2}^{\mathrm{T}} \\\boldsymbol{e}_{3}^{\mathrm{T}}\end{array}\right] \text {, } ⎣ ⎡e1Te2Te3T⎦ ⎤, 则左边的系数就为单位矩阵所以：

[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = def R a ′ \left[\begin{array}{l}a_{1} \\a_{2} \\a_{3}\end{array}\right]=\left[\begin{array}{lll}\boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\\boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime}\end{array}\right]\left[\begin{array}{c}a_{1}^{\prime} \\a_{2}^{\prime} \\ a_{3}^{\prime}\end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{R} \boldsymbol{a}^{\prime} ⎣ ⎡a1a2a3⎦ ⎤=⎣ ⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦ ⎤⎣ ⎡a1′a2′a3′⎦ ⎤= def Ra′

此时我们我们定义R为旋转矩阵。它刻画了旋转前后同一向量的坐标变换关系。
我们可以证明（1）R是一个正交矩阵；（2）R的行列式为1。所以我们将n维旋转矩阵的集合定义为：

S O ( n ) = { R ∈ R n × n ∣ R R T = E , det ⁡ ( R ) = 1 } \mathrm{SO}(n)=\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} \mid \boldsymbol{R} \boldsymbol{R}^{\mathrm{T}}=\boldsymbol{E}, \operatorname{det}(\boldsymbol{R})=1\right\} SO(n)={R∈Rn×n∣RRT=E,det(R)=1}

S O ( n ) \mathrm{SO}(n) SO(n)其实为特殊正交群， S O ( 3 ) \mathrm{SO}(3) SO(3)就是指三维空间的旋转。
此时我们可以反推世界坐标系的向量在相机坐标系下的坐标 a ′ = R − 1 a = R T a \boldsymbol{a}^{\prime}=\boldsymbol{R}^{-1} \boldsymbol{a}=\boldsymbol{R}^{\mathrm{T}} \boldsymbol{a} a′=R−1a=RTa。显然 R T \boldsymbol{R}^{\mathrm{T}} RT 刻画了一个相反的旋转。
这时我们加上平移，在世界坐标系的向量a，经过一次旋转（R）和一次平移(t)得到 a ′ \boldsymbol{a}^{\prime} a′,将两者结合在一起有：
a ′ = R a + t . a^{\prime}=\boldsymbol{R a}+\boldsymbol{t} . a′=Ra+t.
变换矩阵与齐次坐标
首先强调一下这里的变换不是线性的，假设进行了两次变换 R 1 , t 1 和 R 2 , t 2 \boldsymbol{R}_{1}, \boldsymbol{t}_{1} \text { 和 } \boldsymbol{R}_{2}, \boldsymbol{t}_{2} R1,t1 和 R2,t2：
b = R 1 a + t 1 , c = R 2 b + t 2 b=R_{1} a+t_{1}, \quad c=R_{2} b+t_{2} b=R1a+t1,c=R2b+t2
那么从a到c的变换为 c = R 2 ( R 1 a + t 1 ) + t 2 \boldsymbol{c}=\boldsymbol{R}_{2}\left(\boldsymbol{R}_{1} a+\boldsymbol{t}_{1}\right)+\boldsymbol{t}_{2} c=R2(R1a+t1)+t2。
但是这样看来非常的冗余，尤其是经过多次变换后，因为我们引入齐次坐标和变换矩阵：

[ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] = def T [ a 1 ] . \left[\begin{array}{l}\boldsymbol{a}^{\prime} \\1\end{array}\right]=\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\\mathbf{0}^{\mathrm{T}} &1\end{array}\right]\left[\begin{array}{l}\boldsymbol{a} \\1\end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{T}\left[\begin{array}{l}\boldsymbol{a} \\1\end{array}\right] . [a′1]=[R0Tt1][a1]= def T[a1].

这是一个数学技巧，我们在一个三维向量的末尾添加1，变为思维向量，称为齐次坐标，对于这个四维向量我们可以把旋转和平移写在一个矩阵里，使得整个变为线性关系。则T称为变换矩阵。若用 a ~ \tilde{a} a~表示向量a的齐次坐标，那么依靠齐次坐标和变换矩阵，两次变换叠加就可以表示为：

b ~ = T 1 a ~ , c ~ = T 2 b ~ ⇒ c ~ = T 2 T 1 a ~ \tilde{b}=T_{1} \tilde{a}, \tilde{c}=T_{2} \tilde{b} \quad \Rightarrow \tilde{c}=T_{2} T_{1} \tilde{a} b~=T1a~,c~=T2b~⇒c~=T2T1a~

关于这种变换矩阵T左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1。这种矩阵又称为特殊欧氏群。

S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ SO ⁡ ( 3 ) , t ∈ R 3 } \mathrm{SE}(3)=\left\{T=\left[\begin{array}{cc}R & t \\0^{\mathrm{T}} & 1\end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in \operatorname{SO}(3), t \in \mathbb{R}^{3}\right\} SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}

与SO(3)一样，求解该矩阵的逆表示一个反向的变换:

T − 1 = [ R T R T t 0 T 1 ] \boldsymbol{T}^{-1}=\left[\begin{array}{cc}\boldsymbol{R}^{\mathrm{T}} & \boldsymbol{R}^{\mathrm{T}} \boldsymbol{t} \\\mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] T−1=[RT0TRTt1]

旋转向量
SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。同理，变换矩阵用16个量表达了6自由度的变换。那么，是否有更紧凑的表示呢?旋转矩阵自身带有约束:它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变得更困难。
任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量。
假设旋转轴单位长度的向量为n,角度为 θ \theta θ，那么 θ \theta θn也可以描述这个旋转。

R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) n n T + sin ⁡ θ n ∧ \boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) n n^{\mathrm{T}}+\sin \theta \boldsymbol{n}^{\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧

tr ⁡ ( R ) = cos ⁡ θ tr ⁡ ( I ) + ( 1 − cos ⁡ θ ) tr ⁡ ( n n ⊤ ) + sin ⁡ θ tr ⁡ ( n ∧ ) = 3 cos ⁡ θ + ( 1 − cos ⁡ θ ) = 1 + 2 cos ⁡ θ \begin{aligned}\operatorname{tr}(\boldsymbol{R}) &=\cos \theta \operatorname{tr}(\boldsymbol{I})+(1-\cos \theta) \operatorname{tr}\left(n n^{\top}\right)+\sin \theta \operatorname{tr}\left(\boldsymbol{n}^{\wedge}\right) \\&=3 \cos \theta+(1-\cos \theta) \\ &=1+2 \cos \theta\end{aligned} tr(R)=cosθtr(I)+(1−cosθ)tr(nn⊤)+sinθtr(n∧)=3cosθ+(1−cosθ)=1+2cosθ
因此： θ = arccos ⁡ tr ⁡ ( R ) − 1 2 . \theta=\arccos \frac{\operatorname{tr}(\boldsymbol{R})-1}{2} . θ=arccos2tr(R)−1.

【旋转矩阵和旋转向量】相关推荐

罗德里格斯公式推导，以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...
三维重建学习(1)：基础知识：旋转矩阵与旋转向量
前言由于摄像机标定中会使用到旋转矩阵以及旋转向量的知识,所以就整理了一下有关与这一部分基础知识的笔记,并进行详细的数学推导. 旋转矩阵假设坐标系分别绕着xxx轴旋转ϕ\phiϕ角,绕yyy轴旋转θ ...
eigen 编译_头条 | 使用eigen实现四元数、欧拉角、旋转矩阵、旋转向量间的转换...
点击上方蓝字,关注本公众号,获得更多资源上一篇文章介绍了四元数.欧拉角.旋转矩阵.轴角如何相互转换,本篇文章介绍如何用eigen来实现. 旋转向量 1,初始化旋转向量:旋转角为alpha,旋转轴为(x ...
三维坐标变换——旋转矩阵与旋转向量
用 opencv 进行过双目相机标定的同学都知道,单目标定 calibrateCamera() 函数能够对每一张标定图像计算出一对 rvec 和 tvec,即旋转平移向量,代表世界坐标系到相机坐标系的 ...
一、旋转矩阵，旋转向量，单位四元数的相互转换总结
文章目录前言一.要点 1. 旋转矩阵 2. 旋转向量 3. 单位四元数二.旋转向量--->旋转矩阵(罗德里格斯公式) 三.旋转矩阵--->旋转向量四.单位四元数--->旋转矩 ...
[转] 罗德里格斯公式推导，以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...
旋转矩阵、旋转向量（轴角）、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)...
1 #include <iostream> 2 #include <cmath> 3 using namespace std; 4 5 #include <Eigen/C ...
刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数
坐标变换及其方法 1.转化关系图 2 换算关系 3.1 旋转矩阵换算至其他 3.2 四元数换算至其他 3.3 旋转向量转换至旋转矩阵与四元数 3.3 欧拉角转换到旋转矩阵和四元数 3 坐标变换 4 坐 ...
旋转矩阵及旋转向量相互转化 Rodrigues矩阵及matlab实现
处理三维旋转问题时,通常采用旋转矩阵的方式来描述.一个向量乘以旋转矩阵等价于向量以某种方式进行旋转.除了采用旋转矩阵描述外,还可以用旋转向量来描述旋转,旋转向量的长度(模)表示绕轴逆时针旋转的角度(弧 ...

【旋转矩阵和旋转向量】

【旋转矩阵和旋转向量】相关推荐

最新文章

热门文章