diff options
Diffstat (limited to 'src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py | 617 |
1 files changed, 307 insertions, 310 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py index a6f7b43..fd33664 100644 --- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py +++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py @@ -8,7 +8,7 @@ This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with -this program. If not, see <http://www.gnu.org/licenses/>. +this program. If not, see <https://www.gnu.org/licenses/>. """ __author__ = "Théo de la Hogue" @@ -33,447 +33,444 @@ T0 = numpy.array([0., 0., 0.]) R0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) """Define no rotation matrix.""" + 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 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 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]]) - # 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)) - # Return intrinsic rotation matrix - return Rx.dot(Ry.dot(Rz)) -def is_rotation_matrix(R): +def is_rotation_matrix(mat): + rt = numpy.transpose(mat) + should_be_identity = numpy.dot(rt, mat) + i = numpy.identity(3, dtype=mat.dtype) + n = numpy.linalg.norm(i - should_be_identity) - Rt = numpy.transpose(R) - shouldBeIdentity = numpy.dot(Rt, R) - I = numpy.identity(3, dtype = R.dtype) - n = numpy.linalg.norm(I - shouldBeIdentity) + return n < 1e-3 - return n < 1e-3 @dataclass(frozen=True) -class Place(): - """Define a place as list of corners position and a marker. +class Place: + """Define a place as list of corners position and a marker. + + Parameters: + corners: 3D corners position in group referential. + marker: ArUco marker linked to the place. + """ - Parameters: - corners: 3D corners position in group referential. - marker: ArUco marker linked to the place. - """ + corners: numpy.array + marker: ArUcoMarker.ArUcoMarker - corners: numpy.array - marker: ArUcoMarker.ArUcoMarker class ArUcoMarkersGroup(DataFeatures.PipelineStepObject): - """ - 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. + """ + + # noinspection PyMissingConstructor + @DataFeatures.PipelineStepInit + def __init__(self, **kwargs): + """Initialize ArUcoMarkersGroup""" + + # Init private attributes + self.marker_size = None + self.__dictionary = None + self.__places = {} + self.__translation = numpy.zeros(3) + self.__rotation = numpy.zeros(3) - # noinspection PyMissingConstructor - @DataFeatures.PipelineStepInit - def __init__(self, **kwargs): - """Initialize ArUcoMarkersGroup""" + @property + def dictionary(self) -> ArUcoMarkersDictionary.ArUcoMarkersDictionary: + """Expected dictionary of all markers in the group.""" + return self.__dictionary - # Init private attributes - self.marker_size = None - self.__dictionary = None - self.__places = {} - self.__translation = numpy.zeros(3) - self.__rotation = numpy.zeros(3) + @dictionary.setter + def dictionary(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary): - @property - def dictionary(self) -> ArUcoMarkersDictionary.ArUcoMarkersDictionary: - """Expected dictionary of all markers in the group.""" - return self.__dictionary + self.__dictionary = dictionary - @dictionary.setter - def dictionary(self, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary): + @property + def places(self) -> dict: + """Expected markers place.""" + return self.__places - self.__dictionary = dictionary - - @property - def places(self) -> dict: - """Expected markers place.""" - return self.__places + @places.setter + def places(self, places: dict): - @places.setter - def places(self, places: dict): + # Normalize places data + new_places = {} - # Normalize places data - new_places = {} + for identifier, data in places.items(): - for identifier, data in places.items(): + # Convert string identifier to int value + if type(identifier) is str: - # Convert string identifier to int value - if type(identifier) == str: + identifier = int(identifier) - identifier = int(identifier) + # Get translation vector + tvec = numpy.array(data.pop('translation')).astype(numpy.float32) - # Get translation vector - tvec = numpy.array(data.pop('translation')).astype(numpy.float32) + # Check rotation value shape + rvalue = numpy.array(data.pop('rotation')).astype(numpy.float32) - # Check rotation value shape - rvalue = numpy.array(data.pop('rotation')).astype(numpy.float32) + # Rotation matrix + if rvalue.shape == (3, 3): - # Rotation matrix - if rvalue.shape == (3, 3): + rmat = rvalue - rmat = rvalue + # Rotation vector (expected in degree) + elif rvalue.shape == (3,): - # Rotation vector (expected in degree) - elif rvalue.shape == (3,): + rmat = make_rotation_matrix(rvalue[0], rvalue[1], rvalue[2]).astype(numpy.float32) - rmat = make_rotation_matrix(rvalue[0], rvalue[1], rvalue[2]).astype(numpy.float32) + else: - else: + raise ValueError(f'Bad rotation value: {rvalue}') - raise ValueError(f'Bad rotation value: {rvalue}') + assert (is_rotation_matrix(rmat)) - assert(is_rotation_matrix(rmat)) + # Get marker size + size = float(numpy.array(data.pop('size')).astype(numpy.float32)) - # Get marker size - size = float(numpy.array(data.pop('size')).astype(numpy.float32)) + new_marker = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, size) - new_marker = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, size) + # Build marker corners thanks to translation vector and rotation matrix + place_corners = numpy.array([[-size / 2, size / 2, 0], [size / 2, size / 2, 0], [size / 2, -size / 2, 0], [-size / 2, -size / 2, 0]]) + place_corners = place_corners.dot(rmat) + tvec - # Build marker corners thanks to translation vector and rotation matrix - place_corners = numpy.array([[-size/2, size/2, 0], [size/2, size/2, 0], [size/2, -size/2, 0], [-size/2, -size/2, 0]]) - place_corners = place_corners.dot(rmat) + tvec + new_places[identifier] = Place(place_corners, new_marker) - new_places[identifier] = Place(place_corners, new_marker) + # else places are configured using detected markers estimated points + elif isinstance(data, ArUcoMarker.ArUcoMarker): - # else places are configured using detected markers estimated points - elif isinstance(data, ArUcoMarker.ArUcoMarker): + new_places[identifier] = Place(data.points, data) - new_places[identifier] = Place(data.points, data) + # else places are already at expected format + elif (type(identifier) is int) and isinstance(data, Place): - # else places are already at expected format - elif (type(identifier) == int) and isinstance(data, Place): + new_places[identifier] = data - new_places[identifier] = data + self.__places = new_places - self.__places = new_places + @property + def identifiers(self) -> list: + """List place marker identifiers belonging to the group.""" + return list(self.__places.keys()) - @property - def identifiers(self) -> list: - """List place marker identifiers belonging to the group.""" - return list(self.__places.keys()) + @property + def translation(self) -> numpy.array: + """Get ArUco marker group translation vector.""" + return self.__translation - @property - def translation(self) -> numpy.array: - """Get ArUco marker group translation vector.""" - return self.__translation + @translation.setter + def translation(self, tvec): + """Set ArUco marker group translation vector.""" + self.__translation = tvec - @translation.setter - def translation(self, tvec): - """Set ArUco marker group translation vector.""" - self.__translation = tvec + @property + def rotation(self) -> numpy.array: + """Get ArUco marker group rotation matrix.""" + return self.__translation - @property - def rotation(self) -> numpy.array: - """Get ArUco marker group rotation matrix.""" - return self.__translation + @rotation.setter + def rotation(self, rmat): + """Set ArUco marker group rotation matrix.""" + self.__rotation = rmat - @rotation.setter - def rotation(self, rmat): - """Set ArUco marker group rotation matrix.""" - self.__rotation = rmat + def as_dict(self) -> dict: + """Export ArUco marker group properties as dictionary.""" - def as_dict(self) -> dict: - """Export ArUco marker group properties as dictionary.""" + return { + **DataFeatures.PipelineStepObject.as_dict(self), + "dictionary": self.__dictionary, + "places": self.__places + } - return { - **DataFeatures.PipelineStepObject.as_dict(self), - "dictionary": self.__dictionary, - "places": self.__places - } - - @classmethod - def from_obj(cls, obj_filepath: str) -> Self: - """Load ArUco markers group from .obj file. + @classmethod + def from_obj(cls, obj_filepath: str) -> Self: + """Load ArUco markers group from .obj file. - !!! note - Expected object (o) name format: <DICTIONARY>#<IDENTIFIER>_Marker + !!! note + Expected object (o) name format: <DICTIONARY>#<IDENTIFIER>_Marker - !!! note - All markers have to belong to the same dictionary. + !!! note + All markers have to belong to the same dictionary. - """ + """ - new_dictionary = None - new_places = {} - - # Regex rules for .obj file parsing - OBJ_RX_DICT = { - 'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'), - 'vertices': re.compile(r'v ([+-]?[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 - } + new_dictionary = None + new_places = {} - # Regex .obj line parser - def __parse_obj_line(line): + # Regex rules for .obj file parsing + obj_rx_dict = { + 'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'), + 'vertices': re.compile(r'v ([+-]?[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 + } - for key, rx in OBJ_RX_DICT.items(): - match = rx.search(line) - if match: - return key, match + # Regex .obj line parser + def __parse_obj_line(ln): - # If there are no matches - return None, None - - # Start parsing - try: + for k, rx in obj_rx_dict.items(): + m = rx.search(ln) + if m: + return k, m - identifier = None - vertices = [] - faces = {} + # If there are no matches + return None, None - # Open the file and read through it line by line - with open(obj_filepath, 'r') as file: + # Start parsing + try: - line = file.readline() + identifier = None + vertices = [] + faces = {} - while line: + # Open the file and read through it line by line + with open(obj_filepath, 'r') as file: - # At each line check for a match with a regex - key, match = __parse_obj_line(line) + line = file.readline() - # Extract comment - if key == 'comment': - pass + while line: - # Extract marker dictionary and identifier - elif key == 'object': + # At each line check for a match with a regex + key, match = __parse_obj_line(line) - dictionary = str(match.group(1)) - identifier = int(match.group(2)) - last = str(match.group(3)) + # Extract comment + if key == 'comment': + pass - # Init new group dictionary with first dictionary name - if new_dictionary == None: + # Extract marker dictionary and identifier + elif key == 'object': - new_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(dictionary) + dictionary = str(match.group(1)) + identifier = int(match.group(2)) - # Check all others marker dictionary are equal to new group dictionary - elif dictionary != new_dictionary.name: + # Init new group dictionary with first dictionary name + if new_dictionary is None: - raise NameError(f'Marker {identifier} dictionary is not {new_dictionary.name}') + new_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(dictionary) - # Fill vertices array - elif key == 'vertices': + # Check all others marker dictionary are equal to new group dictionary + elif dictionary != new_dictionary.name: - vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])) + raise NameError(f'Marker {identifier} dictionary is not {new_dictionary.name}') - # Extract vertices ids - elif key == 'face': + # Fill vertices array + elif key == 'vertices': - faces[identifier] = [int(match.group(1)), int(match.group(2)), int(match.group(3)), int(match.group(4))] + vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])) - # Go to next line - line = file.readline() + # Extract vertices ids + elif key == 'face': - file.close() + faces[identifier] = [int(match.group(1)), int(match.group(2)), int(match.group(3)), int(match.group(4))] - # Retrieve marker vertices thanks to face vertices ids - for identifier, face in faces.items(): + # Go to next line + line = file.readline() - # Gather place corners in clockwise order - cw_corners = numpy.array([ vertices[i-1] for i in reversed(face) ]) + file.close() - # Edit place axis from corners positions - place_x_axis = cw_corners[2] - cw_corners[3] - place_x_axis_norm = numpy.linalg.norm(place_x_axis) - - place_y_axis = cw_corners[0] - cw_corners[3] - place_y_axis_norm = numpy.linalg.norm(place_y_axis) + # Retrieve marker vertices thanks to face vertices ids + for identifier, face in faces.items(): - # Check axis size: they should be almost equal - if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3): + # Gather place corners in clockwise order + cw_corners = numpy.array([vertices[i - 1] for i in reversed(face)]) - new_marker_size = place_x_axis_norm + # Edit place axis from corners positions + place_x_axis = cw_corners[2] - cw_corners[3] + place_x_axis_norm = numpy.linalg.norm(place_x_axis) - else: + place_y_axis = cw_corners[0] - cw_corners[3] + place_y_axis_norm = numpy.linalg.norm(place_y_axis) - raise ValueError(f'{new_dictionary}#{identifier}_Marker is not a square.') + # Check axis size: they should be almost equal + if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3): - # Create a new place related to a new marker - new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) - new_places[identifier] = Place(cw_corners, new_marker) + new_marker_size = place_x_axis_norm - except IOError: - raise IOError(f'File not found: {obj_filepath}') + else: - # Instantiate ArUco markers group - data = { - 'dictionary': new_dictionary, - 'places': new_places - } + raise ValueError(f'{new_dictionary}#{identifier}_Marker is not a square.') - return ArUcoMarkersGroup(**data) + # Create a new place related to a new marker + new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) + new_places[identifier] = Place(cw_corners, new_marker) - def filter_markers(self, detected_markers: dict) -> tuple[dict, dict]: - """Sort markers belonging to the group from given detected markers dict (cf ArUcoDetector.detect_markers()). + except IOError: + raise IOError(f'File not found: {obj_filepath}') - Returns: - dict of markers belonging to this group - dict of remaining markers not belonging to this group - """ + # Instantiate ArUco markers group + data = { + 'dictionary': new_dictionary, + 'places': new_places + } - group_markers = {} - remaining_markers = {} + return ArUcoMarkersGroup(**data) - for (marker_id, marker) in detected_markers.items(): + def filter_markers(self, detected_markers: dict) -> tuple[dict, dict]: + """Sort markers belonging to the group from given detected markers dict (cf ArUcoDetector.detect_markers()). - if marker_id in self.__places.keys(): + Returns: + dict of markers belonging to this group + dict of remaining markers not belonging to this group + """ - group_markers[marker_id] = marker + group_markers = {} + remaining_markers = {} - else: - - remaining_markers[marker_id] = marker + for (marker_id, marker) in detected_markers.items(): - return group_markers, remaining_markers + if marker_id in self.__places.keys(): - 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. + group_markers[marker_id] = marker - Parameters: - markers: detected markers to use for pose estimation. - K: intrinsic camera parameters - D: camera distorsion matrix + else: - Returns: - success: True if the pose estimation succeeded - tvec: scene translation vector - rvec: scene rotation vector - """ + remaining_markers[marker_id] = marker - markers_corners_2d = [] - places_corners_3d = [] + return group_markers, remaining_markers - for identifier, marker in markers.items(): + 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: + Parameters: + markers: detected markers to use for pose estimation. + k: intrinsic camera parameters + d: camera distortion matrix - place = self.__places[identifier] + Returns: + success: True if the pose estimation succeeded + tvec: scene translation vector + rvec: scene rotation vector + """ - for marker_corner in marker.corners: - markers_corners_2d.append(list(marker_corner)) + markers_corners_2d = [] + places_corners_3d = [] - for place_corner in place.corners: - places_corners_3d.append(list(place_corner)) + for identifier, marker in markers.items(): - except KeyError: + try: - raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.') + place = self.__places[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) + for marker_corner in marker.corners: + markers_corners_2d.append(list(marker_corner)) - # Refine pose estimation using Gauss-Newton optimisation - if success : + for place_corner in place.corners: + places_corners_3d.append(list(place_corner)) - rvec, tvec = cv2.solvePnPRefineVVS(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), rvec, tvec) + except KeyError: - self.__translation = tvec.T - self.__rotation = rvec.T + raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.') - return success, self.__translation, self.__rotation + # 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) - def draw_axes(self, image: numpy.array, K, D, thickness: int = 0, length: float = 0): - """Draw group axes.""" + # Refine pose estimation using Gauss-Newton optimisation + if success: + rvec, tvec = cv2.solvePnPRefineVVS(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(k), numpy.array(d), rvec, tvec) - try: - axisPoints = numpy.float32([[length, 0, 0], [0, length, 0], [0, 0, length], [0, 0, 0]]).reshape(-1, 3) - axisPoints, _ = cv2.projectPoints(axisPoints, self.__rotation, self.__translation, numpy.array(K), numpy.array(D)) - axisPoints = axisPoints.astype(int) + self.__translation = tvec.T + self.__rotation = rvec.T - 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) + return success, self.__translation, self.__rotation - # Ignore errors due to out of field axis: their coordinate are larger than int32 limitations. - except cv2.error: - pass + def draw_axes(self, image: numpy.array, k: numpy.array, d: numpy.array, thickness: int = 0, length: float = 0): + """Draw group axes.""" - def draw_places(self, image: numpy.array, K, D, color: tuple = None, border_size: int = 0): - """Draw group places.""" + try: + axis_points = numpy.float32([[length, 0, 0], [0, length, 0], [0, 0, length], [0, 0, 0]]).reshape(-1, 3) + axis_points, _ = cv2.projectPoints(axis_points, self.__rotation, self.__translation, numpy.array(k), numpy.array(d)) + axis_points = axis_points.astype(int) - l = self.marker_size / 2 + cv2.line(image, tuple(axis_points[3].ravel()), tuple(axis_points[0].ravel()), (0, 0, 255), thickness) # X (red) + cv2.line(image, tuple(axis_points[3].ravel()), tuple(axis_points[1].ravel()), (0, 255, 0), thickness) # Y (green) + cv2.line(image, tuple(axis_points[3].ravel()), tuple(axis_points[2].ravel()), (255, 0, 0), thickness) # Z (blue) - for identifier, place in self.__places.items(): + # Ignore errors due to out of field axis: their coordinate are larger than int32 limitations. + except cv2.error: + pass - try: + def draw_places(self, image: numpy.array, k: numpy.array, d: numpy.array, color: tuple = None, border_size: int = 0): + """Draw group places.""" - placePoints, _ = cv2.projectPoints(place.corners, self.__rotation, self.__translation, numpy.array(K), numpy.array(D)) - placePoints = placePoints.astype(int) - - 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) + for identifier, place in self.__places.items(): - # Ignore errors due to out of field places: their coordinate are larger than int32 limitations. - except cv2.error: - pass + try: - def draw(self, image: numpy.array, K: numpy.array, D: numpy.array, draw_axes: dict = None, draw_places: dict = None): - """Draw group axes and places. - - Parameters: - image: where to draw. - K: - D: - draw_axes: draw_axes parameters (if None, no axes drawn) - draw_places: draw_places parameters (if None, no places drawn) - """ + place_points, _ = cv2.projectPoints(place.corners, self.__rotation, self.__translation, numpy.array(k), numpy.array(d)) + place_points = place_points.astype(int) - # Draw axes if required - if draw_axes is not None: + cv2.line(image, tuple(place_points[0].ravel()), tuple(place_points[1].ravel()), color, border_size) + cv2.line(image, tuple(place_points[1].ravel()), tuple(place_points[2].ravel()), color, border_size) + cv2.line(image, tuple(place_points[2].ravel()), tuple(place_points[3].ravel()), color, border_size) + cv2.line(image, tuple(place_points[3].ravel()), tuple(place_points[0].ravel()), color, border_size) - self.draw_axes(image, K, D, **draw_axes) + # Ignore errors due to out of field places: their coordinate are larger than int32 limitations. + except cv2.error: + pass - # Draw places if required - if draw_places is not None: + def draw(self, image: numpy.array, k: numpy.array, d: numpy.array, draw_axes: dict = None, draw_places: dict = None): + """Draw group axes and places. + + Parameters: + image: where to draw. + k: intrinsic camera parameters + d: camera distortion matrix + draw_axes: draw_axes parameters (if None, no axes drawn) + draw_places: draw_places parameters (if None, no places drawn) + """ - self.draw_places(image, K, D, **draw_places) + # Draw axes if required + if draw_axes is not None: + self.draw_axes(image, k, d, **draw_axes) - def to_obj(self, obj_filepath): - """Save group to .obj file.""" + # Draw places if required + if draw_places is not None: + self.draw_places(image, k, d, **draw_places) - with open(obj_filepath, 'w', encoding='utf-8') as file: + def to_obj(self, obj_filepath): + """Save group to .obj file.""" - file.write('# ArGaze OBJ File\n') - file.write('# http://achil.recherche.enac.fr/features/eye/argaze/\n') + with open(obj_filepath, 'w', encoding='utf-8') as file: - v_count = 0 + file.write('# ArGaze OBJ File\n') + file.write('# https://achil.recherche.enac.fr/features/eye/argaze/\n') - for p, (identifier, place) in enumerate(self.__places.items()): + v_count = 0 - file.write(f'o {self.__dictionary.name}#{identifier}_Marker\n') + for p, (identifier, place) in enumerate(self.__places.items()): - vertices = '' + file.write(f'o {self.__dictionary.name}#{identifier}_Marker\n') - # Write vertices in reverse order - for v in [3, 2, 1, 0]: + vertices = '' - file.write(f'v {" ".join(map(str, place.corners[v]))}\n') - v_count += 1 + # Write vertices in reverse order + for v in [3, 2, 1, 0]: + file.write(f'v {" ".join(map(str, place.corners[v]))}\n') + v_count += 1 - vertices += f' {v_count}' + vertices += f' {v_count}' - #file.write('s off\n') - file.write(f'f{vertices}\n') + # file.write('s off\n') + file.write(f'f{vertices}\n') |