Add Homography Matrices as a type

Author: Adimote

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),

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."""