aboutsummaryrefslogtreecommitdiff
path: root/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
diff options
context:
space:
mode:
Diffstat (limited to 'src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py')
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py412
1 files changed, 76 insertions, 336 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
index 5b6c69d..37bceec 100644
--- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
+++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
@@ -17,8 +17,7 @@ import re
from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator
import numpy
-import cv2 as cv
-import cv2.aruco as aruco
+import cv2
T0 = numpy.array([0., 0., 0.])
"""Define no translation vector."""
@@ -58,50 +57,31 @@ def is_rotation_matrix(R):
return n < 1e-3
-def make_euler_rotation_vector(R):
-
- assert(is_rotation_matrix(R))
-
- sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
-
- singular = sy < 1e-6
-
- if not singular :
- x = math.atan2(R[2,1] , R[2,2])
- y = math.atan2(-R[2,0], sy)
- z = math.atan2(R[1,0], R[0,0])
- else :
- x = math.atan2(-R[1,2], R[1,1])
- y = math.atan2(-R[2,0], sy)
- z = 0
-
- return numpy.array([numpy.rad2deg(x), numpy.rad2deg(y), numpy.rad2deg(z)])
-
@dataclass(frozen=True)
class Place():
- """Define a place as a pose and a marker."""
-
- translation: numpy.array
- """Position in group referential."""
+ """Define a place as list of corners position and a marker.
- rotation: numpy.array
- """Rotation in group referential."""
+ Parameters:
+ corners: 3D corners position in group referential.
+ marker: ArUco marker linked to the place.
+ """
+ corners: numpy.array
marker: dict
- """ArUco marker linked to the place."""
@dataclass
class ArUcoMarkersGroup():
- """Handle group of ArUco markers as one unique spatial entity and estimate its pose."""
+ """Handle group of ArUco markers as one unique spatial entity and estimate its pose.
- marker_size: float = field(default=0.)
- """Expected size of all markers in the group."""
+ Parameters:
+ marker_size: expected size of all markers in the group.
+ dictionary: expected dictionary of all markers in the group.
+ places: expected markers place.
+ """
+ marker_size: float = field(default=0.)
dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary)
- """Expected dictionary of all markers in the group."""
-
places: dict = field(default_factory=dict)
- """Expected markers place"""
def __post_init__(self):
"""Init group pose and places pose."""
@@ -144,12 +124,16 @@ class ArUcoMarkersGroup():
new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size)
- new_places[identifier] = Place(tvec, rmat, new_marker)
+ # Build marker corners thanks to translation vector and rotation matrix
+ place_corners = numpy.array([[-self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, -self.marker_size/2, 0], [-self.marker_size/2, -self.marker_size/2, 0]])
+ place_corners = place_corners.dot(rmat) + tvec
+
+ new_places[identifier] = Place(place_corners, new_marker)
- # else places are configured using detected markers
+ # else places are configured using detected markers estimated points
elif isinstance(data, ArUcoMarker.ArUcoMarker):
- new_places[identifier] = Place(data.translation, data.rotation, data)
+ new_places[identifier] = Place(data.points, data)
# else places are already at expected format
elif (type(identifier) == int) and isinstance(data, Place):
@@ -158,21 +142,15 @@ class ArUcoMarkersGroup():
self.places = new_places
- # Init place consistency
- self.init_places_consistency()
-
@classmethod
def from_obj(self, obj_filepath: str) -> ArUcoMarkersGroupType:
"""Load ArUco markers group from .obj file.
!!! note
- Expected object (o) name format: <DICTIONARY>#<IDENTIFIER>_Marker
+ Expected object (o) name format: <DICTIONARY>#<IDENTIFIER>_Marker
!!! note
- All markers have to belong to the same dictionary.
-
- !!! note
- Marker normal vectors (vn) expected.
+ All markers have to belong to the same dictionary.
"""
@@ -184,8 +162,7 @@ class ArUcoMarkersGroup():
OBJ_RX_DICT = {
'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'),
'vertice': re.compile(r'v ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'),
- 'normal': re.compile(r'vn ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'),
- 'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n'),
+ 'face': re.compile(r'f ([0-9]+) ([0-9]+) ([0-9]+) ([0-9]+)\n'),
'comment': re.compile(r'#(.*)\n') # keep comment regex after object regex because the # is used in object string too
}
@@ -205,7 +182,6 @@ class ArUcoMarkersGroup():
identifier = None
vertices = []
- normals = {}
faces = {}
# Open the file and read through it line by line
@@ -244,15 +220,10 @@ class ArUcoMarkersGroup():
vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]))
- # Extract normal to calculate rotation matrix
- elif key == 'normal':
-
- normals[identifier] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])
-
# Extract vertice ids
elif key == 'face':
- faces[identifier] = [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(2)), int(match.group(3)), int(match.group(4))]
# Go to next line
line = file.readline()
@@ -262,32 +233,20 @@ class ArUcoMarkersGroup():
# Retreive marker vertices thanks to face vertice ids
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 ])
-
- # Edit translation (Tp) allowing to move world axis (W) at place axis (P)
- Tp = corners.mean(axis=0)
+ # Gather place corners in clockwise order
+ cw_corners = numpy.array([ vertices[i-1] for i in reversed(face) ])
# Edit place axis from corners positions
- place_x_axis = corners[1:3].mean(axis=0) - Tp
+ place_x_axis = cw_corners[2] - cw_corners[3]
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 = cw_corners[0] - cw_corners[3]
place_y_axis_norm = numpy.linalg.norm(place_y_axis)
- place_y_axis = place_y_axis / place_y_axis_norm
- 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)
-
# 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
+ current_marker_size = place_x_axis_norm
# Check that all markers size are almost equal
if new_marker_size > 0:
@@ -300,7 +259,7 @@ class ArUcoMarkersGroup():
# Create a new place related to a new marker
new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size)
- new_places[identifier] = Place(Tp, Rp, new_marker)
+ new_places[identifier] = Place(cw_corners, new_marker)
except IOError:
raise IOError(f'File not found: {obj_filepath}')
@@ -335,18 +294,7 @@ class ArUcoMarkersGroup():
output += '\n\n\tPlaces:'
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\tAngle cache:'
- for A_identifier, A_angle_cache in self.__rotation_cache.items():
- for B_identifier, angle in A_angle_cache.items():
- output += f'\n\t\t- {A_identifier}/{B_identifier}: [{angle[0]:3f} {angle[1]:3f} {angle[2]:3f}]'
-
- output += '\n\n\tDistance cache:'
- for A_identifier, A_distance_cache in self.__translation_cache.items():
- for B_identifier, distance in A_distance_cache.items():
- output += f'\n\t\t- {A_identifier}/{B_identifier}: {distance:3f}'
+ output += f'\n{place.corners}'
return output
@@ -360,8 +308,8 @@ class ArUcoMarkersGroup():
"""Sort markers belonging to the group from given detected markers dict (cf ArUcoDetector.detect_markers()).
Returns:
- dict of markers belonging to this group
- dict of remaining markers not belonging to this group
+ dict of markers belonging to this group
+ dict of remaining markers not belonging to this group
"""
group_markers = {}
@@ -379,148 +327,22 @@ class ArUcoMarkersGroup():
return group_markers, remaining_markers
- def init_places_consistency(self):
- """Initialize places consistency to speed up further markers consistency checking."""
-
- # Process expected rotation between places combinations to speed up further calculations
- self.__rotation_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):
-
- AB_rvec = [0., 0., 0.]
- BA_rvec = [0., 0., 0.]
-
- else:
-
- # Calculate euler angle representation of AB and BA rotation matrix
- AB_rvec = make_euler_rotation_vector(B.dot(A.T))
- BA_rvec = make_euler_rotation_vector(A.dot(B.T))
-
- try:
- self.__rotation_cache[A_identifier][B_identifier] = AB_rvec
- except:
- self.__rotation_cache[A_identifier] = {B_identifier: AB_rvec}
-
- try:
- self.__rotation_cache[B_identifier][A_identifier] = BA_rvec
- except:
- self.__rotation_cache[B_identifier] = {A_identifier: BA_rvec}
-
- # Process translation between each places combinations to speed up further calculations
- self.__translation_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 translation between A and B position
- AB_tvec = numpy.linalg.norm(B - A)
-
- try:
- self.__translation_cache[A_identifier][B_identifier] = AB_tvec
- except:
- self.__translation_cache[A_identifier] = {B_identifier: AB_tvec}
+ def estimate_pose_from_markers_corners(self, markers: dict, K: numpy.array, D: numpy.array) -> Tuple[bool, numpy.array, numpy.array]:
+ """Estimate pose from markers corners and places corners.
- try:
- self.__translation_cache[B_identifier][A_identifier] = AB_tvec
- except:
- self.__translation_cache[B_identifier] = {A_identifier: AB_tvec}
-
- def check_markers_consistency(self, group_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict, dict]:
- """Evaluate if given markers configuration match related places configuration.
+ Parameters:
+ markers: detected markers to use for pose estimation.
+ K: intrinsic camera parameters
+ D: camera distorsion matrix
Returns:
- dict of consistent markers
- dict of unconsistent markers
- dict of identified distance or angle unconsistencies and out-of-bounds values
+ success: True if the pose estimation succeeded
+ tvec: scene translation vector
+ rvec: scene rotation vector
"""
- consistent_markers = {}
- unconsistencies = {'rotation': {}, 'translation': {}}
-
- for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(group_markers.items(), 2):
-
- try:
-
- # Rotation matrix from A marker to B marker
- AB = B_marker.rotation.dot(A_marker.rotation.T)
-
- # Calculate euler angle representation of AB rotation matrix
- AB_rvec = make_euler_rotation_vector(AB)
- expected_rvec= self.__rotation_cache[A_identifier][B_identifier]
-
- # Calculate distance between A marker center and B marker center
- AB_tvec = numpy.linalg.norm(A_marker.translation - B_marker.translation)
- expected_tvec = self.__translation_cache[A_identifier][B_identifier]
-
- # Check angle and distance according given tolerance then normalise marker pose
- consistent_rotation = numpy.allclose(AB_rvec, expected_rvec, atol=angle_tolerance)
- consistent_translation = math.isclose(AB_tvec, expected_tvec, abs_tol=distance_tolerance)
-
- if consistent_rotation and consistent_translation:
-
- if A_identifier not in consistent_markers.keys():
-
- # Remember this marker is already validated
- consistent_markers[A_identifier] = A_marker
-
- if B_identifier not in consistent_markers.keys():
-
- # Remember this marker is already validated
- consistent_markers[B_identifier] = B_marker
-
- else:
-
- if not consistent_rotation:
- unconsistencies['rotation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_rvec, 'expected': expected_rvec}
-
- if not consistent_translation:
- unconsistencies['translation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_tvec, 'expected': expected_tvec}
-
- except KeyError:
-
- raise ValueError(f'Marker {A_identifier} or {B_identifier} don\'t belong to the group.')
-
- # Gather unconsistent markers
- unconsistent_markers = {}
-
- for identifier, marker in group_markers.items():
-
- if identifier not in consistent_markers.keys():
-
- unconsistent_markers[identifier] = marker
-
- return consistent_markers, unconsistent_markers, unconsistencies
-
- 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 group.')
-
- 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 = []
+ markers_corners_2d = []
+ places_corners_3d = []
for identifier, marker in markers.items():
@@ -528,72 +350,30 @@ class ArUcoMarkersGroup():
place = self.places[identifier]
- # Rotation matrix that transform marker to related place
- R = marker.rotation.dot(place.rotation.T)
+ for marker_corner in marker.corners:
+ markers_corners_2d.append(list(marker_corner))
- # 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)
+ for place_corner in place.corners:
+ places_corners_3d.append(list(place_corner))
except KeyError:
raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.')
- # Consider ArUcoMarkersGroup 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 ArUcoMarkersGroup translation as the mean of all marker translations
- self._translation = numpy.mean(numpy.array(translations), axis=0)
-
- return self._translation, self._rotation
-
- 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_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]
+ # SolvPnP using cv2.SOLVEPNP_SQPNP flag
+ # TODO: it works also with cv2.SOLVEPNP_EPNP flag so we need to test which is the faster.
+ # About SolvPnP flags: https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html
+ success, rvec, tvec = cv2.solvePnP(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), flags=cv2.SOLVEPNP_SQPNP)
- # Place axis
- OA = A_place.translation - O_place.translation
- OA = OA / numpy.linalg.norm(OA)
+ # Refine pose estimation using Gauss-Newton optimisation
+ if success :
- OB = B_place.translation - O_place.translation
- OB = OB / numpy.linalg.norm(OB)
+ rvec, tvec = cv2.solvePnPRefineVVS(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), rvec, tvec)
- # Detect and correct bad place axis orientation
- X_sign = numpy.sign(OA)[0]
- Y_sign = numpy.sign(OB)[1]
+ self._translation = tvec.T
+ self._rotation = rvec.T
- P = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)])
-
- # Marker axis
- OA = A_marker.translation - O_marker.translation
- OA = OA / numpy.linalg.norm(OA)
-
- OB = B_marker.translation - O_marker.translation
- OB = OB / numpy.linalg.norm(OB)
-
- # Detect and correct bad place axis orientation
- X_sign = numpy.sign(OA)[0]
- Y_sign = -numpy.sign(OB)[1]
-
- M = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)])
-
- # Then estimate ArUcoMarkersGroup rotation
- self._rotation = P.dot(M.T)
-
- # Consider ArUcoMarkersGroup translation as the translation of the marker at axis origin
- self._translation = O_marker.translation - O_place.translation.dot(O_place.rotation).dot(M.T)
-
- return self._translation, self._rotation
+ return success, self._translation, self._rotation
@property
def translation(self) -> numpy.array:
@@ -622,15 +402,15 @@ class ArUcoMarkersGroup():
try:
axisPoints = numpy.float32([[length, 0, 0], [0, length, 0], [0, 0, length], [0, 0, 0]]).reshape(-1, 3)
- axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
+ axisPoints, _ = cv2.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
axisPoints = axisPoints.astype(int)
- cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red)
- cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green)
- cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue)
+ cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red)
+ cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green)
+ cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue)
# Ignore errors due to out of field axis: their coordinate are larger than int32 limitations.
- except cv.error:
+ except cv2.error:
pass
def draw_places(self, image: numpy.array, K, D, color: tuple = None, border_size: int = 0):
@@ -642,52 +422,24 @@ class ArUcoMarkersGroup():
try:
- T = self.places[identifier].translation
- R = self.places[identifier].rotation
-
- placePoints = (T + numpy.float32([R.dot([-l, -l, 0]), R.dot([l, -l, 0]), R.dot([l, l, 0]), R.dot([-l, l, 0])])).reshape(-1, 3)
- placePoints, _ = cv.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
+ placePoints, _ = cv2.projectPoints(place.corners, self._rotation, self._translation, numpy.array(K), numpy.array(D))
placePoints = placePoints.astype(int)
- cv.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), color, border_size)
- cv.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), color, border_size)
- cv.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), color, border_size)
- cv.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), color, border_size)
+ cv2.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), color, border_size)
+ cv2.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), color, border_size)
+ cv2.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), color, border_size)
+ cv2.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), color, border_size)
# Ignore errors due to out of field places: their coordinate are larger than int32 limitations.
- except cv.error:
+ except cv2.error:
pass
- def draw_places_axes(self, image: numpy.array, K, D, thickness: int = 0, length: float = 0):
- """Draw group place axes."""
-
- for identifier, place in self.places.items():
-
- try:
-
- T = self.places[identifier].translation
- R = self.places[identifier].rotation
-
- axisPoints = (T + numpy.float32([R.dot([length, 0, 0]), R.dot([0, length, 0]), R.dot([0, 0, length]), R.dot([0, 0, 0])])).reshape(-1, 3)
- axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D))
- axisPoints = axisPoints.astype(int)
-
- cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red)
- cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green)
- cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue)
-
- # Ignore errors due to out of field places: their coordinate are larger than int32 limitations.
- except cv.error:
- pass
-
- def draw(self, image: numpy.array, K, D, draw_axes: dict = None, draw_places: dict = None, draw_places_axes: dict = None):
+ def draw(self, image: numpy.array, K, D, draw_axes: dict = None, draw_places: dict = None):
"""Draw group axes and places.
Parameters:
-
draw_axes: draw_axes parameters (if None, no axes drawn)
draw_places: draw_places parameters (if None, no places drawn)
- draw_places_axes: draw_places_axes parameters (if None, no places axes drawn)
"""
# Draw axes if required
@@ -700,11 +452,6 @@ class ArUcoMarkersGroup():
self.draw_places(image, K, D, **draw_places)
- # Draw places axes if required
- if draw_places_axes is not None:
-
- self.draw_places_axes(image, K, D, **draw_places_axes)
-
def to_obj(self, obj_filepath):
"""Save group to .obj file."""
@@ -715,26 +462,19 @@ class ArUcoMarkersGroup():
v_count = 0
- for identifier, place in self.places.items():
+ for p, (identifier, place) in enumerate(self.places.items()):
file.write(f'o {self.dictionary.name}#{identifier}_Marker\n')
vertices = ''
- T = place.translation
- R = place.rotation
-
- points = (T + numpy.float32([R.dot(place.marker.points[0]), R.dot(place.marker.points[1]), R.dot(place.marker.points[2]), R.dot(place.marker.points[3])])).reshape(-1, 3)
-
- print(points)
-
# Write vertices in reverse order
- for i in [3, 2, 1, 0]:
+ for v in [3, 2, 1, 0]:
- file.write(f'v {" ".join(map(str, points[i]))}\n')
+ file.write(f'v {" ".join(map(str, place.corners[v]))}\n')
v_count += 1
vertices += f' {v_count}'
- file.write('s off\n')
+ #file.write('s off\n')
file.write(f'f{vertices}\n')