Eigen之矩阵基本操作
开篇介绍
在实际应用中，当要进行姿态解算时，必不可少的便是矩阵操作，这里我们引入Eigen这一矩阵操作库。Eigen是一个C++开源线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。本博客通过一些实例应用，演示Eigen下矩阵逆运算、伴随矩阵运算、行列式计算和特征值、特征向量的计算等等。
一、旋转矩阵
我们知道向量的内积可以描述向量之间的投影关系，而向量的外积，能表示向量的旋转。在欧氏变换中，首先来考虑旋转。我们设某个单位正交基 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) (e1,e2,e3)经过一次旋转，变成了 ( e 1 ′ , e 2 ′ , e 3 ′ ) (e_{1}^{'},e_{2}^{'},e_{3}^{'}) (e1′,e2′,e3′)。那么，对于同一个向量a，它在两个坐标系下的坐标为 [ a 1 , a 2 , a 3 ] T [a_1,a_2,a_3]^T [a1,a2,a3]T和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1^{'},a_2^{'},a_3^{'}]^T [a1′,a2′,a3′]T。根据坐标的定义，有： [ e 1 , e 2 , e 3 ] [ a 1 , a 2 , a 3 ] T = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ , a 2 ′ , a 3 ′ ] T \left [ e_{1},e_2,e_3 \right ][a_1,a_2,a_3]^T=[e_1^{'},e_2^{'},e_3^{'}][a_1^{'},a_2^{'},a_3^{'}]^T [e1,e2,e3][a1,a2,a3]T=[e1′,e2′,e3′][a1′,a2′,a3′]T
为了描述两个坐标之间的关系，对上面等式左右同时左乘 [ e 1 T , e 2 T , e 3 T ] T [e_1^T,e_2^T,e_3^T]^T [e1T,e2T,e3T]T，那么左边的系数变成了单位矩阵，所以： [ a 1 , a 2 , a 3 ] T = [ 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 ′ [a_1,a_2,a_3]^T=\begin{bmatrix} 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{bmatrix} \begin{bmatrix} a_1^{'}\\ a_2^{'}\\ a_3^{'} \end{bmatrix}\triangleq Ra^{'} [a1,a2,a3]T=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
我们把中间阵拿出来，定义成一个矩阵R。这个矩阵由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。可以说，矩阵R描述了旋转本身。因此它又称为旋转矩阵。
旋转矩阵是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，我们可以把旋转矩阵的集合定义如下：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\left \{ R\in \mathrm{R}^{n×n}|RR^T=I,det(R)=1 \right \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
SO(n)是特殊正交群。这个集合由n维空间的旋转矩阵组成，特别的，SO(3)就是三维空间的旋转。通过旋转矩阵，我们可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起了。换句话说，旋转矩阵可以描述相机的旋转。
由于旋转矩阵为正交阵，它的逆描述了一个相反的旋转。按照上面的定义方式，有：
a ′ = R − 1 a = R T a a^{'}=R^{-1}a=R^{T}a a′=R−1a=RTa
然而，在欧氏变换中，除了旋转之外还有一个平移。考虑世界坐标系中的向量a，经过一次旋转(用R描述)和一次平移t后，得到了 a ′ a^{'} a′，那么把旋转和平移合到一起，有 a ′ = R a + t a^{'}=Ra+t a′=Ra+t
其中t为平移向量。
二、变换矩阵
旋转矩阵完整的表达了欧氏空间的平移和旋转，但是还存在一个小问题：这里的变换关系不是一个线性关系。假设进行了两次变换： R 1 , t 1 R_1,t_1 R1,t1和 R 2 , t 2 R_2,t_2 R2,t2，满足： b = R 1 a + t 1 , c = R 2 b + t 2 b=R_1a+t_1,c=R_2b+t_2 b=R1a+t1,c=R2b+t2
但是从a到c的变换为： c = R 2 ( R 1 a + t 1 ) + t 2 c=R_2(R_1a+t_1)+t_2 c=R2(R1a+t1)+t2
这样的形式在变换多次后会显得过于复杂。因此我们要引入齐次坐标和变换矩阵重写旋转矩阵的变换等式： [ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] ≜ T [ a 1 ] \begin{bmatrix} \mathbf{a}^{'}\\ 1 \end{bmatrix}= \begin{bmatrix} \mathbf{R}&\mathbf{t} \\ \mathbf{0}^T& 1 \end{bmatrix}\begin{bmatrix} \mathbf{a}\\ 1 \end{bmatrix}\triangleq T\begin{bmatrix} \mathbf{a}\\ 1 \end{bmatrix} [a′1]=[R0Tt1][a1]≜T[a1]
这是一个数学技巧：我们把一个三维向量末尾添加1，变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。该式中，矩阵T称为变换矩阵。同样地，根据分块矩阵求逆的方法，我们可以轻易的求出变换矩阵的逆： T − 1 = [ R T − R T t 0 T 1 ] \mathbf{T}^{-1}=\begin{bmatrix} \mathbf{R}^T&\mathbf{-R}^Tt \\ \mathbf{0}^T& 1 \end{bmatrix} T−1=[RT0T−RTt1]
三、Eigen实践
在清楚了旋转矩阵和变换矩阵后，我们可以在Kdevelop上尝试着操作矩阵。
点击构建后直接执行，在调试窗口显示打印结果如下：
四、总结
在初步实践了Eigen的矩阵操作后，我们很容易便求得了想要矩阵的一些特征，以及一些矩阵运算，这一切都为后续的姿态解算和后端处理提供坚实基础。