mirror of
https://github.com/opencv/opencv_contrib.git
synced 2025-10-18 08:44:11 +08:00
49 lines
1.6 KiB
Python
49 lines
1.6 KiB
Python
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)
|