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_get_sample_annotation(self,): mock_annotation = {'token': '1', 'instance_token': 'instance_1', 'sample_token': 'sample_1'} mock_sample = {'token': 'sample_1', 'timestamp': 0} nusc = MockNuScenes([mock_annotation], [mock_sample]) helper = PredictHelper(nusc) self.assertDictEqual(mock_annotation, helper.get_sample_annotation('instance_1', 'sample_1'))