三维空间坐标系变换——旋转矩阵
空间中三维坐标变换通常由三种方式实现，第一种是旋转矩阵和旋转向量；第二种是欧拉角；第三种是四元数。这里先介绍旋转矩阵(旋转向量)与欧拉角实现三维空间坐标变换的方法以及二者之间的关系。code
这里以常见的世界坐标系与相机坐标系间的变换为例。orm
1、首先介绍从相机坐标系转换到世界坐标系，也就是比较通用的body到世界坐标系间的转换。blog
那么旋转的欧拉角按从世界坐标系转换到相机坐标系的过程，先按z轴旋转、以后y轴旋转、以后x轴旋转，最终获得相机坐标系，获得的角度分别是yaw、pitch、roll，那么从相机坐标系到世界坐标系的旋转矩阵按以下方式定义:it
那么获得的相机（body）坐标系到世界坐标系间的旋转矩阵为：io
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，最终获得的坐标变换方程以下：class
其理解过程为从世界坐标系通过z，y，x的旋转以及平移T后获得相机坐标系，那么从相机坐标转换的世界坐标其实是反变换过程，最后一步是x旋转，那么经过定义Rx为反旋转将相机坐标反旋转过来，以后依次是y，z，旋转后获得的值为与世界坐标系同方向，可是有一个平移的T的坐标系下的坐标，以后再加一个T坐标就获得了在世界坐标系下的坐标。float
2、下面介绍从世界坐标系旋转到相机坐标系，这是一个正向旋转的过程，首先绕z，y，x旋转并平移T获得相机坐标系，那么从世界坐标系到与相机坐标系同向的坐标系是只通过旋转，那么旋转矩阵为：方法
RxRyRz，而此时三个维度的旋转分别是：im
对于平移矩阵是，此时坐标已是与相机坐标系同向，可是与世界坐标系原点重合的坐标下的坐标，转换相机坐标系还须要加上的平移是世界坐标系原点在相机坐标系下的坐标T。db
其理解过程就是一个在世界坐标系依照顺序进行z，y，x的旋转，以后再平移的过程。
其中对于z，y，x方向的旋转都是按照右手系定则，大拇指指向轴线方向，四指方向为旋转的正方向。
下面为以前整理，可能有错误，仅借鉴。
界坐标系下的坐标（Xw,Yw,Zw）转换到相机坐标系下（Xc，Yc,Zc）。
那么这里涉及到旋转矩阵和平移矩阵。对于旋转咱们有欧拉角描述，根据欧拉角计算出旋转矩阵。对于欧拉角指的是坐标系绕某个轴旋转的角度，那么从一个坐标系旋转到另外一个坐标系须要三次旋转可以完成。对于绕坐标轴旋转分为两种，一种是绕原坐标系的固定轴旋转，一种是绕部分旋转后的坐标轴旋转，这里介绍比较经常使用的绕部分旋转后的坐标轴旋转。同时，对于绕坐标轴旋转其旋转的顺序也是有关系，如绕XYZ旋转和绕ZYX旋转。那么一种比较经常使用的表示方式是航空航天的偏航-俯仰-滚转表示法，其中坐标能够沿机头朝向为x轴，飞机左侧为y轴，飞机正上方为z轴。那么偏航为绕z轴旋转，以后俯仰是绕y轴旋转，滚转是绕x轴旋转，即该欧拉角的旋转顺序为ZYX.
上面介绍了旋转方式-欧拉角，下面介绍旋转矩阵的生成。这里分开不一样轴讨论，先绕X轴旋转。咱们定义旋转变换公式为：
其中，M为旋转矩阵，公式定义的是从世界坐标系向相机坐标系的转换，那么咱们就按世界坐标向相机坐标旋转，以下：
这里咱们定义了从左侧的世界坐标转到右侧的相机坐标，其旋转为绕X轴正向旋转90度，这里的正向旋转是以沿X轴方向看，顺时针旋转为正向，不然为负向。那么从世界坐标系向相机坐标系的旋转矩阵即为：
此时从世界坐标向相机坐标的转换过程以下：
因此，这里须要注意的是几点：（1）坐标是从世界坐标系向相机坐标系旋转，全部对于旋转方程是旋转矩阵乘以世界坐标获得相机坐标；（2）旋转是从世界坐标向相机坐标的旋转，即从源坐标系向目标坐标系旋转，这里是从世界坐标系向相机坐标系旋转；（3）旋转角度是沿着坐标轴的正向看，顺时针为正，逆时针为负；（4）旋转矩阵的形式如上，绕坐标轴旋转的轴坐标是不会变化的，因此是对应位置是1，接下来是0 cos(theta) sin(theta), 在接下来是0 -sin(theta) cos(theta)。
以上四个点的方向是与最终的变换公式相关的，有一个地方发生变化，那么最终的旋转变换公式也会发生变化，为了统一，那么这里按这样的规定，从而肯定旋转变换公式的固定。
上面介绍了绕X轴旋转的状况，其它绕Y轴和Z轴的旋转矩阵相同，分别是：
绕Y轴旋转矩阵：
绕Z轴旋转矩阵：
那么假如从世界坐标系向相机坐标系的旋转过程当中，显示绕Z轴旋转了theta，以后绕旋转后坐标系的Y轴旋转了beta，以后绕旋转后坐标系X轴旋转了alpha，那么最终的旋转矩阵以下：
以上是对旋转矩阵的描述，那么对三维坐标系的转换，除了包含旋转，还要包含平移。即坐标系先从源旋转到与目标坐标系相同的方向，以后再平移。对于平移这里也规定从源坐标向目标坐标系。
这里目标坐标系在源坐标系下的坐标是(1,3,2),那么从源坐标系转到目标坐标系的变换即为：
因而这里规定几点：（1）变换是从源坐标系到目标坐标系，这里是从世界坐标系到相机坐标系；（2）平移变换是目标坐标系原点在源坐标系旋转到与目标坐标系相同方向后的坐标系下的坐标，这里即变换矩阵是目标坐标系原点在世界坐标系变换为与目标坐标系相同方向后在变换后世界坐标系中的坐标位置；（3）此时的变换矩阵须要加上负号。
若是不加负号，也能够将变换矩阵定义为源坐标系原点在目标坐标系下的位置，这里不用变换源坐标系。通过以上步骤能够将源坐标系中的坐标点转换到目标坐标系下。
对于旋转矩阵的旋转向量以及欧拉角的关系与变换：
对于旋转矩阵与旋转向量：
上面介绍了经过旋转矩阵和平移矩阵描述两个坐标系间的变换，其实也至关于表示了6自由度的三维刚体运行。可是旋转矩阵有几个缺点：SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。所以，这种表达式冗余。并且对于旋转矩阵自身也有约束，它必须是正交矩阵，且行列式为1，这些约束会使求解变得困难。一种更紧凑的方式是使用旋转xian向量描述。即对于坐标系的旋转，任意旋转均可以用一个旋转轴和一个旋转角来刻画。因而，咱们可使用一个向量，其方向与旋转轴yizh一致，而长度等于旋转角。这种向量称为旋转向量(或轴角，Axis-Angle)。这种表示法只需一个三维向量便可描述旋转。对于xuan旋转向量和旋转矩阵间的转换能够经过罗德里格斯公司（Rodrigues Formula）来计算。
对于旋转矩阵与欧拉角：