Slam编程一 旋转向量、旋转矩阵，欧拉角、变换矩阵和四元数

任意旋转可用一个旋转轴和一个旋转角度来表示。

旋转向量，旋转向量的方向与旋转轴一致，长度为旋转角度。

在三维坐标下定义旋转轴为向量n(0,1,0)即旋转轴为y轴，旋转角度为k=30°但是用弧度来表示为3.14/6 rad.

则旋转向量可用k*n表示。使用库eigen可用下面来表示

Eigen::AngleAxisd rotationVector(3.14/6,Eigen::Vector3d(0,1,0));

即绕y轴旋转30°。

旋转向量转换为旋转矩阵可由罗德里格公式进行转换：

使用rotationVector.matrix();

或者使用Eigen::Matrix3d rotationMatrix;

rotationMatrix = rotationVector.toRotationMatrix();

实现旋转向量转换为旋转矩阵。

利用旋转向量实现旋转 只需将待旋转向量*旋转向量即可，如：

向量n1(1,1,1)欲绕Y轴旋转30° ：rotationVector * n1；（注意俩个向量的位置，即矩阵相乘的条件）

同理，利用旋转矩阵实现旋转也是如此。

欧拉角时采用易懂的方式来描述旋转，把一次旋转分解成绕各轴的旋转。在视觉处理邻域通常采用先绕Z轴 再Y轴最后X轴的顺序表示。

旋转矩阵到欧拉角的转换：rotationMatrix.eulerAngles(2,1,0);此处（2，1，0）表示先Z轴再Y轴最后Z轴 可更换顺序。

欧式变换矩阵

Eigen::Isometry3d T_matrix = Eigen::Isometry3d::Identity();//3d并非3*3 而是4*4 可参考欧式矩阵的定义

T_matrix.rotate(rotationVector); //将旋转向量转化为变换矩阵

T_matrix.pretranslate(Eigen::Vector(2,5,6)); //偶是变换矩阵中是包括偏移量的即水平移动。

实现旋转也是相乘即可

四元数

Eigen::Quaterniond q = Eigen::Quaterniond (rotationVector);//使用rotationMatrix也可

q.coeffs();

当使用向量时转换公式：q = {cos(k/2),nxsin(k/2),nysin(k/2),nysin(k/2),nzsin(k/2)}

转换为的四元数为(x,y,z,w);

实现旋转也是相乘即可.