如何计算给定两个 4x4 仿射矩阵的相对位姿
how to calculate relative pose given two 4x4 affine matrix
我有两个 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]])
所以我假设您在齐次坐标中使用实体变换矩阵 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)
@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)
我有两个 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]])
所以我假设您在齐次坐标中使用实体变换矩阵 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)
@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)