0

我有两个 4x4 仿射矩阵 A 和 B。它们代表两个对象在世界坐标系中的姿态。

我如何通过矩阵乘法计算它们的相对姿势？（其实我想知道物体B在坐标系中的位置（x_A,y_A））

我尝试过相对姿势 = A * B^-1

relative_pose = torch.multiply(A, torch.inverse(B)).

但是，相对翻译太大了。（A 和 B 彼此非常接近，而它们在世界坐标中远离原点。）

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]])
4

2 回答 2

1

所以我假设您M在齐次坐标中使用实体变换矩阵，换句话说，4x4 矩阵包含一个 3x3 旋转矩阵R、一个 3x1 平移向量T和一个[0,0,0,1]齐次“填充”行向量。而且你想找到从一个姿势到另一个姿势的转换（我不知道如何逐块编写矩阵，但这就像(R | T \\ 0 | 1)

然后我认为你的公式是错误的：如果Y_1 = M_1 X和Y_2 = M_2 X，那么你有Y_2 = M_2 M_1^-1 X，你的相对姿势矩阵是M_rel = M_2 M_1^-1

所以你需要反转你的实体变换矩阵M_1 = (R_1 | T_1 \\ 0 | 1) 如果你写方程，如果我们注意到P = R_1^-1，那么你会发现M_1^-1 = (P | -PT \\ 0 | 1)

于 2021-05-31T13:29:52.000 回答
1

@trialNerror 的数学解决方案是完全正确的。这是关于计算仿射矩阵逆的结构良好的答案

我在pytorch中犯了一个错误。提供 torch.multiply逐元素乘法。对于乘法矩阵，人应该使用torch.mm().

在我将批处理作为额外维度的情况下，代码应该是这样的

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

于 2021-06-01T02:13:08.837 回答