def test_prune_missing_timestamps_scattered_set(self):
a_times = np.arange(10.0)
a_xs = np.arange(10.0)
a_ys = np.arange(10.0) + 1.0
a_zs = np.arange(10.0) + 2.0
b_times = np.array([1.0, 5.0, 6.0, 9.0])
pruned_a = ekf_graph.prune_missing_timestamps(a_xs, a_ys, a_zs,
a_times, b_times)
self.assertEqual(len(pruned_a), len(b_times))
self.assertTrue(np.allclose(pruned_a[:, 0], b_times))
self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0))
self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0))
def test_prune_missing_timestamps_end_set(self):
a_times = np.arange(10.0)
a_xs = np.arange(10.0)
a_ys = np.arange(10.0) + 1.0
a_zs = np.arange(10.0) + 2.0
b_times = np.arange(7.0, 10.0)
pruned_a = ekf_graph.prune_missing_timestamps(a_xs, a_ys, a_zs,
a_times, b_times)
self.assertEqual(len(pruned_a), len(b_times))
self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0))
self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0))
self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0))