四元数

Quaternion 的定义

四元数一般定义如下：
    q=w+xi+yj+zk
其中 w,x,y,z是实数。同时，有:
    i*i=-1
    j*j=-1
    k*k=-1
四元数也可以表示为：
    q=[w,v]
其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的。
通俗的讲，一个四元数（Quaternion）描述了一个旋转轴和一个旋转角度。这个旋转轴和这个角度可以通过 Quaternion::ToAngleAxis转换得到。当然也可以随意指定一个角度一个旋转轴来构造一个Quaternion。这个角度是相对于单位四元数而言的，也可以说是相对于物体的初始方向而言的。
当用一个四元数乘以一个向量时，实际上就是让该向量围绕着这个四元数所描述的旋转轴，转动这个四元数所描述的角度而得到的向量。

四元数的优点

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

四元数下的坐标系转换

机器人坐标系下的点的坐标，转换到全局坐标系下的坐标

例如：坐标系YO1X下P1(x,y)，坐标系YO1X在全局坐标系下的位姿（四元数）T1，O1点在全局坐标系下的坐标a（X0,Y0）。