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 ``PoseSE3`` instance can be created.
"""
p1 = PoseSE3([1, 2, 3], [0, 0, 0, 1])
p2 = PoseSE3(np.array([4, 5, 6]), np.array([1, 0, 0, 0]))
self.assertIsInstance(p1, PoseSE3)
self.assertIsInstance(p2, PoseSE3)
def test_add(self):
"""Test that the overloaded ``__add__`` method works as expected.
"""
np.random.seed(0)
# PoseSE3 (+) PoseSE3
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()
expected = np.dot(p1.to_matrix(), p2.to_matrix())
self.assertAlmostEqual(
np.linalg.norm((p1 + p2).to_matrix() - expected), 0.)
# PoseSE3 [+] numpy.ndarray
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))
p2_compact = p2.to_compact()
if np.linalg.norm(p2.orientation[:3]) > 1.0:
p2[3:] = [0., 0., 0., 1.]
else:
p2.normalize()
p2_compact[3:] = p2.orientation[:3]
p1.normalize()
expected = np.dot(p1.to_matrix(), p2.to_matrix())
self.assertAlmostEqual(
np.linalg.norm((p1 + p2_compact).to_matrix() - expected), 0.)
p1 += p2_compact
self.assertAlmostEqual(np.linalg.norm(p1.to_matrix() - expected),
0.)
with self.assertRaises(NotImplementedError):
p1 = PoseSE3(np.random.random_sample(3),
np.random.random_sample(4))
_ = p1 + 5
def test_normalize(self):
"""Test that the ``normalize`` method works as expected.
"""
p1 = PoseSE3([1, 2, 3], [2, 2, 2, 2])
p2 = PoseSE3(np.array([4, 5, 6]), np.array([2, 0, 0, 0]))
p1.normalize()
p2.normalize()
self.assertAlmostEqual(
np.linalg.norm(p1.to_array() -
np.array([1, 2, 3, 0.5, 0.5, 0.5, 0.5])), 0.)
self.assertAlmostEqual(
np.linalg.norm(p2.to_array() - np.array([4, 5, 6, 1, 0, 0, 0])),
0.)
def test_orientation(self):
"""Test that the ``orientation`` property works as expected.
"""
p1 = PoseSE3([1, 2, 3], [0, 0, 0, 1])
self.assertAlmostEqual(
np.linalg.norm(p1.orientation - np.array([0, 0, 0, 1])), 0.)
def test_copy(self):
"""Test that the ``copy`` method works as expected.
"""
p1 = PoseSE3([1, 2, 3], [0, 0, 0, 1])
p2 = p1.copy()
p2[0] = 0
self.assertEqual(p1[0], 1)
def test_sub(self):
"""Test that the overloaded ``__sub__`` method works as expected.
"""
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()
expected = np.dot(np.linalg.inv(p2.to_matrix()), p1.to_matrix())
self.assertAlmostEqual(
np.linalg.norm((p1 - p2).to_matrix() - expected), 0.)
def test_position(self):
"""Test that the ``position`` property works as expected.
"""
p1 = PoseSE3([1, 2, 3], [0, 0, 0, 1])
pos = p1.position
true_pos = np.array([1, 2, 3])
self.assertIsInstance(pos, np.ndarray)
self.assertNotIsInstance(pos, PoseSE3)
self.assertAlmostEqual(np.linalg.norm(true_pos - pos), 0.)
def test_to_compact(self):
"""Test that the ``to_compact`` method works as expected.
"""
p1 = PoseSE3([1, 2, 3], [0, 0, 0, 1])
arr = p1.to_compact()
self.assertIsInstance(arr, np.ndarray)
self.assertNotIsInstance(arr, PoseSE3)
self.assertAlmostEqual(
np.linalg.norm(arr - np.array([1, 2, 3, 0, 0, 0])), 0.)
def test_inverse(self):
"""Test that the ``inverse`` property works as expected.
"""
np.random.seed(0)
for _ in range(10):
p = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
p.normalize()
expected = np.linalg.inv(p.to_matrix())
self.assertAlmostEqual(
np.linalg.norm(p.inverse.to_matrix() - expected), 0.)
def setUp(self):
r"""Setup a simple ``Graph`` in :math:`SE(3)`.
"""
np.random.seed(0)
p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
p2 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
p3 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
estimate = PoseSE3([0, 0, 0], [0, 0, 0, 1])
p1.normalize()
p2.normalize()
p3.normalize()
v1 = Vertex(1, p1)
v2 = Vertex(2, p2)
v3 = Vertex(3, p3)
e1 = EdgeOdometry([1, 2], np.eye(6), estimate, [v1, v2])
e2 = EdgeOdometry([3, 2], 2 * np.eye(6), estimate, [v3, v2])
self.g = Graph([e1, e2], [v1, v2, v3])
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()