文章目录
本节目标
理解三维空间的刚体运动描述方式:旋转矩阵、变换矩阵、四元数和欧拉角。
掌握 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]
a1a2a3
=a1e1+a2e2+a3e3.
坐标的具体取值,一个是和向量本身有关,第二也和坐标系的选取有关。给定 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∑aibi=∣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= ia1b1ja2b2ka3b3 = a2b3−a3b2a3b1−a1b3a1b2−a2b1 = 0a3−a2−a30a1a2−a10 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] a1a2a3 =[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] 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 ′ ] ≜ 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}^{'}. a1a2a3 = e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3e3Te3′ 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=R1a+t1,c=R2b+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(R1a+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]=[R0Tt1][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=[R0Tt1]具有比较特别的结构:
- 左上角为旋转矩阵 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=[R0Tt1]∈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 小节
- 向量在坐标系中的表达:
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] a1a2a3 =a1e1+a2e2+a3e3. - 两向量的内积:
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∑aibi=∣a∣∣b∣cos⟨a,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= ia1b1ja2b2ka3b3 = a2b3−a3b2a3b1−a1b3a1b2−a2b1 = 0a3−a2−a30a1a2−a10 b=Δa†b. - 欧式变换——旋转矩阵:
[ 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] a1a2a3 =[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}^{'}. a1a2a3 = e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3e3Te3′ a1′a2′a3′ ≜Ra′. -
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}. - 反向旋转:
a ′ = R − 1 a = R T a . \boldsymbol{a^{'}}=\boldsymbol{R^{-1}a}=\boldsymbol{R^{T}a}. a′=R−1a=RTa. - 欧式变换:
a ′ = R a + t . \boldsymbol{a}^{'}=\boldsymbol{Ra}+\boldsymbol{t}. a′=Ra+t. - 齐次坐标定义:一个三维向量的末尾添加 1,变成了四维向量。
- 欧式变换的线性变换:
[ 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]=[R0Tt1][a1]ΔT[a1]. - 变换矩阵 T \boldsymbol{T} T= [ R t 0 T 1 ] \left[\begin{array}{cc}R&t\\ 0^T&1\end{array}\right] [R0Tt1]
-
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=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}. -
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
- Eigen 头文件
#include <Eigen/Core>
#include <Eigen/Dense>
- 矩阵的声明、赋值、打印
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
- 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
- 其他快速声明方法
- 初始化为零
- 动态大小的矩阵(不确定矩阵大小)
- 更简单的
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
- 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
两种方法的结果一致,且矩阵分解法用时更短。