def draw_markers(
frame,
corners,
ids,
only_top_left=False,
marker_color=(0, 255, 0),
marker_size=10,
marker_type=cv.MARKER_CROSS,
font_color=(0, 255, 0),
font_scale=0.8,
):
if ids.any():
for i, c in enumerate(corners):
c = np.squeeze(c)
if only_top_left:
c = c[:1, :]
frame = draw_markers(
frame,
c.T,
color=marker_color,
marker_size=marker_size,
marker_type=marker_type,
)
frame = cv.putText(
frame,
str(int(ids[i])),
tuple(c.mean(axis=0)),
cv.FONT_HERSHEY_SIMPLEX,
font_scale,
font_color,
thickness=2,
)
return frame
border_dim)
frame = 255 * np.ones((1000, 1000), dtype=np.uint8)
x, y = 500, 300
frame[y:y + marker_dim, x:x + marker_dim] = img_m
parameters = cv.aruco.DetectorParameters_create()
corners, ids, _ = cv.aruco.detectMarkers(frame,
marker_dictionary,
parameters=parameters)
corners = np.squeeze(corners[0]).T
ids = ids[0]
frame_c = draw_markers(frame,
corners,
color=(0, 0, 0),
marker_type=cv.MARKER_CROSS)
print(corners)
print(ids)
plt.figure()
plt.imshow(img_m, cmap="gray")
plt.show()
plt.figure()
plt.imshow(frame, cmap="gray")
plt.show()
plt.figure()
plt.imshow(frame_c, cmap="gray")
print(f"img {str(img.shape)} {img.dtype} {img.min()}-{img.max()}")
print(f"xy {str(xy.shape)} {xy.dtype} XYZ {str(XYZ.shape)} {XYZ.dtype}")
xy = to_homogeneous_coordinates(xy)
XYZ = to_homogeneous_coordinates(XYZ)
print(f"xy {str(xy.shape)} {xy.dtype} XYZ {str(XYZ.shape)} {XYZ.dtype}")
xy_gs = to_homogeneous_coordinates(xy_gs)
XYZ_gs = to_homogeneous_coordinates(XYZ_gs)
print(f"xy_gs {str(xy_gs.shape)} {xy_gs.dtype} XYZ {str(XYZ_gs.shape)} {XYZ_gs.dtype}")
print_matrix(xy, "xy")
print_matrix(XYZ, "XYZ")
img_m = draw_markers(img, xy[:-1, :])
"""
Direct Linear Transform.
"""
P, K, R, t, C, error = run_dlt(xy, XYZ)
print(f"reprojection error {error}")
print_matrix(C, "C")
print_matrix(P @ C, "P * C")
xy_p = project_xyz(construct_P_matrix(K, R, t), XYZ)
img_mp = draw_markers(
img_m, xy_p[:-1, :], color=(255, 0, 0), marker_type=cv.MARKER_TILTED_CROSS
)
print_matrix(dist, "dist")
print_matrix(R, "R")
print_matrix(np.linalg.inv(R) @ t, "t")
# Camera undistortion
height, width = img.shape[:2]
K_, roi = cv.getOptimalNewCameraMatrix(K, dist, (width, height), 1,
(width, height))
img_u = cv.undistort(img, K, dist, None, K_)
P = construct_P_matrix(K, R, t.flatten())
xy_grid = project_xyz(P, to_homogeneous_coordinates(grid.T))
img_grid = draw_markers(
img,
xy_grid[:-1, :],
color=(0, 255, 0),
marker_type=cv.MARKER_CROSS,
marker_size=10,
)
# Visualise
cv.drawChessboardCorners(img, grid_dims, corners2, found)
cv.imshow("preview", img)
# cv.imshow("preview", img_grid)
else:
cv.imshow("preview", img)
key = cv.waitKey(20)
if key == 27 or key == ord("q"): # exit on ESC
break
frame = reader.get_frame(fraction=0.4, rot=3)
# frame = resize(cv.imread("./data/aruco.jpg"), 0.3)
corners, ids = aruco.detect_markers(frame, verbose=False)
xy, XYZ = aruco.get_positions_by_id(corners, ids)
print(xy.shape)
if xy.shape[1] > 6:
P, _, R, t, _, error = run_gold_standard(
to_homogeneous_coordinates(xy), to_homogeneous_coordinates(XYZ),
2000)
frame = draw_markers(
frame,
project_xyz(P, to_homogeneous_coordinates(XYZ))[:-1, :],
color=(0, 0, 255),
marker_type=cv.MARKER_TILTED_CROSS,
marker_size=10,
)
print_matrix(-np.linalg.inv(R) @ t, "t")
frame = aruco.draw_markers(frame, corners, ids)
key = reader.show_frame(frame)
if key == 27 or key == ord("q"):
break
cv.destroyAllWindows()