add unrotate() test

borglab · Dec 24, 2022 · 3a2816e · 3a2816e
1 parent fd55e09
commit 3a2816e
Showing 1 changed file with 13 additions and 1 deletion.
diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py
@@ -2037,14 +2037,26 @@ 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]))
         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())
 
+    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()