Improving ArUcoScene loading.

2 files changed, 73 insertions, 4 deletions

diff --git a/src/argaze.test/ArUcoMarkers/ArUcoScene.py b/src/argaze.test/ArUcoMarkers/ArUcoScene.py
index 24e5347..8d344dc 100644
--- a/src/argaze.test/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze.test/ArUcoMarkers/ArUcoScene.py
@@ -11,15 +11,25 @@ import numpy
 
 class TestArUcoSceneClass(unittest.TestCase):
 
-    def setUp(self):
-        """Initialize ArUcoScene class test."""
+    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_scene = ArUcoScene.ArUcoScene(obj_filepath)
+        self.aruco_scene = ArUcoScene.ArUcoScene.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_scene = ArUcoScene.ArUcoScene.from_json(json_filepath)
+
+    def setup_markers(self):
 
         # Prepare detected markers
         self.detected_markers = {
@@ -32,9 +42,35 @@ class TestArUcoSceneClass(unittest.TestCase):
         # Prepare scene markers and remaining markers
         self.scene_markers, self.remaining_markers = self.aruco_scene.filter_markers(self.detected_markers)
 
-    def test_new(self):
+    def test_new_from_obj(self):
         """Test ArUcoScene creation."""
 
+        self.new_from_obj()
+        self.setup_markers()
+
+         # Check ArUcoScene creation
+        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_new_from_json(self):
+        """Test ArUcoScene creation."""
+
+        self.new_from_json()
+        self.setup_markers()
+
          # Check ArUcoScene creation
         self.assertEqual(len(self.aruco_scene.places), 3)
         self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.identifiers, [0, 1, 2]))
@@ -55,6 +91,9 @@ class TestArUcoSceneClass(unittest.TestCase):
     def test_filter_markers(self):
         """Test ArUcoScene 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)
@@ -65,6 +104,9 @@ class TestArUcoSceneClass(unittest.TestCase):
     def test_check_markers_consistency(self):
         """Test ArUcoScene 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.]])
@@ -102,6 +144,9 @@ class TestArUcoSceneClass(unittest.TestCase):
     def test_estimate_pose_from_single_marker(self):
         """Test ArUcoScene 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.]])
@@ -115,6 +160,9 @@ class TestArUcoSceneClass(unittest.TestCase):
     def test_estimate_pose_from_markers(self):
         """Test ArUcoScene 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.]])
@@ -134,6 +182,9 @@ class TestArUcoSceneClass(unittest.TestCase):
     def test_estimate_pose_from_axis_markers(self):
         """Test ArUcoScene 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.]])
diff --git a/src/argaze.test/ArUcoMarkers/utils/scene.json b/src/argaze.test/ArUcoMarkers/utils/scene.json
new file mode 100644
index 0000000..3a33b53
--- /dev/null
+++ b/src/argaze.test/ArUcoMarkers/utils/scene.json
@@ -0,0 +1,18 @@
+{
+	"dictionary": "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]
+		}
+	}
+}