From a83fc97a48d39db389d556bf2d68723d38585a87 Mon Sep 17 00:00:00 2001
From: Richard Gerum <14153051+rgerum@users.noreply.github.com>
Date: Tue, 26 Sep 2023 12:13:23 -0400
Subject: [PATCH] added SpatialOrientationYawPitchRoll (#28)

---
 cameratransform/spatial.py | 65 ++++++++++++++++++++++++++++++++++++++
 docs/source/spatial.rst    |  2 ++
 2 files changed, 67 insertions(+)

diff --git a/cameratransform/spatial.py b/cameratransform/spatial.py
index 879ab5f..51227ba 100644
--- a/cameratransform/spatial.py
+++ b/cameratransform/spatial.py
@@ -213,3 +213,68 @@ def load(self, filename):
             variables = json.loads(fp.read())
         for key in variables:
             setattr(self.parameters, key, variables[key])
+
+
+class SpatialOrientationYawPitchRoll(SpatialOrientation):
+    r"""
+    The orientation in the yaw-pitch-roll system. The definition is based on
+    `this <https://en.wikipedia.org//wiki/Aircraft_principal_axes>`_. It relates to the tilt-heading-roll system used
+    by SpatialOrientation like this:
+
+    .. math::
+        R_{\mathrm{pitch}} &= 90+R_{\mathrm{tilt}} \\
+        R_{\mathrm{yaw}} &= -R_{\mathrm{heading}} \\
+        R_{\mathrm{roll}} &= -R_{\mathrm{roll}}
+
+    """
+
+    def __init__(self, elevation_m=None, yaw_deg=None, pitch_deg=None, roll_deg=None, pos_x_m=None, pos_y_m=None):
+        self.parameters = ParameterSet(
+            # the extrinsic parameters if the camera will not be compared to other cameras or maps
+            elevation_m=Parameter(elevation_m, default=30, range=(0, None), type=TYPE_EXTRINSIC1),
+            # the elevation of the camera above sea level in m
+            yaw_deg=Parameter(yaw_deg, default=85, range=(-90, 90), type=TYPE_EXTRINSIC1),  # the tilt angle of the camera in degrees
+            roll_deg=Parameter(roll_deg, default=0, range=(-180, 180), type=TYPE_EXTRINSIC1),  # the roll angle of the camera in degrees
+
+            # the extrinsic parameters if the camera will be compared to other cameras or maps
+            pitch_deg=Parameter(pitch_deg, default=0, type=TYPE_EXTRINSIC2),  # the heading angle of the camera in degrees
+            pos_x_m=Parameter(pos_x_m, default=0, type=TYPE_EXTRINSIC2),  # the x position of the camera in m
+            pos_y_m=Parameter(pos_y_m, default=0, type=TYPE_EXTRINSIC2),  # the y position of the camera in m
+        )
+        for name in self.parameters.parameters:
+            self.parameters.parameters[name].callback = self._initCameraMatrix
+        self._initCameraMatrix()
+
+    def __str__(self):
+        string = ""
+        string += "  position:\n"
+        string += "    x:\t%f m\n    y:\t%f m\n    h:\t%f m\n" % (self.parameters.pos_x_m, self.parameters.pos_y_m, self.parameters.elevation_m)
+        string += "  orientation:\n"
+        string += "    yaw:\t\t%f°\n    pitch:\t\t%f°\n    roll:\t%f°\n" % (self.parameters.yaw_deg, self.parameters.pitch_deg, self.parameters.roll_deg)
+        return string
+
+    def _initCameraMatrix(self):
+        if self.pitch_deg < -360 or self.pitch_deg > 360:  # pragma: no cover
+            self.pitch_deg = self.pitch_deg % 360
+        # convert the angle to radians
+        tilt = np.pi/2+np.deg2rad(self.parameters.pitch_deg)
+        roll = -np.deg2rad(self.parameters.roll_deg)
+        heading = np.deg2rad(self.parameters.yaw_deg)
+
+        # get the translation matrix and rotate it
+        self.t = np.array([self.parameters.pos_x_m, self.parameters.pos_y_m, self.parameters.elevation_m])
+
+        # construct the rotation matrices for tilt, roll and heading
+        self.R_roll = np.array([[+np.cos(roll), np.sin(roll), 0],
+                                [-np.sin(roll), np.cos(roll), 0],
+                                [0, 0, 1]])
+        self.R_tilt = np.array([[1, 0, 0],
+                                [0, np.cos(tilt), np.sin(tilt)],
+                                [0, -np.sin(tilt), np.cos(tilt)]])
+        self.R_head = np.array([[np.cos(heading), -np.sin(heading), 0],
+                                [np.sin(heading), np.cos(heading), 0],
+                                [0, 0, 1]])
+
+        self.R = np.dot(np.dot(self.R_roll, self.R_tilt), self.R_head)
+        self.R_inv = np.linalg.inv(self.R)
+
diff --git a/docs/source/spatial.rst b/docs/source/spatial.rst
index 453c099..60d607b 100644
--- a/docs/source/spatial.rst
+++ b/docs/source/spatial.rst
@@ -34,3 +34,5 @@ Transformation
 
 .. automethod:: SpatialOrientation.cameraFromSpace
 .. automethod:: SpatialOrientation.spaceFromCamera
+
+.. autoclass:: SpatialOrientationYawPitchRoll