def test_prune_missing_timestamps_some_overlap(self):
a_times = np.arange(10.0)
xs = np.arange(10.0)
ys = np.arange(10.0) + 1.0
zs = np.arange(10.0) + 2.0
b_times = np.arange(8.0, 20.0)
poses_a = make_poses(a_times, xs, ys, zs)
poses_b = make_poses(b_times, xs, ys, zs)
expected_time_range = np.arange(8.0, 10.0)
trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(
poses_a, poses_b)
self.assertEqual(len(trimmed_a.times), len(trimmed_b.times))
self.assertTrue(np.allclose(trimmed_a.times, trimmed_b.times, rtol=0))
self.assertTrue(
np.allclose(trimmed_a.times, expected_time_range, rtol=0))
self.assertTrue(
np.allclose(trimmed_a.positions.xs, expected_time_range, rtol=0))
self.assertTrue(
np.allclose(trimmed_a.positions.ys,
expected_time_range + 1,
rtol=0))
self.assertTrue(
np.allclose(trimmed_a.positions.zs,
expected_time_range + 2,
rtol=0))
def test_prune_missing_timestamps_disjoint_set(self): a_times = np.arange(10.0) xs = np.arange(10.0) ys = np.arange(10.0) + 1.0 zs = np.arange(10.0) + 2.0 b_times = np.arange(11, 20) poses_a = make_poses(a_times, xs, ys, zs) poses_b = make_poses(b_times, xs, ys, zs) trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) self.assertEqual(len(trimmed_a.times), 0) self.assertEqual(len(trimmed_b.times), 0)
def test_prune_missing_timestamps_scattered_set(self): a_times = np.arange(10.0) xs = np.arange(10.0) ys = np.arange(10.0) + 1.0 zs = np.arange(10.0) + 2.0 b_times = np.array([1.0, 5.0, 6.0, 9.0]) poses_a = make_poses(a_times, xs, ys, zs) poses_b = make_poses(b_times, xs, ys, zs) trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) self.assertEqual(len(trimmed_a.times), len(trimmed_b.times)) self.assertTrue(np.allclose(trimmed_a.times, b_times, rtol=0)) self.assertTrue(np.allclose(trimmed_a.positions.xs, b_times, rtol=0)) self.assertTrue(np.allclose(trimmed_a.positions.ys, b_times + 1, rtol=0)) self.assertTrue(np.allclose(trimmed_a.positions.zs, b_times + 2, rtol=0))