diff --git a/symforce/geo/rot2.py b/symforce/geo/rot2.py index 95dd012fe..2e0847992 100644 --- a/symforce/geo/rot2.py +++ b/symforce/geo/rot2.py @@ -137,6 +137,14 @@ def from_angle(cls, theta: T.Scalar) -> Rot2: """ return cls.from_tangent([theta]) + def to_angle(self, epsilon: T.Scalar = sf.epsilon()) -> T.Scalar: + """ + Get the angle of this Rot2 in radians + + This is equivalent to ``to_tangent()[0]`` + """ + return self.to_tangent(epsilon)[0] + def to_rotation_matrix(self) -> Matrix22: """ A matrix representation of this element in the Euclidean space that contains it. diff --git a/test/geo_rot2_test.py b/test/geo_rot2_test.py index 5bc14d52a..59f13af18 100644 --- a/test/geo_rot2_test.py +++ b/test/geo_rot2_test.py @@ -49,6 +49,19 @@ def test_angle_constructor(self) -> None: rot2 = sf.Rot2.from_tangent([1.5]) self.assertEqual(rot1, rot2) + def test_from_to_angle(self) -> None: + """ + Tests: + Rot2.from_angle + Rot2.to_angle + """ + for angle, angle_gt in zip( + [0.0, np.pi / 2, np.pi, 3 * np.pi / 2, 2 * np.pi], + [0.0, np.pi / 2, np.pi, -np.pi / 2, 0.0], + ): + rot = sf.Rot2.from_angle(angle).evalf() + self.assertLess(abs(angle_gt - rot.to_angle()), 1e-8) + def test_lie_exponential(self) -> None: """ Tests: