[Math] How to refer a 3D pose (position + orientation) to a different coordinate system

coordinate systemslinear algebramatricesrotations

I'm working on a robotics project where all poses and marker positions/orientations are stored as a matrix:
$$
\mathbf{P} =\begin{bmatrix}
\mathbf{R} & \mathbf{t}\\
\mathbf{0} & 1\\
\end{bmatrix}
$$

With $\mathbf{R}$ being a rotation matrix and $\mathbf{t}$ being a translation vector.

Now if I have two poses like that: $\mathbf{P_1}$ and $\mathbf{P_2}$ with their rotation and translation expressed with respect to some absolute coordinate system, how can I express $\mathbf{P_2}$ with respect to $\mathbf{P_1}$?

I understand how that would happen if $\mathbf{P_1}$ had the same orientation as the original absolute coordinate system but I can't quite get my head around it when rotations come into play…

Best Answer

With $P_1 = \begin{bmatrix} \mathbf{R}_1 & \mathbf{t}_1\\ \mathbf{0} & 1\\ \end{bmatrix}$, $P_2 = \begin{bmatrix} \mathbf{R}_2 & \mathbf{t}_2\\ \mathbf{0} & 1\\ \end{bmatrix}$: \begin{align} P_2 = P_2 P^{-1}_1 P_1 = P_2 \begin{bmatrix} \mathbf{R}^{\mathrm{T}}_1 & -\mathbf{R}^{\mathrm{T}}_1\mathbf{t}_1\\ \mathbf{0} & 1\\ \end{bmatrix}P_1 = \begin{bmatrix} \mathbf{R}_2\mathbf{R}^{\mathrm{T}}_1 & -\mathbf{R}_2\mathbf{R}^{\mathrm{T}}_1\mathbf{t}_1+t_2\\ \mathbf{0} & 1\\ \end{bmatrix}P_1. \end{align} Notice that since $R_i$ are rotation matrices, $R^\mathrm{T}_iR_i = I$