def detect(self, image, return_image=False):
data = []
if self.detector == None:
ids, corners, centers, Hs = pyAprilTag.find(image)
for i in range(len(ids)):
# reversing corners since that's the order they were in for the older library
corners_list = corners[i].tolist()
corners_list.reverse()
data.append(
Detection(ids[i], centers[i], corners_list,
util.compute_angle(corners_list)))
else:
data = [
Detection(detection.tag_id, detection.center,
detection.corners,
util.compute_angle(detection.corners))
for detection in self.detector.detect(image, False)
]
data.sort(key=lambda entry: entry.tag_id)
if return_image:
return data, image
else:
return data
fig = plt.figure()
fig.canvas.mpl_connect('key_press_event', press)
while cap.isOpened() and not user_exit:
# Capture frame-by-frame
ret, frame = cap.read()
if not ret:
break
if frame.shape[0] > 640:
frame = cv2.resize(frame, (640, 480))
gframe = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ids, corners, centers, Hs = pyAprilTag.find(gframe)
# Display the resulting frame
if frame.ndim == 3:
frame = frame[:, :, ::-1] #BGR => RGB
plt.cla()
plt.imshow(frame)
for i in range(len(ids)):
plt.plot(corners[i, [0, 1, 2, 3, 0], 0], corners[i, [0, 1, 2, 3, 0],
1], 'r-')
plt.annotate('{:d}'.format(ids[i]),
xy=centers[i],
color='red',
fontsize=12)
def Score(x1, x2):
x1 = x1[2, :]
x2 = x2[2, :]
score = 0
for i in range(np.shape(x1)[0]):
if x1[i] > 0 and x2[i] > 0:
score = score + 1
return score
if __name__ == "__main__":
img1 = cv.imread('AprilCalib_orgframe_00007.png', 0)
img2 = cv.imread('AprilCalib_orgframe_00008.png', 0)
ids1, corners1, centers1, Hs1 = pyAprilTag.find(img1)
ids2, corners2, centers2, Hs2 = pyAprilTag.find(img2)
points2 = np.empty([40, 4, 2])
C_2 = np.empty([40, 2])
ids1 = list(ids1)
ids2 = list(ids2)
for i in range(len(ids1)):
idx_curr_2 = ids2.index(ids1[i])
points2[i] = corners2[idx_curr_2]
C_2[i, :] = centers2[idx_curr_2]
points2_new = np.empty([200, 2])
for i in range(np.shape(points2)[0]):
points2_new[5 * i, :] = points2[i][0]
points2_new[5 * i + 1, :] = points2[i][1]
points2_new[5 * i + 2, :] = points2[i][2]
def main():
# Calibration Matrix from Task 3
K = np.array([[1.17136545e+03, 0.00000000e+00, 8.01203424e+02],
[0.00000000e+00, 1.17497866e+03, 5.38637778e+02],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]],
dtype='float64')
print(f"\nK = \n{K}\n")
# detect AprilTag pattern and homography matrix, Hs
image_path = 'Task4_03.png'
img = cv2.imread(image_path, 0)
ids, corners, centers, Hs = pyAprilTag.find(img)
H_matrix = np.array(Hs[0])
print(f"H = \n{H_matrix}")
# store tag corner coordinates to show in resulting image later
tag_corners = corners[0]
print(f"\nCorner Image Coordinates = \n{tag_corners}\n")
# Calculate camera extrinsics using homography and calibration matrix
A_matrix = np.matmul(LA.inv(K), H_matrix)
print(f"\nA = K^-1 * H = \n{A_matrix}\n")
a1_n = LA.norm(A_matrix[:, 0])
r1 = A_matrix[:, 0]
r2 = A_matrix[:, 1]
r3 = np.cross(r1, r2)
r1 = r1 / a1_n
r2 = r2 / a1_n
r3 = r3 / (a1_n * a1_n)
tr = A_matrix[:, 2]
tr = tr / a1_n
print(f"\nTranslation = \n{tr}\n")
R_matrix = np.array([r1, r2, r3]).transpose()
print(f"\nR Matrix = \n{R_matrix}\n")
P_matrix = np.array([r1, r2, r3, tr]).transpose()
print(f"\nExtrinsics [ R | t ] = \n{P_matrix}\n")
# Recalculate Camera Matrix P for converting 3D points to 2D image coordinates
P_matrix = np.matmul(K, P_matrix)
print(f"\nCamera Matrix K * [ R | t ] = \n{P_matrix}\n")
# Cube corners world coordinates, sorted in a way that makes it easy for line drawing later
world_coordinates = np.array([
[1, 1, 0, 1],
[-1, 1, 0, 1],
[-1, -1, 0,
1], # since our cube "base" is from -1 to 1, its side length is 2
[-1, -1, 2,
1], # so z-coordinates should be equal to 2 for it to be a cube!
[-1, 1, 2, 1],
[1, 1, 2, 1],
[1, -1, 2, 1],
[1, -1, 0, 1]
])
print(f"\nWorld Coordinates = \n{world_coordinates}\n")
# Convert to image coordinates uing Camera Matrix, P
image_coordinates = np.matmul(P_matrix,
world_coordinates.transpose()).transpose()
# Homogenous form requires division by last vector value to complete the perspective projection
for i in range(0, len(image_coordinates)):
image_coordinates[i] = image_coordinates[i] / image_coordinates[i][2]
print(f"\nImage Coordinates = \n{image_coordinates}\n")
im = np.array(Image.open(image_path), dtype=np.uint8)
fig, ax = plt.subplots(1)
# Display the image
ax.imshow(im)
# Display AprilTag corners as detected by pyAprilTag
ax.scatter(tag_corners[:, 0], tag_corners[:, 1], color='red')
# Draw cube lines based on image coordinates of corners
for i in range(0, 4):
for j in range(0, 3):
x = []
y = []
x.append(image_coordinates[i * 2, 0])
y.append(image_coordinates[i * 2, 1])
if j == 0:
x.append(image_coordinates[i * 2 + 1, 0])
y.append(image_coordinates[i * 2 + 1, 1])
elif j == 1:
if i * 2 + 5 > len(image_coordinates):
x.append(image_coordinates[i * 2 - 3, 0])
y.append(image_coordinates[i * 2 - 3, 1])
else:
x.append(image_coordinates[i * 2 + 5, 0])
y.append(image_coordinates[i * 2 + 5, 1])
elif j == 2:
if i * 2 + 7 > len(image_coordinates):
x.append(image_coordinates[i * 2 - 1, 0])
y.append(image_coordinates[i * 2 - 1, 1])
else:
x.append(image_coordinates[i * 2 + 7, 0])
y.append(image_coordinates[i * 2 + 7, 1])
line = Line2D(x, y)
ax.add_line(line)
plt.show()
import cv2
import matplotlib.pyplot as plt
import pyAprilTag
img = cv2.imread(pyAprilTag.calib_pattern_path)
ids, corners, centers, Hs = pyAprilTag.find(img)
if img.ndim == 3:
img = img[:, :, ::-1] #BGR => RGB
print(ids)
plt.imshow(img)
plt.show()
import numpy as np
import cv2 as cv
import pyAprilTag
img1 = cv.imread('AprilCalib_orgframe_00007.png', 0)
img2 = cv.imread('AprilCalib_orgframe_00008.png', 0)
ids_1, corners_1, centers_1, Hs_1 = pyAprilTag.find(img1)
ids_2, corners_2, centers_2, Hs_2 = pyAprilTag.find(img2)
K = np.array([[611.4138483174528, 0, 315.6207318252974],
[0, 611.9537184774845, 259.6803148373927], [0, 0, 1]],
dtype='float64')
dist = np.array([[-0.4509210354054127, 0.1895690122992456, 0, 0, 0]],
dtype='float64')
ids_2 = list(ids_2)
ids_1 = list(ids_1)
ids_2_index = ids_2.index(ids_1[0])
corners_1_R = corners_1[0]
Hs_1_R = Hs_1[0]
obj1_list = np.zeros((4, 3))
corners_2_R = corners_2[ids_2_index]
Hs_2_R = Hs_2[ids_2_index]
obj2_list = np.zeros((4, 3))
for i, j in enumerate(corners_1_R):
j = np.append(j, np.ones(1))
p0 = np.matmul(np.linalg.inv(Hs_1_R), j)
p0 = p0 / p0[2]
p0[2] = 0
obj1_list[i, :] = p0
ret_1, rvecs_1, tvecs_1 = cv.solvePnP(obj1_list, corners_1_R, K, dist)
r_1, _ = cv.Rodrigues(rvecs_1)
H1 = np.append(r_1, tvecs_1, axis=1)
H1 = np.append(H1, np.array([[0, 0, 0, 1]]), axis=0)
capture_store.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
print('{:d}x{:d} Pixel ratio @ {:d} Frequency (Hz)'.format(
int(capture_store.get(cv2.CAP_PROP_FRAME_WIDTH)),
int(capture_store.get(cv2.CAP_PROP_FRAME_HEIGHT)),
int(capture_store.get(cv2.CAP_PROP_FPS))))
if not capture_store.isOpened():
print("Camera not working")
exit(0)
while capture_store.isOpened():
RET, frame_read = capture_store.read()
frame_copy = frame_read.copy()
if not RET:
break
if frame_read.shape[0] > 640:
frame_read = cv2.resize(frame_read, (640, 480))
ID, corners, centers, _hs_ = pyAprilTag.find(frame_read)
if (corners.size != 0):
points_src = np.float64((5, 3))
points_dist = np.float32((5, 3))
x = 0
for i in range(ID.shape[0]):
if ID[i] == 2:
x = i
points_dist = np.float64([[[corners[x, 0, 0]],
[corners[x, 0, 1]]],
[[corners[x, 1, 0]],
[corners[x, 1, 1]]],
[[corners[x, 2, 0]],
[corners[x, 2, 1]]],
[[corners[x, 3, 0]],
[corners[x, 3, 1]]],