Extract the rotation support of a rigid transformation matrix

geometryrigid transformation

Problem

I have a rigid transformation matrix, which consists of a rotation and a translation in $ \mathbb R^3 $.
I have trouble determining its rotation axis, in particular the support vector of the rotation axis.

$$ \mathbf T =
\begin{pmatrix}
\mathbf R & \mathbf t \\
\mathbf 0 & 1
\end{pmatrix} \in \mathbb R^{4 \times 4}
$$

$ \mathbf R \in \mathbb R^{3 \times 3}$ is a rotation matrix (thus $ \mathbf R^{-1} = \mathbf R^T$) and $ \mathbf t \in \mathbb R^3$ is a translation vector.

I am searching for $ \mathbf s, \mathbf a \in \mathbb R^3 $, such that $ \mathbf T \mathbf p = \mathbf p $ for all $ c \in \mathbb R $ with $ \mathbf p = (\mathbf{s}_4 + c \cdot \mathbf{a}_4) $

$$
\begin{align}
\mathbf s_4 := & \begin{pmatrix} \mathbf s \\ 1 \end{pmatrix} = \begin{pmatrix} s_x \\ s_y \\ s_z \\ 1 \end{pmatrix} \\
\mathbf a_4 := & \begin{pmatrix} \mathbf a \\ 0 \end{pmatrix} = \begin{pmatrix} a_x \\ a_y \\ a_z \\ 0 \end{pmatrix} \\
\end{align}
$$

Currently using this matrix here:

T = [[   0.99907,   -0.0001 ,    0.04308,  -20.58843],
     [  -0.01148,    0.96321,    0.26852, -124.81325],
     [  -0.04152,   -0.26876,    0.96231,  -28.07112],
     [   0.     ,    0.     ,    0.     ,    1.     ]]

Current Approach

This transformation matrix is a rotation around an axis, which does not necessarily touch the origin $ (0 \ 0 \ 0)^T $.
I have already determined that axis direction $ \mathbf a$, which is a real eigenvector of $\mathbf R$ (as well as $\mathbf T$).

So I am looking for a support vector of the rotation axis
$ \mathbf s_4 = \mathbf T \mathbf{s}_4$.

There are infinitely many possible support vectors $\mathbf s $ along the the rotation axis $ \mathbf a$.
I found one using the following approach, but it seems to be numerically unstable.

Any transformation $ \mathbf{\tilde p} $ of a point $ \mathbf p \in \mathbb R^3 $ by $ \mathbf T $ can be described as the following approach. It considers the idea, that any point can be moved to some "rotation invariant origin" $ \mathbf s \in \mathbb R^3 $, then the rotation can be applied, and afterwards the point is moved back.

$$ \begin{align}
\begin{pmatrix} \mathbf{\tilde p} \\ 1 \end{pmatrix} & = \mathbf T \begin{pmatrix} \mathbf{p} \\ 1 \end{pmatrix} \\
\mathbf{\tilde p} & = \mathbf R \mathbf p + \mathbf t \\
& = \mathbf R (\mathbf p – \mathbf s) + \mathbf s \\
& = \mathbf R \mathbf p \ \underbrace{- \mathbf R \mathbf s + \mathbf s}_{\mathbf{t}} \\
\mathbf t &= – \mathbf R \mathbf s + \mathbf s \\
& = ( \mathbf I – \mathbf R) \mathbf s
& \Leftrightarrow \\
\mathbf s & = ( \mathbf I – \mathbf R)^{-1} \mathbf t
\end{align}
$$

This approach, however, seems to be numerically unstable since that inverse becomes very large.

Are there some other recommendations to get to this support vector $ \mathbf s $?

An approach which constraints the support vector $ \mathbf s $ to its smallest norm would be preferable.

Thanks for your time!

Best Answer

First, derivation of the solution OP mentioned:

We can rewrite $\mathbf{p} = \mathbf{T}\mathbf{p}$ as $$\vec{p} = \mathbf{R} \vec{p} + \vec{t} \tag{1}\label{AC1}$$ where $\mathbf{R}$ is the (orthonormal) rotation part of the transform $\mathbf{T}$, and $\vec{t}$ is the translation part.

The vectors $\vec{p}$ we are interested in are $$\vec{p} = \vec{s} + c \vec{a} \tag{2}\label{AC2}$$ where $\vec{s}$ is the support vector, $c \in \mathbb{R}$, and $\vec{a}$ is the axis vector of $\mathbf{R}$. Since points on the axis stay put in a rotation, $$c \vec{a} = \mathbf{R} \left ( c \vec{a} \right ) = c \mathbf{R} \vec{a} \tag{3}\label{AC3}$$ As OP noted, the axis vector $\vec{a}$ is the eigenvector of $\mathbf{R}$ corresponding to eigenvalue $1$. It (and the rotation angle $\theta$) can also be extracted directly from the components of $\mathbf{R}$, if $\mathbf{R}$ does not have much numerical error.

Substituting $\eqref{AC2}$ into $\eqref{AC1}$ we get $$\vec{s} + c \vec{a} = \mathbf{R} \left ( \vec{s} + c \vec{a} \right ) + \vec{t}$$ which is equivalent to $$\vec{s} + c \vec{a} = \mathbf{R} \vec{s} + c \mathbf{R} \vec{a} + \vec{t} \tag{4}\label{AC4}$$ Applying $\eqref{AC3}$ we get $$\vec{s} + c \vec{a} = \mathbf{R} \vec{s} + c \vec{a} + \vec{t}$$ and obviously the $c \vec{a}$ terms cancel, so we have: $$\vec{s} = \mathbf{R} \vec{s} + \vec{t} \tag{5}\label{AC5}$$ Because $$\vec{s} - \mathbf{R} \vec{s} = \left( \mathbf{R} - \mathbf{I} \right) \vec{s}$$ where $\mathbf{I}$ is the identity matrix, we have $$\left( \mathbf{R} - \mathbf{I} \right) \vec{s} = \vec{t}$$ If $\mathbf{R} - \mathbf{I}$ is invertible, we have a simple solution for $\vec{s}$: $$\vec{s} = \left( \mathbf{R} - \mathbf{I} \right)^{-1} \vec{t} \tag{6} \label{AC6}$$ This is what OP mentioned finding already.

The key equation is $\eqref{AC5}$. When the rotation is very small, $\mathbf{R} \approx \mathbf{I}$, we have $$\vec{s} \approx \vec{s} + \vec{t} \tag{7}\label{EQ7}$$ This is only true for very large $\vec{s}$. Essentially, when $\mathbf{R} = \mathbf{I}$, $\vec{s}$ is at infinity.

This is not a numerical problem, but a problem of definition. Simply put, the support vector is not useful for small rotations, because it necessarily becomes very large then.

Related Question