diff options
Diffstat (limited to 'src/argaze.test/ArUcoMarker')
17 files changed, 882 insertions, 0 deletions
diff --git a/src/argaze.test/ArUcoMarker/ArUcoBoard.py b/src/argaze.test/ArUcoMarker/ArUcoBoard.py new file mode 100644 index 0000000..b20be13 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/ArUcoBoard.py @@ -0,0 +1,50 @@ +""" + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see <https://www.gnu.org/licenses/>. +""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "GPLv3" + +import unittest +import os + +from argaze.ArUcoMarker import ArUcoBoard, ArUcoMarkerDictionary + +import numpy + +class TestArUcoBoardClass(unittest.TestCase): + """Test ArUcoBoard class.""" + + def test_new(self): + """Test ArUcoBoard creation using a dictionary instance.""" + + columns = 4 + rows = 3 + square_size = 2 + marker_size = 1 + + # Check ArUco board creation + aruco_dictionary = ArUcoMarkerDictionary.ArUcoMarkerDictionary('DICT_APRILTAG_16h5') + aruco_board = ArUcoBoard.ArUcoBoard(columns, rows, square_size, marker_size, aruco_dictionary) + + # Check ArUco board dictionary name + self.assertEqual(aruco_board.dictionary.name, 'DICT_APRILTAG_16h5') + self.assertIsNone(numpy.testing.assert_array_equal(aruco_board.identifiers, [i for i in range(int((columns*rows)/2))])) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_board.size, [columns, rows])) + self.assertEqual(aruco_board.markers_number, int((columns*rows)/2)) + self.assertEqual(aruco_board.corners_number, (columns-1)*(rows-1)) + +if __name__ == '__main__': + + unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/ArUcoCamera.py b/src/argaze.test/ArUcoMarker/ArUcoCamera.py new file mode 100644 index 0000000..eb930ab --- /dev/null +++ b/src/argaze.test/ArUcoMarker/ArUcoCamera.py @@ -0,0 +1,80 @@ +""" + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see <https://www.gnu.org/licenses/>. +""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "GPLv3" + +import unittest +import os + +import argaze + +import numpy + +class TestArUcoCameraClass(unittest.TestCase): + """Test ArUcoCamera class.""" + + def test_from_json(self): + """Test ArUcoCamera creation from json file.""" + + # Edit test aruco camera file path + current_directory = os.path.dirname(os.path.abspath(__file__)) + json_filepath = os.path.join(current_directory, 'utils/aruco_camera.json') + + # Load test aruco camera + with argaze.load(json_filepath) as aruco_camera: + + # Check aruco camera meta data + self.assertEqual(aruco_camera.name, "TestArUcoCamera") + + # Check ArUco detector + self.assertEqual(aruco_camera.aruco_detector.dictionary.name, "DICT_ARUCO_ORIGINAL") + self.assertEqual(aruco_camera.aruco_detector.parameters.cornerRefinementMethod, 3) + self.assertEqual(aruco_camera.aruco_detector.parameters.aprilTagQuadSigma, 2) + self.assertEqual(aruco_camera.aruco_detector.parameters.aprilTagDeglitch, 1) + + # Check ArUco detector optic parameters + self.assertEqual(aruco_camera.aruco_detector.optic_parameters.rms, 1.0) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.aruco_detector.optic_parameters.dimensions, [1920, 1080])) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.aruco_detector.optic_parameters.K, [[1.0, 0.0, 1.0], [0.0, 1.0, 1.0], [0.0, 0.0, 1.0]])) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.aruco_detector.optic_parameters.D, [-1.0, -0.5, 0.0, 0.5, 1.0])) + + # Check camera scenes + self.assertEqual(len(aruco_camera.scenes), 2) + self.assertIsNone(numpy.testing.assert_array_equal(list(aruco_camera.scenes.keys()), ["TestSceneA", "TestSceneB"])) + + # Load test scene + ar_scene = aruco_camera.scenes["TestSceneA"] + + # Check Aruco scene + self.assertEqual(len(ar_scene.aruco_markers_group.places), 2) + self.assertIsNone(numpy.testing.assert_allclose(ar_scene.aruco_markers_group.places[0].corners[0], [-0.5, 1.5, 0.], rtol=0, atol=1e-3)) + self.assertEqual(ar_scene.aruco_markers_group.places[0].marker.identifier, 0) + + self.assertIsNone(numpy.testing.assert_allclose(ar_scene.aruco_markers_group.places[1].corners[0], [0., 2.5, -1.5], rtol=0, atol=1e-3)) + self.assertEqual(ar_scene.aruco_markers_group.places[1].marker.identifier, 1) + + # Check layers and AOI scene + self.assertEqual(len(ar_scene.layers.items()), 1) + self.assertEqual(len(ar_scene.layers["Main"].aoi_scene), 1) + self.assertEqual(ar_scene.layers["Main"].aoi_scene['Test'].points_number, 4) + + # Check ArScene + self.assertEqual(ar_scene.angle_tolerance, 1.0) + self.assertEqual(ar_scene.distance_tolerance, 2.0) + +if __name__ == '__main__': + + unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/ArUcoDetector.py b/src/argaze.test/ArUcoMarker/ArUcoDetector.py new file mode 100644 index 0000000..40b7d00 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/ArUcoDetector.py @@ -0,0 +1,149 @@ +""" + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see <https://www.gnu.org/licenses/>. +""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "GPLv3" + +import unittest +import os +import math + +from argaze.ArUcoMarker import ArUcoMarkerDictionary, ArUcoOpticCalibrator, 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.optic_parameters.dimensions, [0, 0])) + self.assertEqual(aruco_detector.detected_markers_number(), 0) + self.assertEqual(aruco_detector.detected_markers(), {}) + + aruco_dictionary = ArUcoMarkerDictionary.ArUcoMarkerDictionary('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.optic_parameters.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.optic_parameters.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__)) + image = cv.imread(os.path.join(current_directory, 'utils/full_hd_marker.png')) + + # Check ArUcoMarker detection + aruco_detector.detect_markers(image) + + 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__)) + image = cv.imread(os.path.join(current_directory, 'utils/full_hd_board.png')) + + # Check ArUcoMarker board detection + aruco_detector.detect_board(image, 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()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/ArUcoMarker.py b/src/argaze.test/ArUcoMarker/ArUcoMarker.py new file mode 100644 index 0000000..6518c8c --- /dev/null +++ b/src/argaze.test/ArUcoMarker/ArUcoMarker.py @@ -0,0 +1,40 @@ +""" + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see <https://www.gnu.org/licenses/>. +""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "GPLv3" + +import unittest + +from argaze.ArUcoMarker import ArUcoMarkerDictionary, ArUcoMarker + +class TestArUcoMarkerClass(unittest.TestCase): + """Test ArUcoMarker class.""" + + def test_new(self): + """Test ArUcoMarker creation.""" + + # Check DICT_ARUCO_ORIGINAL ArUcoMarker creation + aruco_dictionary = ArUcoMarkerDictionary.ArUcoMarkerDictionary('DICT_ARUCO_ORIGINAL') + + aruco_marker = aruco_dictionary.create_marker(0, 3) + + self.assertEqual(aruco_marker.dictionary, aruco_dictionary) + self.assertEqual(aruco_marker.identifier, 0) + self.assertEqual(aruco_marker.size, 3) + +if __name__ == '__main__': + + unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/ArUcoMarkerDictionary.py b/src/argaze.test/ArUcoMarker/ArUcoMarkerDictionary.py new file mode 100644 index 0000000..2f80a67 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/ArUcoMarkerDictionary.py @@ -0,0 +1,50 @@ +""" + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see <https://www.gnu.org/licenses/>. +""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "GPLv3" + +import unittest + +from argaze.ArUcoMarker import ArUcoMarkerDictionary + +class TestArUcoMarkerDictionaryClass(unittest.TestCase): + """Test ArUcoMarkerDictionary class.""" + + def test_new(self): + """Test ArUcoMarkerDictionary creation.""" + + # Check that ArUcoMarkerDictionary creation fails with bad name + with self.assertRaises(NameError): + + aruco_dictionary = ArUcoMarkerDictionary.ArUcoMarkerDictionary('BAD_DICT_NAME') + + # Check default ArUcoMarkerDictionary creation + aruco_dictionary = ArUcoMarkerDictionary.ArUcoMarkerDictionary() + + self.assertEqual(aruco_dictionary.name, 'DICT_ARUCO_ORIGINAL') + self.assertEqual(aruco_dictionary.format, '5X5') + self.assertEqual(aruco_dictionary.number, 1024) + + # Check DICT_APRILTAG_16h5 ArUcoMarkerDictionary creation + aruco_dictionary = ArUcoMarkerDictionary.ArUcoMarkerDictionary('DICT_APRILTAG_16h5') + + self.assertEqual(aruco_dictionary.name, 'DICT_APRILTAG_16h5') + self.assertEqual(aruco_dictionary.format, '4X4') + self.assertEqual(aruco_dictionary.number, 30) + +if __name__ == '__main__': + + unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/ArUcoOpticCalibrator.py b/src/argaze.test/ArUcoMarker/ArUcoOpticCalibrator.py new file mode 100644 index 0000000..d019a5d --- /dev/null +++ b/src/argaze.test/ArUcoMarker/ArUcoOpticCalibrator.py @@ -0,0 +1,61 @@ +""" + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see <https://www.gnu.org/licenses/>. +""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "GPLv3" + +import unittest +import os + +from argaze.ArUcoMarker import ArUcoOpticCalibrator + +import numpy + +class TestOpticParametersClass(unittest.TestCase): + """Test OpticParameters class.""" + + def test_new(self): + """Test OpticParameters creation.""" + + # Check defaut optic parameters creation + optic_parameters = ArUcoOpticCalibrator.OpticParameters() + + # Check ArUco optic parameters + self.assertEqual(optic_parameters.rms, 0.0) + + #self.assertEqual(type(optic_parameters.K), numpy.array) + + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.dimensions, [0, 0])) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.K, ArUcoOpticCalibrator.K0)) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.D, ArUcoOpticCalibrator.D0)) + + def test_from_json(self): + + # Edit optic parameters file path + current_directory = os.path.dirname(os.path.abspath(__file__)) + json_filepath = os.path.join(current_directory, 'utils/optic_parameters.json') + + # Load optic parameters + optic_parameters = ArUcoOpticCalibrator.OpticParameters.from_json(json_filepath) + + # Check ArUco camera + self.assertEqual(optic_parameters.rms, 1.0) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.dimensions, [1920, 1080])) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.K, [[1.0, 0.0, 1.0], [0.0, 1.0, 1.0], [0.0, 0.0, 1.0]])) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.D, [-1.0, -0.5, 0.0, 0.5, 1.0])) + +if __name__ == '__main__': + + unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/ArUcoScene.py b/src/argaze.test/ArUcoMarker/ArUcoScene.py new file mode 100644 index 0000000..67cb668 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/ArUcoScene.py @@ -0,0 +1,227 @@ +""" + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see <https://www.gnu.org/licenses/>. +""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "GPLv3" + +import unittest +import os +import math + +from argaze.ArUcoMarker import ArUcoMarkerGroup, ArUcoMarker + +import cv2 as cv +import numpy + +class TestArUcoMarkerGroupClass(unittest.TestCase): + + def new_from_obj(self): + + # Edit file path + current_directory = os.path.dirname(os.path.abspath(__file__)) + obj_filepath = os.path.join(current_directory, 'utils/scene.obj') + + # Load file + self.aruco_markers_group = ArUcoMarkerGroup.ArUcoMarkerGroup.from_obj(obj_filepath) + + def new_from_json(self): + + # Edit file path + current_directory = os.path.dirname(os.path.abspath(__file__)) + json_filepath = os.path.join(current_directory, 'utils/scene.json') + + # Load file + self.aruco_markers_group = ArUcoMarkerGroup.ArUcoMarkerGroup.from_json(json_filepath) + + def setup_markers(self): + + # Prepare detected markers + self.detected_markers = { + 0: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 0, 1.), + 1: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 1, 1.), + 2: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 2, 1.), + 3: ArUcoMarker.ArUcoMarker('DICT_ARUCO_ORIGINAL', 3, 1.) + } + + # Prepare scene markers and remaining markers + self.scene_markers, self.remaining_markers = self.aruco_markers_group.filter_markers(self.detected_markers()) + + def test_new_from_obj(self): + """Test ArUcoMarkerGroup creation.""" + + self.new_from_obj() + self.setup_markers() + + # Check ArUcoMarkerGroup creation + self.assertEqual(len(self.aruco_markers_group.places), 3) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.identifiers, [0, 1, 2])) + self.assertEqual(self.aruco_markers_group.marker_size, 1.) + + self.assertEqual(self.aruco_markers_group.places[0].marker.identifier, 0) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].translation, [0., 0., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_markers_group.places[1].marker.identifier, 1) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].translation, [10., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_markers_group.places[2].marker.identifier, 2) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].translation, [0., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + def test_new_from_json(self): + """Test ArUcoMarkerGroup creation.""" + + self.new_from_json() + self.setup_markers() + + # Check ArUcoMarkerGroup creation + self.assertEqual(len(self.aruco_markers_group.places), 3) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.identifiers, [0, 1, 2])) + self.assertEqual(self.aruco_markers_group.marker_size, 1.) + + self.assertEqual(self.aruco_markers_group.places[0].marker.identifier, 0) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].translation, [0., 0., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_markers_group.places[1].marker.identifier, 1) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].translation, [10., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_markers_group.places[2].marker.identifier, 2) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].translation, [0., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + def test_filter_markers(self): + """Test ArUcoMarkerGroup markers filtering.""" + + self.new_from_obj() + self.setup_markers() + + # Check scene markers and remaining markers + self.assertEqual(len(self.scene_markers), 3) + self.assertEqual(len(self.remaining_markers), 1) + + self.assertIsNone(numpy.testing.assert_array_equal(list(self.scene_markers.keys()), self.aruco_markers_group.identifiers)) + self.assertIsNone(numpy.testing.assert_array_equal(list(self.remaining_markers.keys()), [3])) + + def test_check_markers_consistency(self): + """Test ArUcoMarkerGroup markers consistency checking.""" + + self.new_from_obj() + self.setup_markers() + + # Edit consistent marker poses + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[1].translation = numpy.array([11., 11., 5.]) + self.scene_markers[1].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[2].translation = numpy.array([1., 11., 5.]) + self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + # Check consistency + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.check_markers_consistency(self.scene_markers, 1, 1) + + # Check consistent markers, unconsistent markers and unconsistencies + self.assertEqual(len(consistent_markers), 3) + self.assertEqual(len(unconsistent_markers), 0) + self.assertEqual(len(unconsistencies['rotation']), 0) + self.assertEqual(len(unconsistencies['translation']), 0) + + self.assertIsNone(numpy.testing.assert_array_equal(list(consistent_markers.keys()), self.aruco_markers_group.identifiers)) + + # Edit unconsistent marker poses + self.scene_markers[2].translation = numpy.array([5., 15., 5.]) + + # Check consistency + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.check_markers_consistency(self.scene_markers, 1, 1) + + # Check consistent markers, unconsistent markers and unconsistencies + self.assertEqual(len(consistent_markers), 2) + self.assertEqual(len(unconsistent_markers), 1) + self.assertEqual(len(unconsistencies['rotation']), 0) + self.assertEqual(len(unconsistencies['translation']), 2) + + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistent_markers.keys()), [2])) + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistencies['translation'].keys()), ['0/2', '1/2'])) + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistencies['translation']['0/2'].keys()), ['current', 'expected'])) + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistencies['translation']['1/2'].keys()), ['current', 'expected'])) + + def test_estimate_pose_from_single_marker(self): + """Test ArUcoMarkerGroup pose estimation from single marker.""" + + self.new_from_obj() + self.setup_markers() + + # Edit marke pose + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + # Estimate pose + tvec, rmat = self.aruco_markers_group.estimate_pose_from_single_marker(self.scene_markers[0]) + + self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) + self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + def test_estimate_pose_from_markers(self): + """Test ArUcoMarkerGroup pose estimation from markers.""" + + self.new_from_obj() + self.setup_markers() + + # Edit markers pose + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[1].translation = numpy.array([11., 11., 5.]) + self.scene_markers[1].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[2].translation = numpy.array([1., 11., 5.]) + self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + # Estimate pose + tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers(self.scene_markers) + + self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) + self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + @unittest.skip("ArUcoMarkerGroup estimate_pose_from_axis_markers method is broken.") + def test_estimate_pose_from_axis_markers(self): + """Test ArUcoMarkerGroup pose estimation from axis markers.""" + + self.new_from_obj() + self.setup_markers() + + # Edit markers pose + self.scene_markers[0].translation = numpy.array([1., 1., 5.]) + self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[1].translation = numpy.array([11., 11., 5.]) + self.scene_markers[1].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + self.scene_markers[2].translation = numpy.array([1., 11., 5.]) + self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) + + # Estimate pose + tvec, rmat = self.aruco_markers_group.estimate_pose_from_axis_markers(self.scene_markers[2], self.scene_markers[1], self.scene_markers[0]) + + self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) + self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]])) + +if __name__ == '__main__': + + unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/__init__.py b/src/argaze.test/ArUcoMarker/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/__init__.py diff --git a/src/argaze.test/ArUcoMarker/utils/aoi_3d.obj b/src/argaze.test/ArUcoMarker/utils/aoi_3d.obj new file mode 100644 index 0000000..92e85bd --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/aoi_3d.obj @@ -0,0 +1,7 @@ +o Test +v 0.000000 0.000000 0.000000 +v 25.000000 0.000000 0.000000 +v 0.000000 14.960000 0.000000 +v 25.000000 14.960000 0.000000 +s off +f 1 2 4 3 diff --git a/src/argaze.test/ArUcoMarker/utils/aruco_camera.json b/src/argaze.test/ArUcoMarker/utils/aruco_camera.json new file mode 100644 index 0000000..980dc9f --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/aruco_camera.json @@ -0,0 +1,98 @@ +{ + "name": "TestArUcoCamera", + "size": [1920, 1080], + "aruco_detector": { + "dictionary": { + "name": "DICT_ARUCO_ORIGINAL" + }, + "optic_parameters": { + "rms": 1.0, + "dimensions": [ + 1920, + 1080 + ], + "K": [ + [ + 1.0, + 0.0, + 1.0 + ], + [ + 0.0, + 1.0, + 1.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "D": [ + -1.0, + -0.5, + 0.0, + 0.5, + 1.0 + ] + }, + "parameters": { + "cornerRefinementMethod": 3, + "aprilTagQuadSigma": 2, + "aprilTagDeglitch": 1 + } + }, + "scenes": { + "TestSceneA" : { + "aruco_markers_group": { + "dictionary": "DICT_ARUCO_ORIGINAL", + "places": { + "0": { + "translation": [1, 0, 0], + "rotation": [0, 0, 0], + "size": 3.0 + }, + "1": { + "translation": [0, 1, 0], + "rotation": [0, 90, 0], + "size": 3.0 + } + } + }, + "layers": { + "Main" : { + "aoi_scene": "aoi_3d.obj" + } + }, + "angle_tolerance": 1.0, + "distance_tolerance": 2.0 + }, + "TestSceneB" : { + "aruco_markers_group": { + "dictionary": "DICT_ARUCO_ORIGINAL", + "places": { + "0": { + "translation": [1, 0, 0], + "rotation": [0, 0, 0], + "size": 3.0 + }, + "1": { + "translation": [0, 1, 0], + "rotation": [0, 90, 0], + "size": 3.0 + } + } + }, + "layers": { + "Main" : { + "aoi_scene": "aoi_3d.obj" + } + }, + "angle_tolerance": 1.0, + "distance_tolerance": 2.0 + } + }, + "layers": { + "Main": {} + } +}
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/utils/detector.json b/src/argaze.test/ArUcoMarker/utils/detector.json new file mode 100644 index 0000000..8aada6d --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/detector.json @@ -0,0 +1,42 @@ +{ + "dictionary": { + "name": "DICT_ARUCO_ORIGINAL" + }, + "marker_size": 3.0, + "optic_parameters": { + "rms": 1.0, + "dimensions": [ + 1920, + 1080 + ], + "K": [ + [ + 1.0, + 0.0, + 1.0 + ], + [ + 0.0, + 1.0, + 1.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "D": [ + -1.0, + -0.5, + 0.0, + 0.5, + 1.0 + ] + }, + "parameters": { + "cornerRefinementMethod": 3, + "aprilTagQuadSigma": 2, + "aprilTagDeglitch": 1 + } +}
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/utils/detector_parameters.json b/src/argaze.test/ArUcoMarker/utils/detector_parameters.json new file mode 100644 index 0000000..d26a3fa --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/detector_parameters.json @@ -0,0 +1,5 @@ +{ + "cornerRefinementMethod": 3, + "aprilTagQuadSigma": 2, + "aprilTagDeglitch": 1 +}
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/utils/full_hd_board.png b/src/argaze.test/ArUcoMarker/utils/full_hd_board.png Binary files differnew file mode 100644 index 0000000..d30b300 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/full_hd_board.png diff --git a/src/argaze.test/ArUcoMarker/utils/full_hd_marker.png b/src/argaze.test/ArUcoMarker/utils/full_hd_marker.png Binary files differnew file mode 100644 index 0000000..42146fe --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/full_hd_marker.png diff --git a/src/argaze.test/ArUcoMarker/utils/optic_parameters.json b/src/argaze.test/ArUcoMarker/utils/optic_parameters.json new file mode 100644 index 0000000..988731c --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/optic_parameters.json @@ -0,0 +1,31 @@ +{ + "rms": 1.0, + "dimensions": [ + 1920, + 1080 + ], + "K": [ + [ + 1.0, + 0.0, + 1.0 + ], + [ + 0.0, + 1.0, + 1.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "D": [ + -1.0, + -0.5, + 0.0, + 0.5, + 1.0 + ] +}
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarker/utils/scene.json b/src/argaze.test/ArUcoMarker/utils/scene.json new file mode 100644 index 0000000..bdd9ae8 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/scene.json @@ -0,0 +1,20 @@ +{ + "dictionary": { + "name": "DICT_ARUCO_ORIGINAL" + }, + "marker_size": 1, + "places": { + "0": { + "translation": [0, 0, 0], + "rotation": [0, 0, 0] + }, + "1": { + "translation": [10, 10, 0], + "rotation": [0, 0, 0] + }, + "2": { + "translation": [0, 10, 0], + "rotation": [0, 0, 0] + } + } +} diff --git a/src/argaze.test/ArUcoMarker/utils/scene.obj b/src/argaze.test/ArUcoMarker/utils/scene.obj new file mode 100644 index 0000000..1eb9f81 --- /dev/null +++ b/src/argaze.test/ArUcoMarker/utils/scene.obj @@ -0,0 +1,22 @@ +# .OBJ file for ArUcoMarkerGroup unitary test +o DICT_ARUCO_ORIGINAL#0_Marker +v -0.500000 -0.500000 0.000000 +v 0.500000 -0.500000 0.000000 +v -0.500000 0.500000 0.000000 +v 0.500000 0.500000 0.000000 +vn 0.0000 0.0000 1.0000 +f 1//1 2//1 4//1 3//1 +o DICT_ARUCO_ORIGINAL#1_Marker +v 9.500000 9.500000 0.000000 +v 10.500000 9.500000 0.000000 +v 9.500000 10.500000 0.000000 +v 10.500000 10.500000 0.000000 +vn 0.0000 0.0000 1.0000 +f 5//2 6//2 8//2 7//2 +o DICT_ARUCO_ORIGINAL#2_Marker +v -0.500000 9.500000 0.000000 +v 0.500000 9.500000 0.000000 +v -0.500000 10.500000 0.000000 +v 0.500000 10.500000 0.000000 +vn 0.0000 0.0000 1.0000 +f 9//3 10//3 12//3 11//3 |
