刚体变换

三维空间坐标的刚体变换可分为旋转和平移两个步骤，用 \(R_{A}^{B} = \begin{bmatrix} u_{x} & v_{x} & w_{x} \\ u_{y} & v_{y} & w_{y} \\ u_{z} & v_{z} & w_{z} \end{bmatrix}\) 表示旋转矩阵，用 \(t_{A}^{B} = \begin{bmatrix} t_{x} \\ t_{y} \\ t_{z} \end{bmatrix}\) 表示平移矩阵。

坐标系A到B刚体变换形式为：

\[\begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \end{bmatrix} = R_{A}^{B} \ast \begin{bmatrix} x \\ y \\ z \end{bmatrix} + t_{A}^{B}\]

用齐次变换矩阵 \(T_{A}^{B}\) 的表示形式为：

\[\begin{bmatrix} x_{'} \\ y_{'} \\ z_{'} \\ 1 \end{bmatrix} = T_{A}^{B} \ast \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} R_{A}^{B} & t_{A}^{B} \\ 0 & 1 \end{bmatrix} \ast \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}\]

那么坐标系B到A的齐次变换矩阵就是求逆的过程。

旋转矩阵的逆等于其转置：

\[\left ( R_{A}^{B} \right ) ^ {-1} = \left ( R_{A}^{B} \right ) ^ T\]

于是：

\begin{split} \begin{aligned} T_{B}^{A} &= \left ( T_{A}^{B} \right ) ^ {-1} \\ &= \begin{bmatrix} R_{A}^{B} & t_{A}^{B} \\ 0 & 1 \end{bmatrix} ^ {-1} \\ &= \left ( \begin{bmatrix} E & t_{A}^{B} \\ 0 & 1 \end{bmatrix} \ast \begin{bmatrix} R_{A}^{B} & 0 \\ 0 & 1 \end{bmatrix} \right ) ^ {-1} \\ &= \begin{bmatrix} R_{A}^{B} & 0 \\ 0 & 1 \end{bmatrix} ^ {-1} \ast \begin{bmatrix} E & t_{A}^{B} \\ 0 & 1 \end{bmatrix} ^ {-1} \\ &= \begin{bmatrix} \left ( R_{A}^{B} \right ) ^ {-1} & 0 \\ 0 & 1 \end{bmatrix} \ast \begin{bmatrix} E & -t_{A}^{B} \\ 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} \left ( R_{A}^{B} \right ) ^ {T} & 0 \\ 0 & 1 \end{bmatrix} \ast \begin{bmatrix} E & -t_{A}^{B} \\ 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} \left ( R_{A}^{B} \right ) ^ {T} & -\left ( R_{A}^{B} \right ) ^ {T} t_{A}^{B} \\ 0 & 1 \end{bmatrix} \end{aligned} \end{split}

在已知 \(T_{A}^{B}\) 和 \(T_{B}^{C}\) 时，可求解 \(T_{A}^{C}\) :

\begin{split} \begin{aligned} T_{A}^{C} &= T_{B}^{C} \ast T_{A}^{B} \\ &= \begin{bmatrix} R_{B}^{C} & t_{B}^{C} \\ 0 & 1 \end{bmatrix} \ast \begin{bmatrix} R_{A}^{B} & t_{A}^{B} \\ 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} R_{B}^{C}R_{A}^{B} & R_{B}^{C}t_{A}^{B} + t_{B}^{C} \\ 0 & 1 \end{bmatrix} \end{aligned} \end{split}