二-旋转矩阵

空间中三维坐标旋转一般有三种方式：旋转矩阵、欧拉角和四元数

为什么BVH文件需要用欧拉角表示，因为欧拉角只用3个角度就可以表示，而旋转矩阵需要用一个包含九个元素的矩阵，浪费空间，当需要变成3D位置坐标时候，需要简单的转换就可以将欧拉角变成旋转矩阵。注意：旋转矩阵是通过欧拉角计算得到的。

先介绍旋转矩阵：

旋转矩阵的目的：旋转矩阵反映了一个坐标系中的坐标在另一个坐标系中表示的转换关系（A坐标系下某点，当坐标系旋转为B坐标系时，求该点在B坐标系的坐标）

这里以常见的世界坐标系与相机坐标系间的变换为例。

一、首先介绍从相机坐标系转换到世界坐标系，也就是比较通用的body到世界坐标系间的转换。

需要先明白一件事，从世界坐标系转换到相机坐标系是这样做的：先按z轴旋转、之后y轴旋转、之后x轴旋转，最终得到相机坐标系，得到的角度（即欧拉角）分别是yaw、pitch、roll，那么从相机坐标系到世界坐标系的旋转矩阵按如下方式定义:

那么得到的相机（body）坐标系到世界坐标系间的旋转矩阵为：

看看这个计算顺序是从右向左的（虽然跟从左计算到右的结果一样，但是意义不一样）

cv::Mat IMUReader::angleToRotation(COORDINATES& carrier)
{
// R
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
R.at<float>(0, 0) = cos(carrier.yaw)*cos(carrier.pitch);
R.at<float>(0, 1) = cos(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch) - cos(carrier.roll)*sin(carrier.yaw);
R.at<float>(0, 2) = sin(carrier.yaw)*sin(carrier.roll) + cos(carrier.yaw)*cos(carrier.roll)*sin(carrier.pitch);
R.at<float>(1, 0) = cos(carrier.pitch)*sin(carrier.yaw);
R.at<float>(1, 1) = cos(carrier.yaw)*cos(carrier.roll) + sin(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch);
R.at<float>(1, 2) = cos(carrier.roll)*sin(carrier.yaw)*sin(carrier.pitch) - cos(carrier.yaw)*sin(carrier.roll);
R.at<float>(2, 0) = - sin(carrier.pitch);
R.at<float>(2, 1) = cos(carrier.pitch)*sin(carrier.roll);
R.at<float>(2, 2) = cos(carrier.roll)*cos(carrier.pitch);

return R;
}

对于平移矩阵为相机坐标系原点在世界坐标系下的平移坐标T，最终得到的坐标变换方程如下：

其理解过程为从世界坐标系经过z，y，x的旋转以及平移T后得到相机坐标系，那么从相机坐标转换的世界坐标实际上是反变换过程。第一步是x反旋转，那么通过定义Rx为反旋转将相机坐标系反旋，之后依次是y，z，旋转后得到的值为与世界坐标系同方向，之后再加一个相机坐标系的原点在世界坐标系的平移坐标T，这才是在世界坐标系下的坐标。
 

二、下面介绍从世界坐标系旋转到相机坐标系，这是一个正向旋转的过程，首先绕z轴，y轴，x轴旋转并平移T得到相机坐标系，那么从世界坐标系到与相机坐标系同向的坐标系是只经过旋转，那么旋转矩阵为：

RxRyRz（从右到左），而此时三个维度的旋转分别是：

请注意，这里的roll、pitch、yaw与介绍一中的不是同样的数值，只是一种表示符号，表示相对各个轴旋转的角度。

此时转换后的世界坐标系已经是与相机坐标系同向，但是相机坐标系的原点与世界坐标系的原点不重合的时候，世界坐标系转换相机坐标系  还需要加上的平移是 世界坐标系原点在相机坐标系下的平移坐标T。

其理解过程就是一个在世界坐标系依照顺序进行z，y，x的旋转，之后再平移的过程。其中对于z，y，x方向的旋转都是按照右手系定则，大拇指指向轴线方向（轴的指向方向），四指方向为旋转的正方向。