diff --git a/sb_vision/coordinates.py b/sb_vision/coordinates.py index 01b9154..469f06c 100644 --- a/sb_vision/coordinates.py +++ b/sb_vision/coordinates.py @@ -1,6 +1,6 @@ """Types and helpers for manipulating coordinates.""" -from typing import NamedTuple, Union +from typing import List, NamedTuple, Union import numpy as np from numpy import arctan2, float64, linalg @@ -24,6 +24,18 @@ def tolist(self): return list(self) +class HomographyMatrix(List[List[AnyFloat]]): + """ + 2D (3x3) Transformation matrix that maps the points of one image to that of another. + """ + + __slots__ = () + + def tolist(self): + """Placeholder helper to ease migration within robotd.""" + return list(self) + + Spherical = NamedTuple('Spherical', ( ('rot_x', AnyFloat), ('rot_y', AnyFloat), diff --git a/sb_vision/tokens.py b/sb_vision/tokens.py index 06ccba4..150f280 100644 --- a/sb_vision/tokens.py +++ b/sb_vision/tokens.py @@ -6,6 +6,7 @@ from .coordinates import ( Cartesian, + HomographyMatrix, cartesian_to_legacy_polar, cartesian_to_spherical, ) @@ -44,6 +45,13 @@ def from_apriltag_detection( pixel_corners = [PixelCoordinate(*l) for l in apriltag_detection.p] + homography_matrix = HomographyMatrix(np.reshape( + [apriltag_detection.H.data[i] for i in range(9)], + (3, 3), + ).tolist()) + # The homography matrix is a 2D transformation of a unit square to the + # co-ordinates of the marker in the image + marker_id = apriltag_detection.id # ************************************************************************* @@ -54,12 +62,9 @@ def from_apriltag_detection( id=marker_id, certainty=apriltag_detection.goodness, ) - arr = [apriltag_detection.H.data[x] for x in range(9)] - homography = np.reshape(arr, (3, 3)) - - instance.update_pixel_coords( + instance.update_2D_transforms( pixel_corners=pixel_corners, - homography_matrix=homography, + homography_matrix=homography_matrix, ) # We don't set coordinates in the absence of a camera model. @@ -81,11 +86,11 @@ def from_apriltag_detection( return instance # noinspection PyAttributeOutsideInit - def update_pixel_coords( + def update_2D_transforms( self, *, pixel_corners: List[PixelCoordinate], - homography_matrix + homography_matrix: HomographyMatrix ): """Set the pixel coordinate information of the Token."""