aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze.test/ArUcoMarkers/ArUcoScene.py139
-rw-r--r--src/argaze.test/ArUcoMarkers/utils/scene.obj25
-rw-r--r--src/argaze.test/utils/environment.json10
-rw-r--r--src/argaze/ArFeatures.py36
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py297
5 files changed, 333 insertions, 174 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",
diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py
index e9b7974..4e1f5cb 100644
--- a/src/argaze/ArFeatures.py
+++ b/src/argaze/ArFeatures.py
@@ -122,7 +122,7 @@ class ArScene():
aruco_scene_value = os.path.join(self.__ar_environment.working_directory, aruco_scene_value)
- self.aruco_scene = ArUcoScene.ArUcoScene(self.__ar_environment.aruco_detector.marker_size, aruco_scene_value, self.__ar_environment.aruco_detector.dictionary)
+ self.aruco_scene = ArUcoScene.ArUcoScene(aruco_scene_value, self.__ar_environment.aruco_detector.dictionary, self.__ar_environment.aruco_detector.marker_size)
# Check aoi_scene value type
aoi_scene_value = kwargs.pop('aoi_scene')
@@ -146,19 +146,6 @@ class ArScene():
# Update all attributes from arguments
self.__dict__.update(kwargs)
- # Convert aruco axis markers identifier into ARUCO_DICT_NAME#ID string
- aruco_axis_string = {}
- for axis_name, markers_id in self.aruco_axis.items():
-
- # Estimate pose from axis markers
- aruco_axis_names = []
- for marker_id in markers_id:
- aruco_axis_names.append(f'{ar_environment.aruco_detector.dictionary.name}#{marker_id}')
-
- aruco_axis_string[axis_name] = aruco_axis_names
-
- self.aruco_axis = aruco_axis_string
-
# Preprocess orthogonal projection to speed up further aruco aoi processings
self.__orthogonal_projection_cache = self.orthogonal_projection
@@ -217,24 +204,25 @@ class ArScene():
# Estimate scene pose from unique marker transformations
elif len(scene_markers) == 1:
- tvec, rmat = self.aruco_scene.estimate_pose_from_any_markers(scene_markers)
+ marker_id, marker = scene_markers.popitem()
+ tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(marker)
- return tvec, rmat, scene_markers
+ return tvec, rmat, {marker_id: marker}
# Try to estimate scene pose from 3 markers defining an orthogonal axis
elif len(scene_markers) >= 3 and len(self.aruco_axis) > 0:
- for axis_name, markers_names in self.aruco_axis.items():
+ for axis_name, axis_markers in self.aruco_axis.items():
try:
- axis_markers = []
- for name in markers_names:
- axis_markers.append((name, scene_markers[name]))
+ origin_marker = scene_markers[axis_markers['origin_marker']]
+ horizontal_axis_marker = scene_markers[axis_markers['horizontal_axis_marker']]
+ vertical_axis_marker = scene_markers[axis_markers['vertical_axis_marker']]
- tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(axis_markers)
+ tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker)
- return tvec, rmat, axis_markers
+ return tvec, rmat, {origin_marker.identifier: origin_marker, horizontal_axis_marker.identifier: horizontal_axis_marker, vertical_axis_marker.identifier: vertical_axis_marker}
except:
pass
@@ -249,8 +237,8 @@ class ArScene():
raise PoseEstimationFailed('Unconsistent marker poses', unconsistencies)
- # Otherwise, estimate scene pose from all markers transformations
- tvec, rmat = self.aruco_scene.estimate_pose_from_any_markers(consistent_markers)
+ # Otherwise, estimate scene pose from all consistent markers pose
+ tvec, rmat = self.aruco_scene.estimate_pose_from_markers(consistent_markers)
return tvec, rmat, consistent_markers
diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 795bfe4..15e7a35 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -38,7 +38,7 @@ class Place():
class ArUcoScene():
"""Handle group of ArUco markers as one unique spatial entity and estimate its pose."""
- def __init__(self, marker_size: float, places: dict | str, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None):
+ def __init__(self, places: dict | str, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None, marker_size: float = 0.):
"""Define scene attributes."""
# Init expected marker size
@@ -62,13 +62,26 @@ class ArUcoScene():
self.places = places
@property
+ def marker_size(self) -> float:
+ """Expected size of all markers in the scene."""
+
+ return self.__marker_size
+
+ @property
def places(self) -> dict:
- """All named places of the scene and their ArUco markers."""
+ """All places of the scene and their related ArUco markers."""
return self.__places
@places.setter
def places(self, data: dict | str):
+ """Load places from dict or .obj file.
+
+ .. warning:: .obj file loading overwrites marker_size attribute
+
+ .. warning:: in .obj file, 'o' tag string format should be DICTIONARY#IDENTIFIER_NAME
+
+ """
# str: path to .obj file
if type(data) == str:
@@ -80,29 +93,27 @@ class ArUcoScene():
self.__places = {}
- for name, place in data.items():
+ for identifier, place in data.items():
+
+ # Convert string identifier to int value
+ identifier = int(identifier)
tvec = numpy.array(place['translation']).astype(numpy.float32)
rmat = self.__make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32)
- marker = ArUcoMarker.ArUcoMarker(self.__dictionary, place['marker'], self.__marker_size)
+ marker = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size)
- self.__places[name] = Place(tvec, rmat, marker)
+ self.__places[identifier] = Place(tvec, rmat, marker)
# Init pose data
self._translation = numpy.zeros(3)
self._rotation = numpy.zeros(3)
- # Process markers ids to speed up further calculations
- self.__identifier_cache = {}
- for name, place in self.__places.items():
- self.__identifier_cache[place.marker.identifier] = name
-
# Process axis-angle between place combination to speed up further calculations
self.__angle_cache = {}
- for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.__places.items(), 2):
+ for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2):
- A = self.__places[A_name].rotation
- B = self.__places[B_name].rotation
+ A = self.__places[A_identifier].rotation
+ B = self.__places[B_identifier].rotation
if numpy.array_equal(A, B):
@@ -117,34 +128,34 @@ class ArUcoScene():
angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
try:
- self.__angle_cache[A_name][B_name] = angle
+ self.__angle_cache[A_identifier][B_identifier] = angle
except:
- self.__angle_cache[A_name] = {B_name: angle}
+ self.__angle_cache[A_identifier] = {B_identifier: angle}
try:
- self.__angle_cache[B_name][A_name] = angle
+ self.__angle_cache[B_identifier][A_identifier] = angle
except:
- self.__angle_cache[B_name] = {A_name: angle}
+ self.__angle_cache[B_identifier] = {A_identifier: angle}
# Process distance between each place combination to speed up further calculations
self.__distance_cache = {}
- for (A_name, A_place), (B_name, B_place) in itertools.combinations(self.__places.items(), 2):
+ for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2):
- A = self.__places[A_name].translation
- B = self.__places[B_name].translation
+ A = self.__places[A_identifier].translation
+ B = self.__places[B_identifier].translation
# Calculate axis-angle representation of AB rotation matrix
distance = numpy.linalg.norm(B - A)
try:
- self.__distance_cache[A_name][B_name] = distance
+ self.__distance_cache[A_identifier][B_identifier] = distance
except:
- self.__distance_cache[A_name] = {B_name: distance}
+ self.__distance_cache[A_identifier] = {B_identifier: distance}
try:
- self.__distance_cache[B_name][A_name] = distance
+ self.__distance_cache[B_identifier][A_identifier] = distance
except:
- self.__distance_cache[B_name] = {A_name: distance}
+ self.__distance_cache[B_identifier] = {A_identifier: distance}
@classmethod
def from_json(self, json_filepath) -> ArUcoSceneType:
@@ -160,32 +171,28 @@ class ArUcoScene():
output = f'\n\n\tDictionary: {self.__dictionary.name}'
output += '\n\n\tPlaces:'
- for name, place in self.__places.items():
- output += f'\n\t\t- {name}:'
+ for identifier, place in self.__places.items():
+ output += f'\n\t\t- {identifier}:'
output += f'\n{place.translation}'
output += f'\n{place.rotation}'
- output += '\n\n\tIdentifier cache:'
- for i, name in self.__identifier_cache.items():
- output += f'\n\t\t- {i}: {name}'
-
output += '\n\n\tAngle cache:'
- for A_name, A_angle_cache in self.__angle_cache.items():
- for B_name, angle in A_angle_cache.items():
- output += f'\n\t\t- {A_name}/{B_name}: {angle:3f}'
+ for A_identifier, A_angle_cache in self.__angle_cache.items():
+ for B_identifier, angle in A_angle_cache.items():
+ output += f'\n\t\t- {A_identifier}/{B_identifier}: {angle:3f}'
output += '\n\n\tDistance cache:'
- for A_name, A_distance_cache in self.__distance_cache.items():
- for B_name, distance in A_distance_cache.items():
- output += f'\n\t\t- {A_name}/{B_name}: {distance:3f}'
+ for A_identifier, A_distance_cache in self.__distance_cache.items():
+ for B_identifier, distance in A_distance_cache.items():
+ output += f'\n\t\t- {A_identifier}/{B_identifier}: {distance:3f}'
return output
@property
def identifiers(self) -> list:
- """List all makers identifier belonging to the scene."""
+ """List place maker identifiers belonging to the scene."""
- return list(self.__identifier_cache.keys())
+ return list(self.__places.keys())
def __make_rotation_matrix(self, x, y, z):
@@ -208,12 +215,9 @@ class ArUcoScene():
return Rx.dot(Ry.dot(Rz))
def __load_places_from_obj(self, obj_filepath: str) -> dict:
- """Load places from .obj file.
-
- .. warning:: 'o' tag string format should be DICTIONARY#IDENTIFIER_NAME
- """
self.__places = {}
+ self.__marker_size = 0
# Regex rules for .obj file parsing
OBJ_RX_DICT = {
@@ -238,7 +242,7 @@ class ArUcoScene():
# Start parsing
try:
- name = None
+ identifier = None
vertices = []
markers = {}
normals = {}
@@ -268,12 +272,11 @@ class ArUcoScene():
# Check that marker dictionary is like the scene dictionary
if dictionary == self.__dictionary.name:
- name = f'{dictionary}#{identifier}' # ignore last part
- markers[name] = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size)
+ markers[identifier] = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size)
else:
- raise NameError(f'Marker#{identifier} dictionary is not {self.__dictionary.name}')
+ raise NameError(f'Marker {identifier} dictionary is not {self.__dictionary.name}')
# Fill vertices array
elif key == 'vertice':
@@ -283,12 +286,12 @@ class ArUcoScene():
# Extract normal to calculate rotation matrix
elif key == 'normal':
- normals[name] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])
+ normals[identifier] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])
# Extract vertice ids
elif key == 'face':
- faces[name] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))]
+ faces[identifier] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))]
# Go to next line
line = file.readline()
@@ -296,7 +299,7 @@ class ArUcoScene():
file.close()
# Retreive marker vertices thanks to face vertice ids
- for name, face in faces.items():
+ for identifier, face in faces.items():
# Gather place corners from counter clockwise ordered face vertices
corners = numpy.array([ vertices[i-1] for i in face ])
@@ -306,24 +309,40 @@ class ArUcoScene():
# Edit place axis from corners positions
place_x_axis = corners[1:3].mean(axis=0) - Tp
- place_x_axis = place_x_axis / numpy.linalg.norm(place_x_axis)
+ place_x_axis_norm = numpy.linalg.norm(place_x_axis)
+ place_x_axis = place_x_axis / place_x_axis_norm
place_y_axis = corners[2:4].mean(axis=0) - Tp
- place_y_axis = place_y_axis / numpy.linalg.norm(place_y_axis)
+ place_y_axis_norm = numpy.linalg.norm(place_y_axis)
+ place_y_axis = place_y_axis / place_y_axis_norm
- place_z_axis = normals[name]
+ place_z_axis = normals[identifier]
# Edit rotation (Rp) allowing to transform world axis (W) into place axis (P)
W = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
P = numpy.array([place_x_axis, place_y_axis, place_z_axis])
Rp = W.dot(P.T)
- self.__places[name] = Place(Tp, Rp, markers[name])
+ # Check axis size: they should be almost equal
+ if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3):
+
+ current_marker_size = place_x_axis_norm*2
+
+ # Check that all markers size are almost equal
+ if self.__marker_size > 0:
+
+ if not math.isclose(current_marker_size, self.__marker_size, rel_tol=1e-3):
+
+ raise ValueError('Markers size should be almost equal.')
+
+ self.__marker_size = current_marker_size
+
+ self.__places[identifier] = Place(Tp, Rp, markers[identifier])
except IOError:
raise IOError(f'File not found: {obj_filepath}')
- def filter_markers(self, detected_markers) -> Tuple[dict, dict]:
+ def filter_markers(self, detected_markers: dict) -> Tuple[dict, dict]:
"""Sort markers belonging to the scene from given detected markers dict (cf ArUcoDetector.detect_markers()).
* **Returns:**
@@ -336,17 +355,17 @@ class ArUcoScene():
for (marker_id, marker) in detected_markers.items():
- try:
- name = self.__identifier_cache[marker_id]
- scene_markers[name] = marker
+ if marker_id in self.__places.keys():
- except KeyError:
+ scene_markers[marker_id] = marker
+
+ else:
remaining_markers[marker_id] = marker
return scene_markers, remaining_markers
- def check_markers_consistency(self, scene_markers, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict]:
+ def check_markers_consistency(self, scene_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict]:
"""Evaluate if given markers configuration match related places configuration.
* **Returns:**
@@ -358,42 +377,48 @@ class ArUcoScene():
consistent_markers = {}
unconsistencies = {}
- for (A_name, A_marker), (B_name, B_marker) in itertools.combinations(scene_markers.items(), 2):
+ for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(scene_markers.items(), 2):
+
+ try:
- # Rotation matrix from A marker to B marker
- AB = B_marker.rotation.dot(A_marker.rotation.T)
+ # Rotation matrix from A marker to B marker
+ AB = B_marker.rotation.dot(A_marker.rotation.T)
- # Calculate axis-angles representation of AB rotation matrix
- angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
- expected_angle = self.__angle_cache[A_name][B_name]
+ # Calculate axis-angles representation of AB rotation matrix
+ angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
+ expected_angle = self.__angle_cache[A_identifier][B_identifier]
- # Calculate distance between A marker center and B marker center
- distance = numpy.linalg.norm(A_marker.translation - B_marker.translation)
- expected_distance = self.__distance_cache[A_name][B_name]
+ # Calculate distance between A marker center and B marker center
+ distance = numpy.linalg.norm(A_marker.translation - B_marker.translation)
+ expected_distance = self.__distance_cache[A_identifier][B_identifier]
- # Check angle and distance according given tolerance then normalise marker pose
- consistent_angle = math.isclose(angle, expected_angle, abs_tol=angle_tolerance)
- consistent_distance = math.isclose(distance, expected_distance, abs_tol=distance_tolerance)
+ # Check angle and distance according given tolerance then normalise marker pose
+ consistent_angle = math.isclose(angle, expected_angle, abs_tol=angle_tolerance)
+ consistent_distance = math.isclose(distance, expected_distance, abs_tol=distance_tolerance)
- if consistent_angle and consistent_distance:
+ if consistent_angle and consistent_distance:
- if A_name not in consistent_markers.keys():
+ if A_identifier not in consistent_markers.keys():
- # Remember this marker is already validated
- consistent_markers[A_name] = A_marker
+ # Remember this marker is already validated
+ consistent_markers[A_identifier] = A_marker
- if B_name not in consistent_markers.keys():
+ if B_identifier not in consistent_markers.keys():
- # Remember this marker is already validated
- consistent_markers[B_name] = B_marker
+ # Remember this marker is already validated
+ consistent_markers[B_identifier] = B_marker
- else:
+ else:
- if not consistent_angle:
- unconsistencies[f'{A_name}/{B_name} angle'] = angle
-
- if not consistent_distance:
- unconsistencies[f'{A_name}/{B_name} distance'] = distance
+ if not consistent_angle:
+ unconsistencies[f'{A_identifier}/{B_identifier} angle'] = angle
+
+ if not consistent_distance:
+ unconsistencies[f'{A_identifier}/{B_identifier} distance'] = distance
+
+ except KeyError:
+
+ raise ValueError(f'Marker {A_identifier} or {B_identifier} don\'t belong to the scene.')
# Gather unconsistent markers
unconsistent_markers = {}
@@ -406,16 +431,70 @@ class ArUcoScene():
return consistent_markers, unconsistent_markers, unconsistencies
- def estimate_pose_from_axis_markers(self, axis_markers) -> Tuple[numpy.array, numpy.array]:
- """Calculate tranformations (rotation and translation) from a list of 3 (name, marker) tuples defining an orthogonal axis."""
+ def estimate_pose_from_single_marker(self, marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]:
+ """Calculate rotation and translation that move a marker to its place."""
+
+ # Get the place related to the given marker
+ try:
+
+ place = self.__places[marker.identifier]
+
+ # Rotation matrix that transform marker to related place
+ self._rotation = marker.rotation.dot(place.rotation.T)
+
+ # Translation vector that transform marker to related place
+ self._translation = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T)
+
+ return self._translation, self._rotation
+
+ except KeyError:
+
+ raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the scene.')
+
+ def estimate_pose_from_markers(self, markers: dict) -> Tuple[numpy.array, numpy.array]:
+ """Calculate average rotation and translation that move markers to their related places."""
+
+ rotations = []
+ translations = []
+
+ for identifier, marker in markers.items():
+
+ try:
+
+ place = self.__places[identifier]
+
+ # Rotation matrix that transform marker to related place
+ R = marker.rotation.dot(place.rotation.T)
+
+ # Translation vector that transform marker to related place
+ T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T)
+
+ rotations.append(R)
+ translations.append(T)
+
+ except KeyError:
+
+ raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the scene.')
+
+ # Consider ArUcoScene rotation as the mean of all marker rotations
+ # !!! WARNING !!! This is a bad hack : processing rotations average is a very complex problem that needs to well define the distance calculation method before.
+ self._rotation = numpy.mean(numpy.array(rotations), axis=0)
+
+ # Consider ArUcoScene translation as the mean of all marker translations
+ self._translation = numpy.mean(numpy.array(translations), axis=0)
+
+ return self._translation, self._rotation
- O_name, O_marker = axis_markers[0]
- A_name, A_marker = axis_markers[1]
- B_name, B_marker = axis_markers[2]
+ def estimate_pose_from_axis_markers(self, origin_marker: ArUcoMarker.ArUcoMarker, horizontal_axis_marker: ArUcoMarker.ArUcoMarker, vertical_axis_marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]:
+ """Calculate rotation and translation from 3 markers defining an orthogonal axis."""
- O_place = self.__places[O_name]
- A_place = self.__places[A_name]
- B_place = self.__places[B_name]
+ O_marker = origin_marker
+ A_marker = horizontal_axis_marker
+ B_marker = vertical_axis_marker
+
+ O_place = self.__places[O_marker.identifier]
+ A_place = self.__places[A_marker.identifier]
+ B_place = self.__places[B_marker.identifier]
# Place axis
OA = A_place.translation - O_place.translation
@@ -450,34 +529,6 @@ class ArUcoScene():
return self._translation, self._rotation
- def estimate_pose_from_any_markers(self, consistent_markers) -> Tuple[numpy.array, numpy.array]:
- """Calculate transformations (rotation and translation) that move each marker to its related place."""
-
- rotations = []
- translations = []
-
- for (name, marker) in consistent_markers.items():
-
- place = self.__places[name]
-
- # Rotation matrix that transform marker to related place
- R = marker.rotation.dot(place.rotation.T)
-
- # Translation vector that transform marker to related place
- T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T)
-
- rotations.append(R)
- translations.append(T)
-
- # Consider ArUcoScene rotation as the mean of all marker rotations
- # !!! WARNING !!! This is a bad hack : processing rotations average is a very complex problem that needs to well define the distance calculation method before.
- self._rotation = numpy.mean(numpy.array(rotations), axis=0)
-
- # Consider ArUcoScene translation as the mean of all marker translations
- self._translation = numpy.mean(numpy.array(translations), axis=0)
-
- return self._translation, self._rotation
-
@property
def translation(self) -> numpy.array:
"""Access to scene translation vector."""
@@ -529,12 +580,12 @@ class ArUcoScene():
n = 95 * consistency if consistency < 2 else 0
f = 159 * consistency if consistency < 2 else 255
- for name, place in self.__places.items():
+ for identifier, place in self.__places.items():
try:
- T = self.__places[name].translation
- R = self.__places[name].rotation
+ T = self.__places[identifier].translation
+ R = self.__places[identifier].rotation
# Draw place axis
axisPoints = (T + numpy.float32([R.dot([l/2, 0, 0]), R.dot([0, l/2, 0]), R.dot([0, 0, l/2]), R.dot([0, 0, 0])])).reshape(-1, 3)