def _kinematics_from_tokens(helper: PredictHelper, instance: str,
sample: str) -> KinematicsData:
"""
Returns the 2D position, velocity and acceleration vectors from the given track records,
along with the speed, yaw rate, (scalar) acceleration (magnitude), and heading.
:param helper: Instance of PredictHelper.
:instance: Token of instance.
:sample: Token of sample.
:return: KinematicsData.
"""
annotation = helper.get_sample_annotation(instance, sample)
x, y, _ = annotation['translation']
yaw = quaternion_yaw(Quaternion(annotation['rotation']))
velocity = helper.get_velocity_for_agent(instance, sample)
acceleration = helper.get_acceleration_for_agent(instance, sample)
yaw_rate = helper.get_heading_change_rate_for_agent(instance, sample)
if np.isnan(velocity):
velocity = 0.0
if np.isnan(acceleration):
acceleration = 0.0
if np.isnan(yaw_rate):
yaw_rate = 0.0
hx, hy = np.cos(yaw), np.sin(yaw)
vx, vy = velocity * hx, velocity * hy
ax, ay = acceleration * hx, acceleration * hy
return x, y, vx, vy, ax, ay, velocity, yaw_rate, acceleration, yaw
def test_acceleration_nan_not_enough_data(self):
mock_samples = [{
'token': '1',
'timestamp': 0
}, {
'token': '2',
'timestamp': 0.5e6
}, {
'token': '3',
'timestamp': 1e6
}]
nusc = MockNuScenes(self.mock_annotations, mock_samples)
helper = PredictHelper(nusc)
self.assertTrue(np.isnan(helper.get_acceleration_for_agent('1', '2')))
def test_acceleration_zero(self):
mock_samples = [{
'token': '1',
'timestamp': 0
}, {
'token': '2',
'timestamp': 0.5e6
}, {
'token': '3',
'timestamp': 1e6
}]
nusc = MockNuScenes(self.mock_annotations, mock_samples)
helper = PredictHelper(nusc)
self.assertEqual(helper.get_acceleration_for_agent('1', '3'), 0)
def test_acceleration_nonzero(self):
mock_samples = [{
'token': '1',
'timestamp': 0
}, {
'token': '2',
'timestamp': 0.5e6
}, {
'token': '3',
'timestamp': 1e6
}]
mock_annotations = copy.copy(self.mock_annotations)
mock_annotations[2]['translation'] = [3, 3, 3]
nusc = MockNuScenes(mock_annotations, mock_samples)
helper = PredictHelper(nusc)
self.assertAlmostEqual(helper.get_acceleration_for_agent('1', '3'),
2 * (np.sqrt(32) - np.sqrt(8)))
