三维空间 旋转矩阵 四元数 欧拉角 相互转换 组合旋转

API和源码 

旋转矩阵 四元数 欧拉角 相互转换的源码

这个是开源的源码。在ROS 的TF2包中就有。

TF2是TF的第二个版本。

API说明：

http://docs.ros.org/hydro/api/tf2/html/classtf2_1_1Matrix3x3.html

http://docs.ros.org/hydro/api/tf2/html/classtf2_1_1Quaternion.html

源码页：

http://docs.ros.org/hydro/api/tf2/html/Quaternion_8h_source.html

http://docs.ros.org/hydro/api/tf2/html/Matrix3x3_8h_source.html

http://docs.ros.org/hydro/api/tf2/html/Vector3_8h.html

理论说明：

参见

http://blog.csdn.net/wangjiannuaa/article/details/8952196

当用一个四元数乘以一个向量时，实际上就是让该向量围绕着这个四元数所描述的旋转轴，转动这个四元数所描述的角度而得到的向量

四元组的优点
有多种方式可表示旋转，如 axis/angle、欧拉角(Euler angles)、矩阵(matrix)、四元组等。 相对于其它方法，四元组有其本身的优点：
四元数不会有欧拉角存在的 gimbal lock 问题
四元数由4个数组成，旋转矩阵需要9个数
两个四元数之间更容易插值
四元数、矩阵在多次运算后会积攒误差，需要分别对其做规范化(normalize)和正交化(orthogonalize)，对四元数规范化更容易
与旋转矩阵类似，两个四元组相乘可表示两次旋转

两个四元数相乘也表示一个旋转
Q1 * Q2 表示先以Q2旋转，再以Q1旋转