From a696c7a25bb0a3b433ae5abc8730ef6152a4fc79 Mon Sep 17 00:00:00 2001 From: "@lukas.elster" Date: Tue, 11 Jan 2022 15:31:34 +0100 Subject: [PATCH 1/2] Added coordinate transformation to documentation Signed-off-by: @lukas.elster --- .../reference_points_coordinate_systems.adoc | 2 +- .../transforming_coordinate_systems.adoc | 70 ++++++++++++++++++- 2 files changed, 70 insertions(+), 2 deletions(-) diff --git a/doc/architecture/reference_points_coordinate_systems.adoc b/doc/architecture/reference_points_coordinate_systems.adoc index 61b75af57..3b7a08153 100644 --- a/doc/architecture/reference_points_coordinate_systems.adoc +++ b/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: diff --git a/doc/usecases/transforming_coordinate_systems.adoc b/doc/usecases/transforming_coordinate_systems.adoc index ab69d80b5..ece7c12e9 100644 --- a/doc/usecases/transforming_coordinate_systems.adoc +++ b/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.[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)[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[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** From 3606291b31caa6b4539c8429be3ed8c50cfcbbfd Mon Sep 17 00:00:00 2001 From: "@lukas.elster" Date: Wed, 19 Jan 2022 08:48:47 +0100 Subject: [PATCH 2/2] Bugfix in citation Signed-off-by: @lukas.elster --- doc/usecases/transforming_coordinate_systems.adoc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/usecases/transforming_coordinate_systems.adoc b/doc/usecases/transforming_coordinate_systems.adoc index ece7c12e9..ccbeb090f 100644 --- a/doc/usecases/transforming_coordinate_systems.adoc +++ b/doc/usecases/transforming_coordinate_systems.adoc @@ -9,7 +9,7 @@ It demonstrates how a global coordinate system, vehicle coordinate system, and s **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.[reuper2020] +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. @@ -29,7 +29,7 @@ Transformation back from target latexmath:[trg] to source latexmath:[src] coordi ++++ -With the rotation matrix (from rotating the coordinate system)[wiki_rotation_matrix]: +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) \\ @@ -59,7 +59,7 @@ With the rotation matrix (from rotating the coordinate system)[wiki_rotation_mat \end{pmatrix} ++++ -Get Tait–Bryan angles from rotation matrix[wiki_euler_angles]: +Get Tait–Bryan angles from rotation matrix cite:[wiki_euler_angles]: [latexmath] ++++ \theta = -\arcsin(R_{13}) \\