自动驾驶（三十八）---------视觉SLAM（空间刚体运动）

       三维空间刚体运动，所谓空间刚体运动是指空间旋转矩阵，介绍空间旋转先介绍向量的内积和外积。

       对于向量和而言

1. 内积（点积）                                                                                                                                                                                         点积公式为：      ，点乘的结果是一个标量(数量而不是向量)    

       两个向量a与b的内积为 a·b = |a||b|cos∠(a, b)，内积表征或计算两个向量之间的夹角，结果为b向量在a向量方向上的投影.

2. 外积（叉乘）

      向量a与b的外积a×b是一个向量(法向量)，其长度等于|a×b| = |a||b|sin∠(a,b)，其方向正交于a与b。并且，(a,b,a×b)构成右手系,该向量垂直于a和b向量构成的平面。外积还有另外一个几何意义就是：|a×b|在数值上等于由向量a和向量b构成的平行四边形的面积。

3.欧拉角转旋转矩阵 

       欧拉角通过将刚体绕过原点的轴(i,j,k)旋转，如果将每一个角度用旋转矩阵表示如下：

           

       欧拉角转旋转矩阵如下：

       欧拉角的缺点:

1.  欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。这其实主要是由于万向锁(Gimbal Lock)引起的；
2.  欧拉角的插值比较难；
3.  计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，计算量较大；

4. 四元数

      四元数本质为复数，复数最基础的印象是： x=a+bi，a为实部，b为虚部，i为虚数单位; 

      四元数和这种类似，只是虚数部分为i,j,k，四元数表示为： x=a+bi+cj+dk; 

      欧拉角三个角顺序严格要求，不能想换交换，四元数采用一个旋转解决问题，四元数的本质是一个向量围绕另一个向量进行旋转，假设某个旋转是绕单位向量 n = [nx,ny,nz]T ,找不到一种不带奇异性的三维向量描述方式，四元数是Hamilton 找到的一种扩展的复数. 它既是紧凑的，也没有奇异性。一个四元数q 拥有一个实部和三个虚部。假设某个旋转是绕单位向量n = [nx, ny, nz] 进行了角度为θ 的旋转，四元数形式为

                                                       

    四元数常见的运算有四则运算、数乘、逆、共轭、求模长等。四元数与转换矩阵的转换关系为：