Ego velocity vy seems wrong, even if it is in local frame

[zhangdongkun98]

Describe the bug

Bug1: Ego past velocity and acceleration are transformed into local frame.

• I've already noticed that ego pose is in global frame and ego velocity and acceleration are in local frame. Therefore, it seems unreasonable to transform velocity and acceleration again in GenericAgentsFeatureBuilder (more specifically, in build_generic_ego_features_from_tensor

Bug2: Ego velocity vy is wrong, while vx seems correct.

• I will explain this given the output of my test code below.

Setup

• Following official docs to set up dataset and devkit.
• Dataset version is v1.1.

Steps To Reproduce

Steps to reproduce the behavior:

1. My test code test_data.py (put it in tutorials folder) for reproducing this bug is following:

# Useful imports
import os
from pathlib import Path



from typing import Any, Dict, List, Optional, Tuple, Type

import hydra
from omegaconf import DictConfig, OmegaConf
import tempfile

import numpy as np
import torch
np.set_printoptions(precision=6, linewidth=65536, suppress=True, threshold=np.inf)
torch.set_printoptions(precision=6, threshold=1000, edgeitems=None, linewidth=65536, profile=None, sci_mode=False)


from nuplan.planning.script.builders.model_builder import build_torch_module_wrapper
from nuplan.planning.script.builders.worker_pool_builder import build_worker

from nuplan.planning.script.builders.splitter_builder import build_splitter
from nuplan.planning.script.builders.scenario_builder import build_scenarios
from nuplan.planning.training.preprocessing.feature_preprocessor import FeaturePreprocessor

from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario
from nuplan.planning.scenario_builder.nuplan_db.nuplan_scenario import NuPlanScenario
from nuplan.planning.training.data_augmentation.abstract_data_augmentation import AbstractAugmentor
from nuplan.planning.training.data_loader.scenario_dataset import ScenarioDataset
from nuplan.planning.training.preprocessing.feature_preprocessor import FeaturePreprocessor
from nuplan.planning.training.modeling.types import FeaturesType, ScenarioListType, TargetsType

from nuplan.planning.training.preprocessing.features.generic_agents import GenericAgents
from nuplan.planning.training.preprocessing.features.vector_set_map import VectorSetMap
from nuplan.planning.training.preprocessing.features.trajectory import Trajectory

from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario
from nuplan.planning.simulation.trajectory.trajectory_sampling import TrajectorySampling
from nuplan.planning.training.preprocessing.utils.agents_preprocessing import EgoInternalIndex
from nuplan.common.actor_state.ego_state import EgoState

from nuplan.planning.training.preprocessing.target_builders import ego_trajectory_target_builder


# Location of path with all training configs
CONFIG_PATH = '../nuplan/planning/script/config/training'
CONFIG_NAME = 'default_training'

# Create a temporary directory to store the cache and experiment artifacts
SAVE_DIR = Path(tempfile.gettempdir()) / 'dataset'  # optionally replace with persistent dir
EXPERIMENT = 'training_vector_experiment'
JOB_NAME = 'train'
LOG_DIR = SAVE_DIR / EXPERIMENT / JOB_NAME

# Initialize configuration management system
hydra.core.global_hydra.GlobalHydra.instance().clear()
hydra.initialize(config_path=CONFIG_PATH)


cfg = hydra.compose(config_name=CONFIG_NAME, overrides=[
    f'group={str(SAVE_DIR)}',
    f'cache.cache_path={str(SAVE_DIR)}/cache',
    f'experiment_name={EXPERIMENT}',
    f'job_name={JOB_NAME}',
    'py_func=train',
    '+training=training_urban_driver_open_loop_model',
    'scenario_builder=nuplan_mini',  # use nuplan mini database
    'scenario_filter=one_continuous_log',
    'scenario_filter.limit_total_scenarios=3',
    'lightning.trainer.params.accelerator=ddp_spawn',
    'lightning.trainer.params.max_epochs=10',
    'data_loader.params.batch_size=8',
    'data_loader.params.num_workers=8',

    'model.feature_params.past_trajectory_sampling.num_poses=10',
    'model.feature_params.past_trajectory_sampling.time_horizon=1.0',
    'model.target_params.future_trajectory_sampling.num_poses=80',
    'model.target_params.future_trajectory_sampling.time_horizon=8.0',
])



@hydra.main(config_path=CONFIG_PATH, config_name=CONFIG_NAME)
def train(cfg: DictConfig):
    """
    Main entrypoint for training/validation experiments.
    :param cfg: omegaconf dictionary
    """

    # Build worker
    worker = build_worker(cfg)

    model = build_torch_module_wrapper(cfg.model)
    
    train_dataset = create_dataset(cfg, worker, model)
    for data in train_dataset:
        pass
    return




def create_dataset(cfg, worker, model):
    """
        only support dataset_fraction=1.0
    """
    feature_builders = model.get_list_of_required_feature()
    target_builders = model.get_list_of_computed_target()

    splitter = build_splitter(cfg.splitter)
    scenarios = build_scenarios(cfg, worker, model)

    feature_preprocessor = FeaturePreprocessor(
        cache_path=cfg.cache.cache_path,
        force_feature_computation=cfg.cache.force_feature_computation,
        feature_builders=feature_builders,
        target_builders=target_builders,
    )
    train_samples = splitter.get_train_samples(scenarios, worker)
    train_dataset = NuplanDataset(
        cfg,
        scenarios=train_samples,
        feature_preprocessor=feature_preprocessor,
        augmentors=None,
    )
    return train_dataset





class NuplanDataset(ScenarioDataset):
    def __init__(self, cfg, scenarios: List[AbstractScenario], feature_preprocessor: FeaturePreprocessor, augmentors: Optional[List[AbstractAugmentor]] = None) -> None:
        super().__init__(scenarios, feature_preprocessor, augmentors)
        self.cfg = cfg

        self.ego_trajectory_builder = EgoTrajectoryTargetBuilder(cfg.model.target_params.future_trajectory_sampling)

        self.num_future_poses = self.cfg.model.target_params.future_trajectory_sampling.num_poses
        self.future_time_horizon = self.cfg.model.target_params.future_trajectory_sampling.time_horizon
    

    def __getitem__(self, idx: int) -> Tuple[FeaturesType, TargetsType, ScenarioListType]:
        """
        Retrieves the dataset examples corresponding to the input index
        :param idx: input index
        :return: model features and targets
        """
        scenario: NuPlanScenario = self._scenarios[idx]

        features, targets, _ = self._feature_preprocessor.compute_features(scenario)

        ### timestamps in seconds
        future_timestamps = [scenario.start_time] + list(
            scenario.get_future_timestamps(
                iteration=0, num_samples=self.num_future_poses, time_horizon=self.future_time_horizon
            )
        )  ### contains current timestamp
        future_timestamps = np.array([t.time_us for t in future_timestamps]) * 1e-6
        future_timestamps -= future_timestamps[0]
        future_timestamps = future_timestamps[1:]

        ### data with batch_size 1
        vector_set_map: VectorSetMap = features['vector_set_map']
        generic_agents: GenericAgents = features['generic_agents']
        ego_trajectory_origin: Trajectory = targets['trajectory']

        ego_trajectory_mine = self.ego_trajectory_builder.get_targets(scenario)

        vx_dataset = ego_trajectory_mine[:,3]
        vy_dataset = ego_trajectory_mine[:,4]
        vx_differential = np.diff(ego_trajectory_mine[:,0]) / np.diff(future_timestamps)
        vy_differential = np.diff(ego_trajectory_mine[:,1]) / np.diff(future_timestamps)

        print(f'index {idx} {scenario.scenario_name}-{scenario.scenario_type}:')
        print(f'  ego past origin is\n', ego_trajectory_origin.data, '\n')
        print(f'  ego past mine is\n', ego_trajectory_mine, '\n')
        print(f'  velocity vx from dataset: ', vx_dataset[1:])
        print(f'  velocity vx from differential: ', vx_differential)
        print(f'  error vx: ', np.abs(vx_dataset[1:] - vx_differential).mean())
        print()
        print(f'  velocity vy from dataset: ', vy_dataset[1:])
        print(f'  velocity vy from differential: ', vy_differential)
        print(f'  error vy: ', np.abs(vy_dataset[1:] - vy_differential).mean())
        print('\n\n\n\n')


        ### vis
        import matplotlib.pyplot as plt
        fig = plt.figure()
        gs = fig.add_gridspec(2, 4)

        ax = fig.add_subplot(gs[0, :])
        ax.set_aspect('equal')
        ax.set_title(f'index {idx} {scenario.scenario_name}-{scenario.scenario_type}: x-y plot')
        ax.plot(ego_trajectory_mine[:,0], ego_trajectory_mine[:,1], '-')

        ax = fig.add_subplot(gs[1, 0])
        ax.set_title('t-x plot')
        ax.plot(future_timestamps, ego_trajectory_mine[:,0], '-')

        ax = fig.add_subplot(gs[1, 1])
        ax.set_title('t-y plot')
        ax.plot(future_timestamps, ego_trajectory_mine[:,1], '-')

        ax = fig.add_subplot(gs[1, 2])
        ax.set_title('t-vx plot')
        ax.plot(future_timestamps[1:], vx_dataset[1:], '-', label='dataset')
        ax.plot(future_timestamps[1:], vx_differential, '-', label='differential')
        ax.legend()

        ax = fig.add_subplot(gs[1, 3])
        ax.set_title('t-vx plot')
        ax.plot(future_timestamps[1:], vy_dataset[1:], '-', label='dataset')
        ax.plot(future_timestamps[1:], vy_differential, '-', label='differential')
        ax.legend()
        plt.show()


        features = {key: value.to_feature_tensor() for key, value in features.items()}
        targets = {key: value.to_feature_tensor() for key, value in targets.items()}
        scenarios = [scenario]

        return features, targets, scenarios






class EgoTrajectoryTargetBuilder(ego_trajectory_target_builder.EgoTrajectoryTargetBuilder):
    def get_targets(self, scenario: AbstractScenario) -> Trajectory:
        """
            ego pose is global, ego velocity is local
            https://github.com/motional/nuplan-devkit/issues/227
        """

        current_absolute_state: EgoState = scenario.initial_ego_state
        trajectory_absolute_states = scenario.get_ego_future_trajectory(
            iteration=0, num_samples=self._num_future_poses, time_horizon=self._time_horizon
        )
        absolute_states = [state for state in trajectory_absolute_states]

        absolute_trajectory = np.zeros((len(absolute_states), EgoInternalIndex.dim()), dtype=np.float64)  ### (x, y, heading, vx, vy, ax, ay)
        for i, absolute_state in enumerate(absolute_states):
            absolute_trajectory[i, EgoInternalIndex.x()] = absolute_state.rear_axle.x
            absolute_trajectory[i, EgoInternalIndex.y()] = absolute_state.rear_axle.y
            absolute_trajectory[i, EgoInternalIndex.heading()] = absolute_state.rear_axle.heading
            absolute_trajectory[i, EgoInternalIndex.vx()] = absolute_state.dynamic_car_state.rear_axle_velocity_2d.x
            absolute_trajectory[i, EgoInternalIndex.vy()] = absolute_state.dynamic_car_state.rear_axle_velocity_2d.y
            absolute_trajectory[i, EgoInternalIndex.ax()] = absolute_state.dynamic_car_state.rear_axle_acceleration_2d.x
            absolute_trajectory[i, EgoInternalIndex.ay()] = absolute_state.dynamic_car_state.rear_axle_acceleration_2d.y

        if len(absolute_trajectory) != self._num_future_poses:
            raise RuntimeError(f'Expected {self._num_future_poses} num poses but got {len(absolute_trajectory)}')

        state0 = current_absolute_state.rear_axle.serialize()

        local_pose = world2local(absolute_trajectory[:, [EgoInternalIndex.x(), EgoInternalIndex.y(), EgoInternalIndex.heading()]], state0)
        local_velocity = absolute_trajectory[:, [EgoInternalIndex.vx(), EgoInternalIndex.vy(), EgoInternalIndex.heading()]]
        local_acceleration = absolute_trajectory[:, [EgoInternalIndex.ax(), EgoInternalIndex.ay(), EgoInternalIndex.heading()]]

        local_trajectory = np.zeros(absolute_trajectory.shape, dtype=np.float32)
        local_trajectory[:, EgoInternalIndex.x()] = local_pose[:, 0]
        local_trajectory[:, EgoInternalIndex.y()] = local_pose[:, 1]
        local_trajectory[:, EgoInternalIndex.heading()] = local_pose[:, 2]
        local_trajectory[:, EgoInternalIndex.vx()] = local_velocity[:, 0]
        local_trajectory[:, EgoInternalIndex.vy()] = local_velocity[:, 1]
        local_trajectory[:, EgoInternalIndex.ax()] = local_acceleration[:, 0]
        local_trajectory[:, EgoInternalIndex.ay()] = local_acceleration[:, 1]

        return local_trajectory


def world2local(state, state0):
    '''
        state (array): (n, 3), 3 -> (x, y, heading)
        state0: (3,), 3 -> (x, y, heading)
    '''

    x_world, y_world, theta_world = state[:,0], state[:,1], state[:,2]
    x0, y0, theta0 = state0[0], state0[1], state0[2]

    x_local = (x_world-x0)*np.cos(theta0) + (y_world-y0)*np.sin(theta0)
    y_local =-(x_world-x0)*np.sin(theta0) + (y_world-y0)*np.cos(theta0)
    delta_theta = pi2pi_numpy(theta_world - theta0)
    return np.stack([x_local, y_local, delta_theta], axis=1)

def pi2pi_numpy(theta):
    return (theta + np.pi) % (2 * np.pi) - np.pi



train(cfg)

2. Simply run python test_data.py

Explanation

Bug1 is straightforward, for Bug2, my code visualizes ego future trajectory (8s with interval 0.1s) in 3 scenarios.

• scenario 0:

• scenario 1:

• scenario 2:

In each 't-vx' and 't-vy' plot, blue line is from the dataset, orange line is calculated by difference of pose (for instance, vx[i] = (x[i] - x[i-1]) / dt). Clearly, difference is an inaccurate way to calculate velocity. However, from these figs, one can find that the trend of velocity vx from difference aligns with that from dataset, while vy fails to match, meaning that ego vy is wrong.

[zhangdongkun98]

Hi @christopher-motional , can you help me fix this?

[christopher-motional]

Hi @zhangdongkun98, yes, sorry for the delay --thanks for the catch, those do indeed sound like bugs, will confirm and aim to get a fix up shortly.

[zhangdongkun98]

Hi @zhangdongkun98, yes, sorry for the delay --thanks for the catch, those do indeed sound like bugs, will confirm and aim to get a fix up shortly.

Thanks for your reply!

[MTDzi]

@christopher-motional I was curious to learn if you managed to confirm / deny these are indeed bugs.

[Zigned]

I think the vy you calculated is in the local frame at the initial iteration. However, the EgoState stores dynamic states in the local frame at each iteration. Therefore, the lateral velocity of the ego vehicle is small even when changing lanes. Remind the non-holonomic constraint of vehicles. But the vy in the dataset still seems wrong because it is always negative (-0.15~-0.20).