How to compare pose and position of two objects from rotation and translation matrix?

Question

I have two 4*4 matrices representing the poses and positions of two objects in OpenGL.How to calculate the differences of orientation and origin of these objects. Here is the matrix converted from OpenGL-Style matrix.

r1 r2 r3 t1
r4 r5 r6 t2
r7 r8 r9 t3
 0  0  0  1

Answer 1

Let's say the two matrices are M1 and M2, which are applied to vectors from the right:

x1 = M1 * x
x2 = M2 * x

The "difference" between the two matrices can be defined as the matrix Md that needs to applied to x1 to get x2:

Md * x1 = x2
Md * (M1 * x) = M2 * x

To create this identity for all vectors x, Md needs to satisfy the equation:

Md * M1 = M2

To isolate Md in this equation, we multiply by the inverse of M1:

Md * M1 * inv(M1) = M2 * inv(M1)

Matrix multiplication is associative, so we can group the left side as:

Md * (M1 * inv(M1)) = M2 * inv(M1)

and the result for Md is:

Md = M2 * inv(M1)

So you can solve this with a 4x4 matrix inversion and a matrix multiplication.

An alternative is that you decompose the original matrixes into a rotation and a translation each, which are applied to vectors as:

x1 = R1 * x + t1
x2 = R2 * x + t2

where R1 and R2 are 3x3 matrices, t1 and t2 translation vectors. Following the same steps as before, but also writing the difference broken down into a rotation Rd and a translation td:

Rd * x1 + td = x2
Rd * (R1 * x + t1) + td = R2 * x + t2
Rd * R1 * x + Rd * t1 + td = R2 * x + t2

For the rotation parts of both sides to match, Rd needs to satisfy:

Rd * R1 = R2

Not surprisingly, the calculation of Rd looks very similar to Md above. As a simplification, we can take advantage of the fact that the inverse of a rotation is its transpose:

Rd = R2 * inv(R1)
Rd = R2 * transp(R1)

Now in the equation above, the translation parts also need to match, which gives us td:

Rd * t1 + td = t2
td = t2 - Rd * t1

In summary, you calculate the rotation matrix and translation vector of the difference as:

Rd = R2 * transp(R1)
td = t2 - Rd * t1