1 files changed, 128 insertions, 11 deletions
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__':