diff options
Diffstat (limited to 'src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py | 95 |
1 files changed, 59 insertions, 36 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py index bdcf70c..5b6c69d 100644 --- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py +++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py @@ -617,40 +617,26 @@ class ArUcoMarkersGroup(): self._rotation = rmat - def draw_axis(self, image: numpy.array, K, D, consistency=2): - """Draw group axis according a consistency score.""" - - 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 + def draw_axes(self, image: numpy.array, K, D, thickness: int = 0, length: float = 0): + """Draw group axes.""" try: - - # Draw axis - axisPoints = numpy.float32([[ll, 0, 0], [0, ll, 0], [0, 0, ll], [0, 0, 0]]).reshape(-1, 3) + 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 = axisPoints.astype(int) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 6) # X (red) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue) + 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 axis: their coordinate are larger than int32 limitations. except cv.error: pass - def draw_places(self, image: numpy.array, K, D, consistency=2): - """Draw group places and their axis according a consistency score.""" + def draw_places(self, image: numpy.array, K, D, color: tuple = None, border_size: int = 0): + """Draw group places.""" 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(): @@ -659,29 +645,66 @@ class ArUcoMarkersGroup(): 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) - 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()), (n,n,f), 6) # X (red) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue) - - # Draw place 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 = placePoints.astype(int) - cv.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), (f,f,f), 3) - cv.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), (f,f,f), 3) - cv.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), (f,f,f), 3) - cv.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), (f,f,f), 3) + 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) + + # Ignore errors due to out of field places: their coordinate are larger than int32 limitations. + except cv.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): + """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 + if draw_axes is not None: + + self.draw_axes(image, K, D, **draw_axes) + + # Draw places if required + if draw_places is not None: + + 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.""" |