1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
|
import numpy as np
import cv2 as cv
# aruco config
adict = cv.aruco.Dictionary_get(cv.aruco.DICT_4X4_50)
cv.imshow("marker", cv.aruco.drawMarker(adict, 0, 400))
marker_len = 5
# rapid config
obj_points = np.float32([[-0.5, 0.5, 0], [0.5, 0.5, 0], [0.5, -0.5, 0], [-0.5, -0.5, 0]]) * marker_len
tris = np.int32([[0, 2, 1], [0, 3, 2]]) # note CCW order for culling
line_len = 10
# random calibration data. your mileage may vary.
imsize = (800, 600)
K = cv.getDefaultNewCameraMatrix(np.diag([800, 800, 1]), imsize, True)
# video capture
cap = cv.VideoCapture(0)
cap.set(cv.CAP_PROP_FRAME_WIDTH, imsize[0])
cap.set(cv.CAP_PROP_FRAME_HEIGHT, imsize[1])
rot, trans = None, None
while cv.waitKey(1) != 27:
img = cap.read()[1]
# detection with aruco
if rot is None:
corners, ids = cv.aruco.detectMarkers(img, adict)[:2]
if ids is not None:
rvecs, tvecs = cv.aruco.estimatePoseSingleMarkers(corners, marker_len, K, None)[:2]
rot, trans = rvecs[0].ravel(), tvecs[0].ravel()
# tracking and refinement with rapid
if rot is not None:
for i in range(5): # multiple iterations
ratio, rot, trans = cv.rapid.rapid(img, 40, line_len, obj_points, tris, K, rot, trans)[:3]
if ratio < 0.8:
# bad quality, force re-detect
rot, trans = None, None
break
# drawing
cv.putText(img, "detecting" if rot is None else "tracking", (0, 20), cv.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255))
if rot is not None:
cv.drawFrameAxes(img, K, None, rot, trans, marker_len)
cv.imshow("tracking", img)
|
