Pushing 2020 code from Jetson

Author: EliVerbrugge

.gitignore

@@ -0,0 +1,14 @@
+# PyCharm files
+.idea/
+
+# Mac Temp Files
+.DS_Store
+
+# Python temp files
+*.pyc
+
+#Our Log Files
+*/logs/
+logs/
+*.log
+*.avi

algorithms/ArTag.py

@@ -0,0 +1,67 @@
+import numpy as np
+import cv2
+from ArTagEncoding import encode, HAMMINGCODE_MARKER_POSITIONS
+MARKER_SIZE = 9
+
+class HammingMarker(object):
+    def __init__(self, id, contours=None):
+        self.id = id
+        self.contours = contours
+
+    def __repr__(self):
+        return '<Marker id = {} center = {} >'.format(self.id, self.center)
+
+    @property
+    def center(self):
+        if self.contours is None:
+            return None
+        center_array = np.mean(self.contours, axis=0).flatten()
+        return (int(center_array[0]), int(center_array[1]))
+
+    def generate_image(self):
+        img = np.zeros((MARKER_SIZE, MARKER_SIZE))
+        #img[1, 1] = 255
+        for count1, ar in enumerate(self.id):
+            for count2, ar2 in enumerate(ar):
+                img[count1+1][count2+1] = self.id[count1][count2]
+        # return zoom(img, zoom=50, order=0)
+        height,width = img.shape[:2]
+        res = cv2.resize(img, (50*width, 50*height), interpolation = cv2.INTER_NEAREST)
+        #cv2.imshow("test1", res)
+        return res
+
+    def draw_contour(self, img, color=(0,255,0), linewidth=5):
+        cv2.drawContours(img, [self.contours], -1, color, linewidth)
+
+    def highlite_marker(self, img, contour_color=(0,255,0), text_color=(255,0,0), linewidth=5, text_thickness=2):
+        """
+        This draws a bounding box around the marker on the image. NOTE: it returns
+        a BGR image so the highlite is in color.
+
+        Input:
+          img: image with detected marker
+          contour_color: bounding box color, default is Green (0,255,0)
+          text_color: text color, default is Blue (255,0,0)
+          linewidth: thickness of bonding box line
+          text_thickness: thickness of marker number text
+
+        Output:
+          A color image with the marker drawn on it
+        """
+        if len(img.shape) == 2:
+            img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
+        self.draw_contour(img, color=contour_color, linewidth=linewidth)
+        cv2.putText(img, str(self.id), self.center, cv2.FONT_HERSHEY_SIMPLEX, text_thickness, text_color)
+        return img
+
+    @classmethod
+    def generate(cls):
+        #return HammingMarker(id=np.random.randint(4096))
+        return HammingMarker(id=[[255,255,0,255,255],[255,255,0,255,255],[255,0,255,0,255],[255,255,255,255,255],[255,255,255,255,255]])
+    @property
+    def id_as_binary(self):
+        return np.binary_repr(self.id, width=12)
+
+    @property
+    def hamming_code(self):
+        return encode(self.id_as_binary)

algorithms/ArTagDetection.py

@@ -0,0 +1,126 @@
+
+import cv2
+import numpy as np
+from ArTagEncoding import decode, extract_hamming_code
+from ArTag import MARKER_SIZE, HammingMarker
+
+BORDER_COORDINATES = [
+    [0, 0], [0, 1], [0, 2], [0, 3], [0, 4], [0, 5], [0, 6], [0, 7], [0, 8],
+    [1, 0], [1, 1], [1, 2], [1, 3], [1, 4], [1, 5], [1, 6], [1, 7], [1, 8],
+    [7, 0], [7, 1], [7, 2], [7, 3], [7, 4], [7, 5], [7, 6], [7, 7], [7, 8],
+    [8, 0], [8, 1], [8, 2], [8, 3], [8, 4], [8, 5], [8, 6], [8, 7], [8, 8],
+    [2, 0], [2, 1], [2, 7], [2, 8], [3, 0], [3, 1], [3, 7], [3, 8],
+    [4, 0], [4, 1], [4, 7], [4, 8], [5, 0], [5, 1], [5, 7], [5, 8],
+    [6, 0], [6, 1], [6, 7], [6, 8],
+]
+
+ORIENTATION_MARKER_COORDINATES = [[2, 2], [2, 6], [6, 2], [6, 6]]
+"""BORDER_COORDINATES = [
+    [0, 0], [0, 1], [0, 2], [0, 3], [0, 4], [0, 5], [0, 6], [1, 0], [1, 6], [2, 0], [2, 6], [3, 0],
+    [3, 6], [4, 0], [4, 6], [5, 0], [5, 6], [6, 0], [6, 1], [6, 2], [6, 3], [6, 4], [6, 5], [6, 6],
+]
+
+ORIENTATION_MARKER_COORDINATES = [[1, 1], [1, 5], [5, 1], [5, 5]]"""
+
+def validate_and_turn(marker):
+    for crd in BORDER_COORDINATES:
+        if marker[crd[0], crd[1]] != 0.0:
+            raise ValueError('Border contains not entirely black parts.')
+    #print(marker[1,1])
+    #if marker[1,1] == 0.0 or marker[1,2] == 0.0 or marker[1,4] == 0.0 or marker[1,5] == 0.0:
+        #raise ValueError('No orientation marker found.')
+    print(marker)
+    if marker[2,2] == 0.0 or marker[2,3] == 0.0 or marker [2,5] == 0.0 or marker[2,6] == 0.0:
+        raise ValueError('No orientation marker found.')
+    """ orientation_marker = None
+    for crd in ORIENTATION_MARKER_COORDINATES:
+        marker_found = False
+        if marker[crd[0], crd[1]] == 1.0:
+            marker_found = True
+        if marker_found and orientation_marker:
+            raise ValueError('More than 1 orientation_marker found.')
+        elif marker_found:
+            orientation_marker = crd
+    if not orientation_marker:
+        raise ValueError('No orientation marker found.')
+    rotation = 0
+    if orientation_marker == [2, 6]:
+        rotation = 1
+    elif orientation_marker == [6, 6]:
+        rotation = 2
+    elif orientation_marker == [6, 2]:
+        rotation = 3
+    marker = np.rot90(marker, k=rotation) """
+    return marker
+
+def detect_markers(img):
+    """
+    This is the main function for detecting markers in an image.
+
+    Input:
+      img: a color or grayscale image that may or may not contain a marker.
+
+    Output:
+      a list of found markers. If no markers are found, then it is an empty list.
+    """
+    if len(img.shape) > 2:
+        width, height, _ = img.shape
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+    else:
+        width, height = img.shape
+        gray = img
+    #cv2.imshow("frame", gray)
+    edges = cv2.Canny(gray, 10, 100)
+    contours, hierarchy = cv2.findContours(edges.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
+    cv2.imshow("frame",edges)
+    min_contour_length = min(width, height) / 50
+    contours = [contour for contour in contours if len(contour) > min_contour_length]
+    warped_size = 81
+    canonical_marker_coords = np.array(
+        (
+            (0, 0),
+            (warped_size - 1, 0),
+            (warped_size - 1, warped_size - 1),
+            (0, warped_size - 1)
+        ),
+        dtype = 'float32')
+    #print(canonical_marker_coords)
+    markers_list = []
+    for contour in contours:
+        approx_curve = cv2.approxPolyDP(contour, len(contour) * 0.01, True)
+        if not (len(approx_curve) == 4 and cv2.isContourConvex(approx_curve)):
+            continue
+        
+        sorted_curve = np.array(
+            cv2.convexHull(approx_curve, clockwise=False),
+            dtype='float32'
+        )
+        #print(sorted_curve)
+        persp_trans = cv2.getPerspectiveTransform(sorted_curve, canonical_marker_coords)
+        
+        warped_img = cv2.warpPerspective(img, persp_trans, (warped_size, warped_size))
+        #cv2.imshow("frame", warped_img)
+        if len(warped_img.shape) > 2:
+            warped_gray = cv2.cvtColor(warped_img, cv2.COLOR_BGR2GRAY)
+        else:
+            warped_gray = warped_img
+
+        _, warped_bin = cv2.threshold(warped_gray, 200, 255, cv2.THRESH_BINARY)
+        #warped_bin = cv2.adaptiveThreshold(warped_gray, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 3, 0) # try adaptive thresholding if needed after this actually works
+        #cv2.imshow("frame", warped_bin)
+        #marker = cv2.resize(warped_bin, (9,9), interpolation=cv2.INTER_AREA)
+        marker = warped_bin.reshape([MARKER_SIZE, warped_size // MARKER_SIZE, MARKER_SIZE, warped_size // MARKER_SIZE]
+        )
+        marker = marker.mean(axis=3).mean(axis=1)
+        marker[marker < 127] = 0
+        marker[marker >= 127] = 1
+        #cv2.imshow("frame", marker)
+
+        try:
+            marker = validate_and_turn(marker)
+            #hamming_code = extract_hamming_code(marker)
+            #marker_id = int(decode(hamming_code), 2)
+            markers_list.append(HammingMarker(id=1, contours=approx_curve))
+        except ValueError:
+            continue
+    return markers_list

algorithms/ArTagDetectionTest.py

@@ -0,0 +1,49 @@
+import pyzed.sl as sl
+import numpy as np
+import cv2
+from ArTagDetection import detect_markers
+
+
+
+if __name__ == '__main__':
+    # add zed camera code
+    fourcc = cv2.VideoWriter_fourcc(*'XVID')
+    videoname = "./ArTest.avi"
+    video_out = cv2.VideoWriter(videoname, fourcc, 10, (int(1920), int(1080)))
+    grabbed = False
+    runtime_params = sl.RuntimeParameters()
+    camera = sl.Camera()
+    init_params = sl.InitParameters()
+    init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD1080
+    err = camera.open(init_params)
+    if err != sl.ERROR_CODE.SUCCESS:
+        print("FAILED TO OPEN CAMERA")
+
+    image_size = camera.get_resolution()
+    left = sl.Mat(image_size.width/2, image_size.height, sl.MAT_TYPE.MAT_TYPE_8U_C4)
+    if camera.grab(runtime_params) == sl.ERROR_CODE.SUCCESS:
+        camera.retrieve_image(left, sl.VIEW.VIEW_LEFT, sl.MEM.MEM_CPU, int(1920), int(1080))
+        frame = left.get_data()
+        grabbed = True
+    
+    while grabbed:
+        markers = detect_markers(frame)
+        for marker in markers:
+            marker.highlite_marker(frame)
+        cv2.imshow("IMAGE", frame)
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            break
+        video_out.write(frame)
+        if camera.grab(runtime_params) == sl.ERROR_CODE.SUCCESS:
+            camera.retrieve_image(left, sl.VIEW.VIEW_LEFT, sl.MEM.MEM_CPU, int(1920), int(1080))
+            frame = left.get_data()
+            grabbed = True
+        else:
+            grabbed = False
+        #read zed again
+
+
+
+    cv2.destroyAllWindows()
+    camera.close()
+    video_out.release()

algorithms/ArTagEncoding.py

@@ -0,0 +1,89 @@
+import numpy as np
+
+GENERATOR_MATRIX = np.matrix([
+    [1, 1, 0, 1],
+    [1, 0, 1, 1],
+    [1, 0, 0, 0],
+    [0, 1, 1, 1],
+    [0, 1, 0, 0],
+    [0, 0, 1, 0],
+    [0, 0, 0, 1],
+])
+
+REGENERATOR_MATRIX = np.matrix([
+    [0, 0, 1, 0, 0, 0, 0],
+    [0, 0, 0, 0, 1, 0, 0],
+    [0, 0, 0, 0, 0, 1, 0],
+    [0, 0, 0, 0, 0, 0, 1],
+])
+
+PARITY_CHECK_MATRIX = np.matrix([
+    [1, 0, 1, 0, 1, 0, 1],
+    [0, 1, 1, 0, 0, 1, 1],
+    [0, 0, 0, 1, 1, 1, 1],
+])
+
+HAMMINGCODE_MARKER_POSITIONS = [
+    [2, 2], [2, 3], [2, 4],
+    [3, 2], [3, 3], [3, 4], [3, 5], [3, 6],
+    [4, 2], [4, 3], [4, 4], [4, 5], [4, 6],
+    [5, 2], [5, 3], [5, 4], [5, 5], [5, 6],
+    [6, 2], [6, 3], [6, 4],
+]
+
+def encode(bits):
+    encoded_code = ''
+    if len(bits) % 4 != 0:
+        raise ValueError('Only a multiple of 4 as bits are allowed.')
+    while len(bits) >= 4:
+        four_bits = bits[:4]
+        bit_array = generate_bit_array(four_bits)
+        hamming_code = matrix_array_multiply_and_format(GENERATOR_MATRIX, bit_array)
+        encoded_code += ''.join(hamming_code)
+        bits = bits[4:]
+    return encoded_code
+
+def decode(bits):
+    decoded_code = ''
+    if len(bits) % 7 != 0:
+        raise ValueError('Only a multiple of 7 as bits are allowed.')
+    for bit in bits:
+        if int(bit) not in [0, 1]:
+            raise ValueError('The provided bits contain other values than 0 or 1: %s' % bits)
+    while len(bits) >= 7:
+        seven_bits = bits[:7]
+        uncorrected_bit_array = generate_bit_array(seven_bits)
+        corrected_bit_array = parity_correct(uncorrected_bit_array)
+        decoded_bits = matrix_array_multiply_and_format(REGENERATOR_MATRIX, corrected_bit_array)
+        decoded_code += ''.join(decoded_bits)
+        bits = bits[7:]
+    return decoded_code
+
+def parity_correct(bit_array):
+    checked_parity = matrix_array_multiply_and_format(PARITY_CHECK_MATRIX, bit_array)
+    parity_bits_correct = True
+    for bit in checked_parity:
+        if int(bit) != 0:
+            parity_bits_correct = False
+    if not parity_bits_correct:
+        error_bit = int(''.join(checked_parity),2)
+        for index, bit in enumerate(bit_array):
+            if error_bit == index + 1:
+                if bit == 0:
+                    bit_array[index] = 1
+                else:
+                    bit_array[index] = 0
+    return bit_array
+
+def matrix_array_multiply_and_format(matrix, array):
+    unformated = matrix.dot(array).tolist()[0]
+    return [str(bit % 2) for bit in unformated]
+
+def generate_bit_array(bits):
+    return np.array([int(bit) for bit in bits])
+
+def extract_hamming_code(mat):
+    hamming_code = ''
+    for pos in HAMMINGCODE_MARKER_POSITIONS:
+        hamming_code += str(int(mat[pos[0], pos[1]]))
+    return hamming_code

algorithms/ArTagTest.py

@@ -0,0 +1,17 @@
+import numpy as np
+import cv2
+from ArTag import HammingMarker
+import ArTagEncoding
+import time
+
+print("test")
+#test = HammingMarker()
+test2 = HammingMarker.generate()
+im2 = test2.generate_image()
+#im = test.generate_image()
+#cv2.imshow("frame", im)
+cv2.imwrite("./marker2957.png", im2)
+cv2.waitKey(1)
+time.sleep(5)
+print("test")
+print("test")