From 48e628f23a232d3e69920d2576973188bbfba7a6 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 15 Mar 2023 16:09:11 +0100 Subject: Managing ArUcoScene places by marker identifier. Improving ArUcoScene class tests. --- src/argaze.test/ArUcoMarkers/ArUcoScene.py | 139 ++++++++++++++++++++++++--- src/argaze.test/ArUcoMarkers/utils/scene.obj | 25 +++-- src/argaze.test/utils/environment.json | 10 +- 3 files changed, 147 insertions(+), 27 deletions(-) (limited to 'src/argaze.test') diff --git a/src/argaze.test/ArUcoMarkers/ArUcoScene.py b/src/argaze.test/ArUcoMarkers/ArUcoScene.py index 537469e..24e5347 100644 --- a/src/argaze.test/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze.test/ArUcoMarkers/ArUcoScene.py @@ -4,34 +4,151 @@ import unittest import os import math -from argaze.ArUcoMarkers import ArUcoScene +from argaze.ArUcoMarkers import ArUcoScene, ArUcoMarker import cv2 as cv import numpy class TestArUcoSceneClass(unittest.TestCase): - """Test ArUcoScene class.""" - def test_new(self): - """Test ArUcoScene creation.""" + def setUp(self): + """Initialize ArUcoScene class test.""" # Edit file path current_directory = os.path.dirname(os.path.abspath(__file__)) obj_filepath = os.path.join(current_directory, 'utils/scene.obj') # Load file - aruco_scene = ArUcoScene.ArUcoScene(1, obj_filepath) + self.aruco_scene = ArUcoScene.ArUcoScene(obj_filepath) + + # 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_scene.filter_markers(self.detected_markers) + + def test_new(self): + """Test ArUcoScene creation.""" # Check ArUcoScene creation - self.assertEqual(len(aruco_scene.places), 2) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.identifiers, [0, 1])) + self.assertEqual(len(self.aruco_scene.places), 3) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.identifiers, [0, 1, 2])) + self.assertEqual(self.aruco_scene.marker_size, 1.) + + self.assertEqual(self.aruco_scene.places[0].marker.identifier, 0) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].translation, [0., 0., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_scene.places[1].marker.identifier, 1) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].translation, [10., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + self.assertEqual(self.aruco_scene.places[2].marker.identifier, 2) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].translation, [0., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + + def test_filter_markers(self): + """Test ArUcoScene markers filtering.""" + + # 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_scene.identifiers)) + self.assertIsNone(numpy.testing.assert_array_equal(list(self.remaining_markers.keys()), [3])) + + def test_check_markers_consistency(self): + """Test ArUcoScene markers consistency checking.""" + + # 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_scene.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), 0) + + self.assertIsNone(numpy.testing.assert_array_equal(list(consistent_markers.keys()), self.aruco_scene.identifiers)) + + # Edit unconsistent marker poses + self.scene_markers[2].translation = numpy.array([5., 15., 5.]) + + # Check consistency + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.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), 2) + + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistent_markers.keys()), [2])) + self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistencies.keys()), ['0/2 distance', '1/2 distance'])) + + def test_estimate_pose_from_single_marker(self): + """Test ArUcoScene pose estimation from single marker.""" + + # 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_scene.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 ArUcoScene pose estimation from 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_scene.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.]])) + + def test_estimate_pose_from_axis_markers(self): + """Test ArUcoScene pose estimation from axis 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.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#0'].translation, [0.5, 0.5, 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#0'].rotation, [[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.]]) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#1'].translation, [10.5, 10.5, 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_scene.places['DICT_ARUCO_ORIGINAL#1'].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + # Estimate pose + tvec, rmat = self.aruco_scene.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__': diff --git a/src/argaze.test/ArUcoMarkers/utils/scene.obj b/src/argaze.test/ArUcoMarkers/utils/scene.obj index 51c8148..16c22a0 100644 --- a/src/argaze.test/ArUcoMarkers/utils/scene.obj +++ b/src/argaze.test/ArUcoMarkers/utils/scene.obj @@ -1,17 +1,22 @@ # .OBJ file for ArUcoScene unitary test o DICT_ARUCO_ORIGINAL#0_Marker -v 0.000000 0.000000 0.000000 -v 1.000000 0.000000 0.000000 -v 0.000000 1.000000 0.000000 -v 1.000000 1.000000 0.000000 +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 -s off f 1//1 2//1 4//1 3//1 o DICT_ARUCO_ORIGINAL#1_Marker -v 10.000000 10.000000 0.000000 -v 11.000000 10.000000 0.000000 -v 10.000000 11.000000 0.000000 -v 11.000000 11.000000 0.000000 +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 -s off 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 diff --git a/src/argaze.test/utils/environment.json b/src/argaze.test/utils/environment.json index d5f8639..e4cf43a 100644 --- a/src/argaze.test/utils/environment.json +++ b/src/argaze.test/utils/environment.json @@ -42,15 +42,13 @@ }, "TestScene" : { "aruco_scene": { - "A": { + "0": { "translation": [1, 0, 0], - "rotation": [0, 0, 0], - "marker": 0 + "rotation": [0, 0, 0] }, - "B": { + "1": { "translation": [0, 1, 0], - "rotation": [0, 90, 0], - "marker": 1 + "rotation": [0, 90, 0] } }, "aoi_scene": "aoi.obj", -- cgit v1.1