diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index 5aa22bb0b..e1eeb7fe4 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -2037,7 +2037,6 @@ class TestRot3(GtsamTestCase): 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])) @@ -2045,6 +2044,19 @@ class TestRot3(GtsamTestCase): np.testing.assert_array_equal(actual_p, expected_p) np.testing.assert_array_equal(actual_u.point3(), expected_u.point3()) + def test_unrotate(self) -> None: + """Test that unrotate() after rotate() yields original Point3/Unit3.""" + wRc = Rot3(np.array(R1_R2_pairs[0][0])) + c_p = Point3(1., 1., 1.) + c_u = Unit3(np.array([1, 1, 1])) + w_p = wRc.rotate(c_p) + w_u = wRc.rotate(c_u) + actual_p = wRc.unrotate(w_p) + actual_u = wRc.unrotate(w_u) + + np.testing.assert_almost_equal(actual_p, c_p, decimal=6) + np.testing.assert_almost_equal(actual_u.point3(), c_u.point3(), decimal=6) + if __name__ == "__main__": unittest.main()