def test_rotx_batch():
C_got = SO3.rotx(torch.Tensor([np.pi / 2, np.pi]))
C_expected = torch.cat([
torch.Tensor([[1, 0, 0], [0, 0, -1], [0, 1, 0]]).unsqueeze_(dim=0),
torch.Tensor([[1, 0, 0], [0, -1, 0], [0, 0, -1]]).unsqueeze_(dim=0)
],
dim=0)
assert utils.allclose(C_got.mat, C_expected)
def test_rpy_batch():
rpy = torch.Tensor([[np.pi / 12, np.pi / 6, np.pi / 3], [0, 0, 0]])
C_got = SO3.from_rpy(rpy)
C_expected = SO3.rotz(rpy[:, 2]).dot(
SO3.roty(rpy[:, 1]).dot(SO3.rotx(rpy[:, 0])))
assert utils.allclose(C_got.mat, C_expected.mat)
def test_rpy():
rpy = torch.Tensor([np.pi / 12, np.pi / 6, np.pi / 3])
C_got = SO3.from_rpy(rpy)
C_expected = SO3.rotz(torch.Tensor([rpy[2]])).dot(
SO3.roty(torch.Tensor([rpy[1]])).dot(SO3.rotx(torch.Tensor([rpy[0]]))))
assert utils.allclose(C_got.mat, C_expected.mat)
def test_rotx():
C_got = SO3.rotx(torch.Tensor([np.pi / 2]))
C_expected = torch.Tensor([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
assert utils.allclose(C_got.mat, C_expected)