def test_construct_from_matrix():
theta = np.pi / 3
R = SO2(theta).to_matrix()
so2 = SO2.from_matrix(R)
np.testing.assert_equal(so2.angle, theta)
np.testing.assert_array_equal(so2.to_matrix(), R)
# Matrices not elements of SO(2) should be fitted to SO(2)
R_noisy = R + 0.1 + np.random.rand(2)
so2_fitted = SO2.from_matrix(R_noisy)
R_fitted = so2_fitted.to_matrix()
assert np.any(np.not_equal(R_fitted, R_noisy))
np.testing.assert_almost_equal(np.linalg.det(R_fitted), 1, 14)
np.testing.assert_almost_equal((R_fitted.T @ R_fitted), np.identity(2), 14)
def from_matrix(cls, T):
"""Construct an SE(2) element corresponding from a pose matrix.
The rotation is fitted to the closest rotation matrix, the bottom row of the 3x3 matrix is ignored.
:param T: 3x3 or 2x3 pose matrix.
:return: The SE(2) element.
"""
return cls((SO2.from_matrix(T[:2, :2]), T[:2, 2:3]))
