欧拉角与旋转矩阵（EIgen与书中公式的区别）

1. 欧拉角简介

1.1 定义欧拉角的关键项

  定义一个欧拉角需要确认以下几条：

1、旋转坐标系是右手坐标系还是左手坐标系。
 右手坐标系中的欧拉角逆时针旋转为正，左手系中顺时针旋转为正。
2、旋转组合方式，欧拉角共有322共计12种。
 欧拉角根据旋转方式可分为Tait-Bryan angle（也称作：Cardan angles; nautical angles; heading、elevation、bank; yaw、pitch、roll）和 Proper/classic Euler angle两种。
 Tait-Bryan angle包括:

z-y-x//即为zyx欧拉角，先绕z轴旋转，再绕y轴旋转，再绕x轴旋转，zyx欧拉角是导航和slam技术中常用的欧拉角，
此外，Tait-Bryan angle还包括以下欧拉角：
z-x-y, y-x-z, y-z-x, x-y-z, x-z-y

 Proper/classic Euler angle包括:

z-x-z，x-y-x，y-z-y，z-y-z, x-z-x，y-x-y

3、点或向量旋转（主动旋转）还是坐标系旋转（被动旋转）；二者对应的旋转矩阵也有所不同。
 主动旋转是指将向量逆时针围绕旋转轴旋转，被动旋转是对坐标轴进行的逆时针旋转，相当于主动旋转的逆操作
4、定轴旋转与动轴旋转
 动轴旋转又称为内在旋转，内在旋转每次旋转围绕的轴是上次旋转之后坐标系的某个轴，定轴旋转又称为外在旋转，外在旋转每次旋转的轴是固定坐标系中的轴。内在旋转与外在旋转的转换关系：互换第一次和第三次旋转的位置则两者结果相同

2 正常坐标旋转

正常的旋转如上图所示，在世界坐标系中有一个点P_w1(1,1,0)T, $\theta$表示的是从世界坐标系w1到世界坐标系w2旋转的角度，则可以得到，旋转后的P_w2为$(\sqrt{2},0,0{)}^T$,其对应的旋转矩阵如下图所示

                      图1

3 Eigen及日常应用中的坐标旋转

Eigen及日常应用中的旋转如下图所示，在世界坐标系中有一个点P_w1(1,1,0)T, $\theta$表示的是从向量P_w1到向量P_w2旋转的角度，则可以得到，旋转后的P_w2为$(0,\sqrt{2},0{)}^T$，注意，此时其旋转矩阵如下图所示，与图1中互为逆矩阵：

                      图2

eigen中的轴角、旋转矩阵、四元数、$\theta$表示的都是向量的旋转角度。

以下为eigen中的验证代码：

int main(int argc, char** argv)
{
	Eigen::Vector3d p_c(1,1,0);
	Eigen::AngleAxis<double> rotation_v(M_PI_4,Eigen::Vector3d(0,0,1));
	cout<<"rotation_v: "<<rotation_v.angle()<<" otation_v.axis():  "<<rotation_v.axis()<<endl;
	Eigen::Matrix3d rotation_m(rotation_v);
	cout<<"rotation_m: "<<rotation_m<<endl;
	Eigen::Quaterniond rotation_q(rotation_m);
	cout<<""<<rotation_q.coeffs();
	Eigen::Quaterniond q2(rotation_v);
	cout<<"q2: "<<q2.w()<<"  "<<q2.x()<<"  "<<q2.z()<<endl;

	Eigen::Vector3d p_w1 = rotation_m*p_c;
	cout<<"p_w1: "<<p_w1<<endl;
	Eigen::Quaterniond q_cal;
	q_cal.w() = 0;
	q_cal.vec() = p_c;
	Eigen::Quaterniond p_q = rotation_q*q_cal * rotation_q.inverse();
	cout<<"p_q: "<<p_q.w()<<"  "<<p_q.x()<<"  "<<p_q.y()<<"  "<<p_q.z()<<endl;

	Eigen::Vector3d p_w2 = rotation_q*p_c;
	cout<<"p_w1: "<<p_w2<<endl;
	}

输出结果为：

rotation_v: 0.785398 otation_v.axis():
0
0
1
rotation_m:
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
0
0
0.382683
0.92388
q2: 0.92388 0 0.382683
p_w1:
1.11022e-16
1.41421
0
p_q:
0 -1.11022e-16 1.41421 0
p_w1:
-5.55112e-17
1.41421
0

4 SLAM中常用到的旋转模型推导

这部分是手推的，字有些难看（从小到大一直被吐槽），大家凑乎看吧。

5 关于定轴旋转和动轴旋转

从第4小结第（2）部分可以看出，定轴旋转类似于车从map坐标系转到world坐标系的过程，其为左乘。
动轴旋转类似于第4小结第（3）部分从车的相对运动以及运动前的车体姿态求运动后的车体姿态的过程。其为右乘。
推广到三轴旋转，可以想象到，如果定轴旋转和非定轴旋转的的第一次旋转和第三次旋转互换位置，则二者是等价的。

6 主动旋转与被动旋转的关系

第2小结为被动旋转，第3小结为主动旋转，可以看到，二者旋转矩阵互为逆矩阵。

7 验证四元数左乘对应主动旋转左乘