Rotation Matrix
性质和结论
从B frame相对于A frame的旋转矩阵的转置即为A frame相对于B frame的旋转矩阵，同时等于其逆矩阵： \[^{A}_{B}R = ^{B}_{A}A^{T}=^{B}_{A}A^{-1}\] 旋转矩阵可以将一个点(P)从一个frame(B)变换到另一个frame(A)： \[^{A}P=^{A}_{B}R \cdot ^{B}P\] 用坐标轴绕参考坐标系旋转角度来确定旋转矩阵： \[R_{Z_{A}}({\theta})=\left[\begin{matrix} cos{\theta} & -sin\theta & 0 \\ sin{\theta} & cos{\theta} & 0 \\ 0 & 0 & 1 \end{matrix}\right]=\left[\begin{matrix} c{\theta} & -s\theta & 0 \\ s{\theta} & c{\theta} & 0 \\ 0 & 0 & 1 \end{matrix}\right]\] \[R_{X_{A}}({\theta})=\left[\begin{matrix} 1 & 0 & 0 \\ 0 & cos{\theta} & -sin{\theta} \\ 0 & sin{\theta} & cos{\theta} \end{matrix}\right]=\left[\begin{matrix} 1 & 0 & 0 \\ 0 & c{\theta} & -s{\theta} \\ 0 & s{\theta} & c{\theta} \end{matrix}\right]\] \[R_{Y_{A}}({\theta})=\left[\begin{matrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{matrix}\right]=\left[\begin{matrix} c{\theta} & 0 & s{\theta} \\ 0 & 1 & 0 \\ -s{\theta} & 0 & c{\theta} \end{matrix}\right]\] A表示A frame，参考坐标系，即世界坐标系，B表示Body frame，本地坐标系，即物体坐标系。
描述一个frame(B)相对另一个frame(A)的姿态： \[^{A}_{B}R=\left[\begin{matrix} | & | & | \\ ^{A}X_{B} & ^{A}Y_{B} & ^{A}Z_{B} \\ | & | & | \end{matrix}\right]\] 其中\(^{A}X_{B}\)，\(^{A}Y_{B}\)，\(^{A}Z_{B}\)分别代表B frame各坐标轴在A frame投影的列向量。
Fixed Angles
针对空间中固定的坐标系XYZ进行旋转操作： \[^{A}_{B}R_{XYZ}(\gamma,\beta,\alpha)=R_{Z}(\alpha)R_{Y}(\beta)R_{X}(\gamma)\] \[=\left[\begin{matrix} c{\alpha} & -s\alpha & 0 \\ s{\alpha} & c{\alpha} & 0 \\ 0 & 0 & 1 \end{matrix}\right]\left[\begin{matrix} c{\beta} & 0 & s{\beta} \\ 0 & 1 & 0 \\ -s{\beta} & 0 & c{\beta} \end{matrix}\right]\left[\begin{matrix} 1 & 0 & 0 \\ 0 & c{\gamma} & -s{\gamma} \\ 0 & s{\gamma} & c{\gamma} \end{matrix}\right]\] \[=\left[\begin{matrix}c{\alpha}c{\beta} & c{\alpha}s{\beta}s{\gamma}-s{\alpha}c{\gamma} & c{\alpha}s{\beta}c{\gamma}+s{\alpha}s{\gamma} \\ s{\alpha}c{\beta} & s{\alpha}s{\beta}s{\gamma}+c{\alpha}c{\gamma} & s{\alpha}s{\beta}c{\gamma}-c{\alpha}s{\gamma} \\-s{\beta} & c{\beta}s{\gamma} & c{\beta}c{\gamma}\end{matrix}\right]=\left[\begin{matrix}r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33}\end{matrix}\right]\]
先转的放在后面，转动的顺序（左乘）是不能互换的。
通过R推算Angles： \[^{A}_{B}R_{XYZ}(\gamma,\beta,\alpha)=\left[\begin{matrix}c{\alpha}c{\beta} & c{\alpha}s{\beta}s{\gamma}-s{\alpha}c{\gamma} & c{\alpha}s{\beta}c{\gamma}+s{\alpha}s{\gamma} \\ s{\alpha}c{\beta} & s{\alpha}s{\beta}s{\gamma}+c{\alpha}c{\gamma} & s{\alpha}s{\beta}c{\gamma}-c{\alpha}s{\gamma} \\-s{\beta} & c{\beta}s{\gamma} & c{\beta}c{\gamma}\end{matrix}\right]=\left[\begin{matrix}r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33}\end{matrix}\right]\] \(if\space{\beta} \neq 90^{\circ}\) \[\beta=Atan2(-r_{31},\sqrt{r_{11}^2+r_{21}^2})\] \[\alpha=Atan2(\frac{r_{21}}{c{\beta}},\frac{r_{11}}{c{\beta}})\] \[\gamma=Atan2(\frac{r_{32}}{c{\beta}},\frac{r_{33}}{c{\beta}})\] \(if \space \beta=90^{\circ}\) \[\alpha=0^{\circ}\] \[\gamma=Atan2(r_{12},r_{22})\] \(if \space \beta=-90^{\circ}\) \[\alpha=0^{\circ}\] \[\gamma=-Atan2(r_{12},r_{22})\]
Euler Angles
Z-Y-X \[^{A}_{B}R_{Z^{'}Y^{'}X^{'}}(\alpha,\beta,\gamma)=^{A}_{B^{'}}R^{B^{'}}_{B^{''}}R^{B^{''}}_{B}R=R_{Z^{'}}(\alpha)R_{Y^{'}}(\beta)R_{X^{'}}(\gamma)\] 先转的放在前面，转动的顺序（左乘）是不能互换的。 对Fixed Angle以XYZ的顺序转动，相当于对Euler Angle以ZYX的顺序转动。 Euler Angle的正转和Fixed Angle的反转会得到相同的解。
刚体状态的表达
Homogeneous transformation matrix (4x4): \[^{A}_{B}T=\left[\begin{matrix}^{A}_{B}R_{3\times3} & ^{A}P_{B_{org}3\times1} \\ 0_{1\times3} & 1\end{matrix}\right]\] \[=\left[\begin{matrix} | & | & | & | \\ ^{A}X_{B} & ^{A}Y_{B} & ^{A}Z_{B} & P_{B_{org}} \\ | & | & | & | \\ 0 & 0 & 0 & 1 \end{matrix}\right]\] 左上角为旋转矩阵，右上角为移动状态。
移动和转动复合运算： \[^{A}P_{3\times1}=^{A}_{B}R^{B}P_{3\times1}+^{A}P_{B_{org}3\times1}\] \[\left[\begin{matrix}^{A}P \\ 1\end{matrix}\right]=^{A}_{B}T\left[\begin{matrix}^{B}P \\ 1\end{matrix}\right]\]
可连续操作： \[^{A}_{B}T=^{A}_{C}T^{C}_{D}T^{D}_{B}T\] "sequential transformation"
Mapping and Operator:
transformation matrix既有Mapping（把向量从一个frame转换到另一个frame下来看）的功能也可当作Operator对向量（或点）进行移动或旋转（在同一个frame）。
\[\left[\begin{matrix}^{A}P_2 \\ 1\end{matrix}\right]=T\left[\begin{matrix}^{A}P_1 \\ 1\end{matrix}\right]\]
Homogeneous transformation matrix的三种用法
- 描述一个frame（相对于另一个frame）的空间状态。 \[^{A}_{B}T=\left[\begin{matrix} | & | & | & | \\ ^{A}X_{B} & ^{A}Y_{B} & ^{A}Z_{B} & P_{B_{org}} \\ | & | & | & | \\ 0 & 0 & 0 & 1 \end{matrix}\right]\]
- 将point由某一个frame的表达转换到另一个frame来表达。 \[\left[\begin{matrix}^{A}P \\ 1\end{matrix}\right]=^{A}_{B}T\left[\begin{matrix}^{B}P \\ 1\end{matrix}\right]\]
- 将point(vector)在同一个frame中进行移动和转动。 \[\left[\begin{matrix}^{A}P_2 \\ 1\end{matrix}\right]=T\left[\begin{matrix}^{A}P_1 \\ 1\end{matrix}\right]\]
Transformation Matrix运算
逆矩阵：
\[^{A}_{B}T^{B}_{A}T=^{A}_{B}T^{A}_{B}T^{-1}=I_{4\times4}\]
\[^{A}_{B}T^{-1}=\left[\begin{matrix}^{A}_{B}R^{T} & -^{A}_{B}{R^{T}}^{A}P_{B_{org}} \\ 0_{1\times3} & 1\end{matrix}\right]\] 连续运算法则：
- {B}对{A}的转轴旋转：用"premultiply"，先转的放在后面
- {B}对{B}自身的转轴旋转：用"postmultiply"，先转的放在前面