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:
![0](https://github.com/motional/nuplan-devkit/assets/71307861/5730a310-4d7c-4910-86ba-529db7d69328)

- scenario 1:
![1](https://github.com/motional/nuplan-devkit/assets/71307861/a900824c-ede2-41ae-a582-20d7a796d49c)

- scenario 2:
![2](https://github.com/motional/nuplan-devkit/assets/71307861/fde2cb63-2ecd-4ccb-ac47-1ee7f2e5cf79)

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).