diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index a1ce01ba2..5aa22bb0b 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Rot3 +from gtsam import Point3, Rot3, Unit3 from gtsam.utils.test_case import GtsamTestCase @@ -2032,6 +2032,19 @@ class TestRot3(GtsamTestCase): angle_deg = np.rad2deg(angle) assert angle_deg < 180 + def test_rotate(self) -> None: + """Test that rotate() works for both Point3 and Unit3.""" + R = Rot3(np.array([[1, 0, 0], [0, -1, 0], [0, 0, 1]])) + p = Point3(1., 1., 1.) + u = Unit3(np.array([1, 1, 1])) + print(type(p)) + actual_p = R.rotate(p) + actual_u = R.rotate(u) + expected_p = Point3(np.array([1, -1, 1])) + expected_u = Unit3(np.array([1, -1, 1])) + np.testing.assert_array_equal(actual_p, expected_p) + np.testing.assert_array_equal(actual_u.point3(), expected_u.point3()) + if __name__ == "__main__": unittest.main()