视觉SLAM十四讲学习笔记——三维空间刚体运动(ch3~旋转矩阵Eigen)

三维空间中的几何变换(旋转矩阵、变换矩阵、旋转向量、欧拉角、四元数) 旋转矩阵表示形式   由欧拉旋转定理可得:刚体在三位空间里的一般运动可以分解为刚体上某一点的平移,以及绕过该点旋转轴的转动; 只考虑旋转:   三维空间中的点p在坐标系(e1,e2,e3)(e_1, e_2, e_3)(e1​,e2​,e3​)中的坐标为(a1,a2,a3)(a_1, a_2, a_3)(a1​,a2​,a3​),当坐标系(e1,e2,e3)(e_1, e_2, e_3)(e1​,e2​,e3​)旋转到坐标系(e1′,e2′,e3′)(e_1^{\prime}, e_2^{\prime},. 阅读详情

本节目标

理解三维空间的刚体运动描述方式:旋转矩阵、变换矩阵、四元数和欧拉角。
掌握 Eigen库的矩阵、几何模块使用方法。

一、旋转矩阵

刚体不光有位置,还有自身的姿态。
相机也可以看成三维空间的刚体,位置是指相机在空间中的哪个地方,而姿态则是指相机的朝向。

1.1 点、向量和坐标系

如果我们确定一个坐标系,也就是一个线性空间的基(e1,e2,e3),那就可以谈论向量 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[e_1,e_2,e_3\right]\left[\begin{array}{c}a_1\\ a_2\\ a_3\end{array}\right]=a_1e_1+a_2e_2+a_3e_3. a=[e1​,e2​,e3​] ​a1​a2​a3​​ ​=a1​e1​+a2​e2​+a3​e3​.
坐标的具体取值,一个是和向量本身有关,第二也和坐标系的选取有关。给定 x 和y 轴时, 就可以通过右手 (或左手) 法则由 x×y 定出来。
在这里插入图片描述

根据定义方式的不同,坐标系又分为左手系和右手系。就经验来讲人们更习惯使用右手系。
(左手系右手系都是y轴在x轴的逆时针方向,右手系z轴⬆,左手系z轴⬇)
在这里插入图片描述

1.1.1 向量-向量、向量-数之间的运算

对于三维向量a和b:

  • 内积
    a ⋅ b = a T b = ∑ i = 1 a i b i = ∣ a ∣ ∣ b ∣ cos ⁡ ⟨ a , b ⟩ . \boldsymbol{a}\cdot\boldsymbol{b}=\boldsymbol{a}^{T}\boldsymbol{b}=\sum_{i=1}a_{i}b_{i}=\left|\boldsymbol{a}\right|\left|\boldsymbol{b}\right|\cos\left\langle\boldsymbol{a},\boldsymbol{b}\right\rangle. a⋅b=aTb=i=1∑​ai​bi​=∣a∣∣b∣cos⟨a,b⟩.
    内积可以描述向量间的投影关系。

  • 外积
    外积的方向垂直于这两个向量,大小为 ∣ a ∣ ∣ b ∣ s i n < a , b > |a| |b| sin<a,b> ∣a∣∣b∣sin<a,b>,是两个向量张成的四边形的有向面积。外积引入了^ 符号,把 a a a 写成一个矩阵。事实上是一个反对称矩阵(Skew-symmetric),你可以将记成一个反对称符号。这样就把外积 a × b a×b a×b,写成了矩阵与向量的乘法 a a a^ b b b,把它变成了线性运算。这个符号将在后文经常用到,请记住它。
    在这里插入图片描述
    a × b = [ i j k 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 = Δ a † b . \boldsymbol{a}\times\boldsymbol{b}=\left[\begin{array}{cc}i&j&\boldsymbol{k}\\ a_1&a_2&a_3\\ b_1&b_2&b_3\end{array}\right]=\left[\begin{array}{cc}a_2b_3-a_3b_2\\ a_3b_1-a_1b_3\\ a_1b_2-a_2b_1\end{array}\right]=\left[\begin{array}{cc}0&-a_3&a_2\\ a_3&0&-a_1\\ -a_2&a_1&0\end{array}\right]\boldsymbol{b}\overset{\Delta}{=}\boldsymbol{a}^{\dagger}\boldsymbol{b}. a×b= ​ia1​b1​​ja2​b2​​ka3​b3​​ ​= ​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​ ​= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​b=Δa†b.
    在这里插入图片描述
    外积只对三维向量存在定义,还能用外积表示向量的旋转

1.1.2 外积表示旋转

考虑两个不平行的向量 a a a, b b b,为描述从 a a a 到 b b b之间是如何旋转的。
如图 ,可以用一个向量来描述三维空间中两个向量的旋转关系。
在右手法则下,我们用右手的四个指头从 a a a 转向 b b b,其大拇指朝向就是旋转向量 w w w的方向,事实上也是 a × b a×b a×b的方向。它的大小则由 a a a和 b b b的夹角决定。通过这种方式,我们构造了从 a a a 到 b b b的一个旋转向量。这个向量同样位于三维空间中,在此坐标系下,可以用三个实数来描述它。
在这里插入图片描述

1.2 坐标系间的欧式变换

与向量间的旋转类似,同样可以描述两个坐标系之间的旋转关系,再加上平移,统称为坐标系之间的变换关系。
在机器人的运动过程中,常见的做法是设定一个惯性坐标系(或者叫世界坐标系),可以认为它是固定不动的,如下图的 x x xw、 y y yw、 z z zw 定义的坐标系。
同时,相机或机器人则是一个移动坐标系,例如 x x xc、 y y yc、 z z zc定义的坐标系。
问: 相机视野中某个向量 p p p,它的坐标为 p p pc。而从世界坐标系下看,它的坐标 p p pw。这两个坐标之间是如何转换的呢?
这时,就需要先得到该点针对机器人坐标系坐标值,再根据机器人位姿转换到世界坐标系中,这个转换关系由一个矩阵 T T T 来描述。
在这里插入图片描述
相机运动是一个刚体运动,它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧氏变换。
(可以想象当一个被子抛到空中,被子除了位姿会发生变化,自身的结构是不发生变化的)

一个欧氏变换由一个旋转和一个平移两部分组成。

  • 首先来考虑旋转。我们设某个单位正交基 ( e 1 , e 2 , e 3 ) \left(e_{1}, e_{2}, e_{3}\right) (e1​,e2​,e3​)经过一次旋转,变成了 ( e 1 ′ , e 2 ′ , e 3 ′ ) \left(\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right) (e1′​,e2′​,e3′​)。那么,对于同一个向量 a a a (注意该向量并没有随着坐标系的旋转而发生运动),它在两个坐标系下的坐标为 [ a 1 , a 2 , a 3 ] T \left[a_{1}, a_{2}, a_{3}\right]^{T} [a1​,a2​,a3​]T和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T \left[a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime}\right]^{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}{c} 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​] ​a1​a2​a3​​ ​=[e1′​,e2′​,e3′​] ​a1′​a2′​a3′​​ ​
    为了描述两个坐标之间的关系,对上面等式左右同时左乘 [ e 1 T e 2 T e 3 T ] \left[\begin{array}{c} \boldsymbol{e}_{1}^{T} \\ \boldsymbol{e}_{2}^{T} \\ e_{3}^{T} \end{array}\right] ​e1T​e2T​e3T​​ ​
    那么左边的系数变成了单位矩阵,所以:
    [ 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 ′ ] ≜ R a ′ . \left[\begin{array}{c}a_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}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^{'}\end{array}\right]\left[\begin{array}{c}a_1^{'}\\ a_2^{'}\\ a_3^{'}\\ \end{array}\right]\triangleq\mathbf{R}\mathbf{a}^{'}. ​a1​a2​a3​​ ​= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3​e3T​e3′​​ ​ ​a1′​a2′​a3′​​ ​≜Ra′.
    把中间的阵拿出来,定义成一个矩阵 R R R。这个矩阵刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的,那么这个矩阵也是一样的。因此它又称为旋转矩阵。
    旋转矩阵有一些特别的性质:它是一个行列式为 1的正交矩阵。反之,行列式为 1的正交阵也是一个旋转矩阵。所以,我们可以把旋转矩阵的集合定义如下:
    S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } . SO(n)=\{\boldsymbol{R}\in\mathbb{R}^{n\times n}|\boldsymbol{RR}^T=\boldsymbol{I},\det(\boldsymbol{R})=1\}. SO(n)={R∈Rn×n∣RRT=I,det(R)=1}.
    S O ( n ) SO(n) SO(n)是特殊正交群 (Special Orthogonal Group)的意思。
    这个集合由 n n n 维空间的旋转矩阵组成,特别的, S O ( 3 ) SO(3) SO(3) 就是三维空间的旋转了。通过旋转矩阵可以描述相机的旋转。
    由于旋转矩阵为正交阵,旋转矩阵的逆 (即转置) 描述了一个相反的旋转。按照上面的定义方式,有:
    a ′ = R − 1 a = R T a . \boldsymbol{a^{'}}=\boldsymbol{R^{-1}a}=\boldsymbol{R^{T}a}. a′=R−1a=RTa.
    显然 R T R^T RT 刻画了一个相反的旋转。
    在欧氏变换中,除了旋转之外还有一个平移。考虑世界坐标系中的向量 a a a,经过一次旋转 R R R和一次平移 t t t后,得到了 a ′ a' a′,那么把旋转和平移合到一起,有:
    a ′ = R a + t . a^{'}=Ra+t. a′=Ra+t.

其中, t t t 称为平移向量。相比于旋转,平移部分只需把这个平移量加到旋转之后的坐标上。通过上式,用一个旋转矩阵 R R R 和一个平移向量 t t t 完整地描述了一个欧氏空间的坐标变换关系。

1.3 变换矩阵与齐次坐标

a ′ = R a + t a^{'}=Ra+t a′=Ra+t 完整地表达了欧氏空间的旋转与平移,不过还存在一个小问题: 这里的变换关系不是一个线性关系。假设我们进行了两次变换: R 1 , t 1 \boldsymbol{R}_1,\boldsymbol{t}_1 R1​,t1​和 R 2 , t 2 \boldsymbol{R}_2,\boldsymbol{t}_2 R2​,t2​,满足:
b = R 1 a + t 1 , c = R 2 b + t 2 . b=\boldsymbol{R}_1\boldsymbol{a}+\boldsymbol{t}_1,\quad\boldsymbol{c}=\boldsymbol{R}_2\boldsymbol{b}+\boldsymbol{t}_2. b=R1​a+t1​,c=R2​b+t2​.
但是从 a a a到 c c c的变换为:
c = R 2 ( R 1 a + t 1 ) + t 2 . c=\boldsymbol{R}_2\left(\boldsymbol{R}_1\boldsymbol{a}+\boldsymbol{t}_1\right)+\boldsymbol{t}_2. c=R2​(R1​a+t1​)+t2​.
这样的形式在变换多次之后会过于复杂。因此,要引入齐次坐标和变换矩阵重写
a ′ = R a + t a^{'}=Ra+t a′=Ra+t 。
[ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] Δ ‾ T [ a 1 ] . \left[\begin{array}{c}\boldsymbol{a}^{'}\\ 1\end{array}\right]=\left[\begin{array}{c c}\boldsymbol{R}&t\\ \boldsymbol{0}^{T}&1\end{array}\right]\left[\begin{array}{c c}\boldsymbol{a}\\ \boldsymbol{1}\end{array}\right]\underline{\Delta}\boldsymbol{T}\left[\begin{array}{c}a\\ 1\end{array}\right]. [a′1​]=[R0T​t1​][a1​]Δ​T[a1​].
这是一个数学技巧:把一个三维向量的末尾添加 1,变成了四维向量,称为齐次坐标。对于这个四维向量,旋转和平移矩阵写在一个矩阵里面,使得整个关系变成了线性关系。该式中,矩阵 T T T称为变换矩阵(Transform Matrix)。暂时用 a ~ \tilde{\boldsymbol{a}} a~表示 a a a的齐次坐标。
齐次坐标是射影几何里的概念。通过添加最后一维,用四个实数描述了一个三维向量,这显然多了一个自由度,但允许我们把变换写成线性的形式。
在这里插入图片描述
在齐次坐标中,某个点 a 的每个分量同乘一个非零常数后,仍然表示的是同一个点。因此,一个点的具体坐标值不是唯一的。如 [ 1 , 1 , 1 , 1 ] T [1,1,1,1]^T [1,1,1,1]T和 [ 2 , 2 , 2 , 2 ] T [2,2,2,2]^T [2,2,2,2]T是同一个点。但当最后项不为零时,总可以把所有坐标除以最后一项,强制最后一项为 1,从而得到一个点唯一的坐标表示(也就是转换成非齐次坐标):
x ~ = [ x , y , z , w ] T = [ x / w , y / w , z / w , 1 ] T \tilde{\boldsymbol{x}}=\left[x,y,z,w\right]^T=\left[x/w,y/w,z/w,1\right]^T x~=[x,y,z,w]T=[x/w,y/w,z/w,1]T
区分齐次和非齐次坐标的符号令人厌烦。在不引起歧义的情况下,就直接把它写成 b = T a b=Ta b=Ta的样子,默认其中是齐次坐标了。
关于变换矩阵 T = [ R t 0 T 1 ] T=\left[\begin{array}{cc}R&t\\ 0^T&1\end{array}\right] T=[R0T​t1​]具有比较特别的结构:

  • 左上角为旋转矩阵 R R R
  • 右侧为平移向量 t t t
  • 左下角为0向量
  • 右下角为1

这种矩阵又称为特殊欧氏群 (Special Euclidean Group):
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . SE(3)=\left\{T=\left[\begin{array}{c c}{\boldsymbol{R}}&{t}\\ {\boldsymbol{0}^T}&{1}\end{array}\right]\in\mathbb{R}^{4\times4}|\boldsymbol{R}\in SO(3),t\in\mathbb{R}^3\right\}. SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}.
与 S O ( 3 ) SO(3) SO(3)一样,求解该矩阵的逆表示一个反向的变换:
T − 1 = [ R T − R T t 0 T 1 ] \boldsymbol{T}^{-1}=\left[\begin{array}{cc}\boldsymbol{R}^T&-\boldsymbol{R}^Tt\\ \boldsymbol{0}^T&1\\ \end{array}\right] T−1=[RT0T​−RTt1​]
最后,为了保持符号的简洁,在不引起歧义的情况下,不区别齐次坐标与普通的坐标的符号,默认使用的是符合运算法则的那一种。
例如,写 T a Ta Ta 时,使用的是齐次坐标(不然没法计算)。而写 R a Ra Ra 时,使用的是非齐次坐标。

1.4 小节

  1. 向量在坐标系中的表达:
    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[e_1,e_2,e_3\right]\left[\begin{array}{c}a_1\\ a_2\\ a_3\end{array}\right]=a_1e_1+a_2e_2+a_3e_3. a=[e1​,e2​,e3​] ​a1​a2​a3​​ ​=a1​e1​+a2​e2​+a3​e3​.
  2. 两向量的内积:
    a ⋅ b = a T b = ∑ i = 1 a i b i = ∣ a ∣ ∣ b ∣ cos ⁡ ⟨ a , b ⟩ . \boldsymbol{a}\cdot\boldsymbol{b}=\boldsymbol{a}^{T}\boldsymbol{b}=\sum_{i=1}a_{i}b_{i}=\left|\boldsymbol{a}\right|\left|\boldsymbol{b}\right|\cos\left\langle\boldsymbol{a},\boldsymbol{b}\right\rangle. a⋅b=aTb=i=1∑​ai​bi​=∣a∣∣b∣cos⟨a,b⟩.
  3. 两向量的外积:
    a × b = [ i j k 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 = Δ a † b . \boldsymbol{a}\times\boldsymbol{b}=\left[\begin{array}{cc}i&j&\boldsymbol{k}\\ a_1&a_2&a_3\\ b_1&b_2&b_3\end{array}\right]=\left[\begin{array}{cc}a_2b_3-a_3b_2\\ a_3b_1-a_1b_3\\ a_1b_2-a_2b_1\end{array}\right]=\left[\begin{array}{cc}0&-a_3&a_2\\ a_3&0&-a_1\\ -a_2&a_1&0\end{array}\right]\boldsymbol{b}\overset{\Delta}{=}\boldsymbol{a}^{\dagger}\boldsymbol{b}. a×b= ​ia1​b1​​ja2​b2​​ka3​b3​​ ​= ​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​ ​= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​b=Δa†b.
  4. 欧式变换——旋转矩阵:
    [ 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}{c} 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​] ​a1​a2​a3​​ ​=[e1′​,e2′​,e3′​] ​a1′​a2′​a3′​​ ​
    [ 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 ′ ] ≜ R a ′ . \left[\begin{array}{c}a_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}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^{'}\end{array}\right]\left[\begin{array}{c}a_1^{'}\\ a_2^{'}\\ a_3^{'}\\ \end{array}\right]\triangleq\mathbf{R}\mathbf{a}^{'}. ​a1​a2​a3​​ ​= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3​e3T​e3′​​ ​ ​a1′​a2′​a3′​​ ​≜Ra′.
  5. S O ( n ) SO(n) SO(n)是特殊正交群 (Special Orthogonal Group):
    S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } . SO(n)=\{\boldsymbol{R}\in\mathbb{R}^{n\times n}|\boldsymbol{RR}^T=\boldsymbol{I},\det(\boldsymbol{R})=1\}. SO(n)={R∈Rn×n∣RRT=I,det(R)=1}.
  6. 反向旋转:
    a ′ = R − 1 a = R T a . \boldsymbol{a^{'}}=\boldsymbol{R^{-1}a}=\boldsymbol{R^{T}a}. a′=R−1a=RTa.
  7. 欧式变换:
    a ′ = R a + t . \boldsymbol{a}^{'}=\boldsymbol{Ra}+\boldsymbol{t}. a′=Ra+t.
  8. 齐次坐标定义:一个三维向量的末尾添加 1,变成了四维向量。
  9. 欧式变换的线性变换:
    [ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] Δ ‾ T [ a 1 ] . \left[\begin{array}{c}\boldsymbol{a}^{'}\\ 1\end{array}\right]=\left[\begin{array}{c c}\boldsymbol{R}&t\\ \boldsymbol{0}^{T}&1\end{array}\right]\left[\begin{array}{c c}\boldsymbol{a}\\ \boldsymbol{1}\end{array}\right]\underline{\Delta}\boldsymbol{T}\left[\begin{array}{c}a\\ 1\end{array}\right]. [a′1​]=[R0T​t1​][a1​]Δ​T[a1​].
  10. 变换矩阵 T \boldsymbol{T} T= [ R t 0 T 1 ] \left[\begin{array}{cc}R&t\\ 0^T&1\end{array}\right] [R0T​t1​]
  11. S E ( 3 ) SE(3) SE(3)特殊欧氏群:
    S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . SE(3)=\left\{T=\left[\begin{array}{c c}{\boldsymbol{R}}&{t}\\ {\boldsymbol{0}^T}&{1}\end{array}\right]\in\mathbb{R}^{4\times4}|\boldsymbol{R}\in SO(3),t\in\mathbb{R}^3\right\}. SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}.
  12. S E ( 3 ) SE(3) SE(3)的反向变换与 S O ( 3 ) SO(3) SO(3)一样:
    T − 1 = [ R T − R T t 0 T 1 ] \boldsymbol{T}^{-1}=\left[\begin{array}{cc}\boldsymbol{R}^T&-\boldsymbol{R}^Tt\\ \boldsymbol{0}^T&1\\ \end{array}\right] T−1=[RT0T​−RTt1​]

二、代码实践

本节将介绍如何使用 Eigen 来表示矩阵、向量,随后引申至旋转矩阵与变换矩阵的计算。本节的代码来自链接的slambook/ch3/useEigen 文件。
Eigen是一个 C++ 开源线性代数库,如果还没安装可以参考库的安装。

个人建议:先运行一次slambook/ch3/useEigen文件中的代码。如果跑通了,证明自己的环境没有问题再继续学习,学习时建议自己敲一遍代码。

2.1 矩阵初始化

Eigen 中所有向量和矩阵都是Eigen::Matrix,它是一个模板类。它的前三个参数为:数据类型,行,列

Eigen::Matrix<数据类型,行,列> matrix
  1. Eigen 头文件
#include <Eigen/Core>
#include <Eigen/Dense>
  1. 矩阵的声明、赋值、打印
int main()
{
    Eigen::Matrix<int, 3, 4> matrix;//int型$3*4$大小矩阵
    matrix << 1, 2, 3, 4, 5;        //只附了五个值
    cout << matrix << endl;
    return 0;
}

输出:

(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
         1          2          3          4
         5 1750436672          0  -87321568
 -87317235      32758  -87317312      21945
  1. Eigen 通过 typedef 提供了许多内置类型,不过底层仍是Eigen::Matrix

Eigen::Vector3d和 Eigen::Matrix<float,3,1>是一样的

int main()
{
    Eigen::Vector3d v3d1;
    cout<<v3d1<<endl;
    Eigen::Matrix<float,3,1> v3d2;
    cout<<v3d2<<endl;

    Eigen::Matrix3d m3d1;
    cout<<m3d1<<endl;
    Eigen::Matrix<float,3,3>m3d2;
    cout<<m3d1<<endl;
    
    return 0;
}

输出:

(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
6.90051e-310
6.90051e-310
4.94066e-324
 4.59121e-41
-1.12229e+31
 4.55674e-41
9.88131e-324 4.68625e-310 4.68625e-310
4.68625e-310 6.90051e-310 4.68625e-310
4.94066e-324            0 6.95256e-310
9.88131e-324 4.68625e-310 4.68625e-310
4.68625e-310 6.90051e-310 4.68625e-310
4.94066e-324            0 6.95256e-310
  1. 其他快速声明方法
  • 初始化为零
  • 动态大小的矩阵(不确定矩阵大小)
  • 更简单的
int main()
{
    Eigen::Matrix3d m3d = Eigen::Matrix3d::Zero();
    Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> m_Dy;
    Eigen::MatrixXd m_Xd;

    cout << m3d << endl
         << m_Dy << endl
         << m_Xd << endl;

    return 0;
}

输出:

(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
0 0 0
0 0 0
0 0 0
  1. for输出矩阵元素
int main()
{
    Eigen::Matrix3d m3d = Eigen::Matrix3d::Zero();

    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            cout << m3d(i, j) << endl;
        }
    }

    return 0;
}

输出:

(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
0
0
0
0
0
0
0
0
0

2.2 矩阵运算

这里主要展示矩阵-向量的乘法。

int main()
{
    Eigen::Matrix3d m3d;
    Eigen::Vector3d v3d;

    m3d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    v3d << 1, 2, 3;
    cout << m3d << endl
         << v3d << endl;

    Eigen::Matrix<double, 3, 1> result_matrix = m3d.cast<double>() * v3d;

    cout << result_matrix << endl;

    return 0;
}

输出:

(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
1 2 3
4 5 6
7 8 9
1
2
3
14
32
50

Eigen::Matrix<double, 3, 1> result_matrix = m3d.cast<double>() * v3d;中数据类型是double如果有一个是int就会报错:YOU_MIXED_DIFFERENT_NUMERIC_TYPES...

/home/ubuntu/hl/sl/ch3/useEigen/test/test.cpp:19:64:   required from here
/usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:32:40: error: static assertion failed: YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY

矩阵和矩阵的±*/就不演示了,直接运算就好了Eigen::Matrix2d result_matrix = m2d1 * m2d2;

2.3 矩阵变换

  • 随机数矩阵Eigen::Matrix3d::Random()
  • 转置transpose()
  • 各元素和sum()
  • 迹trace()
  • 数乘
  • 逆inverse()
  • 行列式determinant()
int main()
{
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
    cout << "随机数矩阵" << endl
         << matrix_33 << endl
         << endl;

    cout << "转置" << endl
         << matrix_33.transpose() << endl; // 转置
    cout << "各元素和" << endl
         << matrix_33.sum() << endl; // 各元素和
    cout << "迹" << endl
         << matrix_33.trace() << endl; // 迹
    cout << "数乘" << endl
         << 10 * matrix_33 << endl; // 数乘
    cout << "逆" << endl
         << matrix_33.inverse() << endl; // 逆
    cout << "行列式" << endl
         << matrix_33.determinant() << endl; // 行列式

    return 0;
}
(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
随机数矩阵
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451

转置
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
各元素和
1.61307
迹
1.05922
数乘
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
逆
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
行列式
0.208598

2.4 解方程

2.4.1 特征值/特征向量

int main()
{
    Eigen::Matrix3d matrix_33;
    matrix_33 << 1, 2, 3, 2, 1, 4, 3, 4, 1;
    cout << matrix_33 << endl;
    // 特征值
    // 实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "Eigen values = \n"
         << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n"
         << eigen_solver.eigenvectors() << endl;

    return 0;
}

关于Eigen::SelfAdjointEigenSolver的解释:参考1、参考2

(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
1 2 3
2 1 4
3 4 1
Eigen values = 
0.786398
 10.1626
  50.051
Eigen vectors = 
 0.824038 -0.255232  0.505785
-0.544925 -0.601302  0.584374
-0.154979  0.757161  0.634577

2.4.2 解方程

关于 A x = b Ax=b Ax=b的求解有两种:一种是直接法;一种是矩阵分解法。下面将展示两种方法的代码实现及其计算时长。

#define MATRIX_SIZE 5

using namespace std;
int main()
{
    //声明矩阵
    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    //声明向量
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);
    // 计时
    clock_t time_stt = clock(); 
    // 直接求逆
    Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << x << endl;
    cout << "time use in normal inverse is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "———————————————————————————" << endl;

    // 通常用矩阵分解来求,例如QR分解,速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << x << endl;
    cout << "time use in Qr decomposition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    return 0;
}

输出:

(base) ubuntu@ubuntu:~/hl/sl/ch3/useEigen/test/build$ ./eigenMatrix 
 0.750027
 -0.42772
0.0375574
  1.28299
  2.10191
time use in normal inverse is 0.052ms
———————————————————————————
 0.750027
 -0.42772
0.0375574
  1.28299
  2.10191
time use in Qr decomposition is 0.012ms

两种方法的结果一致,且矩阵分解法用时更短。

旋转矩阵推导过程 二维旋转矩阵和三维旋转矩阵的推导,以及旋转矩阵性质的列举。 阅读详情

相关推荐

使用Eigen计算三维坐标变换

本文讲述物体在三维空间的位姿表示与变换,欧拉角、四元数、旋转矩阵都是表示两个坐标系相对旋转关系,欧拉角有歧义,旋转矩阵有冗余,四元数不易理解,介绍了这三者相互转换公式以及如何使用Eigen库计算。

weixin_50431732的博客 9931

三维空间刚体运动之旋转矩阵与变换矩阵

刚体运动: 两个坐标系之间的运动变换由一个旋转加上一个平移组成,这种运动就是刚体运动。刚体运动过程中,同一个向量在各个坐标系下的长度和夹角都不会发生变化。此时,我们说手机坐标系和世界坐标系之间,相差了一个。我们经常在实际场景中定义各种各样的坐标系,如果考虑运动的机器人(即相机),那么常见的做法是设定一个惯性坐标系(或者叫世界坐标系),可以认为它是固定不动的。的行列式可以看出,它是一个行列式为1的正交矩阵。反过来说,行列式为1的正交矩阵也是一个旋转矩阵。旋转矩阵也有一些特别的性质,我们通过矩阵。

u014374826的博客 4048

《视觉SLAM》学习笔记(二)——ch3三维空间刚体运动

《视觉SLAM》学习笔记(二) 视频地址:https://www.bilibili.com/video/BV1Dz4y1S7gn?t=749 github地址:https://github.com/gaoxiang12 【第一版、第二版的代码】 第一节 《视觉SLAM》学习笔记(一) 第三节 《视觉SLAM》学习笔记(三)【敬请期待】 第四节 《视觉SLAM》学习笔记(四)【敬请期待】 … 【占坑后补】 ...

way7486chundan的博客 908

三维空间旋转矩阵求解(定角旋转矩阵和欧拉角旋转矩阵)

定角(Fixed angles) 围绕固定的坐标系转动。固定坐标系的原点,坐标系再围绕已经固定的轴转动,全程原坐标系不动。 注意!移动位置的顺序可以调换,但是旋转的顺序不能调换,结果不一样。 以X-Y-Z型为例子:即先围绕X轴进行转动γ°,然后围绕Y轴进行转动β°,最后围绕Z轴进行转动α°。注意逆时针为正方向。 X-Y-Z型公式: 重点:先转的轴的放后面运算,如下 举例: 由角度推旋转矩阵 由旋转矩阵推角度 欧拉角(Euler angles) “自旋转..

hy2014x的博客 7593

视觉slam十四讲实践部分记录——ch2 初识SLAM、ch3 三维空间刚体运动

/a.out运行。

m0_69023437的博客 1016

N维空间中的旋转方法(仅是数学推导,不是代码)

任意维数欧氏空间中的旋转矩阵推导。可以用作高维数据处理的快速算法。 注意:不是主轴化算法!不是主轴化算法!不是主轴化算法! 只有数学理论,没有代码,没有代码,没有代码。

视觉SLAM ch3—三维空间的刚体运动

配套高翔老师以及其他视频资源的视觉SLAM笔记。 里面的视频可以在指示下观看,四元数的两个视频很好,建议全部观看!!

Johaden的博客 1234

苹果iOS设备解锁软件:iToolab UnlockGo

iToolab UnlockGo是Mac上一款具有解锁屏幕密码、移除iCloud激活锁、移除Apple ID、关闭FMI、解锁屏幕使用时间和绕过MDM功能的苹果设备解锁工具,UnlockGo只需要几分钟,就能够快速为您解决IOS设备的密码遗忘、设备被禁用、脸部/触控ID不可用、密码已过期等问题! 即时解锁iPhone / iPad的密码 使用iToolab UnlockGo,您可以轻松绕过iPhone密码并获得对该设备的完全访问权限。它适用于以下情况,并且完全支持各种密码。UnlockGo支持轻松删除所有

macxingchendahai的博客 1万+

【视觉SLAM十四讲】ch3笔记【逐行代码带你解析】【适合纯小白 ps:因为我就是】(持续更新中)

上面我们知道了旋转矩阵跟旋转向量还有欧拉角都带有奇异性,所以我们引入四元数。用四元数来表达三维空间旋转时它既是紧凑的,也没有奇异性。它是一种类似于复数的代数。但是四元数也有缺点,它不够直观,而且运算稍微比较复杂。想要了解什么是四元数,我们得先了解复平面,下面给出了一个复平面的学习链接。大家如果不清楚什么是复平面的话,可以去看一下:链接:link以下是我学习到这里的时候做的一点小笔记(不许嫌弃我字丑!!!):下面开始讲四元数:一个四元数q拥有一个实部和三个虚部其中i,j,k。

R_ichun的博客 1529

如何用ISBN API快速获取图书信息?Python实战教程(附完整代码)

本文是一篇Python实战教程,详细介绍了如何利用ISBN API高效获取图书元数据。内容涵盖从API服务选择、密钥管理,到使用requests库构建健壮的请求模块,并利用Pydantic进行数据验证与解析。教程还提供了批量查询、缓存优化及集成到Flask Web应用等高级技巧,帮助开发者快速实现图书信息的自动化获取与处理。

p8q9r0的博客 458

Altium Concord Pro 的应用范围,安装,和使用范围

Altium Concord Pro 的应用范围,安装,和使用范围。保证满足两者软件的最低安装要求。 Altium Concord Pro 是配合Digapcb,和MCAD,Altium 和相应机械设计软件的基础和交换桥梁

hanzhaos的博客 1124

视觉slam十四讲ch3三维空间刚体运动(包含实践部分)

欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)在俯仰角为±90°时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由3次旋转变成了2次旋转)。这被称为奇异性问题,在其他形式的欧拉角中也同样存在。理论上可以证明,只要想用3个实数来表达三维旋转,都会不可避免地碰到奇异性问题。由于这种原理,欧拉角不适用于插值和迭代,往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角表达姿态,同样不会在滤波或优化中使用欧拉角表达旋转(因为它具有奇异性)。

shikaiaixuexi的博客 1116

Kill5.37版本下,使用ARMcomplier6编译__asm 函数报错的问题

Kill5-5.37版本下,使用ARMcomplier6编译__asm 函数报错的问题 问题简要描述: 昨日更新了keil5软件的版本,从5.17更新到了5.37版本,然后编译STM32项目文件时,软件提示编译报错。报错问题如下: Build started: Project: QKsoftware *** Using Compiler 'V6.18', folder: 'E:\MDK-ARM\ARM\ARM\ARMCLANG\Bin' Build target 'QKsoftware' compiling

weixin_43644424的博客 1万+

视觉SLAM十四讲学习笔记——ch3-三维刚体运动理论篇

文章目录3.1 旋转矩阵3.1.1点与坐标系3.1.2 坐标系中的欧式变换3.1.3 变换矩阵与齐次坐标3.2 Eigen库的使用3.3 旋转向量和欧拉角3.3.1 旋转向量3.3.2 欧拉角3.4四元数3.4.1 四元数的定义3.4.2 四元数的运算3.4.3 用四元数表示旋转3.4.4 四元数到旋转矩阵的转换3.5 补充小萝卜的例子解释,坐标在不同坐标系转换参考博客 3.1 旋转矩阵 3.1.1点与坐标系 点乘 叉乘 法向量的方向成右手法则,所以叉乘的顺序是会有影响的。 反对称矩阵 反对成矩阵(

路拾遗的博客 776

避坑实录:用certificate-manager重置vCenter 6.7证书时遇到的三个致命陷阱

本文详细解析了使用certificate-manager重置vCenter 6.7证书时可能遇到的三大致命陷阱,包括进度卡在85%的服务启动僵局、脚本突然退出的静默杀手以及密码错误锁死的隐藏机制。通过实战验证的解决方案和高级恢复技巧,帮助系统管理员有效避免vCenter环境瘫痪,确保证书更新过程顺利进行。

g5h6i7j的博客 920

TC3XX - MCAL知识点(二十七):SENT Mcal配置及代码实战(单边半字节传输协议)

SENT协议只有一根线,异步传输!Sent协议的全称为Single Edge Nibble Trans .isson,单边半字节传输协议,是一种单线异步串行通信协议。Sent报文包含8个Nibble和一个校准位如图1所示,每一个Nibble数据被编码成PWM方式。设计这种协议的目的是用在与汽车ECU快速交互信息的传感器上,以此来取代低精确度的10bit AD和PWM。Sent是一种点对点的单方向从传感器到控制端的协议,控制器不需要同步信号。

梦想技术家 2276

视觉SLAM十四讲学习笔记----ch3 Eigen代码实践

目录1. 编译2 .运行useEigen![ 运行](https://img-blog.csdnimg.cn/29d8df966a33465b9f89c6535938a6a9.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA6Lev5ou-6YGXMzc=,size_20,color_FFFFFF,t_70,g_se,x_16)3. Eigen几何模块 1. 编译 cmake:根据CMakeList.t

路拾遗的博客 2110

【tessy】软件环境配置:Keil C51 Keil uVision UVSC (Default)

本文详细说明了tessy软件如何配置【Keil C51 Keil uVision UVSC (Default)】环境,具体操作步骤和注意事项

qq_37282365的博客 602

SLAM十四讲ch3课后习题

配套高翔视觉SLAM中ch3部分的课后习题,其中第二题(对应书上第三题)的解题过程比大多数博客的解题过程更好理解,有不清楚或错误的地方请多指正。

Johaden的博客 1121

QT+Halcon实现螺纹检测

工业上要检测螺丝的内外直径、内外牙距、内外角度,现在比较好用的图像处理库,开源的opencv,收费的有halcon。为了实现螺纹检测,和对比螺纹检测拿个库比较好。分别用opencv和halcon实现了螺纹检测,通过对比发现用halcon实现的效果比较。现在用博客把实现过程记录下来: 1、需要检测的螺纹数据如下: 2、为了实现检测螺丝以上数据,螺纹内径、螺纹外径、螺纹外牙距、螺纹...

专注现代C++实战,从核心语法到高性能编程,助力开发者深入掌握系统级编程精髓。 7056

efm32 入门.pdf

本PDF共计22页,介绍的是操作驱动API,下载与否自己斟酌。

[学习SLAM]Eigen库使用教程之旋转矩阵,旋转向量和四元数的初始化和相互转换的实现 (ch3)

Eigen: C++开源矩阵计算工具——Eigen的简单用法 Eigen库是一个开源的C++线性代数库,它提供了快速的有关矩阵的线性代数运算,还包括解方程等功能。Eigen是一个用纯头文件搭建起来的库,这意味这你只要能找到它的头文件,就能使用它。Eigen头文件的默认位置是“/usr/include/eigen3”. 由于Eigen库相较于OpenCV中的Mat等库而言更加高效...

开源节流 1904

怀旧版电脑GBC模拟器

再现原版GBC掌机的界面,电脑运行连游戏界面都一样大小,很怀旧,很真实,(英文版)强烈推荐,很小的文件,很难才找到的。

视觉slam14讲 ch3习题

视觉slam14讲

sinat_56310865的博客 331

pscad IEEE标准系统搭建说明

pscad IEEE标准系统搭建说明,包含典型的IEEE 9节点、14节点、30节点、39节点和118节点系统。内含标准测试系统的原始数据和潮流结果。

汉书HE8,好用的汇编代码编写工具

汉书HE8,好用的汇编代码编写工具早期编写汇编代码所用功能强大

视觉SLAM十四讲 ch3 (三维空间刚体运动)笔记

视觉SLAM十四讲 ch3 (三维空间刚体运动):旋转矩阵、变换矩阵、角轴、四元数和欧拉角。

2019菜鸡进化史 520

<视觉SLAM十四讲>ch3 刚体运动

一、关于Eigen Eigen纯纯头文件,没有.so或者.a的二进制文件,不需要链接库文件。(题外话:今早运行French代码vtk的库文件的路径保存好后一直没让他生效,总报错) Vector3d v_3d;//声明3*1的float矩阵 Matrix<float, 3, 1> vd_3d;// 这是一样的 matrix_23 << 1, 2, 3, 4, 5, 6;// 输入数据(初始化) cout << "matrix 2x3 from 1 to 6: \n" &l

weixin_40440816的博客 718

【读书笔记】《视觉SLAM十四讲(高翔著)》 第3讲

在上讲中,我们讲解了视觉 SLAM 的框架与内容。本讲将介绍视觉 SLAM 的基本问 题之一: 一个刚体在三维空间中的运动是如何描述的。

dahailuoa6的博客 774

【视觉SLAM】十四讲ch3习题

由于笔者查找的罗德里格斯公式的推导,符号都比较混乱,笔者重新书写一遍仅供参考。

weixin_68880273的博客 1095
上一篇: 视觉SLAM十四讲学习笔记——初识SLAM(ch1-2)
下一篇: 视觉SLAM十四讲学习笔记——三维空间刚体运动(ch3~旋转向量和欧拉角)
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值
⌂ 换网址