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_heading_change_rate_near_pi(self):
mock_samples = [{'token': '1', 'timestamp': 0}, {'token': '2', 'timestamp': 0.5e6}]
mock_annotations = copy.copy(self.mock_annotations)
mock_annotations[0]['rotation'] = [np.cos((np.pi - 0.05)/2), 0, 0, np.sin((np.pi - 0.05) / 2)]
mock_annotations[1]['rotation'] = [np.cos((-np.pi + 0.05)/2), 0, 0, np.sin((-np.pi + 0.05) / 2)]
nusc = MockNuScenes(mock_annotations, mock_samples)
helper = PredictHelper(nusc)
self.assertAlmostEqual(helper.get_heading_change_rate_for_agent('1', '2'), 0.2)
def test_heading_change_rate(self) -> None:
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_heading_change_rate_for_agent('1', '2'),
np.pi)
