add rotate() test

python/gtsam/tests/test_Rot3.py

@@ -13,7 +13,7 @@
 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 @@ def test_axis_angle_stress_test(self) -> None:
             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()