2021-07-07

右手空间坐标系变换

    在slam(即时定位和建图)算法中，存在多个坐标系，坐标系中的3D点需要变换到其他坐标系中，空间坐标系变换的主要内容是确定坐标系之间的旋转矩阵R和平移向量t;本文以比较直观简单的示例说明空间坐标系变换关系方法的求解过程。
    如下两坐标系：

坐标系a与坐标系b均为右手坐标系。坐标系a的x轴水平向右，y轴竖直向下，z轴垂直向里。坐标系b的x轴数值向上，y轴水平向左，z轴垂直向外。坐标系a的圆点oa在坐标系b中的坐标是（-1，1，1）；
假设三维坐标系绕x、y、z轴旋转角度分别为 、 、 ，三维坐标系绕各坐标轴旋转的旋转矩阵为：

旋转角按照如下方法确定：
右手坐标系各轴的顺序为：

**旋转时，坐标轴按照箭头方向旋转，则旋转角度为正；否则为负；**如绕z轴旋转，旋转方向为y向x方向（顺时针）转30º，则旋转角为-30º。
以坐标系b旋转为坐标系a为例说明旋转过程。
1、将b的z轴旋转到向里方向，与a的z轴一致,即绕x轴旋转180º；如下图所示，沿着坐标系x轴从下往上看，是从z轴转向y轴，顺时针方向，则转角为负，即-180º。旋转后的坐标系为b1所示。

2、将b1坐标系的y轴旋转到水平向右、x轴旋转到数值向下，即绕z轴做如下所示旋转：旋转方向为从x轴转向y轴，旋转90º。为逆时针旋转，旋转角度因此为90º。

3、旋转矩阵计算：绕3个坐标轴旋转的矩阵相乘的顺序一定要与实际的旋转顺序一致，本例中的旋转顺序为x轴、z轴，因此旋转矩阵为：

4、假设a坐标系中的点P(x,y,z)，确定该点从坐标系a坐标转换到b坐标系的转换关系：

其中：

上式可以使用坐标系a中点进行验证，如另P点坐标为(1,1,1)，P点在b中的坐标也可以目测得到（-2，0，0），便于进行验证。
从特例到一般归纳的总结：
把a坐标系的点转换到b坐标系的转换关系如下：1、旋转矩阵为从b坐标系旋转成a坐标系得到的旋转矩阵；2、平移向量为a坐标系原点在b坐标系中的坐标值。