def rand_T():
t = torch.randn(3)
axis = torch.randn(3)
theta = torch.norm(axis)
axis = axis / theta
R = rodrigues(axis, theta)
T = torch.eye(4)
T[:3, :3] = R
T[:3, 3] = t
return T
def test_compute_transfo_error(self):
axis = torch.randn(3)
axis = axis / torch.norm(axis)
theta = 30 * np.pi / 180
R = rodrigues(axis, theta)
T = torch.eye(4)
T[:3, :3] = R
T[0, 3] = 1
rte, rre = compute_transfo_error(torch.eye(4), T)
npt.assert_allclose(rte.item(), 1)
npt.assert_allclose(rre.item(), 30, rtol=1e-3)