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))