Merge pull request #607 from OpenSimulationInterface/604_coordinate_transformation

Added coordinate transformation to documentation

Author: pmai

doc/architecture/reference_points_coordinate_systems.adoc

@@ -1,4 +1,4 @@
-= Coordinate systems and reference points
+= Coordinate systems, reference points and coordinate transformation
 
 OSI uses DIN ISO 8855:2013-11 cite:[iso8855] for coordinate systems and transformations between coordinate systems.
 OSI uses three coordinate systems:

doc/usecases/transforming_coordinate_systems.adoc

@@ -7,7 +7,75 @@ When running simulations, it is frequently necessary to transform coordinates fr
 This section provides an overview of the messages and fields involved and their relationship for this task.
 It demonstrates how a global coordinate system, vehicle coordinate system, and sensor coordinate system are related on the basis of a specific (ego) vehicle.
 
-//TODO: Should we add one or more sentences about the mathematical operations involved?
+**Mathematical Definitions of Coordinate Transformations**
+
+All vectors and matrices are noted with reference frame as a superscript index and the direction of translation as a supscript index. cite:[reuper2020]
+The translation direction is from the first index to the second index (src: source coordinate system, trg: target coordinate system).
+The vector latexmath:[\boldsymbol{v}^x] denotes the 3D position of an object in the coordinate frame latexmath:[x].
+Vector latexmath:[\boldsymbol{t}] is the translation vector between two coordinate systems with the described indices for reference frame and direction.
+The angles yaw latexmath:[\psi] around the z-axis, pitch latexmath:[\theta] around the y-axis and roll latexmath:[\phi] around the x-axis are defined in a right handed coordinate system according to DIN ISO 8855:2013 cite:[iso8855].
+The sign of the angles corresponds to the direction of the transformation.
+
+Transformation from source latexmath:[src] to target latexmath:[trg] coordinates:
+[latexmath]
+++++
+\boldsymbol{v}^{trg} = \boldsymbol{R}_{src}^{trg} (\boldsymbol{v}^{src} - \boldsymbol{t}_{src,trg}^{src})
+++++
+
+Transformation back from target latexmath:[trg] to source latexmath:[src] coordinates
+[latexmath]
+++++
+\boldsymbol{v}^{src} = (\boldsymbol{R}_{src}^{trg})^{-1} \boldsymbol{v}^{trg}  + \boldsymbol{t}_{src,trg}^{src}
+++++
+
+
+With the rotation matrix (from rotating the coordinate system) cite:[wiki_rotation_matrix]:
+[latexmath]
+++++
+\boldsymbol{R}_{srv}^{trg}=\boldsymbol{R}_{yaw,pitch,roll} = \boldsymbol{R}_{z,y,x} = \boldsymbol{R}_{x}(\phi) \boldsymbol{R}_{y}(\theta) \boldsymbol{R}_{z}(\psi) \\
+
+\boldsymbol{R}_{z,y,x} = 
+\begin{pmatrix}
+1 & 0 & 0\\
+0 & \cos(\phi) & \sin(\phi)\\
+0 & -\sin(\phi) & \cos(\phi)
+\end{pmatrix}
+\begin{pmatrix}
+\cos(\theta) & 0 & -\sin(\theta)\\
+0 & 1 & 0\\
+\sin(\theta) & 0 & \cos(\theta)
+\end{pmatrix}
+\begin{pmatrix}
+\cos(\psi) & \sin(\psi) & 0\\
+-\sin(\psi) & \cos(\psi) & 0\\
+0 & 0 &1
+\end{pmatrix} \\
+
+\boldsymbol{R}_{z,y,x} = 
+\begin{pmatrix}
+\cos(\theta)\cos(\psi) & \cos(\theta)\sin(\psi) & -\sin(\theta)\\
+\sin(\phi)\sin(\theta)\cos(\psi)-\cos(\phi)\sin(\psi) & \sin(\phi)\sin(\theta)\sin(\psi)+\cos(\phi)\cos(\psi) & \sin(\phi)\cos(\theta)\\
+\cos(\phi)\sin(\theta)\cos(\psi)+\sin(\phi)\sin(\psi) & \cos(\phi)\sin(\theta)\sin(\psi)-\sin(\phi)\cos(\psi) & \cos(\phi)\cos(\theta)
+\end{pmatrix}
+++++
+
+Get Tait–Bryan angles from rotation matrix cite:[wiki_euler_angles]:
+[latexmath]
+++++
+\theta = -\arcsin(R_{13}) \\
+\psi = \arctan2(R_{12}/\cos(\theta),R_{11}/\cos(\theta)) \\
+\phi = \arctan2(R_{23}/\cos(\theta),R_{33}/\cos(\theta))
+++++
+
+**Relative orientation**:
+
+Object rotation Matrix: latexmath:[\boldsymbol{R}_{object}^{src}] +
+Host vehicle rotation Matrix: latexmath:[\boldsymbol{R}_{ego}^{src}] +
+Resulting rotation matrix between object and host: latexmath:[\boldsymbol{R}_{object}^{src}(\boldsymbol{R}_{ego}^{src})^{T}] +
+
+To transform from world coordinates into vehicle coordinates and back use the formulas from above with the world coordinates frame latexmath:[w] as source system latexmath:[src] and vehicle coordinates frame latexmath:[v] as target system latexmath:[trg].
+To transform from world coordinates into vehicle coordinates and back use the formulas from above with the vehicle coordinates frame latexmath:[v] as source system latexmath:[src] and sensor coordinates frame latexmath:[s] as target system latexmath:[trg].
+
 
 **Corresponding messages**