示例#1
0
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
示例#2
0
    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'))