Inverse of a roto-translation matrix in 3D space

3dinversematricesrotationstransformation

I want to create two roto-translation matrices. The first transforms point $P$ into point $P'$ by performing a translation $T=(x_t, y_t, z_t)$ and two rotations (one around the $x$ axis of $\alpha$ and the second around the $y$ axis of $\beta$). The second undoes the previous transformation by transforming the point $P'$ into $P$. The first transformation works correctly and I calculated it as follows:

$$\pmatrix{P'\\1}=
\pmatrix{P\\1}
\pmatrix{
cos(\alpha) & sin(\beta)sin(\alpha) & cos(\beta)sin(\alpha) & x_t\\
0 & cos(\beta) & -sin(\beta) & y_t\\
-sin(\alpha) & sin(\beta)cos(\alpha) & cos(\beta)cos(\alpha) & z_t\\
0 & 0 & 0 & 1
}=
\pmatrix{
P_x*cos(\alpha) + P_y*sin(\beta)sin(\alpha) + P_z*cos(\beta)sin(\alpha) + x_t\\
P_x*0 + P_y * cos(\beta) – P_z*sin(\beta) + y_t\\
-P_x*sin(\alpha) + P_y*sin(\beta)cos(\alpha) + P_z*cos(\beta)cos(\alpha) + z_t\\
1
}
$$

I have calculated the second transformation as follows but it does not seem to be correct if $x_t$, $y_t$ and $z_t$ is greater than zero:

$$\pmatrix{P\\1}=
\pmatrix{P'\\1}
\pmatrix{
cos(\alpha) & 0 & -sin(\alpha) & -x_t\\
sin(\beta)sin(\alpha) & cos(\beta) & sin(\beta)cos(\alpha) & -y_t\\
cos(\beta)sin(\alpha) & -sin(\beta) & cos(\beta)cos(\alpha) & -z_t\\
0 & 0 & 0 & 1
}=
\pmatrix{
P'_x*cos(\alpha) + P'_y*0 – P'_z*sin(\alpha) – x_t\\
P'_x*sin(\beta)sin(\alpha) + P'_y * cos(\beta) – P'_z*sin(\beta)cos(\alpha) – y_t\\
P'_x*cos(\beta)sin(\alpha) – P'_y*sin(\beta) + P'_z*cos(\beta)cos(\alpha) – z_t\\
1
}
$$

Do you see any errors?

Best Answer

Let $\mathbf{T}$ be the translation matrix, and $\mathbf{R}$ the rotation matrix: $$\mathbf{T} = \left[\begin{matrix} 1 & 0 & 0 & T_x \\ 0 & 1 & 0 & T_y \\ 0 & 0 & 1 & T_z \\ 0 & 0 & 0 & 1 \end{matrix}\right], \quad \mathbf{R} = \left[ \begin{matrix} \cos\alpha & \sin\beta \sin\alpha & \cos\beta \sin\alpha & 0 \\ 0 & \cos\beta & -\sin\beta & 0 \\ -\sin\alpha & \sin\beta \cos\alpha & \cos\beta \cos\alpha & 0 \\ 0 & 0 & 0 & 1 \end{matrix}\right]$$ Because rotations are combined by multiplying the matrices with the applied transform on the left (i.e., oldest rightmost, newest leftmost), the combined transformation matrix $\mathbf{M}_{T,R}$ if the translation is done first and rotation second is $$\mathbf{M}_{T,R} = \mathbf{R} \mathbf{T} = \left[\begin{matrix} \cos\alpha & \sin\beta \sin\alpha & \cos\beta \sin\alpha & T_x \cos\alpha + T_y \sin\beta \sin\alpha + T_z \cos\beta \sin\alpha \\ 0 & \cos\beta & -\sin\beta & T_y \cos\beta - T_z \sin\beta \\ -\sin\alpha & \sin\beta \cos\alpha & \cos\beta \cos\alpha & -T_x \sin\alpha + T_y \sin\beta \cos\alpha + T_z \cos\beta \cos\alpha \\ 0 & 0 & 0 & 1 \end{matrix}\right]$$ On the other hand, if the rotation is done first and translation second, the transformation matrix is $\mathbf{M}_{R,T}$, $$\mathbf{M}_{R,T} = \mathbf{T} \mathbf{R} = \left[\begin{matrix} \cos\alpha & \sin\beta \sin\alpha & \cos\beta \sin\alpha & T_x \\ 0 & \cos\beta & -\sin\beta & T_y \\ -\sin\alpha & \sin\beta \cos\alpha & \cos\beta \cos\alpha & T_z \\ 0 & 0 & 0 & 1 \end{matrix}\right]$$ Any combination (product) of pure rotation and pure translation matrices is invertible. The key thing to note is that inverting the result is the same as inverting each matrix, and the order; see e.g. the Wikipedia article on matrix inverse. That is, $$\mathbf{M}_{T,R}^{-1} = \left( \mathbf{R} \mathbf{T} \right)^{-1} = \mathbf{T}^{-1} \mathbf{R}^{-1}$$ and $$\mathbf{M}_{R,T}^{-1} = \left( \mathbf{T} \mathbf{R} \right)^{-1} = \mathbf{R}^{-1} \mathbf{T}^{-1}$$ Pure rotation matrices are orthonormal, and their inverse is their transpose, $$\mathbf{R}^{-1} = \mathbf{R}^{T} = \left[\begin{matrix} \cos\alpha & 0 & -\sin\alpha & 0 \\ \sin\beta \sin\alpha & \cos\beta & \sin\beta \cos\alpha & 0 \\ \cos\beta \sin\alpha & -\sin\beta & \cos\beta \cos\alpha & 0 \\ 0 & 0 & 0 & 1 \end{matrix}\right]$$ see e.g. Wikipedia orthogonal matrix article. For translation matrices, you negate the translation vector (rightmost column, except for the fourth element in the lower right corner of the matrix), i.e. $$\mathbf{T}^{-1} = \left [ \begin{matrix} 0 & 0 & 0 & -T_x \\ 0 & 0 & 0 & -T_y \\ 0 & 0 & 0 & -T_z \\ 0 & 0 & 0 & 1 \end{matrix} \right ]$$ You can verify this by inverting $\mathbf{T}$ by hand.

Combining all of the above, we see that $$\mathbf{M}_{T,R}^{-1} = \left( \mathbf{R} \mathbf{T} \right)^{-1} = \mathbf{T}^{-1} \mathbf{R}^{-1} = \left[\begin{matrix} \cos\alpha & 0 & -\sin\alpha & -T_x \\ \sin\beta \sin\alpha & \cos\beta & \sin\beta \cos\alpha & -T_y \\ \cos\beta \sin\alpha & -\sin\beta & \cos\beta \cos\alpha & -T_z \\ 0 & 0 & 0 & 1 \end{matrix}\right]$$ and $$\mathbf{M}_{R,T}^{-1} = \left( \mathbf{T} \mathbf{R} \right)^{-1} = \mathbf{R}^{-1} \mathbf{T}^{-1} = \left[\begin{matrix} \cos\alpha & 0 & -\sin\alpha & -T_x \cos\alpha + T_z \sin\alpha \\ \sin\beta \sin\alpha & \cos\beta & \sin\beta \cos\alpha & -T_x \sin\beta \sin\alpha - T_y \cos\beta - T_z \sin\beta \cos\alpha \\ \cos\beta \sin\alpha & -\sin\beta & \cos\beta \cos\alpha & -T_x \cos\beta \sin\alpha + T_y \sin\beta - T_z \cos\beta \cos\alpha \\ 0 & 0 & 0 & 1 \end{matrix}\right]$$

 
Thus, we can easily see the error in OP's inversion: they forgot to also invert the order of the matrices in the product.

Related Question