1

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]])
zheyuanWang
  • 1,158
  • 2
  • 16
  • 30

2 Answers2

1

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)

trialNerror
  • 3,255
  • 7
  • 18
  • 1
    thanks for pointing out my error. Does the solid transformation matrix M_1 be inverted in the way of `M_1^-1 = (P | -PT \\ 0 | 1)` ? Could I calculate it via `M_1^-1`? – zheyuanWang May 31 '21 at 14:19
  • 1
    Yeah now that you mention it, you can simply compute the inverse of `M_1` as a 4x4 matrix, no need to go through the block decomposition. I feel stupid ... – trialNerror May 31 '21 at 14:49
1

The math solution of @trialNerror is totally right. Here is a well structed answer about calculating inverse of affine matrix

I made a mistake in pytorch. The torch.multiply offers element-wise multiplication. For multipying matrices, man shoud use torch.mm().

In my case with batch as extra dimension, the code should be like this

relative_pose = torch.inverse(A).bmm(B)

zheyuanWang
  • 1,158
  • 2
  • 16
  • 30