Skip to content
This repository was archived by the owner on Jul 26, 2020. It is now read-only.
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 13 additions & 1 deletion sb_vision/coordinates.py
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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),
Expand Down
19 changes: 12 additions & 7 deletions sb_vision/tokens.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

from .coordinates import (
Cartesian,
HomographyMatrix,
cartesian_to_legacy_polar,
cartesian_to_spherical,
)
Expand Down Expand Up @@ -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

# *************************************************************************
Expand All @@ -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.
Expand All @@ -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."""

Expand Down