视觉SLAM十四讲 读书编程笔记 Chapte3 三维空间刚体运动

旋转矩阵

内积和外积

内积：对于三维实数空间中的两个向量a,b,内积=a^Tb=a₀b₀+a₁b₁+a₂b₂=|a||b|cos<a,b>，内积可以描述两个向量间的投影关系。
外积：外积的方向垂直与这两个向量，大小为|a||b|sin<a,b>,是两个向量张成的四边形的有向面积。

值得强调的是，对于外积，我们引入了^符号，用这个符号将向量a转换成了一个反对称矩阵，所以该符号可以被看成是一个反对称符号。利用这个反对称符号就可以把外积a×b写成矩阵与向量的乘法，从而把它变成了线性运算。外积可以被用来表示向量的旋转。

w表示旋转向量，它的方向同时垂直于向量a和b，模值反映了旋转角度的大小。

坐标间的欧式变换

对于一个向量p，它在世界坐标系下的坐标p_w和在相机坐标系下的坐标p_c的变换，可以通过坐标系间的变换矩阵T来描述。

相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化，这种变换称为欧式变换。
欧式变换由一个旋转和一个平移两部分组成。
首先考虑旋转：
假设某个单位正交基(e₁,e₂,e₃)经过一次旋转变成了(e₁^’,e₂^’,e₃^’).那么对于同一个向量a，它在两个坐标系下的坐标为[a₁,a₂,a₃]^T和[a₁^’,a₂^’,a₃^’]^T,那么有：

从而

我们把中间的矩阵定义成旋转矩阵R，它描述了同一个向量在旋转前后的坐标变换关系。旋转矩阵是一个行列式为1的正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵，可以把旋转矩阵的集合定义如下：

SO(n)是特殊正交群的意思，显然这里的特殊包括行列式等于1这条性质。通过旋转矩阵而不是基来描述相机的旋转显然更加简洁专业。
由于旋转矩阵为正交矩阵，它的逆(即转置)描述了一个相反的旋转。
a^’=R^-1a=R^Ta

再考虑平移：
a^’=Ra+t
其中，t表示平移向量，通过上式，我们用一个旋转矩阵R和一个平移矩阵t完整地描述了一个欧式空间的坐标变换关系。

变换矩阵与齐次坐标

非齐次坐标表示多次坐标变换是很复杂的，例如，假设我们进行了两次坐标变换R₁,t₁和R₂,t₂,满足b=R₁a+t₁,c=R₂b+t₂,但是从a到c的变换为c=R₂(R₁a+t₁)+t₂,如果再继续多次坐标变换将会更加复杂，究其原因，发现每次变换都要加t,如果引入齐次坐标，式子将会变得非常简洁。

其中，T称为变换矩阵，它具有比较特别的结构：左上角为旋转矩阵R,右侧为平移矩阵t,左下角为0向量，右下角为1,这种矩阵又称为特殊欧式群。

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

为了保持符号的简洁，对齐次坐标的符号与普通坐标的符号不加以区分，默认使用符合运算法则的那一种。
因此，可以将b=T₁a,c=T₂b表示成c=T₂T₁a,形式非常简洁。

实践：Eigen

1.ubuntu下Eigen的安装

sudo apt-get install libeigen3-dev

Eigen头文件的默认位置在"/usr/include/eigen3/"中.如果不确定，可以输入以下命令查找：

sudo updatedb
locate eigen3

2.Eigen头文件

//Eigen部分
#include 
//稠密矩阵的代数运算
#include 

3.Eigen::Matrix()

Eigen以矩阵为基本数据单元。它是一个模板类。它的前三个参数为：数据类型，行，列。
例如：声明一个2行3列的float矩阵

Eigen::Matrix matrix_23;

Vector3d实质上是Eigen::Matrix
Matrix3d实质上是Eigen::Matrix
//动态大小的矩阵
Eigen::Matrix matrix_dynamic;
//更简单的动态大小矩阵
Eigen::MatrixXd matrix_x;

4.用<<为矩阵输入数据

matrix_23<<1,2,3,4,5,6;

5.用()访问矩阵中的元素

matrix_23(i,j);//访问第i行第j列的数据

6.矩阵相乘

注意：这里不能直接混合两种不同类型的矩阵相乘,应该显式转换

v_3d<<3,2,1;
Eigen::Matrix result = matrix_23.cast() * v_3d;

7.矩阵转置

matrix_33.transpose()

8.矩阵各个元素的和

matrix_33.sum()

9,矩阵的迹

matrix_33.trace()

10.矩阵数乘

10*matrix_33

11.矩阵直接求逆

matrix_33.inverse()

12.求矩阵的行列式

matrix_33.determinant()

13.生成随机矩阵

Eigen::Matrix3d::Random()
Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE)等

14.求特征值和特征向量

Eigen::SelfAdjointEigenSolver eigen_solver(matrix_33.transpose() * matrix_33);
cout<< "Eigen values = " << eigen_solver.eigenvalues() <

完整代码：

#include 
#include 
using namespace std;

//Eigen部分
#include 
//稠密矩阵的代数运算（逆、特征值等）
#include 

#define MATRIX_SIZE 50


//演示Eigen基本类型的使用
int main()
{
	//Eigen以矩阵为基本数据单元。它是一个模板类。它的前三个参数为：数据类型，行，列
	//声明一个2×3的float矩阵
	Eigen::Matrix matrix_23;

	//同时，Eigen通过typedef提供了许多内置类型，不过底层仍是Eigen:Matrix
	//例如，Vector3d实质上是Eigen::Matrix
	Eigen::Vector3d v_3d;

	//还有Matrix3d实质上是Eigen::Matrix
	Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();//初始化为0
	
	//如果不确定矩阵大小，可以使用动态大小的矩阵
	Eigen::Matrix matrix_dynamic;

	//更简单的
	Eigen::MatrixXd matrix_x;

	



	//下面是对矩阵的操作
	//输入数据
	matrix_23<<1,2,3,4,5,6;
	//输出
	cout< result_wrong_type = matrix_23 * v_3d;
	//应该显式转换
	Eigen::Matrix result = matrix_23.cast() * v_3d;
	cout << result << endl;
	
	//同样也不能搞错矩阵的维度
	//Eigen::Matrix result_wrong_dimension = matrix_23 * v_3d;


	
	//一些矩阵运算
	matrix_33 = Eigen::Matrix3d::Random();//随机生成一些数据
	cout << matrix_33 << endl;
	
	cout<< matrix_33.transpose() < eigen_solver(matrix_33.transpose() * matrix_33);
	cout<< "Eigen values = " << eigen_solver.eigenvalues() <

CMakeLists.txt文件

#添加Eigen的头文件
include_directories("/usr/include/eigen3")

#添加一个可执行程序
add_executable( eigenMatrix eigenMatrix.cpp )

运行结果：

旋转向量和欧拉角

旋转向量

引入旋转向量的原因：
1.旋转矩阵具有冗余性：SO(3)的旋转矩阵具有9个量，但是一次旋转本质上只有3个自由度。同理，变换矩阵用16个量表达了6个自由度，也具有冗余性。
2.旋转矩阵自身带有约束：它必须是行列式为1的正交矩阵，这些约束将使得优化一个旋转矩阵\变换矩阵变得困难。

旋转向量：
使用一个向量，其方向n与旋转轴一致，而长度等旋转角theta，这种向量称为旋转向量。
同理，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。

旋转向量与旋转矩阵的相互转换由罗德里格斯公式表明：
由旋转向量转换成旋转矩阵：

由旋转矩阵转化为旋转向量：



转轴n是旋转矩阵R特征值1对应的特征向量。

欧拉角

rpy欧拉角采用的旋转顺序是ZYX，ZYX转角相当于把任意旋转分解成以下三个轴上的转角：
1.绕物体的Z轴旋转，得到偏航角yaw
2.绕旋转之后的Y轴旋转，得到俯仰角pitch
3.绕旋转之后的X轴旋转，得到转滚角roll
此时，可以使用[r,p,y]^T这样一个三维的向量描述任意旋转。
欧拉角的一个重大缺点是会碰到著名的万向锁问题：在俯仰角为±90度时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度(由三次旋转变成了两次旋转)。

四元数

四元数的定义

旋转矩阵表达不紧凑，旋转向量表达具有奇异性，事实上，我们找不到不带奇异性的三维向量描述方式。
四元数是一种扩展的复数，它既是紧凑的，也没有奇异性，但是其表达不够直观，运算也稍复杂。
定义：一个四元数q拥有一个实部和三个虚部。q=q₀+q₁i+q₂j+q₃k,其中这三个虚部满足以下关系式：

用用一个标量和一个向量来表达之：

这里s称为四元数的实部，而v称为它的虚部。如果一个四元数的虚部为0，称之为实四元数，反之，若它的实部为0，则称之为虚四元数。
复数的乘法可以表示复平面的旋转，比如，乘上复数i相当于逆时针把一个复向量旋转90度。四元数也具有相似的性质。只不过这里，乘以i对应着绕i轴旋转180度，而绕着i轴旋转360度会得到一个跟原来相反的东西，这个东西要再旋转360度才会和它原来的样子相等。

四元数与旋转向量的相互转换

假设某个旋转是绕单位向量n=[n_x,n_y,n_z]^T进行了角度为theta的旋转，那么
从旋转向量转化为四元数：

从四元数转换为旋转向量：

注意：这里有一些奇怪的地方，旋转向量旋转360度仍然为它本身，而四元数旋转360度则变为它的相反数。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

四元数的运算

现有两个四元数q_a和q_b,它们的向量表示为[s_a,v_a],[s_b,v_b]

1. 加法和减法
   q_a+q_b=[s_a+s_b,v_a+v_b]
   q_a-q_b=[s_a-s_b,v_a-v_b]

2. 乘法
   乘法将q_a 与 q_b 的每一项相乘，然后相加，合并项。
   
   或者
   

3. 共轭
   四元数的共轭是把虚部取成相反数：
   
   四元数共轭与其本身相乘，会得到一个实四元数，其实部为模长的平方
   

4. 模长

可以验证，两个四元数乘积的模即为模的乘积:

5. 逆
   
   按照这个定义，四元数和自己逆的乘积为实四元数1
   
6. 数乘和点乘
   数乘：
   kq=[ks,kv]
   点乘:
   

用四元数表示旋转

假设有一个空间三维点p=[x,y,z]以及一个由轴角n,theta指定的旋转，三维点p经过旋转之后变为p^’。
如果用旋转向量R来表示这个旋转，那么很显然：p^’=Rp
如果用四元数来表示：
把三维空间点用一个虚四元数来表示，p=[0,x,y,z]=[0,v],这相当于把四元数的三个虚部与空间中的三个轴相对应。
把轴角表示成四元数q

那么用四元数来表示这个点的旋转为：
p^’ = qpq^-1

四元数与旋转矩阵的相互转换

设四元数q=q₀+q₁i+q₂j+q₃k,则对应的旋转矩阵为R。
四元数转换为旋转矩阵：

旋转矩阵转换为四元数：
设旋转矩阵
则：

注意：实际上，由于q和 -q 表示同一个旋转，所以一个R对应的四元数表示并不是唯一的。

相似、仿射、射影变换

实践：Eigen几何模块

1.Eigen中各种形式的表达方式

2.将旋转向量转换成旋转矩阵

rotation_vector.matrix()
或
rotation_matrix = rotation_vector.toRotationMatrix();

3.用旋转向量进行坐标转换

Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated = rotation_vector*v;
cout << "(1,0,0)after rotation="<

4.用旋转矩阵进行坐标转换

v_rotated = rotation_matrix*v;

5.将旋转矩阵转换成欧拉角

Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);//ZYX顺序，即yaw pitch roll顺序
cout << "yaw,pitch,roll = "<

6.用旋转向量和平移向量构建欧式变换矩阵

Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//虽然称为3d，实质上是4×4的矩阵
T.rotate(rotation_vector);//按照旋转向量rotation_vector进行旋转
T.pretranslate(Eigen::Vector3d(1,3,4));//把平移向量设置成(1,3,4)
cout<<"Transform matrix = \n"<

7.用变换矩阵进行坐标变换

Eigen::Vector3d v_transformed = T*v;//这相当于R*v+t
cout<<"v transformed = "<

8.将旋转向量转换成四元数

Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
cout<<"quaternion = \n"<

9.将旋转矩阵转换成四元数

q = Eigen::Quaterniond(rotation_matrix);

10.用四元数进行坐标变换

v_rotated = q*v;//这里用的是重载的乘法，数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = "<

完整代码

//2019.08.04
#include 
#include 
using namespace std;

#include 
//Eigen几何模块
#include 

//Eigen几何模块使用方法演示
int main()
{
	//Eigen/Geometry模块提供了各种旋转和平移的表示
	//3D旋转矩阵直接使用Matrix3d或者Matrix3f
	Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//将旋转矩阵初始化为单位阵
	//旋转向量使用AngleAxis,它底层不直接是Matrix，但运算可以当作矩阵(因为重载了运算符)
	Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));//将旋转向量初始化为沿Z轴旋转度

	//将旋转向量转换成旋转矩阵
	//cout.percision(3);
	cout << "rotation matrix = \n"<