位置、方向和坐标变换

为了表示一个刚体相对于另一个刚体的相对位姿（位置和方向），需要将坐标系附加到每个刚体上，然后规定了这两个坐标系之间的几何关系。

位置

一个帧的原点相对于另一个帧的位置可以用 translation vector （3x1），即平移向量来表述：

\begin{array}{r} t = [\begin{array}{c} x \\ y \\ z \end{array}] \end{array}

一个坐标系相对于另一个坐标系的方向可以用 rotation matrix （3x3），即旋转矩阵来表述：

\begin{array}{r} R = [\begin{array}{c} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}] \end{array}

一个 homogeneous transformation matrix （4x4），即齐次变换矩阵，可同时表示一个坐标系相对于另一个坐标系的位置和方向：

\begin{array}{r} H = [\begin{array}{c} R & t \\ 0 & 1 \end{array}] = [\begin{array}{c} r_{11} & r_{12} & r_{13} & x \\ r_{21} & r_{22} & r_{23} & y \\ r_{31} & r_{32} & r_{33} & z \\ 0 & 0 & 0 & 1 \end{array}] \end{array}

一个刚体最多具有三个旋转自由度。换句话说，只需要三个独立的量足以描述任意刚体方向。一种表示方法为 roll-pitch-yaw representation，由Roll（ $ϕ$ ），Pitch（ $θ$ ）和Yaw（ $ψ$ ）角度定义：

R (ϕ, θ, ψ)

Roll-pitch-yaw表示方法的优点是它只需要三个值。与其他表示方法相比，这些值在几何上也很容易理解。缺点是：

机器人厂家常使用Roll-Pitch-Yaw表示方法。但是，并非所有机器人供应商都采用相同的惯例。了解旋转顺序以及它们是内部旋转还是外部旋转，这些是将Roll-Pitch-Yaw角度转换为其他表示形式的必要条件。

Axis-angle 表示方法描述了以下内容：

一共有四个参数。为了最大程度地减少参数数量，通常将单位向量的每个元素乘以角度，其结果就是旋转向量 ( $r$ )：

r = [\begin{matrix} r_{x} & r_{y} & r_{z} \end{matrix}] = [\begin{matrix} u_{x} θ & u_{y} θ & u_{z} θ \end{matrix}]

轴-角表示方法相对于roll-pitch-yaw表示方法的优点在于它没有连续性和旋转顺序的问题。但是很难在物理方向和旋转矢量的数值之间进行匹配。另一个缺点是不能将旋转直接应用于3D点，而需要转换为其它表示方法。

Unit quaternions 是一种简单但具有鲁棒性的方法，用四个参数对轴-角表示方法进行编码。

q = [\begin{matrix} q_{w} & q_{x} & q_{y} & q_{z} \end{matrix}]

单位四元数被认为是表示两个坐标系之间方向的最佳方法，因为它们比旋转矩阵更紧凑、数值更稳定且更高效。与roll-pitch-yaw表示方法相比，单位四元数不会受到万向节死锁的影响（不可能唯一地表示方向）。此外，与轴-角表示不同，可以将单位四元数直接应用于3D点。

备注

3D 中刚体的任何坐标变换都可以用旋转和平移来描述。例如，可以将一个点（或点云）从一个坐标系（ $b$ ）变换到另一个坐标系（ $a$ ）。这可以通过旋转矩阵和平移向量来实现：

p^{a} = R_{b}^{a} p^{b} + t^{a}

也可以使用描述两个坐标系之间的位姿的齐次变换矩阵来实现：

{\tilde{p}}^{a} = H_{b}^{a} {\tilde{p}}^{b}

其中 ${\tilde{p}}^{a}$ 和 ${\tilde{p}}^{b}$ 分别是向量 $p^{a}$ 和 $p^{b}$ 的同质表示。

上式中的齐次变换矩阵 $H_{b}^{a}$ 表示：

在代码示例中，我们使用以下符号表示该变换矩阵： a_to_b_transform 或者 b_pose_in_a_frame 。