I have two 4x4 affine matrix, A and B. They represent the pose of two objects in the world coordinate system.
How could I calculate their relative pose via matrix multiplication ? (Actually, I want to know the position(x_A,y_A) in the coordinate system of object B)
I've tried with relative pose = A * B^-1
relative_pose = torch.multiply(A, torch.inverse(B))
.
However, the relative translation is way too big. (A and B are pretty close to each other, while they are far away from origin point in world coordinate.)
test data for pytorch:
import torch
A = torch.tensor([[-9.3793e-01, -3.4481e-01, -3.7340e-02, -4.6983e+03],
[ 3.4241e-01, -9.3773e-01, 5.8526e-02, 1.0980e+04],
[-5.5195e-02, 4.2108e-02, 9.9759e-01, -2.3445e+01],
[ 0.0000e+00, 0.0000e+00, 0.0000e+00, 1.0000e+00]])
B = torch.tensor([[-9.7592e-01, -2.1022e-01, -5.8136e-02, -4.6956e+03],
[ 2.0836e-01, -9.7737e-01, 3.6429e-02, 1.0979e+04],
[-6.4478e-02, 2.3438e-02, 9.9764e-01, -2.3251e+01],
[ 0.0000e+00, 0.0000e+00, 0.0000e+00, 1.0000e+00]])
So I assume you are using solid transformation matrices M
in homogeneous coordinates, in other words 4x4 matrices containing a 3x3 rotation matrix R
, a 3x1 translation vector T
and a [0,0,0,1]
homogeneous "padding" row vector. And you want to find the transformation to go from one pose to the other (I don't know how to write matrices by block, but that would be something like (R | T \\ 0 | 1)
Then I think your formula is wrong : if Y_1 = M_1 X
and Y_2 = M_2 X
, then you have Y_2 = M_2 M_1^-1 X
, and your relative pose matrix is M_rel = M_2 M_1^-1
So you need to invert your solid transformation matrix M_1 = (R_1 | T_1 \\ 0 | 1)
If you write the equations, and if we note P = R_1^-1
, then you'll find that M_1^-1 = (P | -PT \\ 0 | 1)