def createDict(): # creates dictionary of markers based on marker bit arrays markers = aruco.Dictionary_create(bit_arrays.shape[0], bit_arrays.shape[1]) byte_arrays = [] for array in bit_arrays: byte_arrays.append(aruco.Dictionary_getByteListFromBits(array)) bytesList = np.concatenate(byte_arrays, 0) markers.bytesList = bytesList return markers
def get_dictionary():
aruco_dict = aruco.custom_dictionary(0, 5, 1)
aruco_dict.bytesList = np.empty(shape=(11, 4, 4), dtype=np.uint8)
# LEG 1
mybits = np.array([
[1, 1, 1, 1, 1],
[1, 1, 0, 0, 1],
[1, 1, 1, 1, 1],
[0, 0, 1, 0, 1],
[0, 0, 1, 0, 1]
], dtype=np.uint8)
aruco_dict.bytesList[0] = aruco.Dictionary_getByteListFromBits(mybits)
# LEG 2
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[0, 0, 1, 1, 0],
[1, 1, 1, 0, 1]
], dtype=np.uint8)
aruco_dict.bytesList[1] = aruco.Dictionary_getByteListFromBits(mybits)
# LEG 3
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[1, 0, 1, 1, 0],
[1, 0, 1, 1, 0]
], dtype=np.uint8)
aruco_dict.bytesList[2] = aruco.Dictionary_getByteListFromBits(mybits)
# LEG 4
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[0, 1, 1, 1, 1],
[1, 0, 1, 0, 0]
], dtype=np.uint8)
aruco_dict.bytesList[3] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[0, 1, 1, 1, 0],
[0, 1, 1, 1, 0]
], dtype=np.uint8)
aruco_dict.bytesList[4] = aruco.Dictionary_getByteListFromBits(mybits)
# LEG 5
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[1, 0, 1, 1, 1],
[0, 1, 1, 0, 0]
], dtype=np.uint8)
aruco_dict.bytesList[5] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[0, 0, 1, 1, 1],
[0, 0, 1, 1, 1]
], dtype=np.uint8)
aruco_dict.bytesList[6] = aruco.Dictionary_getByteListFromBits(mybits)
# LEG 6
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[1, 1, 1, 1, 0],
[0, 0, 1, 0, 1]
], dtype=np.uint8)
aruco_dict.bytesList[7] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[0, 0, 1, 0, 1],
[1, 1, 1, 1, 0]
], dtype=np.uint8)
aruco_dict.bytesList[8] = aruco.Dictionary_getByteListFromBits(mybits)
# LEG 7
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[1, 1, 1, 0, 0],
[1, 1, 1, 0, 0]
], dtype=np.uint8)
aruco_dict.bytesList[9] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([
[1, 1, 0, 1, 1],
[1, 1, 0, 1, 1],
[1, 0, 1, 0, 1],
[0, 1, 1, 0, 0],
[1, 0, 1, 1, 1]
], dtype=np.uint8)
aruco_dict.bytesList[10] = aruco.Dictionary_getByteListFromBits(mybits)
return aruco_dict
import matplotlib.pyplot as plt
import matplotlib.image as img
# we will not use a built-in dictionary, but we could
# aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
# define an empty custom dictionary with
aruco_dict = aruco.custom_dictionary(44, 5, 1)
# add empty bytesList array to fill with 3 markers later
aruco_dict.bytesList = np.empty(shape=(44, 4, 4), dtype=np.uint8)
# add new marker(s)
mybits = np.array([[0, 1, 1, 0, 0], [1, 0, 1, 0, 0], [0, 0, 1, 0, 0],
[0, 0, 1, 0, 0], [1, 1, 1, 1, 1]],
dtype=np.uint8)
aruco_dict.bytesList[0] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[0, 1, 1, 1, 0], [1, 0, 0, 0, 1], [0, 0, 0, 1, 0],
[0, 0, 1, 0, 0], [1, 1, 1, 1, 1]],
dtype=np.uint8)
aruco_dict.bytesList[1] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[0, 1, 1, 1, 0], [1, 0, 0, 0, 1], [0, 0, 1, 1, 0],
[1, 0, 0, 0, 1], [0, 1, 1, 1, 0]],
dtype=np.uint8)
aruco_dict.bytesList[2] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[0, 0, 0, 1, 0], [0, 0, 1, 1, 0], [0, 1, 0, 1, 0],
[1, 1, 1, 1, 1], [0, 0, 0, 1, 0]],
dtype=np.uint8)
aruco_dict.bytesList[3] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[1, 1, 1, 1, 1], [1, 0, 0, 0, 0], [0, 1, 1, 1, 0],
[0, 0, 0, 0, 1], [1, 1, 1, 1, 0]],
from cv_bridge import CvBridge, CvBridgeError
import cv2.aruco as aruco
from scipy.spatial.transform import Rotation as R
import time
import tf
HEIGHT_CUT = 200
VERBOSE = True
aruco_dict = aruco.custom_dictionary(0, 7)
aruco_dict.bytesList = np.zeros(shape=(16, 7, 4), dtype=np.uint8)
ar = np.array([[1, 1, 0, 1, 1], [1, 1, 0, 1, 1], [1, 0, 1, 0, 1],
[0, 0, 1, 1, 0], [1, 1, 1, 0, 1]],
dtype=np.uint8)
aruco_dict.bytesList[1] = aruco.Dictionary_getByteListFromBits(
np.pad(ar, pad_width=1, mode='constant', constant_values=0))
ar = np.array([[1, 1, 0, 1, 1], [1, 1, 0, 1, 1], [1, 0, 1, 0, 1],
[1, 0, 1, 1, 0], [1, 0, 1, 1, 0]],
dtype=np.uint8)
aruco_dict.bytesList[2] = aruco.Dictionary_getByteListFromBits(
np.pad(ar, pad_width=1, mode='constant', constant_values=0))
ar = np.array([[1, 1, 0, 1, 1], [1, 1, 0, 1, 1], [1, 0, 1, 0, 1],
[0, 1, 1, 1, 1], [1, 0, 1, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[3] = aruco.Dictionary_getByteListFromBits(
np.pad(ar, pad_width=1, mode='constant', constant_values=0))
ar = np.array([[1, 1, 0, 1, 1], [1, 1, 0, 1, 1], [1, 0, 1, 0, 1],
[0, 1, 1, 1, 0], [0, 1, 1, 1, 0]],
# define an empty custom dictionary with
# aruco_dict = aruco.custom_dictionary(0, 4, 1)
aruco_dict = aruco.Dictionary_create_from(
1000, 5, aruco.getPredefinedDictionary(aruco.DICT_5X5_1000), 2)
# add empty bytesList array to fill with 3 markers later
print(aruco_dict.bytesList.shape)
aruco_dict.bytesList = np.append(aruco_dict.bytesList,
np.empty(shape=(4, 4, 4), dtype=np.uint8),
axis=0)
# add new marker(s)
mybits = np.array([[1, 1, 1, 0, 0], [1, 0, 1, 0, 0], [1, 1, 1, 0, 0],
[1, 0, 1, 0, 0], [1, 0, 1, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[1000] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[0, 1, 1, 0, 0], [1, 0, 0, 1, 0], [1, 1, 1, 0, 0],
[1, 0, 0, 1, 0], [0, 1, 1, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[1001] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[1, 1, 1, 1, 0], [1, 0, 0, 0, 0], [1, 0, 0, 0, 0],
[1, 0, 0, 0, 0], [1, 1, 1, 1, 0]],
dtype=np.uint8)
aruco_dict.bytesList[1002] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[0, 1, 1, 1, 0], [1, 0, 0, 0, 0], [1, 1, 1, 0, 0],
[1, 0, 0, 0, 0], [1, 1, 1, 1, 0]],
dtype=np.uint8)
aruco_dict.bytesList[1003] = aruco.Dictionary_getByteListFromBits(mybits)
global aruco_dict
global parameters, folder_path
global detectamos, rango, direccion, profundidad, CoorMsg, x1, x2, y1, y2, constante
global CoorMsg, imgMsg, imgMsgMask
aruco_dict = aruco.custom_dictionary(20, 7)
aruco_dict.bytesList = np.empty(shape=(20, 7, 4), dtype=np.uint8)
#0
mybits1 = np.array(
[[0, 0, 0, 0, 0, 0, 0], [0, 1, 1, 0, 1, 1, 0], [0, 1, 1, 0, 1, 1, 0],
[0, 1, 0, 1, 0, 1, 0], [0, 1, 1, 1, 1, 1, 0], [0, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[0] = aruco.Dictionary_getByteListFromBits(mybits1)
#1
mybits2 = np.array(
[[0, 0, 0, 0, 0, 0, 0], [0, 1, 1, 0, 1, 1, 0], [0, 1, 1, 0, 1, 1, 0],
[0, 1, 0, 1, 0, 1, 0], [0, 0, 0, 1, 1, 0, 0], [0, 1, 1, 1, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[1] = aruco.Dictionary_getByteListFromBits(mybits2)
#2
mybits3 = np.array(
[[0, 0, 0, 0, 0, 0, 0], [0, 1, 1, 0, 1, 1, 0], [0, 1, 1, 0, 1, 1, 0],
[0, 1, 0, 1, 0, 1, 0], [0, 1, 0, 1, 1, 0, 0], [0, 1, 0, 1, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0]],
dtype=np.uint8)
aruco_dict.bytesList[2] = aruco.Dictionary_getByteListFromBits(mybits3)
#3
def detect_markers(img_filepath):
# img_filepath should be a .png
img = cv2.imread(img_filepath)
arr = [
[0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 0, 1, 1, 0],
[0, 1, 1, 0, 1, 1, 0],
[0, 1, 0, 1, 0, 1, 0],
[0, 0, 1, 1, 1, 0, 0],
[0, 0, 1, 1, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0],
]
arr_flat = []
for a in arr:
for b in a:
arr_flat.append(b)
mat = np.array(arr_flat)
bytesList = aruco.Dictionary_getByteListFromBits(mat)
marker_dict = aruco.custom_dictionary(1, 7)
marker_dict.bytesList = bytesList
# marker_dict.maxCorrectionBits = 10
img1 = aruco.drawMarker(marker_dict, 0, 500)
cv2.imwrite("mtest.jpg", img1)
#parameters = aruco.DetectorParameters_create()
# marker_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
corners, ids, rejectedImgPoints = aruco.detectMarkers(img, marker_dict)
print(corners)
#IPython.embed()
for corner_set in corners:
#corner_set is 1, 4, 2
corner1 = corner_set[0, 0, :]
corner2 = corner_set[0, 1, :]
corner3 = corner_set[0, 2, :]
corner4 = corner_set[0, 3, :]
corner1 = np.expand_dims(corner1, axis=1)
corner2 = np.expand_dims(corner2, axis=1)
corner3 = np.expand_dims(corner3, axis=1)
corner4 = np.expand_dims(corner4, axis=1)
axis1 = corner3 - corner1
axis2 = corner2 - corner4
print(axis1.shape)
A = np.hstack((axis1, -axis2))
#IPython.embed()
b = corner4 - corner1
x = np.linalg.solve(A, b)
square_center = corner4 + x[1][0] * axis2
red = [0, 0, 255]
# Change one pixel
#IPython.embed()
x = int(round(square_center[0][0]))
y = int(round(square_center[1][0]))
#print(x, y)
cv2.circle(img, (x, y), 4, red, -1)
#img[x,y]=red
# img = cv2.resize(img, (int(img.shape[0]/4), int(img.shape[1]/4)))
img_with_markers = aruco.drawDetectedMarkers(img, corners)
cv2.imwrite("btest.jpg", img_with_markers)
cv2.imshow('image', img_with_markers)
cv2.waitKey(0)
cv2.destroyAllWindows()
import numpy as np
import cv2
import cv2.aruco as aruco
# we will not use a built-in dictionary, but we could
# aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
# define an empty custom dictionary with
aruco_dict = aruco.custom_dictionary(0, 4, 1)
# add empty bytesList array to fill with 3 markers later
aruco_dict.bytesList = np.empty(shape = (3, 2, 4), dtype = np.uint8)
# add new marker(s)
mybits = np.array([[1,1,1,1],[1,0,0,0],[1,1,1,0],[1,0,0,0]], dtype = np.uint8)
aruco_dict.bytesList[0] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[0,1,1,0],[1,0,0,1],[1,1,1,1],[1,0,0,1],], dtype = np.uint8)
aruco_dict.bytesList[1] = aruco.Dictionary_getByteListFromBits(mybits)
mybits = np.array([[0,1,1,1],[1,0,0,0],[1,0,0,0],[1,1,1,1]], dtype = np.uint8)
aruco_dict.bytesList[2] = aruco.Dictionary_getByteListFromBits(mybits)
# save marker images
for i in range(len(aruco_dict.bytesList)):
cv2.imwrite("custom_aruco_" + str(i) + ".png", aruco.drawMarker(aruco_dict, i, 128))
# open video capture from (first) webcam
cap = cv2.VideoCapture(0)
while(True):
# Capture frame-by-frame
ret, frame = cap.read()