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

Cleaned up Pose3 unit test, added unit test for adjoint.

release/4.3a0
Frank Dellaert 2018-12-31 15:38:12 -05:00
parent 409a0215b8
commit 1999fba7ae
1 changed files with 25 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
cython/gtsam/tests/test_Pose3.py
View File

@ -1,25 +1,32 @@
"""Pose3 unit tests."""
import math import math
import unittest import unittest
from gtsam import Point3, Rot3, Pose3 import numpy as np
from gtsam import Point3, Pose3, Rot3
class TestPose3(unittest.TestCase): class TestPose3(unittest.TestCase):
"""Test selected Pose3 methods."""
def test__between(self): def test_between(self):
T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)) """Test between method."""
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
expected = T2.inverse().compose(T3) expected = T2.inverse().compose(T3)
actual = T2.between(T3) actual = T2.between(T3)
self.assertTrue(actual.equals(expected, 1e-6)) self.assertTrue(actual.equals(expected, 1e-6))
def test_transform_to(self): def test_transform_to(self):
transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0)) """Test transform_to method."""
actual = transform.transform_to(Point3(3,2,10)) transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
expected = Point3 (2,1,10) actual = transform.transform_to(Point3(3, 2, 10))
expected = Point3(2, 1, 10)
self.assertTrue(actual.equals(expected, 1e-6)) self.assertTrue(actual.equals(expected, 1e-6))
def test_range(self): def test_range(self):
"""Test range method."""
l1 = Point3(1, 0, 0) l1 = Point3(1, 0, 0)
l2 = Point3(1, 1, 0) l2 = Point3(1, 1, 0)
x1 = Pose3() x1 = Pose3()
@ -28,16 +35,23 @@ class TestPose3(unittest.TestCase):
xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
# establish range is indeed zero # establish range is indeed zero
self.assertEqual(1,x1.range(point=l1)) self.assertEqual(1, x1.range(point=l1))
# establish range is indeed sqrt2 # establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0),x1.range(point=l2)) self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
# establish range is indeed zero # establish range is indeed zero
self.assertEqual(1,x1.range(pose=xl1)) self.assertEqual(1, x1.range(pose=xl1))
# establish range is indeed sqrt2 # establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2)) self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
def test_adjoint(self):
"""Test adjoint method."""
xi = np.array([1, 2, 3, 4, 5, 6])
expected = np.dot(Pose3.adjointMap(xi), xi)
actual = Pose3.adjoint(xi, xi)
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__": if __name__ == "__main__":