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

cython/gtsam/tests/test_Pose3.py

@@ -1,12 +1,17 @@
+"""Pose3 unit tests."""
 import math
 import unittest
 
-from gtsam import Point3, Rot3, Pose3
+import numpy as np
+
+from gtsam import Point3, Pose3, Rot3
 
 
 class TestPose3(unittest.TestCase):
+    """Test selected Pose3 methods."""
 
-    def test__between(self):
+    def test_between(self):
+        """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))
         expected = T2.inverse().compose(T3)
@@ -14,12 +19,14 @@ class TestPose3(unittest.TestCase):
         self.assertTrue(actual.equals(expected, 1e-6))
 
     def test_transform_to(self):
+        """Test transform_to method."""
         transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
         actual = transform.transform_to(Point3(3, 2, 10))
         expected = Point3(2, 1, 10)
         self.assertTrue(actual.equals(expected, 1e-6))
 
     def test_range(self):
+        """Test range method."""
         l1 = Point3(1, 0, 0)
         l2 = Point3(1, 1, 0)
         x1 = Pose3()
@@ -39,6 +46,13 @@ class TestPose3(unittest.TestCase):
         # establish range is indeed sqrt2
         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__":
     unittest.main()