aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorThéo de la Hogue2023-03-27 16:08:06 +0200
committerThéo de la Hogue2023-03-27 16:08:06 +0200
commit279229706fec35fa73a0c11dd1c67ebb1d59a914 (patch)
tree34cef23176ab99da64344193962aa2623c53cd01
parent1a4e3283623afc065bbe6b46722f468fe7ea1779 (diff)
downloadargaze-279229706fec35fa73a0c11dd1c67ebb1d59a914.zip
argaze-279229706fec35fa73a0c11dd1c67ebb1d59a914.tar.gz
argaze-279229706fec35fa73a0c11dd1c67ebb1d59a914.tar.bz2
argaze-279229706fec35fa73a0c11dd1c67ebb1d59a914.tar.xz
Improving ArEnvironment, ArScene and ArUcoScene loading.
-rw-r--r--src/argaze.test/ArFeatures.py12
-rw-r--r--src/argaze/ArFeatures.py145
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py271
3 files changed, 202 insertions, 226 deletions
diff --git a/src/argaze.test/ArFeatures.py b/src/argaze.test/ArFeatures.py
index 8e7ff49..af8cfa8 100644
--- a/src/argaze.test/ArFeatures.py
+++ b/src/argaze.test/ArFeatures.py
@@ -45,13 +45,13 @@ class TestArEnvironmentClass(unittest.TestCase):
# Check Aruco scene
self.assertEqual(len(ar_scene.aruco_scene.places), 2)
- self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places['A'].translation, [1, 0, 0]))
- self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places['A'].rotation, [[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]]))
- self.assertEqual(ar_scene.aruco_scene.places['A'].marker.identifier, 0)
+ self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[0].translation, [1, 0, 0]))
+ self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[0].rotation, [[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]]))
+ self.assertEqual(ar_scene.aruco_scene.places[0].marker.identifier, 0)
- self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places['B'].translation, [0, 1, 0]))
- self.assertIsNone(numpy.testing.assert_array_almost_equal(ar_scene.aruco_scene.places['B'].rotation, [[0.,0.,1.],[0., 1.,0.],[-1.,0.,0.]]))
- self.assertEqual(ar_scene.aruco_scene.places['B'].marker.identifier, 1)
+ self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[1].translation, [0, 1, 0]))
+ self.assertIsNone(numpy.testing.assert_array_almost_equal(ar_scene.aruco_scene.places[1].rotation, [[0.,0.,1.],[0., 1.,0.],[-1.,0.,0.]]))
+ self.assertEqual(ar_scene.aruco_scene.places[1].marker.identifier, 1)
# Check AOI scene
self.assertEqual(len(ar_scene.aoi_scene.items()), 1)
diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py
index 4e1f5cb..3706aac 100644
--- a/src/argaze/ArFeatures.py
+++ b/src/argaze/ArFeatures.py
@@ -24,31 +24,72 @@ class ArEnvironment():
"""Define an Augmented Reality environment based ArUco marker detection."""
name: str
- """Environement name"""
+ """Environment name"""
- aruco_detector: ArUcoDetector.ArUcoDetector = field(init=False, default_factory=ArUcoDetector.ArUcoDetector)
- """ArUco detector"""
-
- def __init__(self, **kwargs):
-
- self.name = kwargs.pop('name')
+ working_directory: str
+ """Environment working directory"""
- self.aruco_detector = ArUcoDetector.ArUcoDetector(**kwargs.pop('aruco_detector'))
-
- self.__scenes = {}
- for name, scene_kwargs in kwargs.items():
+ aruco_detector: ArUcoDetector.ArUcoDetector = field(default_factory=ArUcoDetector.ArUcoDetector)
+ """ArUco detector"""
- self.__scenes[name] = ArScene(self, **scene_kwargs)
+ scenes: dict = field(default_factory=dict)
+ """All environment scenes"""
@classmethod
def from_json(self, json_filepath: str) -> ArSceneType:
"""Load ArEnvironment from .json file."""
+ new_name = ''
+ new_working_directory = '.'
+ new_aruco_detector = None
+ new_scenes = {}
+
with open(json_filepath) as configuration_file:
- self.__working_directory = os.path.dirname(json_filepath)
+ data = json.load(configuration_file)
+
+ new_name = data.pop('name')
+
+ new_working_directory = os.path.dirname(json_filepath)
+
+ new_aruco_detector = ArUcoDetector.ArUcoDetector(**data.pop('aruco_detector'))
+
+ for scene_name, scene_data in data.items():
+
+ new_aruco_scene = None
+ new_aoi_scene = None
+
+ # Check aruco_scene value type
+ aruco_scene_value = scene_data.pop('aruco_scene')
- return ArEnvironment(**json.load(configuration_file))
+ # str: relative path to .obj file
+ if type(aruco_scene_value) == str:
+
+ aruco_scene_value = os.path.join(new_working_directory, aruco_scene_value)
+ new_aruco_scene = ArUcoScene.ArUcoScene.from_obj(aruco_scene_value)
+
+ # dict:
+ else:
+
+ new_aruco_scene = ArUcoScene.ArUcoScene(new_aruco_detector.marker_size, new_aruco_detector.dictionary, aruco_scene_value)
+
+ # Check aoi_scene value type
+ aoi_scene_value = scene_data.pop('aoi_scene')
+
+ # str: relative path to .obj file
+ if type(aoi_scene_value) == str:
+
+ obj_filepath = os.path.join(new_working_directory, aoi_scene_value)
+ new_aoi_scene = AOI3DScene.AOI3DScene.from_obj(obj_filepath)
+
+ # dict:
+ else:
+
+ new_aoi_scene = AOI3DScene.AOI3DScene(aoi_scene_value)
+
+ new_scenes[scene_name] = ArScene(self, new_aruco_scene, new_aoi_scene, **scene_data)
+
+ return ArEnvironment(new_name, new_working_directory, new_aruco_detector, new_scenes)
def __str__(self) -> str:
"""String display"""
@@ -60,17 +101,6 @@ class ArEnvironment():
return output
- @property
- def working_directory(self):
- """Working directory path."""
- return self.__working_directory
-
- @property
- def scenes(self) -> Tuple[str, ArSceneType]:
- """Access to scenes dictionary."""
-
- return self.__scenes
-
class PoseEstimationFailed(Exception):
"""Exception raised by ArScene project method when the pose can't be estimated due to unconsistencies."""
@@ -91,72 +121,33 @@ class SceneProjectionFailed(Exception):
class ArScene():
"""Define an Augmented Reality scene with ArUco markers and AOI scenes."""
- aruco_scene: ArUcoScene.ArUcoScene = field(init=False, default_factory=ArUcoScene.ArUcoScene)
+ ar_environment: ArEnvironment = field(default_factory=ArEnvironment)
+ """AR Environment to which the scene belongs."""
+
+ aruco_scene: ArUcoScene.ArUcoScene = field(default_factory=ArUcoScene.ArUcoScene)
"""ArUco markers 3D scene description used to estimate scene pose from detected markers: see `estimate_pose` function below."""
- aoi_scene: AOI3DScene.AOI3DScene = field(init=False, default_factory=AOI3DScene.AOI3DScene)
+ aoi_scene: AOI3DScene.AOI3DScene = field(default_factory=AOI3DScene.AOI3DScene)
"""AOI 3D scene description that will be projected onto estimated scene once its pose will be estimated : see `project` function below."""
- aruco_axis: dict
+ aruco_axis: dict = field(default_factory=dict)
"""Optional dictionary to define orthogonal axis where each axis is defined by list of 3 markers identifier (first is origin). \
This pose estimation strategy is used by `estimate_pose` function when at least 3 markers are detected."""
- aruco_aoi: dict
+ aruco_aoi: dict = field(default_factory=dict)
"""Optional dictionary of AOI defined by list of markers identifier and markers corners index tuples: see `build_aruco_aoi_scene` function below."""
- angle_tolerance: float
+ angle_tolerance: float = field(default=0.)
"""Optional angle error tolerance to validate marker pose in degree used into `estimate_pose` function."""
- distance_tolerance: float
+ distance_tolerance: float = field(default=0.)
"""Optional distance error tolerance to validate marker pose in centimeter used into `estimate_pose` function."""
- def __init__(self, ar_environment: ArEnvironment, **kwargs):
-
- self.__ar_environment = ar_environment
-
- # Check aruco_scene value type
- aruco_scene_value = kwargs.pop('aruco_scene')
-
- # str: relative path to .obj file
- if type(aruco_scene_value) == str:
-
- aruco_scene_value = os.path.join(self.__ar_environment.working_directory, aruco_scene_value)
-
- 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')
-
- # str: relative path to .obj file
- if type(aoi_scene_value) == str:
-
- obj_filepath = os.path.join(self.__ar_environment.working_directory, aoi_scene_value)
- self.aoi_scene = AOI3DScene.AOI3DScene.from_obj(obj_filepath)
-
- # dict: all AOI
- else:
- self.aoi_scene = AOI3DScene.AOI3DScene(aoi_scene_value)
-
- # Init aruco axis
- self.aruco_axis = {}
-
- # Init aruco aoi
- self.aruco_aoi = {}
-
- # Update all attributes from arguments
- self.__dict__.update(kwargs)
+ def __post_init__(self):
# Preprocess orthogonal projection to speed up further aruco aoi processings
self.__orthogonal_projection_cache = self.orthogonal_projection
- @classmethod
- def from_json(self, json_filepath: str) -> ArSceneType:
- """Load ArScene from .json file."""
-
- with open(json_filepath) as configuration_file:
-
- return ArScene(**json.load(configuration_file))
-
def __str__(self) -> str:
"""String display"""
@@ -270,7 +261,7 @@ class ArScene():
aoi_scene_copy = self.aoi_scene.copy()
- aoi_scene_projection = aoi_scene_copy.project(tvec, rvec, self.__ar_environment.aruco_detector.camera.K)
+ aoi_scene_projection = aoi_scene_copy.project(tvec, rvec, self.ar_environment.aruco_detector.camera.K)
# Warn user when the projected scene is empty
if len(aoi_scene_projection) == 0:
@@ -320,11 +311,11 @@ class ArScene():
def draw_axis(self, frame):
"""Draw scene axis into frame."""
- self.aruco_scene.draw_axis(frame, self.__ar_environment.aruco_detector.camera.K, self.__ar_environment.aruco_detector.camera.D)
+ self.aruco_scene.draw_axis(frame, self.ar_environment.aruco_detector.camera.K, self.ar_environment.aruco_detector.camera.D)
def draw_places(self, frame):
"""Draw scene places into frame."""
- self.aruco_scene.draw_places(frame, self.__ar_environment.aruco_detector.camera.K, self.__ar_environment.aruco_detector.camera.D)
+ self.aruco_scene.draw_places(frame, self.ar_environment.aruco_detector.camera.K, self.ar_environment.aruco_detector.camera.D)
diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index bc89287..19fcf07 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -35,31 +35,119 @@ class Place():
marker: dict
"""ArUco marker linked to the place."""
+@dataclass
class ArUcoScene():
"""Handle group of ArUco markers as one unique spatial entity and estimate its pose."""
- def __init__(self, places: dict, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None, marker_size: float = 0.):
- """Define scene attributes."""
+ marker_size: float = field(default=0.)
+ """Expected size of all markers in the scene."""
- # Init expected marker size
- self.__marker_size = marker_size
+ dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary)
+ """Expected dictionary of all markers in the scene."""
- # Handle dictionary str or instance
- if dictionary != None:
+ places: dict = field(default_factory=dict)
+ """Expected markers place"""
+
+ def __post_init__(self):
+ """Initialize cached data to speed up further processings."""
+
+ # Init pose data
+ self._translation = numpy.zeros(3)
+ self._rotation = numpy.zeros(3)
+
+ # Normalize places data
+ new_places = {}
+
+ def __make_rotation_matrix(x, y, z):
+
+ # Create rotation matrix around x axis
+ c = numpy.cos(numpy.deg2rad(x))
+ s = numpy.sin(numpy.deg2rad(x))
+ Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]])
+
+ # Create rotation matrix around y axis
+ c = numpy.cos(numpy.deg2rad(y))
+ s = numpy.sin(numpy.deg2rad(y))
+ Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
+
+ # Create rotation matrix around z axis
+ c = numpy.cos(numpy.deg2rad(z))
+ s = numpy.sin(numpy.deg2rad(z))
+ Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
+
+ # Return intrinsic rotation matrix
+ return Rx.dot(Ry.dot(Rz))
+
+ for identifier, place in self.places.items():
+
+ # Convert string identifier to int value
+ if type(identifier) == str:
+
+ identifier = int(identifier)
+
+ # Convert translation and rotation keys to Place object
+ tvec = numpy.array(place.pop('translation')).astype(numpy.float32)
+ rmat = __make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32)
+
+ new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size)
+
+ new_places[identifier] = Place(tvec, rmat, new_marker)
+
+ # else places are already at expected format
+ elif (type(identifier) == int) and isinstance(place, Place):
+
+ new_places[identifier] = place
+
+ self.places = new_places
+
+ # Process axis-angle between place combination to speed up further calculations
+ self.__angle_cache = {}
+ for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2):
+
+ A = self.places[A_identifier].rotation
+ B = self.places[B_identifier].rotation
+
+ if numpy.array_equal(A, B):
+
+ angle = 0.
- if type(dictionary) == str:
- self.__dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(dictionary)
- elif isinstance(dictionary, ArUcoMarkersDictionary.ArUcoMarkersDictionary):
- self.__dictionary = dictionary
else:
- raise ValueError(f'dictionary: {dictionary}')
- else:
+ # Rotation matrix from A place to B place
+ AB = B.dot(A.T)
+
+ # Calculate axis-angle representation of AB rotation matrix
+ angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
+
+ try:
+ self.__angle_cache[A_identifier][B_identifier] = angle
+ except:
+ self.__angle_cache[A_identifier] = {B_identifier: angle}
- self.__dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary()
+ try:
+ self.__angle_cache[B_identifier][A_identifier] = angle
+ except:
+ self.__angle_cache[B_identifier] = {A_identifier: angle}
+
+ # Process distance between each place combination to speed up further calculations
+ self.__distance_cache = {}
+ for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2):
- # NEVER USE {} as default function argument
- self.places = places
+ 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_identifier][B_identifier] = distance
+ except:
+ self.__distance_cache[A_identifier] = {B_identifier: distance}
+
+ try:
+ self.__distance_cache[B_identifier][A_identifier] = distance
+ except:
+ self.__distance_cache[B_identifier] = {A_identifier: distance}
@classmethod
def from_obj(self, obj_filepath: str) -> ArUcoSceneType:
@@ -76,9 +164,9 @@ class ArUcoScene():
"""
- new_places = {}
- new_dictionary = None
new_marker_size = 0
+ new_dictionary = None
+ new_places = {}
# Regex rules for .obj file parsing
OBJ_RX_DICT = {
@@ -205,138 +293,35 @@ class ArUcoScene():
except IOError:
raise IOError(f'File not found: {obj_filepath}')
- return ArUcoScene(new_places, new_dictionary, new_marker_size)
+ return ArUcoScene(new_marker_size, new_dictionary, new_places)
@classmethod
def from_json(self, json_filepath: str) -> ArUcoSceneType:
"""Load ArUco scene from .json file."""
- new_places = {}
- new_dictionary = None
new_marker_size = 0
-
- def __make_rotation_matrix(x, y, z):
-
- # Create rotation matrix around x axis
- c = numpy.cos(numpy.deg2rad(x))
- s = numpy.sin(numpy.deg2rad(x))
- Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]])
-
- # Create rotation matrix around y axis
- c = numpy.cos(numpy.deg2rad(y))
- s = numpy.sin(numpy.deg2rad(y))
- Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
-
- # Create rotation matrix around z axis
- c = numpy.cos(numpy.deg2rad(z))
- s = numpy.sin(numpy.deg2rad(z))
- Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
-
- # Return intrinsic rotation matrix
- return Rx.dot(Ry.dot(Rz))
+ new_dictionary = None
+ new_places = {}
with open(json_filepath) as configuration_file:
data = json.load(configuration_file)
- new_dictionary = data.pop('dictionary')
new_marker_size = data.pop('marker_size')
+ new_dictionary = data.pop('dictionary')
+ new_places = data.pop('places')
- for identifier, place in data.pop('places').items():
-
- # Convert string identifier to int value
- identifier = int(identifier)
-
- tvec = numpy.array(place.pop('translation')).astype(numpy.float32)
- rmat = __make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32)
-
- new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size)
-
- new_places[identifier] = Place(tvec, rmat, new_marker)
-
- return ArUcoScene(new_places, new_dictionary, new_marker_size)
-
- @property
- def marker_size(self) -> float:
- """Expected size of all markers in the scene."""
-
- return self.__marker_size
-
- @property
- def places(self) -> dict:
- """All places of the scene and their related ArUco markers."""
-
- return self.__places
-
- @places.setter
- def places(self, places: dict):
- """Initialize cached data to speed up further processings."""
-
- # Init places
- self.__places = places
-
- # Init pose data
- self._translation = numpy.zeros(3)
- self._rotation = numpy.zeros(3)
-
- # Process axis-angle between place combination to speed up further calculations
- self.__angle_cache = {}
- for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2):
-
- A = self.__places[A_identifier].rotation
- B = self.__places[B_identifier].rotation
-
- if numpy.array_equal(A, B):
-
- angle = 0.
-
- else:
-
- # Rotation matrix from A place to B place
- AB = B.dot(A.T)
-
- # Calculate axis-angle representation of AB rotation matrix
- angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
-
- try:
- self.__angle_cache[A_identifier][B_identifier] = angle
- except:
- self.__angle_cache[A_identifier] = {B_identifier: angle}
-
- try:
- self.__angle_cache[B_identifier][A_identifier] = angle
- except:
- self.__angle_cache[B_identifier] = {A_identifier: angle}
-
- # Process distance between each place combination to speed up further calculations
- self.__distance_cache = {}
- for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.__places.items(), 2):
-
- 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_identifier][B_identifier] = distance
- except:
- self.__distance_cache[A_identifier] = {B_identifier: distance}
-
- try:
- self.__distance_cache[B_identifier][A_identifier] = distance
- except:
- self.__distance_cache[B_identifier] = {A_identifier: distance}
+ return ArUcoScene(new_marker_size, new_dictionary, new_places)
def __str__(self) -> str:
"""String display"""
- output = f'\n\n\tDictionary: {self.__dictionary.name}'
+ output = f'\n\n\tDictionary: {self.dictionary.name}'
- output = f'\n\n\tMarker size: {self.__marker_size} cm'
+ output = f'\n\n\tMarker size: {self.marker_size} cm'
output += '\n\n\tPlaces:'
- for identifier, place in self.__places.items():
+ for identifier, place in self.places.items():
output += f'\n\t\t- {identifier}:'
output += f'\n{place.translation}'
output += f'\n{place.rotation}'
@@ -357,7 +342,7 @@ class ArUcoScene():
def identifiers(self) -> list:
"""List place maker identifiers belonging to the scene."""
- return list(self.__places.keys())
+ return list(self.places.keys())
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()).
@@ -372,7 +357,7 @@ class ArUcoScene():
for (marker_id, marker) in detected_markers.items():
- if marker_id in self.__places.keys():
+ if marker_id in self.places.keys():
scene_markers[marker_id] = marker
@@ -454,7 +439,7 @@ class ArUcoScene():
# Get the place related to the given marker
try:
- place = self.__places[marker.identifier]
+ place = self.places[marker.identifier]
# Rotation matrix that transform marker to related place
self._rotation = marker.rotation.dot(place.rotation.T)
@@ -478,7 +463,7 @@ class ArUcoScene():
try:
- place = self.__places[identifier]
+ place = self.places[identifier]
# Rotation matrix that transform marker to related place
R = marker.rotation.dot(place.rotation.T)
@@ -509,9 +494,9 @@ class ArUcoScene():
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]
+ 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
@@ -571,8 +556,8 @@ class ArUcoScene():
def draw_axis(self, frame, K, D, consistency=2):
"""Draw scene axis according a consistency score."""
- l = self.__marker_size / 2
- ll = self.__marker_size
+ l = self.marker_size / 2
+ ll = self.marker_size
# Select color according consistency score
n = 95 * consistency if consistency < 2 else 0
@@ -590,19 +575,19 @@ class ArUcoScene():
def draw_places(self, frame, K, D, consistency=2):
"""Draw scene places and their axis according a consistency score."""
- l = self.__marker_size / 2
- ll = self.__marker_size
+ l = self.marker_size / 2
+ ll = self.marker_size
# Select color according consistency score
n = 95 * consistency if consistency < 2 else 0
f = 159 * consistency if consistency < 2 else 255
- for identifier, place in self.__places.items():
+ for identifier, place in self.places.items():
try:
- T = self.__places[identifier].translation
- R = self.__places[identifier].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)