2-1 Rotation Matrix与转角

这里想说明旋转矩阵与转角之间的对应关系，旋转矩阵是描述最终物体的姿态，而转角可以理解成旋转的过程。举个例子，旋转矩阵就像一张最终摆好姿势的照片，而转角则是怎么一步一步摆成这个姿势的，而这里有两种旋转方式，一种是Fixed Angles：绕固定不动的坐标轴转，另一种是Euler Angles：绕自身坐标轴旋转（轴会变）。同时，转动和移动不一样的是：旋转有先后顺序，而移动改变先后顺序也能到达同一个位置。

2-2 Fixed Angles 01

分别绕固定的X-Y-Z轴旋转（逆时针），计算所得的旋转矩阵，先转的矩阵放在后面（右边）再做连乘：

旋转动画：

Ex：旋转的先后顺序不同，旋转矩阵也会不同

Answer：

2-2 Fixed Angles 02

由旋转矩阵反推Angles（转角）：

由上一张图片，直接计算转角：

2-3 Euler Angles 01

如 2-1 所述，Euler Angles每次旋转是对自身坐标轴旋转，并且与Fixed Angles不同的是，先转的矩阵放在前面（左边）再做连乘

可以发现先后分别绕Z-Y-X的Euler Angles得到的旋转矩阵与分别绕X-Y-Z的Fixed Angles的旋转矩阵是相同的，这在几何上也表明，旋转后的最后姿态是一样的。
旋转动画：


Ex1：与Fixed Angles一致的是，Euler Angles旋转的先后顺序不同，旋转矩阵也会不同

Ex2：绕Z-Y-X的Euler Angles得到的旋转矩阵与分别绕X-Y-Z的Fixed Angles的旋转矩阵是相同的（Fixed Angles的正转与Euler Angles的反转旋转矩阵是一样的）

2-3 Euler Angles 02

直接小结：Fix Angles与Euler Angles的绕X，Y，Z旋转的排列组合方式有12种（第一次转3 轴可选 × 第二次转2轴可选 × 第三次转2轴可选）


四元数与旋转矩阵的介绍可以参考以下的博客：1、四元数与欧拉角（Yaw、Pitch、Roll）的转换
2、欧拉角、四元数和旋转矩阵之间的关系

2-4 Mapping

齐次变换矩阵（Homogeneous Transformation Matrix）是机器人学和计算机视觉中统一描述位置与姿态的核心工具。它将三维旋转矩阵${}_B^A\mathbf{R}$与平移向量（${}^A{P}_{Borg}$）整合为一个 4×4 矩阵，既能表示坐标系间的关系，也能直接对点或向量进行变换。

Mapping所讲述的是已知一个向量（或点）在坐标系{B}下的表达，考虑该向量在另一坐标系{A}的表达，以下幻灯片分别考虑了当{A}和{B}坐标系之间仅有移动或转动、同时有转动和移动，已知向量P在{B}坐标系下的表达，如何用齐次变换矩阵变换到{A}坐标系下的表达。
仅有转动或移动：

同时有转动和移动：

Ex：用齐次变换矩阵将在坐标系{B}中的点[3，7，0]‘变换到{A}中，需要求出{B}相对于{A}的旋转矩阵${}_B^A\mathbf{R}$与{B}坐标系的原点相对于{A}的向量${}^A{P}_{Borg}$，套上一张幻灯片即可求出${}^{A}P$：

2-5 Operators 01

齐次变换矩阵除了Mapping之外，还可以当算子，对向量或点进行移动或转动，即当需要移动一个向量（或点）时只需要乘以对应的其次变换矩阵，如下所示，为仅有移动或转动的情况：

移动和转动的复合：
先转动再移动不等于先移动再转动

Ex：选C，A选项是先转动再移动的齐次变换矩阵，排除A；D选项除了先转动再移动又右乘了一个仅转动的齐次变换矩阵，排除D；看B和C，先后顺序的区别，矩阵乘法是“右边先执行”，所以先移动（右边）再转动（左边），选C。

2-5 Operators 02

小结：齐次变换矩阵的三种用法
第一，描述一个坐标系相对于另一个坐标系的空间状态
第二，将点由某一个坐标系的表达转换到另一个坐标系来表达（Mapping）
第三，将点（向量）在同一个坐标系中进行移动和转动（Operators）

2-6 Transformation Matrix的运算

1、连续运算：
这里需要注意的是矩阵之间相加的操作必须要在同一坐标系的表达下才能进行，例如{C}坐标原点相对于{B}坐标的向量${}^B{P}_{Corg}$是在坐标系{B}下的表达，需要转换到{A}坐标系下才能与向量${}^A{P}_{Borg}$相加。为此，需要乘以{B}相对于{A}的旋转矩阵${}_B^A\mathbf{R}$，得到${}_B^A\mathbf{R}$${}^B{P}_{Corg}$ + ${}^A{P}_{Borg}$。

2、齐次变换矩阵的逆矩阵：
{B}对{A}的齐次变换矩阵乘以{A}对{B}的其次变换矩阵 = 单位阵（还是{A}自己）
${}_B^{A}T{}_A^{B}T={}_B^{A}T{\left({}_B^{A}T\right)}^{-1}=I_{4\times 4}$

展开旋转变换矩阵，进行相乘
$\left[\begin{array}{cc}{}_B^{A}R& {}^A{P}_{Borg}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{}_A^{B}R& {}^B{P}_{Aorg}\\ 0& 1\end{array}\right]$

$=\left[\begin{array}{cc}{}_B^{A}R{}_A^{B}R& {}^A{P}_{Borg}+{}_B^{A}R{}^B{P}_{Aorg}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{I}_{3\times 3}& 0\\ 0& 1\end{array}\right]$

等式两边矩阵元素相等
${}_B^{A}R{}_A^{B}R=I_{3\times 3}$ 因为${}_B^{A}R$的逆 = ${\left({}_B^{A}R\right)}^T$且${}_B^{A}R$的逆 = ${}_A^{B}R$ $\Rightarrow {}_A^{B}R={\left({}_B^{A}R\right)}^T$

${}^A{P}_{Borg}+{}_B^{A}R{}^B{P}_{Aorg}=0$
$\Rightarrow {}^B{P}_{Aorg}=-{\left({}_B^{A}R\right)}^T{}^A{P}_{Borg}$

最终得到：

${\left({}_B^{A}T\right)}^{-1}=\left[\begin{array}{cc}{}_A^{B}R& {}^B{P}_{Aorg}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}({}_B^{A}R{)}^T& -({}_B^{A}R{)}^T{}^A{P}_{B,org}\\ 0& 1\end{array}\right]$

3、求未知的齐次变换矩阵：

${}_D^{U}T={}_A^{U}T{}_D^{A}T={}_B^{U}T{}_C^{B}T{}_D^{C}T$

运用线性代数矩阵的逆乘法可以求任意未知的齐次变换矩阵：

例如，${}_D^{C}T$ = $({}_B^{U}T{}_C^{B}T{)}^{-1}{}_A^{U}T{}_D^{A}T$ = ${}_C^B{T}^{-1}{}_B^U{T}^{-1}{}_A^{U}T{}_D^{A}T$

${}_C^{B}T$ = ${}_B^U{T}^{-1}{}_A^{U}T{}_D^{A}T{}_D^C{T}^{-1}$

总结：第一，初始条件，{A}和{B}重合，即${}_B^{A}T$是单位阵；
第二，{B}对{A}的转轴旋转（Fixed Angles），用“premultiply”，先转的矩阵放在后面（右边）再做连乘；
第三，{B}对{B}自身的转轴旋转（Euler Angles），用“postmultiply”，先转的矩阵放在前面（左边）再做连乘；
第四，而对于齐次变换矩阵的乘法是“右边先执行”；