From 3a2816e4fcaac539b9a092632e9b96317ddc27b1 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 24 Dec 2022 10:15:08 -0500 Subject: [PATCH] add unrotate() test --- python/gtsam/tests/test_Rot3.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index 5aa22bb0b4..e1eeb7fe4f 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -2037,7 +2037,6 @@ def test_rotate(self) -> None: 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 @@ def test_rotate(self) -> None: 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()