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

add rotate() test

release/4.3a0
Akshay Krishnan 2022-12-24 10:06:28 -05:00
parent f2ab4afda4
commit fd55e09bcc
1 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
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,19 @@ 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]))
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()