Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update geometry.i : wrap multiplication of Rot3 and Unit3 #1302

Merged
merged 5 commits into from
Dec 25, 2022
Merged

Update geometry.i : wrap multiplication of Rot3 and Unit3 #1302

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
7766428
Update geometry.i
akshay-krishnan Oct 5, 2022
e2f8ad8
Update geometry.i
akshay-krishnan Oct 5, 2022
f2ab4af
remove * overator
akshay-krishnan Dec 24, 2022
fd55e09
add rotate() test
akshay-krishnan Dec 24, 2022
3a2816e
add unrotate() test
akshay-krishnan Dec 24, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions gtsam/geometry/geometry.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -340,6 +340,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);
Expand Down
27 changes: 26 additions & 1 deletion python/gtsam/tests/test_Rot3.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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


Expand Down Expand Up @@ -2032,6 +2032,31 @@ 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]))
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()