SLAM——刚体运动以及Eigen

Posted on Edited on In

这次主要介绍一些刚体运动中需要的数学知识以及Eigen库的基本使用。

什么是刚体运动？运动过程中不会发生形变。所以实际上刚体的运动也就只有两种：旋转和平移。

对于三维空间的旋转平移表示，之前的计算机图形学中介绍的比较清楚了。不过还有一些别的概念没有接触，在这里做个补充。

旋转向量

之前的旋转使用的是旋转矩阵，旋转矩阵是单位正交矩阵（行列式为1）。实际上会有个问题：旋转操作只需要3个自由度，但是却用了9个量来表示。这说明使用旋转矩阵是有冗余的。因此这里介绍旋转向量。

我们知道任何旋转都可以使用一个旋转轴和旋转角来描述。旋转向量是一个非常聪明的做法。它的方向代表了旋转轴，而它的长度代表了旋转的角度。

那么旋转向量与旋转矩阵如何转换呢？如果旋转轴（单位向量）为𝒏\mathbf{n}，旋转角度为θ\theta，那么旋转向量为θ𝒏\theta\mathbf{n}，旋转矩阵可以使用Rodrigues公式来计算出来： R=cos⁡θI+(1−cos⁡θ)𝒏𝒏T+sin⁡θ𝒏^ R = \cos \theta I + (1-\cos \theta)\mathbf{nn}^T + \sin \theta \mathbf{n}^\^

𝒏^\mathbf{n}^\^为向量𝒏\mathbf{n}的对偶矩阵。这个公式也在之前的图形学博客中做过介绍了。

如何从旋转矩阵得到旋转向量？

首先，对于旋转角：

tr(R)=cos⁡θtr(I)+(1−cos⁡θ)tr(𝒏𝒏T)+sin⁡θtr(𝒏^)=3cos⁡θ+(1−cos⁡θ)=1+2cos⁡θ \begin{aligned} tr(R) &= \cos\theta tr(I) +(1 - \cos \theta) tr(\mathbf{nn}^T) + \sin \theta tr (\mathbf{n}^\^)\\ &= 3\cos \theta + (1-\cos \theta)\\ &= 1+ 2\cos\theta \end{aligned}

所以根据上式可以很简单求得： θ=arccos⁡(tr(R)−12) \theta = \arccos\left( \frac{tr(R)-1}{2}\right)

至于𝒏\mathbf{n}，由于旋转轴的向量旋转后不会发生变化，因此R𝒏=𝒏R\mathbf{n} = \mathbf{n}.

所以𝒏\mathbf{n}为矩阵R特征值为1的对应的特征向量。

欧拉角

旋转向量以及旋转矩阵对于人的角度来说都不够直观。因此有了欧拉角的诞生。其实欧拉角就是将一个旋转转化为3个绕坐标轴的转动。因此这个转动的顺序就不唯一了。在航空里可能经常听到“偏航-俯仰-滚转”（yam-pitch-roll），实际上就是欧拉角的一种，等价与ZYX轴的旋转。

因此欧拉角就是用一个向量[r,p,y]T[ r,p,y ]^T（3个角度）来表示一个旋转。但是欧拉角有个著名的万能锁问题。如果pitch转动了±90°\pm90°,则最后一个转动绕的轴实际上和z轴一样，这就使得损失了一个自由度，这被称为奇异性问题（？）。因此欧拉角不适用与插值和迭代，往往用于人工交互。

四元数

旋转矩阵用9个量描述，有冗余，而旋转向量和欧拉角却具有奇异性。实际上我们找不到不带奇异性的三维向量描述方式。 因此在这里再介绍一个四元数。它用四个量来描述旋转足够紧凑同时也没有奇异性。

一个四元数𝒒=q0+q1i+q2j+q3k\mathbf{q} = q_0+q_1i+q_2j+q_3k,有一个实部，3个虚部。其中虚部满足下面的一组式子： {i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j \left\{ \begin{aligned} i^2 = j^2 = k^2 = -1\\ ij=k,ji = -k\\ jk=i,kj = -i\\ ki=j,ik = -j \end{aligned} \right.

有时候也使用一个标量和一个向量来表示四元数：

$ = [ s, ],s = q_0 , = [ q_1,q_2,q_3 ]^T ^3 $.

如果实部为0,称为虚四元数，如果虚部为0称为实四元数。

我们能用单位四元数表示三维空间中的任意旋转。不过由于复数的引入，它的表示是有点反直觉的。假如旋转向量为𝒏θ\mathbf{n} \theta，则对应的四元数为： 𝒒=[cos⁡θ2,nxsin⁡θ2,nysin⁡θ2,nz,θ2]T. \mathbf{q} = [ \cos \frac \theta 2, n_x\sin \frac \theta 2 ,n_y \sin \frac \theta 2,n_z,\frac \theta 2]^T.

因此从单位四元数中也很容易得到对应的旋转轴和旋转角度： {θ=2arccos⁡q0;[nx,ny,nz]=[q1,q2,q3]T/sin⁡θ2 \left \{ \begin{aligned} \theta = 2 \arccos q_0; [ n_x,n_y,n_z] = [ q_1, q_2,q_3]^T / \sin \frac \theta 2 \end{aligned} \right .

这个式子确实反直觉，给我们赚了一般的感觉。如果对$$加上2π2\pi，相当于没有转动，但是他们对应的四元数却变成原来的相反数了。因此，互为四元数的相反数表示同一个旋转。

四元数的概念的话现在还很懵比，不知道为什么要这样来表示一个旋转。但是能发展到现在还留下来的一定是有自己的道理。因为四元数每个值都是经过处理的轴和角结合，因此它方便进行插值。听说四元数在游戏开发里应用广泛。具体这些以后再去弄明白，先看一看四元数的基本运算吧。

加减

$_a _b = [ s_a s_b ,_a _b] $ ### 乘法 ### 乘法就是各项轮着相称，最后相加，虚部要按照虚部的规则来乘，最后得到结果： 𝒒a𝒒b=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb) \begin{aligned} \mathbf{q}_a \mathbf{q}_b &= s_as_b - x_ax_b - y_ay_b - z_az_b\\ &+(s_ax_b+x_as_b+y_az_b-z_ay_b)i\\ &+(s_ay_b - x_az_b + y_as_b + z_ax_b)j\\ &+(s_az_b+x_ay_b-y_ax_b+z_as_b) \end{aligned}

如果写成向量形式： 𝒒a𝒒b=[sasb−𝒗aT𝒗b,sa𝒗b+sb𝒗a+𝒗a×𝒗b] \mathbf{q}_a \mathbf{q}_b = [ s_as_b - \mathbf{v}_a^T\mathbf{v}_b, s_a\mathbf{v}_b + s_b\mathbf{v}_a + \mathbf{v}_a\times \mathbf{v}_b] 由于最后一项存在，四元数的乘法通常是不可交换的。 ### 共轭 ### 𝒒a*=sa−xai−yaj−zak=[sa,−𝒗a]\mathbf{q}_a^* = s_a - x_ai - y_aj-z_ak = [ s_a,-\mathbf{v}_a]

𝒒*𝒒=𝒒𝒒*=[sa2+𝒗𝒗T,0]\mathbf{q^ *q} = \mathbf{qq^ *} = [s_a^2 + \mathbf{vv}^T ,0]

可以看到四元数和共轭相称得到一个实数。 ### 模长 ### ‖𝒒‖=s2+x2+y2+z2 \Vert \mathbf{q}\Vert = \sqrt{s^2 +x^2+y^2+z^2} 可以验证：‖𝒒a𝒒b‖=‖𝒒a‖‖𝒒b‖\Vert \mathbf{q}_a\mathbf{q}_b \Vert = \Vert \mathbf{q}_a\Vert \Vert \mathbf{q}_b\Vert.

这保证了单位四元数的乘积依然为单位四元数。

逆

𝒒−1=𝒒*‖𝒒‖2\mathbf{q}^{-1} = \frac {\mathbf{q}^*}{\Vert \mathbf{q}\Vert^2}

𝒒−1𝒒=𝒒𝒒−1=1\mathbf{q}^{-1}\mathbf{q} = \mathbf{qq}^{-1} = 1

同时可以知道，单位四元数的逆就是单位四元数的共轭，因为𝒒𝒒*=1\mathbf{qq}^* = 1.

乘积的逆和矩阵的逆有同样的性质：(𝒒𝒂𝒒𝒃)−1=𝒒𝒃−1𝒒𝒂−1(\mathbf{q_a}\mathbf{q_b})^{-1} = \mathbf{q_b}^{-1}\mathbf{q_a}^{-1}

数乘和点乘

k𝒒=[ks,k𝒗]k\mathbf{q} = [ks,k\mathbf{v} ]

𝒒𝒂⋅𝒒𝒃=sasb+xaxb+yayb+zazb\mathbf{q_a} \cdot \mathbf{q_b} = s_as_b + x_ax_b + y_ay_b + z_az_b

用四元数表示旋转

对于一个三维点p=[x,y,z]p=[x,y,z ],绕着转轴𝒏θ\mathbf{n}\theta旋转，变为p′p'.我们知道使用矩阵的话，可以这样描述：p′=Rpp' = Rp.但是使用四元数如何描述呢？

我们把空间的点用虚四元数来描述，则𝒑=[0,x,y,z]\mathbf{p} = [0,x,y,z ].

用𝒒\mathbf{q}来表示旋转: 𝒒=[cos⁡θ2,𝒏sin⁡θ2]\mathbf{q} = [\cos \frac \theta 2,\mathbf{n} \sin \theta 2].

则：𝒑′=𝒒𝒑𝒒−1\mathbf{p}' = \mathbf{qpq}^{-1}.

可以验证的是经过计算的实部为0,虚部对应的就是q′q'的坐标点。

可以看到使用四元数来旋转的话也是非常方便的。

四元数和旋转矩阵的转换

四元数与旋转矩阵的转换，我们可以想到的是利用旋转向量来做中间的桥梁。不过其中有个arccos函数代价较大，但是实际上可以通过一定的技巧绕过。在这里直接给出四元数到旋转矩阵的转换结果（省略推导过程）：

设四元数为：𝒒=q0+q1i+q2j+q3k\mathbf{q} = q_0+q_1i+q_2j+q_3k,则： R=[1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22] R = \begin{bmatrix} 1-2q_2^2 - 2q_3^2&2q_1q_2 - 2q_0q_3& 2q_1q_3 + 2q_0q_2\\ 2q_1q_2+2q_0q_3& 1-2q_1^2-2q_3^2& 2q_2q_3 - 2q_0q_1 \\ 2q_1q_3-2q_0q_2&2q_2q_3+2q_0q_1&1 - 2q_1^2 - 2q_2^2 \end{bmatrix}

如果知道了旋转矩阵，想要得到四元数：

q0=tr(R)+12,q1=r2,3−r3,24q0,q2=r3,1−r1,34q0,q3=r1,2−r2,14q0q_0 = \frac{\sqrt{tr(R) + 1} }{2},q_1 = \frac{r_ {2,3} - r_ {3,2} }{4q_0},q_2 = \frac{r_ {3,1} - r_ {1,3} }{4q_0},q_3 = \frac{r_ {1,2} - r_ {2,1} }{4q_0}

这里面ri,jr_ {i,j}表示RR的第i行j列。在计算过程中，如果q0q_0接近于0,则其他3个量就会很大，这是很需要考虑别的方式来表示旋转。 ## 相似，仿射，射影变换 ##

相似变换

相似变换比之前的欧式变换多了一个自由度： TS=[sR𝒕01] T_S = \begin{bmatrix} sR&\mathbf{t}\\ 0&1 \end{bmatrix}

这个s允许我们对物体进行均匀缩放。

仿射变换

TA=[A𝒕01] T_A = \begin{bmatrix} A&\mathbf{t}\\ 0&1 \end{bmatrix} 仿射变换不要求A为正交矩阵，只要可逆即可。仿射变换又叫正交投影变换。 ### 射影变换 ### 射影变换是最易般的变换。 TP=[sR𝒕𝒂Tv] T_P = \begin{bmatrix} sR&\mathbf{t}\\ \mathbf{a}^T&v \end{bmatrix} 左上角可逆，右上角为平移t,左下角为缩放𝒂T\mathbf{a}^T

从真实世界到相机照片的变换可以看作为一个射影变换。如果焦距为无限远，则为仿射变换。

Eigen

最后介绍一些Eigen库相关的东西。Eigen是一个C++开源线性代数库.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
#include<eigen/core>
#include<eigen/dense>
/*3*3矩阵float型*/
Eigen::Matrix<float,3,3,> matrix_33;
/*3维向量，但实际上就是Eigen::Matrix<double,3,1>*/
Eigen::Vector3d v_3d;
//输入
v_3d<<1,2,3;
//输出
cout<<v_3d<<endl;
//访问i行j列
cout<<v_3d(1,0);
//转置
matrix_33.transpose();
//各项和
matrix_33.sum();
//迹
matrix_33.trace();
//逆
matrix_33.inverse();
//行列式
matrix_33.determinant();
//特征值和特征向量，实对称矩阵确保对角化成功
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33*matrix_33.transpose());
cout << eigen_solver.eigenvalues() << endl;
cout<< eigen_solver.eigenvectors() << endl;

Module Header file Contents
Core #include < Eigen/Core > Matrix和Array类，基础的线性代数运算和数组操作
Geometry #include< Eigen/Geometry > 旋转、平移、缩放、2维和3维的各种变换
LU #include< Eigen/LU > 求逆，行列式，LU分解
Cholesky #include < Eigen/Cholesky > LLT和LDLT Cholesky分解
Householder #include< Eigen/Householder > 豪斯霍尔德变换，用于线性代数运算
SVD #include< Eigen/SVD > SVD分解
QR #include< Eigen/QR > QR分解
Eigenvalues #include< Eigen/Eigenvalues > 特征值，特征向量分解
Sparse #include< Eigen/Sparse > 稀疏矩阵的存储和一些基本的线性运算
稠密矩阵 #include< Eigen/Dense > 包含Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块
矩阵 #include< Eigen/Eigen > 包括Dense和Sparse(整合库)

这些东西都被整合在dense模块中。

eigen几何模块：

旋转矩阵直接使用 Matrix3d 或 Matrix3f：

Eigen::Matrix3d rotationMatrix=Eigen::Matrix3d::Identity();//初始化为一个单位阵。

旋转向量使用 AngleAxis：

Eigen::AngleAxisd rotationVector(M_PI/4,Eigen::Vector3d(0,0,1)); //角+轴：沿 Z 轴旋转 45 度

欧拉角：

Eigen::Vector3d ea0(yaw,pitching,droll);

旋转向量->旋转矩阵：rotationMatrix=rotation_vector.toRotationMatrix();

旋转向量->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );

旋转矩阵->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_matrix );

四元素->旋转矩阵：Eigen::Matrix3d Rx = q.toRotationMatrix();

旋转向量->欧拉角：Eigen::Vector3d eulerAngle=rotationVector.matrix().eulerAngles(0,1,2);

旋转矩阵->欧拉角：Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序

Note

这一讲最后说明了世界坐标和相机坐标的转换。世界坐标下的坐标为𝒑w\mathbf{p}_w，相机坐标下的坐标为mathbfpcmathbf{p}_c，则二者转换为： 𝒑w=Tc2w𝒑c \mathbf{p}_w = T_ {c2w}\mathbf{p}_c

𝒑c=Tw2c𝒑w \mathbf{p}_c = T_ {w2c}\mathbf{p}_w

值得注意的是，它提到了一般更常用的是从世界坐标到相机坐标的转换，但是从相机坐标到世界坐标的转换却更直观。因为如果相机坐标𝒑c\mathbf{p}_c下为0,则世界坐标𝒑w\mathbf{p}_w就是相机在世界坐标的位置: 𝒑w=Tc2w=tc2w \mathbf{p}_w = T_ {c2w} = t_ {c2w}

上式中tc2wt_ {c2w}正是相机的位置。也是平移向量。

所以从相机坐标到世界坐标的转换可以直接看到相机位置。这也是因为从相机坐标到世界坐标的转换是先旋转后平移的特性。先旋转后平移，则平移向量是不用改变的。