Comparing two rotation matrices

Question

Problem

I want to compare two rotation matrices $R_A$ and $R_B$ both representing the orientation of the same point cloud in space, but computed from different methods. The idea is to have an estimation of the error between those two matrices.

Method

My idea was to do it as follows:

1. Compute the rotation $R_{AB}$ between $R_A$ and $R_B$ as $R_{AB} = R_A^TR_B$
2. Compute the axis-angle ($\omega$, $\theta$) representation of $R_{AB}$ using the following formula: $$Tr(R_A) = 1 + 2cos(\theta)$$
3. Use the angle $\theta$ as the rotation error.

In Python, I do:

r_oa = import(R_A) // See R_A below, in "Data"
r_ob = import(R_B) // See R_B below, in "Data"

r_oa_t = np.transpose(r_oa)
r_ab = r_oa_t * r_ob

np.rad2deg(np.arccos((np.trace(r_ab) - 1) / 2))

That seems quite straightforward to me, but I get $\theta = 23.86\unicode{xb0}$, which seems really unrealistic. This intuition is confirmed by the use of a software comparing the matrices, as described below in "Verification".

Am I doing something wrong there? Is it just not the right way to compare my matrices?

Verification

In a processing software, I have the possibility to "compare" two matrices (each being a 4x4 matrix defining a position and an orientation: [R | t]).

The documentation does not explain exactly how it works, but the output of this comparison is:

• 4 values for the rotation [deg]: Omega, Phi, Kappa, Total
• 4 values for the translation: $\Delta$x, $\Delta$y, $\Delta$z, Total.

I assumed that the "Total" angle for the rotation is my $\theta$ (as described above). For the same matrices $R_A$ and $R_B$ as above, the software outputs: Total = 0.036477551. That seems much more realistic, but then I don't know why it is not what I get.

Just to check my computation of $\theta$, I tried to compute it on $R_A$ and $R_B$ (instead of $R_{AB}$) and to compare that to the output of the software when running its comparison between $R_A$ and $I$, and also between $R_B$ and $I$. Even though I don't get the exact same $\theta$ as the software, my result is really close (to the 4th decimal).

Assuming that the software loses precision somewhere, I believe that my computation of $\theta$ is correct. And my computation of $R_{AB}$ is quite straightforward, so I don't understand why I don't find the same $\theta$ for $R_{AB}$.

Data

My matrices are:

$R_A = \begin{bmatrix} -0.956395958000000 & 0.292073230000000 & 0.000014880000000 \\ -0.292073218000000 & -0.956395931000000 & 0.000242173000000 \\ 0.000084963000000 & 0.000227268000000 & 0.999999971000000\end{bmatrix}$

$R_B = \begin{bmatrix} -0.956227882000000 & 0.292623030000000 & -0.000013768000000 \\ -0.292623029000000 & -0.956227882000000 & -0.000029806000000 \\ -0.000021887000000 & -0.000024473000000 & 0.999999999000000 \end{bmatrix}$

The software outputs, for the comparison between $R_A$ and $R_B$: "Total = 0.036477551". Following my method, I get $\theta = 23.86$, which is completely different.

Related questions

• "Change in rotation" matrix

Answer 1

Apparently you have mistaken array multiplication for matrix multiplication. In Octave, acos((trace(A'*B)-1)/2)*360/2/pi gives 0.0365 degree, but acos((trace(A'.*B)-1)/2)*360/2/pi (note it's .* here instead of *) gives 23.86 degrees.

In NumPy, * means array multiplication, not matrix multiplication. Your Python code seems to be incorrect.

Answer 2

A direct way to measure the angle between matrices is to view them as vectors in $\mathbb{R}^{n^2}$ and compute the cosine between these vectors as usual. Notation:
$\qquad x, y$ : 1d vectors
$\qquad A, B$ : $n \times n$ matrices
$\qquad A_{flat}, B_{flat}$ : the corresponding vectors in $\mathbb{R}^{n^2}$

$\qquad dot( x, y ) = \sum x_i y_i$
$\qquad \|x\| = \sqrt \, dot( x, x )$
$\qquad cos( x, y ) = {dot( x, y ) \over {\|x\| \, \|y\|}}$

$\qquad Fdot( x, y ) = dot( \, A_{flat}, \, B_{flat} \, ) = \sum \sum A_{ij} B_{ij}$
$\qquad \|A\|_F = \sqrt \, Fdot( A, A ) \ \ $ -- Frobenius norm
$\qquad Fcos( A, B ) = {Fdot( \, A, \, B\, ) \over {\|A\|_F \, \|B\|_F}}$

Rotation matrices have $\|R\|_F = \sqrt n$ (check $\|I\|_F$), so $Fcos( R, S ) = {1 \over n} Fdot( R, S )$. ($trace( A^T B )$ is also $\sum \sum A_{ij} B_{ij}$, but your formula is missing the factor ${1 \over n}$.)

In python numpy, the same block of numbers can be viewed as a matrix (actually ndarray), or as a 1d vector:

def cos( A, B ):
    """ comment cos between vectors or matrices """
    Aflat = A.reshape(-1)  # views
    Bflat = B.reshape(-1)
    return (np.dot( Aflat, Bflat )
        / max( norm(Aflat) * norm(Bflat), 1e-10 ))

Answer 3

I think your approach was correct. Here is my implementation in Python which works. It outputs the angle between two rotation matrices $Q$ and $P$.

def getAngle(P, Q):
    R = np.dot(P, Q.T)
    cos_theta = (np.trace(R)-1)/2
    return np.arccos(cos_theta) * (180/np.pi)

I took the math from this website, which is a good reference: http://www.boris-belousov.net/2016/12/01/quat-dist/