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

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

$$\begin{bmatrix} x^{’} \newline y^{’} \newline z^{’} \end{bmatrix} = R_{A}^{B} \ast \begin{bmatrix} x \newline y \newline z \end{bmatrix} + t_{A}^{B}$$

用齐次变换矩阵 $T_{A}^{B}$ 的表示形式为:

$$\begin{bmatrix} x_{’} \newline y_{’} \newline z_{’} \newline 1 \end{bmatrix} = T_{A}^{B} \ast \begin{bmatrix} x \newline y \newline z \newline 1 \end{bmatrix} = \begin{bmatrix} R_{A}^{B} & t_{A}^{B} \newline 0 & 1 \end{bmatrix} \ast \begin{bmatrix} x \newline y \newline z \newline 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} \newline &= \begin{bmatrix} R_{A}^{B} & t_{A}^{B} \newline 0 & 1 \end{bmatrix} ^ {-1} \newline &= \left ( \begin{bmatrix} E & t_{A}^{B} \newline 0 & 1 \end{bmatrix} \ast \begin{bmatrix} R_{A}^{B} & 0 \newline 0 & 1 \end{bmatrix} \right ) ^ {-1} \newline &= \begin{bmatrix} R_{A}^{B} & 0 \newline 0 & 1 \end{bmatrix} ^ {-1} \ast \begin{bmatrix} E & t_{A}^{B} \newline 0 & 1 \end{bmatrix} ^ {-1} \newline &= \begin{bmatrix} \left ( R_{A}^{B} \right ) ^ {-1} & 0 \newline 0 & 1 \end{bmatrix} \ast \begin{bmatrix} E & -t_{A}^{B} \newline 0 & 1 \end{bmatrix} \newline &= \begin{bmatrix} \left ( R_{A}^{B} \right ) ^ {T} & 0 \newline 0 & 1 \end{bmatrix} \ast \begin{bmatrix} E & -t_{A}^{B} \newline 0 & 1 \end{bmatrix} \newline &= \begin{bmatrix} \left ( R_{A}^{B} \right ) ^ {T} & -\left ( R_{A}^{B} \right ) ^ {T} t_{A}^{B} \newline 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} \newline &= \begin{bmatrix} R_{B}^{C} & t_{B}^{C} \newline 0 & 1 \end{bmatrix} \ast \begin{bmatrix} R_{A}^{B} & t_{A}^{B} \newline 0 & 1 \end{bmatrix} \newline &= \begin{bmatrix} R_{B}^{C}R_{A}^{B} & R_{B}^{C}t_{A}^{B} + t_{B}^{C} \newline 0 & 1 \end{bmatrix} \end{aligned} \end{split} $$