From 77664288cffc3200ec8a43926efe0aa6eac80d5a Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Wed, 5 Oct 2022 16:34:28 -0400 Subject: [PATCH 1/5] Update geometry.i --- gtsam/geometry/geometry.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 988727b98d..3919af496a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -329,6 +329,7 @@ class Rot3 { // Operator Overloads gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; + gtsam::Rot3 operator*(const gtsam::Unit3& p) const; // Manifold // gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both From e2f8ad8962de897d2c8dbeb68f0bb262b0b73f7e Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Wed, 5 Oct 2022 16:37:51 -0400 Subject: [PATCH 2/5] Update geometry.i --- gtsam/geometry/geometry.i | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 3919af496a..5675265a4c 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -329,7 +329,7 @@ class Rot3 { // Operator Overloads gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; - gtsam::Rot3 operator*(const gtsam::Unit3& p) const; + gtsam::Unit3 operator*(const gtsam::Unit3& p) const; // Manifold // gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both @@ -341,6 +341,10 @@ class Rot3 { gtsam::Point3 rotate(const gtsam::Point3& p) const; gtsam::Point3 unrotate(const gtsam::Point3& p) const; + // Group action on Unit3 + gtsam::Unit3 rotate(const gtsam::Unit3& p) const; + gtsam::Unit3 unrotate(const gtsam::Unit3& p) const; + // Standard Interface static gtsam::Rot3 Expmap(Vector v); static Vector Logmap(const gtsam::Rot3& p); From f2ab4afda488648ec1bbf18da41c3e15dcca8cc7 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 24 Dec 2022 10:06:15 -0500 Subject: [PATCH 3/5] remove * overator --- gtsam/geometry/geometry.i | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 5675265a4c..310ae42897 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -329,7 +329,6 @@ class Rot3 { // Operator Overloads gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; - gtsam::Unit3 operator*(const gtsam::Unit3& p) const; // Manifold // gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both From fd55e09bcc766310b5e2946b38619c69ffc42ed8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 24 Dec 2022 10:06:28 -0500 Subject: [PATCH 4/5] add rotate() test --- python/gtsam/tests/test_Rot3.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index a1ce01ba2c..5aa22bb0b4 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/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() From 3a2816e4fcaac539b9a092632e9b96317ddc27b1 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 24 Dec 2022 10:15:08 -0500 Subject: [PATCH 5/5] 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()