Source code for zoloto.coords

from typing import Iterator, NamedTuple, Tuple

from cached_property import cached_property
from cv2 import Rodrigues
from pyquaternion import Quaternion

Coordinates = NamedTuple("Coordinates", [("x", float), ("y", float)])
Coordinates.__doc__ = """
:param float x: X coordinate
:param float y: Y coordinate
"""

ThreeDCoordinates = NamedTuple(
    "ThreeDCoordinates", [("x", float), ("y", float), ("z", float)]
)
ThreeDCoordinates.__doc__ = """
:param float x: X coordinate
:param float y: Y coordinate
:param float z: Z coordinate
"""

Spherical = NamedTuple("Spherical", [("rot_x", float), ("rot_y", float), ("dist", int)])
Spherical.__doc__ = """
:param float rot_x: Rotation around the X-axis, in radians
:param float rot_y: Rotation around the Y-axis, in radians
:param float dist: Distance
"""

ThreeTuple = Tuple[float, float, float]
RotationMatrix = Tuple[ThreeTuple, ThreeTuple, ThreeTuple]


[docs]class Orientation: """The orientation of an object in 3-D space."""
[docs] def __init__(self, e_x: float, e_y: float, e_z: float): """ Construct a quaternion given the components of a rotation vector. More information: https://w.wiki/Fci """ rotation_matrix, _ = Rodrigues((e_x, e_y, e_z)) self._quaternion = Quaternion(matrix=rotation_matrix)
@property def rot_x(self) -> float: """Get rotation angle around x axis in radians.""" return self.roll @property def rot_y(self) -> float: """Get rotation angle around y axis in radians.""" return self.pitch @property def rot_z(self) -> float: """Get rotation angle around z axis in radians.""" return self.yaw @property def yaw(self) -> float: """Get rotation angle around z axis in radians.""" return self.yaw_pitch_roll[0] @property def pitch(self) -> float: """Get rotation angle around y axis in radians.""" return self.yaw_pitch_roll[1] @property def roll(self) -> float: """Get rotation angle around x axis in radians.""" return self.yaw_pitch_roll[2] @cached_property def yaw_pitch_roll(self) -> ThreeTuple: """ Get the equivalent yaw-pitch-roll angles. Specifically intrinsic Tait-Bryan angles following the z-y'-x'' convention. Returns: A three-tuple of floating point angles: yaw: rotation angle around the z-axis in radians, in the range `[-pi, pi]` pitch: rotation angle around the y'-axis in radians, in the range `[-pi/2, -pi/2]` roll: rotation angle around the x''-axis in radians, in the range `[-pi, pi]` """ return self._quaternion.yaw_pitch_roll
[docs] def __iter__(self) -> Iterator[float]: """ Get an iterator over the rotation angles. Returns: An iterator of floating point angles in order x, y, z. """ return iter([self.rot_x, self.rot_y, self.rot_z])
@cached_property def rotation_matrix(self) -> RotationMatrix: """ Get the rotation matrix represented by this orientation. Returns: A 3x3 rotation matrix as a tuple of tuples. """ r_m = self._quaternion.rotation_matrix return ( (r_m[0][0], r_m[0][1], r_m[0][2]), (r_m[1][0], r_m[1][1], r_m[1][2]), (r_m[2][0], r_m[2][1], r_m[2][2]), ) @property def quaternion(self) -> Quaternion: """Get the quaternion represented by this orientation.""" return self._quaternion def __repr__(self) -> str: return "Orientation(rot_x={},rot_y={},rot_z={})".format( self.rot_x, self.rot_y, self.rot_z )