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()