sb-vision/tests/test_orientation.py at 361ce0935cc83e803f00956705613c8b85a0a387 · sourcebots/sb-vision · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
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")