%% Convert Target Poses Between Ego Vehicle and Scenario Coordinates
% In a simple driving scenario, obtain the poses of target vehicles in the coordinate system of the ego vehicle. Then convert these poses back to the world coordinates of the driving scenario.
% Create a driving scenario.

scenario = drivingScenario;

% Create target actors.

actor(scenario,'ClassID',1, ...
    'Position',[10 20 30], ...
    'Velocity',[12 113 14], ...
    'Yaw',54, ...
    'Pitch',25, ...
    'Roll',22, ...
    'AngularVelocity',[24 42 27]);

actor(scenario,'ClassID',1, ...
    'Position',[17 22 12], ...
    'Velocity',[19 13 15], ...
    'Yaw',45, ...
    'Pitch',52, ...
    'Roll',2, ...
    'AngularVelocity',[42 24 29]);

% Add an ego vehicle actor.

egoActor = actor(scenario,'ClassID',1, ...
    'Position',[1 2 3], ...
    'Velocity',[1.2 1.3 1.4], ...
    'Yaw',4, ...
    'Pitch',5, ...
    'Roll',2, ...
    'AngularVelocity',[4 2 7]);

% Use the actorPoses function to return the poses of all actors in the scenario. Pose properties (position, velocity, and orientation) are in the world coordinates of the driving scenario. Save the target actors to a separate variable and inspect the pose of the first target actor.

allPoses = actorPoses(scenario);
targetPosesScenarioCoords = allPoses(1:2);
targetPosesScenarioCoords(1)

% Use the driving.scenario.targetsToEgo function to convert the target poses to the ego-centric coordinates of the ego actor. Inspect the pose of the first actor.

targetPosesEgoCoords = driving.scenario.targetsToEgo(targetPosesScenarioCoords,egoActor);
targetPosesEgoCoords(1)

% Alternatively, use the targetPoses function to obtain all target actor poses in ego vehicle coordinates. Display the first target pose, which matches the previously calculated pose.

targetPosesEgoCoords = targetPoses(egoActor);
targetPosesEgoCoords(1)

% Use the driving.scenario.targetsToScenario to convert the target poses back to the world coordinates of the scenario. Display the first target pose, which matches the original target pose.

targetPosesScenarioCoords = driving.scenario.targetsToScenario(targetPosesEgoCoords,egoActor);
targetPosesScenarioCoords(1)

%
% Copyright 2012 The MathWorks, Inc.