def test_calc_error(self):
"""Test that the ``calc_error`` method is not implemented.
"""
p = PoseSE2([0, 0], 0)
v = Vertex(0, p)
e = BaseEdge(0, 1, 0, [v])
with self.assertRaises(NotImplementedError):
_ = e.calc_error()
def test_plot(self):
"""Test that the ``plot`` method is not implemented.
"""
p = PoseSE2([0, 0], 0)
v = Vertex(0, p)
e = BaseEdge(0, 1, 0, [v])
with self.assertRaises(NotImplementedError):
e.plot()
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 = PoseSE3(np.random.random_sample(3),
np.random.random_sample(4))
p2 = PoseSE3(np.random.random_sample(3),
np.random.random_sample(4))
p1.normalize()
p2.normalize()
v1 = Vertex(1, p1)
v2 = Vertex(2, p2)
e = EdgeOMinusCompact([1, 2], np.eye(7), np.zeros(7), [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., 5)
def test_constructor(self):
"""Test that a ``BaseEdge`` object can be created.
"""
p = PoseSE2([0, 0], 0)
v = Vertex(0, p)
e = BaseEdge(0, 1, 0, [v])
self.assertEqual(e.vertices[0].id, 0)
self.assertEqual(e.information, 1)
def test_jacobian_self_oplus_other(self):
"""Test that the ``jacobian_self_oplus_other_wrt_self`` and ``jacobian_self_oplus_other_wrt_other`` methods are correctly implemented.
"""
np.random.seed(0)
for _ in range(10):
p1 = PoseR2(np.random.random_sample(2))
p2 = PoseR2(np.random.random_sample(2))
v1 = Vertex(1, p1)
v2 = Vertex(2, p2)
e = EdgeOPlus([1, 2], np.eye(2), np.zeros(2), [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.)
