Managing ArUcoScene places by marker identifier. Improving ArUcoScene class tests.

3 files changed, 147 insertions, 27 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__':
 
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",