diff --git a/README.md b/README.md index 2449530..bb01e0b 100644 --- a/README.md +++ b/README.md @@ -86,6 +86,14 @@ markers are roughly level with the observer: Available as a typed named tuple `Spherical` from `Token.spherical`. +### Orientation + +Rotation of the marker around its centre (relative to the observer) + - `rot_x`: rotation about the Cartesian x-axis, positive angles represent the front going downwards. + - `rot_y`: rotation about the Cartesian y-axis, positive angles represent the front going leftwards. + - `rot_z`: rotation about the Cartesian z-axis, positive angles represent the top going rightwards. + +Available as a typed named tuple `Orientation` from `Token.orientation`. ## Camera support diff --git a/calibrations/tecknet_rotations/-22.5rot_x0rot_y0.6z.png b/calibrations/tecknet_rotations/-22.5rot_x0rot_y0.6z.png new file mode 100644 index 0000000..669eabf Binary files /dev/null and b/calibrations/tecknet_rotations/-22.5rot_x0rot_y0.6z.png differ diff --git a/calibrations/tecknet_rotations/-45rot_x0rot_y1.1z.png b/calibrations/tecknet_rotations/-45rot_x0rot_y1.1z.png new file mode 100644 index 0000000..be898d9 Binary files /dev/null and b/calibrations/tecknet_rotations/-45rot_x0rot_y1.1z.png differ diff --git a/calibrations/tecknet_rotations/-90rot_z.png b/calibrations/tecknet_rotations/-90rot_z.png new file mode 100644 index 0000000..b9679f2 Binary files /dev/null and b/calibrations/tecknet_rotations/-90rot_z.png differ diff --git a/calibrations/tecknet_rotations/0rot_x-22.5rot_y0.6z.png b/calibrations/tecknet_rotations/0rot_x-22.5rot_y0.6z.png new file mode 100644 index 0000000..66ef0be Binary files /dev/null and b/calibrations/tecknet_rotations/0rot_x-22.5rot_y0.6z.png differ diff --git a/calibrations/tecknet_rotations/0rot_x-45rot_y1.1z.png b/calibrations/tecknet_rotations/0rot_x-45rot_y1.1z.png new file mode 100644 index 0000000..1e13476 Binary files /dev/null and b/calibrations/tecknet_rotations/0rot_x-45rot_y1.1z.png differ diff --git a/calibrations/tecknet_rotations/0rot_x0rot_y0.55z.png b/calibrations/tecknet_rotations/0rot_x0rot_y0.55z.png new file mode 100644 index 0000000..30ce1e9 Binary files /dev/null and b/calibrations/tecknet_rotations/0rot_x0rot_y0.55z.png differ diff --git a/calibrations/tecknet_rotations/135rot_z.png b/calibrations/tecknet_rotations/135rot_z.png new file mode 100644 index 0000000..d3ca2cc Binary files /dev/null and b/calibrations/tecknet_rotations/135rot_z.png differ diff --git a/calibrations/tecknet_rotations/180rot_z.png b/calibrations/tecknet_rotations/180rot_z.png new file mode 100644 index 0000000..fe4c052 Binary files /dev/null and b/calibrations/tecknet_rotations/180rot_z.png differ diff --git a/calibrations/tecknet_rotations/45rot_z.png b/calibrations/tecknet_rotations/45rot_z.png new file mode 100644 index 0000000..529a45e Binary files /dev/null and b/calibrations/tecknet_rotations/45rot_z.png differ diff --git a/calibrations/tecknet_rotations/90rot_z.png b/calibrations/tecknet_rotations/90rot_z.png new file mode 100644 index 0000000..5152427 Binary files /dev/null and b/calibrations/tecknet_rotations/90rot_z.png differ diff --git a/sb_vision/__init__.py b/sb_vision/__init__.py index 21b3492..075d596 100644 --- a/sb_vision/__init__.py +++ b/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', diff --git a/sb_vision/coordinates.py b/sb_vision/coordinates.py index 01b9154..7c4b40f 100644 --- a/sb_vision/coordinates.py +++ b/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)]) diff --git a/sb_vision/cv3d.py b/sb_vision/cv3d.py index eb69579..9df0495 100644 --- a/sb_vision/cv3d.py +++ b/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), ) diff --git a/sb_vision/find_3D_coords.py b/sb_vision/find_3D_coords.py index eaa8543..931ebd5 100644 --- a/sb_vision/find_3D_coords.py +++ b/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( diff --git a/sb_vision/tokens.py b/sb_vision/tokens.py index ab93538..d9d6226 100644 --- a/sb_vision/tokens.py +++ b/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) diff --git a/tests/test_orientation.py b/tests/test_orientation.py new file mode 100644 index 0000000..2ec5209 --- /dev/null +++ b/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(-90)), + ), + ( + '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")