..
  *******************************************************************************
  Copyright (c) 2021 in-tech GmbH
                2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)

  This program and the accompanying materials are made available under the
  terms of the Eclipse Public License 2.0 which is available at
  http://www.eclipse.org/legal/epl-2.0.

  SPDX-License-Identifier: EPL-2.0
  *******************************************************************************

.. _coordinatesystems:

Coordinate Systems
==================

|op| uses three different coordinate systems.
All systems rotate counterclockwise and use radian.

.. _coordinatesystems_world:

World Coordinate System
-----------------------

The world coordinate system is absolute. It is the parent coordinate system and all other systems are relative to the world coordinate system.
This system consists of *x*, *y* and *z* (*z* is currently unused).

.. figure:: ./images/WorldCoordinateSystem.png

   World Coordinate System

.. _coordinatesystems_road:

Road Coordinate System
----------------------

The road coordinate system is relative to the position of the road in the world coordinate system.
It consists of *s* and *t* coordinates, where *s* describes the longitudinal position along the road.
At the beginning of the road *s* = 0, increasing with the downstream direction of the road.
The coordinate *t* describes the lateral position relative to the center of the road, where *t* > 0 indicates left side and *t* < 0 right side of the road with respect to the road direction.

.. figure:: ./images/RoadCoordinateSystem.png

   Road Coordinate System

.. _coordinatesystems_agent:

Agent Coordinate System
~~~~~~~~~~~~~~~~~~~~~~~

The agent coordinate system is relative to the position of the agent in the world coordinate system.
It uses longitudinal, lateral and height (height currently unused).
The system originates at the *reference point*, which is located at the center of the rear axle.
The longitudinal axis is parallel to the length of the car and the lateral axis is parallel to the width of the car

.. figure:: ./images/AgentCoordinateSystem.png

   Agent Coordinate System

.. _coordinatesystems_sensor:

Sensor Coordinate System
------------------------

Each sensor has its own coordinate system whose origin is the mounting position of the sensor.
The *x* axis points in the direction of the sensor as defined by the mounting orientation.
All values in the OSI SensorData generated by a sensor are in its sensor coordinate system.
Vectorial values such as velocity and acceleration are relative to the own vehicle's velocity and acceleration at the sensor position.

.. _coordinatesystems_sensor_transformation:

Conversions between SensorData coordinate systems
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Since all values in the SensorData are relative to the sensor the following conversions are needed.

In the Sensor_OSI where the SensorData is written, data for other objects is calculated according to these steps:

#) Get position, velocity and acceleration from the SensorView in world coordinates
#) For the position:
    #) Translation by the sensor position (in world coordinates, inverse)
    #) Rotation by the sensor orientation (in world coordinates, inverse)
#) For the velocity and acceleration (vectorial):
    #) Subtract own velocity / acceleration at sensor position
    #) Rotation by the sensor orientation (in world coordinates, inverse)

ADAS using SensorData may need to convert the values back to the agent coordinate system.
The SensorFusion_Errorless also converts the merged SensorData to agent coordinate and sets the mounting position to the agents reference point.
This calculation follows the following steps:

#) Get position, velocity and acceleration from the SensorData in sensor coordinates
#) For the position:
    #) Rotation by the sensor orientation (mounting position in agent coordinates)
    #) Translation by the sensor position (mounting position in agent coordinates)
#) For the velocity and acceleration (vectorial):
    #) Calculate the difference between the ego velocity / acceleration at the sensor position to the reference point (in world coordinates)
    #) Rotation of this difference by the inverse agent orientation
    #) Rotation of objects' velocity / acceleration by the sensor orientation (mounting position in agent coordinates)
    #) Add results of the two previous steps
