12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1302 from borglab/wrap-unit3-rot

release/4.3a0
Varun Agrawal 2022-12-24 23:43:39 -05:00 committed by GitHub
parent d7491a187e 3a2816e4fc
commit af6a4f2417
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 30 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/geometry.i
View File

@ -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);

27
python/gtsam/tests/test_Rot3.py
View File

@ -13,7 +13,7 @@ import unittest
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,31 @@ class TestRot3(GtsamTestCase):
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()