位姿的刚体变换与坐标系转换

1 前言
在一些场景里，可能会遇到坐标系转换与刚体变换的问题，对于空间点来说，它们的变换形式是一致的，而对于带旋转的位姿来说，它们的变换方式却不一致。

一个对象进行刚体变换，整体表现为旋转与平移，不改变坐标系本身约定的性质（各个轴之间的相对关系不会改变）。刚体变换可以理解成在同一个约定的坐标系下，从一个相对坐标系到另一个相对坐标系的变换过程！（PS：世界坐标系也可看成一个特殊的相对坐标系）

坐标系转换，从名字上就可以看出，其本身是对约定的系统进行改变。比如，左右坐标系的转换，它们各个轴的相对关系因坐标系的不同而不同。

不管是刚体变换还是坐标系之间的转换，主要的难点是旋转矩阵的表现形式。本文这里仅简单记录。

注意： 
(1) 本文中没有特别说明的，其表示方式均为世界坐标系的表示方式。 
(2) 本文当中的3D点$\mathbf{p}$为了表示方便，均以齐次坐标形式表示，即 
(1.1)

$$\mathbf{p}=\begin{bmatrix}  x\\ y\\ z\\ 1 \end{bmatrix}$$

 

2 刚体变换

刚体变换的主体思想是，对操作对象进行旋转与平移，这里约定有如下(2.1)的变换矩阵，然后用这个矩阵对操作对象进行操作即达到刚体变换效果。 
(2.1)

$$T =  \begin{bmatrix}  R & \mathbf{t}\\ \mathbf{0}^T & 1 \end{bmatrix}$$

 

2.1 点的变换
对于空间点$\mathbf{p}_0$，假设在刚体变换$T$后变为$\mathbf{p}_1$，那么变换前后的关系可以表示为 
(2.2)

$$\mathbf{p}_1 = T \mathbf{p}_0$$

 

2.2 位姿的变换
对于空间中，有一个相机的位姿为$T_0$，假设在刚体变换$T$后变为$T_1$。同样，空间点$\mathbf{p}_0$，在刚体变换$T$后变为$\mathbf{p}_1$。由于刚体变换不会改变空间中对象的相对位置信息，因此可知点$\mathbf{p}_0$在相机$T_0$里的相对位置不会改变，因此可以得到 
(2.3)

$$T_0\mathbf{p}_0 = T_1\mathbf{p}_1=T_1T\mathbf{p}_0$$

由于对任意$\mathbf{p}_0$均要满足公式(2.3)，因此有 
(2.4)

$$T_1 = T_0 T^{-1}$$

注意，这里的位姿$T_0、T_1$是世界坐标系到相机坐标系的表示方式, 即如果一个世界坐标系的点$\mathbf{p}_0$，通过公式$T_0 \mathbf{p}_0$将变换到相机坐标系下的位置。如果$T_0、T_1$相机坐标系到世界坐标系的表示方式, 即如果一个世界坐标系的点$\mathbf{p}_0$，通过公式$T_0^{-1} \mathbf{p}_0$将变换到相机坐标系下的位置，那么根据公式(2.4)，可以得到如下位姿变换结果

(2.5)

$$T_1 = TT_0$$

3 坐标系变换
有时我们会在左右手坐标系之间进行转换，有时我们需要将x、y轴进行对调，有时我们需要将当前坐标系按某种形式进行旋转……这一系列的坐标系变换，可以看成一个镜像、旋转、偏移的组合，这里以如下的变换矩阵来表示 
(3.1)

$$T =  \begin{bmatrix}  R' & \mathbf{t}\\ \mathbf{0}^T & 1 \end{bmatrix}$$
注意：这里的$R'$的行列式不一定为1，也可能为-1。

 

3.1 点的坐标系变换
对于空间点$\mathbf{p}_0$，假设在坐标系变换$T$后变为$\mathbf{p}_1$，那么变换前后的关系可以表示为 
(3.2)

$$\mathbf{p}_1 = T \mathbf{p}_0$$

 

3.2 位姿的坐标系变换

3.2.1 相机坐标系按约定变换，世界坐标系按约定变换
对于空间中，有一个相机的位姿为$T_0$，假设经过坐标系变换系数$T$后变为$T_1$。同样，空间点$\mathbf{p}_0$，在经过坐标系变换$T$后变为$\mathbf{p}_1$。由于$\mathbf{p}_0$在相机$T_0$里的相对位置不会改变。但是，需要注意的是，虽然相对位置不会改变，但是按约定，坐标系进行了变化（例如，在世界坐标系，我们约定x轴向前，y轴向右，z轴向上；在相机坐标系，我们也会默认x轴向前，y轴向右，z轴向上），则可得到如下结果 

这里假设，在变换前的坐标系下，$\mathbf{p}_0$相对于$T_0$的位置为$\mathbf{p}'$，则可以得到 
(3.3)

$$\mathbf{p}' = T_0 \mathbf{p}_0$$

$\mathbf{p}'$在经过坐标系变换$T$后变为$\mathbf{p}''$，则有  
(3.4)

$$\mathbf{p}'' = T \mathbf{p}'$$

由于在变换后的坐标系下，$\mathbf{p}_1$相对于$T_1$的位置也为$\mathbf{p}''$，即 
(3.5)

$$\mathbf{p}'' = T_1 \mathbf{p}_1$$
根据公式(3.2)-(3.5)可得到 
(3.6)

$$(T_1T – TT_0)\mathbf{p}_0 = \mathbf{0}$$
由于对任意$\mathbf{p}_0$均要满足公式(3.6)，因此有 
(3.7)

$$T_1 = T T_0 T^{-1}$$

对(3.7)进行展开有   
(3.8)

$$\begin{bmatrix}  R_1 & \mathbf{t}_1\\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix}  RR_0R^{-1} &R\mathbf{t}_0 + \mathbf{t} – RR_0R^{-1}\mathbf{t}\\ \mathbf{0}^T & 1 \end{bmatrix}$$

即旋转部分与偏移部分的变换结果分别为 
(3.9)

$$\begin{aligned}  R_1 &= RR_0R^{-1}\\ \mathbf{t}_1 &= R\mathbf{t}_0 + \mathbf{t} – RR_0R^{-1}\mathbf{t} \end{aligned}$$

特别地，当$\mathbf{t}=\mathbf{0}$时，有 
(3.10)

$$\begin{aligned}  R_1 &= RR_0R^{-1}\\ \mathbf{t}_1 &= R\mathbf{t}_0 \end{aligned}$$

 

3.2.2 相机坐标系不变，世界坐标系按约定变换

这里同样使用3.2.1当中的符号表达，由于这里约定相机坐标系不变，因此(3.5)式变为

(3.11)

$$\mathbf{p}' = T_1 \mathbf{p}_1$$

由公式(3.2),(3.3),(3.11)可以得到

(3.12)

$$(T_1T – T_0)\mathbf{p}_0 = \mathbf{0}$$

由于对任意$\mathbf{p}_0$均要满足公式(3.12)，因此有 
(3.13)

$$T_1 = T_0T^{-1}$$

 

3.2.3 相机坐标系按约定变换，世界坐标系不变

这里同样使用3.2.1当中的符号表达，由于这里约定世界坐标系不变，因此(3.5)式变为

(3.14)

$$\mathbf{p}'' = T_1 \mathbf{p}_0$$

由公式(3.3),(3.4),(3.14)可以得到

(3.15)

$$(TT_0 – T_1)\mathbf{p}_0 = \mathbf{0}$$

由于对任意$\mathbf{p}_0$均要满足公式(3.15)，因此有 
(3.16)

$$T_1 = TT_0$$

 

3.3 相机坐标系变换总结

最终，相机坐标系变换方式，可分3种情况进行：

情况1：相机坐标系按约定变换，世界坐标系按约定变换，这时使用公式(3.7)

情况2：相机坐标系不变，世界坐标系按约定变换，这时使用公式(3.13)

情况3：相机坐标系按约定变换，世界坐标系不变，这时使用公式(3.16)

 

4 例子
现在有一个原始的相机位姿为$T_0$ 
(4.1)

$$T_0 =  \begin{bmatrix}  R_0 & \mathbf{t}_0\\ \mathbf{0}^T & 1 \end{bmatrix}$$
因为一些接口需要，我们希望将原始坐标系的x轴指向新坐标系的z轴反向，原坐标系的y轴指向新坐标系的x轴反向，原坐标系的z轴指向新坐标系y轴。根据需求，可得到变换矩阵的旋转部分 
(4.2)

$$R = \begin{bmatrix}  0 & -1 & 0\\ 0 & 0 & 1\\ -1 & 0 & 0 \end{bmatrix}$$
根据需求，偏移部分为 
(4.3)

$$\mathbf{t} = \begin{bmatrix}  0\\ 0\\ 0 \end{bmatrix}$$
因此最终得到扩展后的变换矩阵 
(4.4)
$$T = \begin{bmatrix}  0 & -1 & 0 & 0\\ 0 & 0 & 1 &0 \\ -1 & 0 & 0 &0\\ 0 & 0 & 0 & 1 \end{bmatrix}$$

按公式(3.7)，可得到最终的坐标系变换结果 

$$T_1 = T T_0 T^{-1}$$

 

5 延伸

5.1 按指定向量旋转

很多时候，我们可能希望得到某些点以$\mathbf{p}_0$为原心，由向量$\mathbf{n}_1$ 旋转到向量$\mathbf{n}_2$后的位置，这个本质上是求公式（3.2）当中的变换矩阵$T$。

很明显，第一步，可以将所有点以$\mathbf{p}_0$为原心，进行平移；第二步，由于向量$\mathbf{n}_1$ 与向量$\mathbf{n}_2$叉乘后的单位轴向量$\mathbf{a}$同时垂直于$\mathbf{n}_1$、$\mathbf{n}_2$，因此$\mathbf{a}$可以作为旋转轴，而旋转角度，可以通过余弦公式计算得出。因此可先按如下方式确定对应变量：

(5.1)

$$\begin{aligned} \mathbf{a} &=\frac{ \mathbf{n}_1 \times \mathbf{n}_2}{|\mathbf{n}_1||\mathbf{n}_2|}\\ \theta &= \mathbf{arccos}(\frac{\mathbf{n}_1 \cdot \mathbf{n}_2}{|\mathbf{n}_1||\mathbf{n}_2|}) \end{aligned}$$

结合SLAM之李群李代数工具中的公式(1.5)，可以得到旋转矩阵$R$，最终变换矩阵形式如下

(5.2)

$$T = \begin{bmatrix}   R & -R\mathbf{p}_0\\ \mathbf{0} & 1 \end{bmatrix}$$