Add marker orientations

sb_vision/__init__.py

@@ -7,7 +7,13 @@
 """
 
 from .camera import Camera, FileCamera
-from .coordinates import Cartesian, LegacyPolar, Spherical, cartesian_to_spherical
+from .coordinates import (
+    Cartesian,
+    Orientation,
+    LegacyPolar,
+    Spherical,
+    cartesian_to_spherical,
+)
 from .tokens import Token
 from .vision import Vision
 
@@ -17,6 +23,7 @@
     'FileCamera',
     'Token',
     'Cartesian',
+    'Orientation',
     'LegacyPolar',
     'Spherical',
     'cartesian_to_spherical',

sb_vision/coordinates.py

@@ -36,6 +36,11 @@ def tolist(self):
     ('dist', AnyFloat),
 ))
 
+Orientation = NamedTuple('Orientation', (
+    ('rot_x', AnyFloat),
+    ('rot_y', AnyFloat),
+    ('rot_z', AnyFloat),
+))
 
 PixelCoordinate = NamedTuple('PixelCoordinate', [('x', AnyFloat), ('y', AnyFloat)])
 

sb_vision/cv3d.py

@@ -6,7 +6,7 @@
 
 from sb_vision.native import _cv3d  # type: ignore
 
-from .coordinates import Cartesian, PixelCoordinate
+from .coordinates import Cartesian, Orientation, PixelCoordinate
 
 
 class Cv3dError(RuntimeError):
@@ -39,7 +39,7 @@ def solve_pnp(
     pixel_corners: Sequence[PixelCoordinate],
     camera_matrix: Sequence[Sequence[float]],
     distance_coefficients: Sequence[Sequence[float]],
-) -> Tuple[Cartesian, Tuple[float, float, float]]:
+) -> Tuple[Cartesian, Orientation]:
     """
     Wrapper around OpenCV solvePnP.
 
@@ -64,9 +64,5 @@ def solve_pnp(
 
     return (
         Cartesian(*translation_vector),
-        (
-            float(orientation_vector[0]),
-            float(orientation_vector[1]),
-            float(orientation_vector[2]),
-        ),
+        Orientation(*orientation_vector),
     )

sb_vision/find_3D_coords.py

@@ -13,7 +13,7 @@
 
 import numpy as np
 
-from sb_vision.coordinates import Cartesian, PixelCoordinate
+from sb_vision.coordinates import Cartesian, Orientation, PixelCoordinate
 
 from . import cv3d
 
@@ -151,7 +151,7 @@ def calculate_transforms(
     pixel_corners: List[PixelCoordinate],
     camera_matrix: List[List[float]],
     distance_coefficients: List[List[float]],
-) -> Tuple[Cartesian, Tuple[float, float, float]]:
+) -> Tuple[Cartesian, Orientation]:
     """
     Calculate the position of a marker.
 
@@ -171,10 +171,10 @@ def calculate_transforms(
 
     # create the rectangle representing the marker in 3D
     object_points = [
+        [-width_from_centre, height_from_centre, 0],
         [width_from_centre, height_from_centre, 0],
         [width_from_centre, -height_from_centre, 0],
         [-width_from_centre, -height_from_centre, 0],
-        [-width_from_centre, height_from_centre, 0],
     ]
 
     translation_vector, orientation_vector = cv3d.solve_pnp(

sb_vision/tokens.py

@@ -6,6 +6,7 @@
 
 from .coordinates import (
     Cartesian,
+    Orientation,
     cartesian_to_legacy_polar,
     cartesian_to_spherical,
 )
@@ -79,6 +80,7 @@ def from_apriltag_detection(
 
             instance.update_3D_transforms(
                 translation=translation,
+                orientation=orientation,
             )
         return instance
 
@@ -106,13 +108,16 @@ def update_pixel_coords(
     def update_3D_transforms(
         self,
         *,
-        translation: Cartesian
+        translation: Cartesian,
+        orientation: Orientation
     ):
         """Calculate 3D coordinate information from the given transformations."""
         # Cartesian Co-ordinates in the 3D World, relative to the camera
         # (as opposed to somehow being compass-aligned)
         self.cartesian = translation
 
+        self.orientation = orientation
+
         # Polar co-ordinates in the 3D world, relative to the camera
         self.polar = cartesian_to_legacy_polar(self.cartesian)
         self.legacy_polar = cartesian_to_legacy_polar(self.cartesian)

tests/test_orientation.py

@@ -0,0 +1,97 @@
+"""Tests for marker orientations."""
+
+import math
+from pathlib import Path
+
+import pytest
+from pytest import approx
+
+from sb_vision import FileCamera, Orientation, Vision
+
+ROTATION_TOLERANCE_DEGREES = 6
+
+CALIBRATIONS = Path(__file__).parent.parent / 'calibrations' / 'tecknet_rotations'
+
+TEST_IMAGES = [
+    (
+        '0rot_x-45rot_y1.1z.png',
+        False,
+        Orientation(rot_x=0, rot_y=math.radians(-45), rot_z=0),
+    ),
+    (
+        '-45rot_x0rot_y1.1z.png',
+        False,
+        Orientation(rot_x=math.radians(-45), rot_y=0, rot_z=0),
+    ),
+    (
+        '-22.5rot_x0rot_y0.6z.png',
+        False,
+        Orientation(rot_x=math.radians(-22.5), rot_y=0, rot_z=0),
+    ),
+    (
+        '0rot_x-22.5rot_y0.6z.png',
+        False,
+        Orientation(rot_x=0, rot_y=math.radians(-22.5), rot_z=0),
+    ),
+    (
+        '0rot_x0rot_y0.55z.png',
+        False,
+        Orientation(rot_x=0, rot_y=0, rot_z=0),
+    ),
+    (
+        '-90rot_z.png',
+        False,
+        Orientation(rot_x=0, rot_y=0, rot_z=math.radians(270)),
+    ),
+    (
+        '90rot_z.png',
+        False,
+        Orientation(rot_x=0, rot_y=0, rot_z=math.radians(90)),
+    ),
+    (
+        '180rot_z.png',
+        True,
+        Orientation(rot_x=0, rot_y=0, rot_z=math.radians(180)),
+    ),
+    (
+        '135rot_z.png',
+        False,
+        Orientation(rot_x=0, rot_y=0, rot_z=math.radians(135)),
+    ),
+    (
+        '45rot_z.png',
+        False,
+        Orientation(rot_x=0, rot_y=0, rot_z=math.radians(45)),
+    ),
+]
+
+
+@pytest.mark.parametrize(
+    "photo, allow_wrapping, expected_orientation",
+    TEST_IMAGES,
+)
+def test_image_coordinates(photo, allow_wrapping, expected_orientation):
+    """Make sure that this particular file gives these particular tokens."""
+    camera = FileCamera(CALIBRATIONS / photo, camera_model='C016')
+    vision = Vision(camera)
+    token, = vision.snapshot()
+
+    def approx_ang(expected_degrees):
+        return approx(expected_degrees, abs=ROTATION_TOLERANCE_DEGREES)
+
+    def assert_angle(angle_radians, expected_degrees, message):
+        expected_degrees = approx_ang(math.degrees(expected_degrees))
+        # Check both +0 and +360 so approx can cover the jump between -180 and 180
+        if allow_wrapping:
+            assert \
+                math.degrees(angle_radians) == expected_degrees or \
+                math.degrees(angle_radians) + 360 == expected_degrees, \
+                message
+        else:
+            assert math.degrees(angle_radians) == expected_degrees, message
+
+    rot_x, rot_y, rot_z = token.orientation
+
+    assert_angle(rot_x, expected_orientation.rot_x, "Wrong Orientation rot_x")
+    assert_angle(rot_y, expected_orientation.rot_y, "Wrong Orientation rot_y")
+    assert_angle(rot_z, expected_orientation.rot_z, "Wrong Orientation rot_z")