def test_constructor(self): """Test that a ``PoseR3`` instance can be created. """ r3a = PoseR3([1, 2, 3]) r3b = PoseR3(np.array([3, 4, 5])) self.assertIsInstance(r3a, PoseR3) self.assertIsInstance(r3b, PoseR3)
def test_sub(self): """Test that the overloaded ``__sub__`` method works as expected. """ r3a = PoseR3([1, 2, 3]) r3b = PoseR3([3, 4, 5]) expected = PoseR3([2, 2, 2]) self.assertAlmostEqual( np.linalg.norm((r3b - r3a).to_array() - expected), 0.)
def test_orientation(self): """Test that the ``orientation`` property works as expected. """ r3 = PoseR3([1, 2, 3]) self.assertEqual(r3.orientation, 0.)
def test_copy(self): """Test that the ``copy`` method works as expected. """ p1 = PoseR3([1, 2, 3]) p2 = p1.copy() p2[0] = 0 self.assertEqual(p1[0], 1)
def setUp(self): r"""Setup a simple ``Graph`` in :math:`\mathbb{R}^3`. """ np.random.seed(0) p1 = PoseR3(np.random.random_sample(3)) p2 = PoseR3(np.random.random_sample(3)) p3 = PoseR3(np.random.random_sample(3)) estimate = PoseR3([0, 0, 0]) v1 = Vertex(1, p1) v2 = Vertex(2, p2) v3 = Vertex(3, p3) e1 = EdgeOdometry([1, 2], np.eye(3), estimate, [v1, v2]) e2 = EdgeOdometry([3, 2], 2 * np.eye(3), estimate, [v3, v2]) self.g = Graph([e1, e2], [v1, v2, v3])
def test_inverse(self): """Test that the ``inverse`` property works as expected. """ r3 = PoseR3([1, 2, 3]) true_inv = np.array([-1, -2, -3]) self.assertAlmostEqual( np.linalg.norm(r3.inverse.to_array() - true_inv), 0.)
def test_to_compact(self): """Test that the ``to_compact`` method works as expected. """ r3 = PoseR3([1, 2, 3]) arr = r3.to_compact() self.assertIsInstance(arr, np.ndarray) self.assertNotIsInstance(arr, PoseR3) self.assertAlmostEqual(np.linalg.norm(arr - np.array([1, 2, 3])), 0.)
def test_position(self): """Test that the ``position`` property works as expected. """ r3 = PoseR3([1, 2, 3]) pos = r3.position true_pos = np.array([1, 2, 3]) self.assertIsInstance(pos, np.ndarray) self.assertNotIsInstance(pos, PoseR3) self.assertAlmostEqual(np.linalg.norm(true_pos - pos), 0.)
def test_jacobian_self_ominus_other_compact(self): """Test that the ``jacobian_self_ominus_other_wrt_self_compact`` and ``jacobian_self_ominus_other_wrt_other_compact`` methods are correctly implemented. """ np.random.seed(0) for _ in range(10): p1 = PoseR3(np.random.random_sample(3)) p2 = PoseR3(np.random.random_sample(3)) v1 = Vertex(1, p1) v2 = Vertex(2, p2) e = EdgeOMinusCompact([1, 2], np.eye(3), np.zeros(3), [v1, v2]) numerical_jacobians = BaseEdge.calc_jacobians(e) analytical_jacobians = e.calc_jacobians() self.assertEqual(len(numerical_jacobians), len(analytical_jacobians)) for n, a in zip(numerical_jacobians, analytical_jacobians): self.assertAlmostEqual(np.linalg.norm(n - a), 0.)
def test_plot(self): """Test that the ``plot`` method is not implemented. """ v_none = Vertex(0, None) v_r2 = Vertex(1, PoseR2([1, 2])) v_se2 = Vertex(2, PoseSE2([1, 2], 3)) v_r3 = Vertex(3, PoseR3([1, 2, 3])) v_se3 = Vertex(4, PoseSE3([1, 2, 3], [0.5, 0.5, 0.5, 0.5])) with self.assertRaises(NotImplementedError): e = EdgeOdometry(0, 1, 0, [v_none, v_none]) e.plot() for v in [v_r2, v_se2, v_r3, v_se3]: e = EdgeOdometry(0, 1, 0, [v, v]) e.plot()
def test_plot(self): """Test that a ``Vertex`` can be plotted. """ v_none = Vertex(0, None) v_r2 = Vertex(1, PoseR2([1, 2])) v_se2 = Vertex(2, PoseSE2([1, 2], 3)) v_r3 = Vertex(3, PoseR3([1, 2, 3])) v_se3 = Vertex(4, PoseSE3([1, 2, 3], [0.5, 0.5, 0.5, 0.5])) with self.assertRaises(NotImplementedError): v_none.plot() for v in [v_r2, v_se2, v_r3, v_se3]: fig = plt.figure() if len(v.pose.position) == 3: fig.add_subplot(111, projection='3d') v.plot()