def test_identity():
T = SE2.identity()
assert isinstance(T, SE2) \
and isinstance(T.rot, SO2) \
and T.rot.mat.dim() == 2 \
and T.trans.shape == (2,)
def test_identity_batch():
T = SE2.identity(5)
assert isinstance(T, SE2) \
and isinstance(T.rot, SO2) \
and T.rot.mat.dim() == 3 \
and T.trans.shape == (5, 2)
def test_inv_batch():
T = SE2.exp(0.1 * torch.Tensor([[1, 2, 3], [4, 5, 6], [7, 8, 9]]))
assert utils.allclose(
T.dot(T.inv()).as_matrix(),
SE2.identity(T.trans.shape[0]).as_matrix())