aboutsummaryrefslogtreecommitdiff
path: root/src/argaze.test/ArUcoMarkers/ArUcoDetector.py
blob: 5a45fadd073c8a4ca7d217b769b1bc4fafc5301a (plain)
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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#!/usr/bin/env python

import unittest
import os
import math

from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoCamera, ArUcoDetector, ArUcoBoard

import cv2 as cv
import numpy

class TestDetectorParametersClass(unittest.TestCase):
    """Test DetectorParameters class."""

    def test_from_json(self):
        """Test DetectorParameters creation from json file."""
        
        # Edit file path
        current_directory = os.path.dirname(os.path.abspath(__file__))
        json_filepath = os.path.join(current_directory, 'utils/detector_parameters.json')

        # Load file
        detector_parameters = ArUcoDetector.DetectorParameters.from_json(json_filepath)

        # Check data
        self.assertEqual(detector_parameters.cornerRefinementMethod, 3)
        self.assertEqual(detector_parameters.aprilTagQuadSigma, 2)
        self.assertEqual(detector_parameters.aprilTagDeglitch, 1)

        # Check bad data access fails
        with self.assertRaises(AttributeError):

            detector_parameters.unknown_data = 1

class TestArUcoDetectorClass(unittest.TestCase):
    """Test ArUcoDetector class."""

    def test_new(self):
        """Test ArUcoDetector creation."""

        aruco_detector = ArUcoDetector.ArUcoDetector(marker_size=3)

         # Check ArUcoDetector creation
        self.assertEqual(aruco_detector.dictionary.name, 'DICT_ARUCO_ORIGINAL')
        self.assertEqual(aruco_detector.marker_size, 3)
        self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [0, 0]))
        self.assertEqual(aruco_detector.detected_markers_number, 0)
        self.assertEqual(aruco_detector.detected_markers, {})

        aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary('DICT_APRILTAG_16h5')
        aruco_detector = ArUcoDetector.ArUcoDetector(aruco_dictionary, 5.2)

         # Check ArUcoDetector creation
        self.assertEqual(aruco_detector.dictionary.name, 'DICT_APRILTAG_16h5')
        self.assertEqual(aruco_detector.marker_size, 5.2)
        self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [0, 0]))
        self.assertEqual(aruco_detector.detected_markers_number, 0)
        self.assertEqual(aruco_detector.detected_markers, {})

    def test_from_json(self):
        """Test ArUcoDetector creation."""

        # Edit file path
        current_directory = os.path.dirname(os.path.abspath(__file__))
        json_filepath = os.path.join(current_directory, 'utils/detector.json')

        # Load file
        aruco_detector = ArUcoDetector.ArUcoDetector.from_json(json_filepath)

         # Check ArUcoDetector creation
        self.assertEqual(aruco_detector.dictionary.name, 'DICT_ARUCO_ORIGINAL')
        self.assertEqual(aruco_detector.marker_size, 3)
        self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [1920, 1080]))
        self.assertEqual(aruco_detector.parameters.cornerRefinementMethod, 3)
        self.assertEqual(aruco_detector.parameters.aprilTagQuadSigma, 2)
        self.assertEqual(aruco_detector.parameters.aprilTagDeglitch, 1)

    def test_detect(self):
        """Test detect method."""

        aruco_detector = ArUcoDetector.ArUcoDetector(marker_size=3)

        # Load picture Full HD to test ArUcoMarker detection
        current_directory = os.path.dirname(os.path.abspath(__file__))
        frame = cv.imread(os.path.join(current_directory, 'utils/full_hd_marker.png'))

        # Check ArUcoMarker detection
        aruco_detector.detect_markers(frame)

        self.assertEqual(aruco_detector.detected_markers_number, 1)

        self.assertEqual(aruco_detector.detected_markers[0].dictionary, aruco_detector.dictionary)
        self.assertEqual(aruco_detector.detected_markers[0].identifier, 0)
        self.assertEqual(aruco_detector.detected_markers[0].size, 3)

        # Check corner positions with -/+ 10 pixels precision
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][0].astype(int), numpy.array([3823, 2073]), decimal=-1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][1].astype(int), numpy.array([4177, 2073]), decimal=-1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][2].astype(int), numpy.array([4177, 2427]), decimal=-1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][3].astype(int), numpy.array([3823, 2427]), decimal=-1))

        # Check marker pose estimation
        aruco_detector.estimate_markers_pose([0])

        # Check marker translation with -/+ 0.1 cm precision and rotation with -/+ 0.001 radian precision
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].translation, numpy.array([33.87, 19.05, 0.]), decimal=1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].rotation, numpy.array([[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]]), decimal=3))

        # Check detect metrics
        detect_count, markers_count = aruco_detector.detection_metrics
        self.assertEqual(detect_count, 1)
        self.assertEqual(markers_count[0], 1)

    def test_detect_board(self):
        """Test detect board method."""

        aruco_board = ArUcoBoard.ArUcoBoard(7, 5, 5, 3)
        aruco_detector = ArUcoDetector.ArUcoDetector(marker_size=3)

        # Load picture Full HD to test ArUcoMarker board detection
        current_directory = os.path.dirname(os.path.abspath(__file__))
        frame = cv.imread(os.path.join(current_directory, 'utils/full_hd_board.png'))

        # Check ArUcoMarker board detection
        aruco_detector.detect_board(frame, aruco_board, aruco_board.markers_number)

        self.assertEqual(aruco_detector.board_corners_number, aruco_board.corners_number)
        self.assertEqual(len(aruco_detector.board_corners), 24)
        self.assertEqual(len(aruco_detector.board_corners_identifier), 24)

if __name__ == '__main__':

    unittest.main()