如何计算给定两个 4x4 仿射矩阵的相对姿势

如何解决如何计算给定两个 4x4 仿射矩阵的相对姿势

我有两个 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,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],1.0000e+00]])

解决方法

所以我假设您在齐次坐标中使用实体变换矩阵 M，换句话说，4x4 矩阵包含一个 3x3 旋转矩阵 R、一个 3x1 平移向量 T 和一个 {{ 1}} 同构“填充”行向量。并且您想找到从一种姿势到另一种姿势的变换（我不知道如何按块编写矩阵，但这类似于 [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

@trialNerror 的数学解决方案是完全正确的。 Here is a well structed answer about calculating inverse of affine matrix

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

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

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