Make the Spherical coordinates useful

docs/coordinates.rst

@@ -6,21 +6,21 @@ Orientation
 .. autoclass:: zoloto.coords.Orientation
     :members:
 
-Coordinates
------------
-.. autoclass:: zoloto.coords.Coordinates
+PixelCoordinates
+----------------
+.. autoclass:: zoloto.coords.PixelCoordinates
     :members:
     :no-inherited-members:
 
-ThreeDCoordinates
------------------
-.. autoclass:: zoloto.coords.ThreeDCoordinates
+CartesianCoordinates
+--------------------
+.. autoclass:: zoloto.coords.CartesianCoordinates
     :members:
     :no-inherited-members:
 
-Spherical
----------
-.. autoclass:: zoloto.coords.Spherical
+SphericalCoordinates
+--------------------
+.. autoclass:: zoloto.coords.SphericalCoordinates
     :members:
     :no-inherited-members:
 

tests/test_coords.py

@@ -1,11 +1,14 @@
 """Tests for coordinates classes."""
 from __future__ import annotations
 
+import math
+
+import pytest
 from hypothesis import given
 from hypothesis.strategies import floats, tuples
 from pyquaternion import Quaternion
 
-from zoloto.coords import Orientation
+from zoloto.coords import CartesianCoordinates, Orientation, SphericalCoordinates
 
 
 @given(tuples(floats(), floats(), floats()))
@@ -62,3 +65,57 @@ def test_repr(euler_angles: tuple[float, float, float]) -> None:
 
     for name, val in zip(names, ypr):
         assert f"{name}={val}" in repr_str
+
+
+@pytest.mark.parametrize(
+    "cartesian,expected",
+    [
+        pytest.param(
+            CartesianCoordinates(0, 0, 0),
+            SphericalCoordinates(0, 0, 0),
+            id="origin",
+        ),
+        pytest.param(
+            CartesianCoordinates(0, 0, 1),
+            SphericalCoordinates(
+                theta=math.pi / 2,
+                phi=math.pi / 2,
+                distance=1,
+            ),
+            id="in-front-of-you",
+        ),
+        pytest.param(
+            CartesianCoordinates(0, 1, 0),
+            SphericalCoordinates(
+                theta=0,
+                phi=0,
+                distance=1,
+            ),
+            id="above-you",
+        ),
+        pytest.param(
+            CartesianCoordinates(1, 0, 0),
+            SphericalCoordinates(
+                theta=math.pi / 2,
+                phi=0,
+                distance=1,
+            ),
+            id="to-one-side",
+        ),
+        pytest.param(
+            CartesianCoordinates(1000, 1000, 0),
+            SphericalCoordinates(
+                theta=0.7853981633974484,  # math.pi / 4, with floating point error
+                phi=0,
+                distance=1414,
+            ),
+            id="to-one-side-and-up",
+        ),
+    ],
+)
+def test_spherical_from_cartesian(
+    cartesian: CartesianCoordinates,
+    expected: SphericalCoordinates,
+) -> None:
+    spherical = SphericalCoordinates.from_cartesian(cartesian)
+    assert spherical == expected

tests/test_marker.py

@@ -1,6 +1,7 @@
 from __future__ import annotations
 
 import json
+import math
 from typing import Any
 from unittest import TestCase
 from unittest.mock import patch
@@ -71,10 +72,10 @@ def test_cartesian_coordinates(self) -> None:
         self.assertAlmostEqual(int(z), 910, delta=100)  # HACK: Sometimes it changes
 
     def test_spherical_coordinates(self) -> None:
-        rot_x, rot_y, dist = self.marker.spherical
+        dist, rot_x, rot_y = self.marker.spherical
         self.assertEqual(dist, self.marker.distance)
-        self.assertAlmostEqual(rot_x, 0, delta=0.1)
-        self.assertAlmostEqual(rot_y, 0, delta=0.1)
+        self.assertAlmostEqual(rot_x, math.pi / 2, delta=0.1)
+        self.assertAlmostEqual(rot_y, math.pi / 2, delta=0.1)
 
     def test_as_dict(self) -> None:
         marker_dict = self.marker.as_dict()
@@ -119,7 +120,7 @@ def test_marker_types(self) -> None:
 
             self.assertIsType(self.marker.spherical.rot_x, float)
             self.assertIsType(self.marker.spherical.rot_y, float)
-            self.assertIsType(self.marker.spherical.dist, int)
+            self.assertIsType(self.marker.spherical.distance, int)
 
             self.assertIsType(self.marker.cartesian.x, float)
             self.assertIsType(self.marker.cartesian.y, float)

tests/test_module.py

@@ -20,7 +20,7 @@ def test_exposes_marker_type() -> None:
 
 @pytest.mark.parametrize(
     "coordinate_struct",
-    ["Coordinates", "Orientation", "ThreeDCoordinates", "Spherical"],
+    ["PixelCoordinates", "Orientation", "CartesianCoordinates", "SphericalCoordinates"],
 )
 def test_exposes_coordinates(coordinate_struct: str) -> None:
     assert getattr(zoloto, coordinate_struct) == getattr(

zoloto/__init__.py

@@ -1,17 +1,22 @@
 from __future__ import annotations
 
-from zoloto.coords import Coordinates, Orientation, Spherical, ThreeDCoordinates
+from zoloto.coords import (
+    CartesianCoordinates,
+    Orientation,
+    PixelCoordinates,
+    SphericalCoordinates,
+)
 from zoloto.marker import Marker
 from zoloto.marker_type import MarkerType
 
 __version__ = "0.9.0"
 
 
 __all__ = [
-    "Coordinates",
+    "CartesianCoordinates",
     "Orientation",
-    "Spherical",
-    "ThreeDCoordinates",
+    "PixelCoordinates",
+    "SphericalCoordinates",
     "Marker",
     "MarkerType",
 ]

zoloto/coords.py

@@ -1,14 +1,20 @@
 from __future__ import annotations
 
+import math
 from typing import Iterator, NamedTuple, Tuple
 
 from cached_property import cached_property
 from cv2 import Rodrigues
 from pyquaternion import Quaternion
 
 
-class Coordinates(NamedTuple):
+class PixelCoordinates(NamedTuple):
     """
+    Coordinates within an image made up from pixels.
+
+    This type allows float values to account for computed locations which are
+    not limited to exact pixel boundaries.
+
     :param float x: X coordinate
     :param float y: Y coordinate
     """
@@ -17,8 +23,24 @@ class Coordinates(NamedTuple):
     y: float
 
 
-class ThreeDCoordinates(NamedTuple):
+class CartesianCoordinates(NamedTuple):
     """
+    Cartesian coordinates, rotated on their side.
+
+    The X axis is horizontal relative to the camera's perspective, i.e: left &
+    right within the frame of the image. Zero is at the centre of the image.
+    Increasing values indicate greater distance to the right.
+
+    The Y axis is vertical relative to the camera's perspective, i.e: up & down
+    within the frame of the image. Zero is at the centre of the image.
+    Increasing values indicate greater distance below the centre of the image.
+
+    The Z axis extends directly away from the camera. Zero is at the camera.
+    Increasing values indicate greater distance from the camera.
+
+    These match traditional cartesian coordinates when the camera is facing
+    upwards.
+
     :param float x: X coordinate
     :param float y: Y coordinate
     :param float z: Z coordinate
@@ -29,16 +51,54 @@ class ThreeDCoordinates(NamedTuple):
     z: float
 
 
-class Spherical(NamedTuple):
+class SphericalCoordinates(NamedTuple):
     """
-    :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
+    SphericalCoordinates coordinates, rotated onto their side.
+
+    This is comparable to the ISO convention for spherical coordinates, applied
+    to our rotated axes. Here θ is measured down from the y-axis (rather than
+    the usual z-axis) while φ is measured around the y-axis.
+
+    See https://en.wikipedia.org/wiki/Spherical_coordinate_system and
+    https://studentrobotics.org/docs/programming/sr/vision/#SphericalCoordinates.
+
+    :param float distance: Radial distance from the origin.
+    :param float theta: Polar angle, θ, in radians. This is the angle "down"
+        from the y-axis to the vector which points to the location. For points
+        with zero cartesian x-coordinate value, this can be viewed as the
+        rotation about the x-axis. Zero is on the positive y-axis.
+    :param float phi: Azimuth angle, φ, in radians. This is the angle from the
+        x-axis around the polar (y-axis) to the projection of the point on the
+        x-z plane. This can be viewed as rotation about the y-axis. Zero is at
+        the centre of the image.
     """
 
-    rot_x: float
-    rot_y: float
-    dist: int
+    distance: int
+    theta: float
+    phi: float
+
+    @property
+    def rot_x(self) -> float:
+        """Approximate rotation around the x-axis, an alias for ``self.theta``."""
+        return self.theta
+
+    @property
+    def rot_y(self) -> float:
+        """Rotation around the y-axis, an alias for ``self.phi``."""
+        return self.phi
+
+    @classmethod
+    def from_cartesian(cls, cartesian: CartesianCoordinates) -> SphericalCoordinates:
+        if not any(cartesian):
+            return SphericalCoordinates(0, 0, 0)
+
+        distance = math.sqrt(sum(x**2 for x in cartesian))
+        x, y, z = cartesian
+        return SphericalCoordinates(
+            distance=int(distance),
+            theta=math.acos(y / distance),
+            phi=math.atan2(z, x),
+        )
 
 
 ThreeTuple = Tuple[float, float, float]
@@ -59,17 +119,58 @@ def __init__(self, e_x: float, e_y: float, e_z: float):
 
     @property
     def rot_x(self) -> float:
-        """Get rotation angle around x axis in radians."""
+        """
+        Get rotation angle around X axis in radians.
+
+        The X axis is horizontal relative to the camera's perspective, i.e: left
+        & right within the frame of the image.
+
+        Increasing values represent an increasing clockwise rotation of the
+        marker as seen from the camera's left.
+
+        Zero values for April Tags markers have the marker facing away from the
+        camera. The practical effect of this is that an April Tags marker facing
+        the camera square-on will have a value of ``pi`` (or equivalently
+        ``-pi``) and the value will decrease as the marker diverges from
+        square-on.
+
+        For observed markers positive values therefore indicate a rotation of
+        the top of the marker away from the camera, such that marker could be
+        said to be leaning backwards, with the value decreasing as the marker
+        leans back further.
+        """
         return self.roll
 
     @property
     def rot_y(self) -> float:
-        """Get rotation angle around y axis in radians."""
+        """
+        Get rotation angle around Y axis in radians.
+
+        The Y axis is vertical relative to the camera's perspective, i.e: up &
+        down within the frame of the image.
+
+        Positive values indicate a rotation of an observed marker towards the
+        camera's right. This is a rotation of the marker counter-clockwise about
+        the Y axis as seen from above the marker.
+
+        Zero values for April Tags markers have the marker facing the camera
+        square-on.
+        """
         return self.pitch
 
     @property
     def rot_z(self) -> float:
-        """Get rotation angle around z axis in radians."""
+        """
+        Get rotation angle around Z axis in radians.
+
+        The Z axis extends directly away from the camera.
+
+        Positive values indicate a rotation counter-clockwise from the
+        perspective of the camera.
+
+        Zero values for April Tags markers have the marker reference point at
+        the top left.
+        """
         return self.yaw
 
     @property