位姿(pose)和变换矩阵(transformation matrix)

理解坐标系之间的转换关系很重要。对于刚体变换矩阵，搞机器人的朋友肯定不陌生，它一般如下表示
$\mathbf{T}_{1}^2 = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & \mathbf{1} \end{bmatrix}$
其中 $\mathbf{t}$ 是位移而 $\mathbf{R}$ 是旋转。它的物理意义或者说作用是什么呢？这个矩阵的作用就是，给出一个坐标系下的点的坐标，乘上这个矩阵，你就可以得到它在另一个坐标系下的坐标。比如说我目前这个矩阵的作用是把位于坐标系2下的点的坐标'转移'到坐标系1下。假设某个点在坐标系2下的位置为 $\mathbf{p}_2$ ，那么我们可以得到它在坐标系1下的位置
$\mathbf{p}_1 = \mathbf{T}_{1}^2 \mathbf{p}_2$
假设我们知道一个坐标系3下的点 $\mathbf{p}_3$ ，坐标系3和2的关系为 $\mathbf{T}_{2}^3$ ，那么我们就可以用下式
$\mathbf{p}_1 = \mathbf{T}_{1}^2 \mathbf{T}_{2}^3 \mathbf{p}_3$
来后去 $\mathbf{p}_3$ 在坐标系1下的位置。注意点的坐标是齐次坐标，即比它的自由度多一维且最后一维为1。比如2维坐标系下的某点坐标是 $(x,y)^T$ ，那么它的齐次坐标就是 $(x,y,1)^T$ (这个 $T$ 是转置，行向量转为列向量)。
这儿一个比较好的记忆方法就是要转换一个点的坐标到不同的坐标系，点总是在右侧，矩阵总是左乘(貌似有过右乘的，但在我接触过的几个机器人应用中还没见过)。上面那种写法有个方便之处就是方便直观，比如这个式子你可以看到 $\mathbf{T}_{1}^2 \mathbf{p}_2$ 把 $\mathbf{p}$ 右下角的2和 $\mathbf{T}_{1}^2$ 右上角的2“抵消”，剩下的就只有1了，你能很直观地看出把 $\mathbf{T}_{1}^2$ 右乘到 $\mathbf{p}_2$ 会得到它在坐标系1下的坐标。
如果一个机器人有多个传感器，那么就会涉及到很重要的传感器之间的变换矩阵的测量，称之为外参(extrinsic parameter)。你最终得到的矩阵一定要搞清楚是那个传感器的坐标到另一个的，别弄反了。
我们说机器人位姿的时候也是通过一个位置和旋转来定义，比如SLAM系统(见我的SLAM介绍或者其他的)如果给出某时刻机器人的位置是 $\mathbf{t}$ ，方向是 $\mathbf{R}$ ，我们也可以写成
$\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & \mathbf{1} \end{bmatrix}$
那么这个 $\mathbf{T}$ 其实也表示的是两个坐标系之间的变换，哪两个？机器人自身的坐标系(body frame)到世界坐标系的，即我们这个 $\mathbf{T}$ 实际就是 $\mathbf{T}_W^B$ 。 $B$ 表示body frame, $W$ 表示世界坐标系。这个body frame可能是基于机器人身上某个传感器比如相机或者某个固定的点。
很多代码或者文献中对于坐标转换矩阵也这么记 $\mathbf{T}_{1}^2$ 记为 $\mathbf{T} \underline{}1\underline{}2$ ，甚至 $\mathbf{T}_{1}^2$ 表示从1坐标系到2坐标系的。具体一定要看文献中是怎么说的。比如下面这个链接是关于SLAM的一个数据集的
https://www.eth3d.net/slam_documentation
你可以看到在extrinsics_1_2.txt的位置它很明显地写到这个txt文件里记录的矩阵的作用A 3D point can be transformed from camera 2 to camera 1 by homogeneously multiplying it with this matrix from the right.很多论文不会写这么详细，但是一般(目前没看到例外)说了某个矩阵是"A coordinate to B coordinate"类似话的，就表示A坐标系下的点乘以该矩阵可以得到点在B坐标下的位置。

最后编辑于：2019.11.04 12:45:42