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_velocity_return_nan_one_obs(self) -> None:
mock_samples = [{'token': '1', 'timestamp': 0}]
nusc = MockNuScenes(self.mock_annotations, mock_samples)
helper = PredictHelper(nusc)
self.assertTrue(np.isnan(helper.get_velocity_for_agent('1', '1')))
def test_velocity(self):
mock_samples = [{'token': '1', 'timestamp': 0},
{'token': '2', 'timestamp': 0.5e6}]
nusc = MockNuScenes(self.mock_annotations, mock_samples)
helper = PredictHelper(nusc)
self.assertEqual(helper.get_velocity_for_agent("1", "2"), np.sqrt(8))
def test_velocity_return_nan_big_diff(self):
mock_samples = [{'token': '1', 'timestamp': 0},
{'token': '2', 'timestamp': 2.5e6}]
nusc = MockNuScenes(self.mock_annotations, mock_samples)
helper = PredictHelper(nusc)
self.assertTrue(np.isnan(helper.get_velocity_for_agent('1', '2')))
