aboutsummaryrefslogtreecommitdiff
path: root/src/argaze.test/ArUcoMarkers
diff options
context:
space:
mode:
Diffstat (limited to 'src/argaze.test/ArUcoMarkers')
-rw-r--r--src/argaze.test/ArUcoMarkers/ArUcoScene.py139
-rw-r--r--src/argaze.test/ArUcoMarkers/utils/scene.obj25
2 files changed, 143 insertions, 21 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
generated by cgit v1.1 at 2024-11-10 11:12:22 +0000