深入理解空间坐标系的矩阵变换
空间中三维坐标变换一般由三种方式实现，第一种是旋转矩阵和旋转向量；第二种是欧拉角；第三种是四元数。这里先介绍旋转矩阵(旋转向量)与欧拉角实现三维空间坐标变换的方法以及两者之间的关系。
空间变换分析：
这里以常见的世界坐标系与相机坐标系间的变换为例。
从相机坐标系转换到世界坐标系，也就是比较通用的body到世界坐标系间的转换。
那么旋转的欧拉角按从世界坐标系转换到相机坐标系的过程，先按z轴旋转、之后y轴旋转、之后x轴旋转，最终得到相机坐标系，得到的角度分别是yaw、pitch、roll，那么从相机坐标系到世界坐标系的旋转矩阵按如下方式定义:
那么得到的相机（body）坐标系到世界坐标系间的旋转矩阵为：
对应四元数变换矩阵：
Opencv实现代码：
cv::Mat IMUReader::angleToRotation(COORDINATES& carrier)
{
// R
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
R.at(0, 0) = cos(carrier.yaw)*cos(carrier.pitch);
R.at(0, 1) = cos(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch) - cos(carrier.roll)*sin(carrier.yaw);
R.at(0, 2) = sin(carrier.yaw)*sin(carrier.roll) + cos(carrier.yaw)*cos(carrier.roll)*sin(carrier.pitch);
R.at(1, 0) = cos(carrier.pitch)*sin(carrier.yaw);
R.at(1, 1) = cos(carrier.yaw)*cos(carrier.roll) + sin(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch);
R.at(1, 2) = cos(carrier.roll)*sin(carrier.yaw)*sin(carrier.pitch) - cos(carrier.yaw)*sin(carrier.roll);
R.at(2, 0) = - sin(carrier.pitch);
R.at(2, 1) = cos(carrier.pitch)*sin(carrier.roll);
R.at(2, 2) = cos(carrier.roll)*cos(carrier.pitch);
return R;
}
对于平移矩阵为相机坐标系原点在世界坐标系下的坐标T，最终得到的坐标变换方程如下：
其理解过程为从世界坐标系经过z，y，x的旋转以及平移T后得到相机坐标系，那么从相机坐标转换的世界坐标实际上是反变换过程，最后一步是x旋转，那么通过定义Rx为反旋转将相机坐标反旋转过来，之后依次是y，z，旋转后得到的值为与世界坐标系同方向，但是有一个平移的T的坐标系下的坐标，之后再加一个T坐标就得到了在世界坐标系下的坐标。
齐次坐标变换矩阵：