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

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}
$$