def test_from_matrix():
T_good = SE2.from_matrix(torch.eye(3))
assert isinstance(T_good, SE2) \
and isinstance(T_good.rot, SO2) \
and T_good.trans.shape == (2,) \
and SE2.is_valid_matrix(T_good.as_matrix()).all()
T_bad = SE2.from_matrix(torch.eye(3).add_(1e-3), normalize=True)
assert isinstance(T_bad, SE2) \
and isinstance(T_bad.rot, SO2) \
and T_bad.trans.shape == (2,) \
and SE2.is_valid_matrix(T_bad.as_matrix()).all()
def test_from_matrix_batch():
T_good = SE2.from_matrix(torch.eye(3).repeat(5, 1, 1))
assert isinstance(T_good, SE2) \
and T_good.trans.shape == (5, 2) \
and SE2.is_valid_matrix(T_good.as_matrix()).all()
T_bad = T_good.as_matrix()
T_bad[3, :, :].add_(0.1)
T_bad = SE2.from_matrix(T_bad, normalize=True)
assert isinstance(T_bad, SE2) \
and T_bad.trans.shape == (5, 2) \
and SE2.is_valid_matrix(T_bad.as_matrix()).all()
def test_normalize_batch():
T = SE2.exp(0.1 * torch.Tensor([[1, 2, 3], [4, 5, 6], [7, 8, 9]]))
assert SE2.is_valid_matrix(T.as_matrix()).all()
T.rot.mat.add_(0.1)
assert (SE2.is_valid_matrix(T.as_matrix()) == torch.ByteTensor([0, 0,
0])).all()
T.normalize(inds=[0, 2])
assert (SE2.is_valid_matrix(T.as_matrix()) == torch.ByteTensor([1, 0,
1])).all()
T.normalize()
assert SE2.is_valid_matrix(T.as_matrix()).all()
def test_normalize():
T = SE2.exp(torch.Tensor([1, 2, 3]))
T.rot.mat.add_(0.1)
T.normalize()
assert SE2.is_valid_matrix(T.as_matrix()).all()