import cv2
import numpy as np
import time
import cv2.aruco as aruco
import yaml
file_path = ("./标定文件.yaml")
###加载文件路径###
with open(file_path, "r") as file:
parameter = yaml.load(file.read(), Loader=yaml.Loader)
mtx = parameter['camera_matrix']
dist = parameter['dist_coeff']
camera_u = parameter['camera_u']
camera_v = parameter['camera_v']
mtx = np.array(mtx)
dist = np.array(dist)
font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below)
#参考链接https://blog.csdn.net/Miaosh999/article/details/106934655/
#本文修改了min_hsv_bound，max_hsv_bound里面的数值，还将cv2.rectangle里面的数值强制转化为int
#这个检测可以和未加卡尔曼滤波的做比对，即tracker5里面
# hsv阈值，便于进行轮廓判断及轨迹绘制，需要根据运动目标的颜色自己进行调整
min_hsv_bound = (25, 52, 72)
max_hsv_bound = (100, 255, 255)
#状态向量
stateSize = 6
#观测向量
measSize = 4
coutrSize = 0
kf = cv2.KalmanFilter(stateSize,measSize,coutrSize)
state = np.zeros(stateSize, np.float32)#[x,y,v_x,v_y,w,h],簇心位置，速度，高宽
meas = np.zeros(measSize, np.float32)#[z_x,z_y,z_w,z_h]
procNoise = np.zeros(stateSize, np.float32)
#状态转移矩阵
cv2.setIdentity(kf.transitionMatrix)#生成单位矩阵
kf.measurementMatrix = np.zeros((measSize,stateSize),np.float32)
kf.measurementMatrix[0,0]=1.0
kf.measurementMatrix[1,1]=1.0
kf.measurementMatrix[2,4]=1.0
kf.measurementMatrix[3,5]=1.0
cv2.setIdentity(kf.processNoiseCov)
kf.processNoiseCov[0,0] = 1e-2
kf.processNoiseCov[1,1] = 1e-2
kf.processNoiseCov[2,2] = 5.0
kf.processNoiseCov[3,3] = 5.0
kf.processNoiseCov[4,4] = 1e-2
kf.processNoiseCov[5,5] = 1e-2
#测量噪声
cv2.setIdentity(kf.measurementNoiseCov)
#video_cap = cv2.VideoCapture(0,cv2.CAP_DSHOW)
#video_cap = cv2.VideoCapture('./zhenAruco.mp4')
video_cap = cv2.VideoCapture('./aruco.mp4')
# 视频输出
fps = video_cap.get(cv2.CAP_PROP_FPS) #获得视频帧率，即每秒多少帧
size = (int(video_cap.get(cv2.CAP_PROP_FRAME_WIDTH)),int(video_cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
#保存检测结果，将预测框和中心点的路径保存到视频中
videoWriter = cv2.VideoWriter('./new_aruco.mp4' ,cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), fps, size)
ticks = 0
i=0
found = False
notFoundCount = 0
prePointCen = [] #存储中心点位置
meaPointCen = []
while(True):
ret, frame = video_cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_1000)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,
aruco_dict,
parameters=parameters)
if ret is False:
break
cv2.imshow('frame',frame)
cv2.waitKey(1)
precTick = ticks
ticks = float(cv2.getTickCount())
res = frame.copy()
# dT = float(1/fps)
dT = float((ticks - precTick)/cv2.getTickFrequency())
if(found):
#预测得到的位置
kf.transitionMatrix[0,2] = dT
kf.transitionMatrix[1,3] = dT
state = kf.predict()
width = state[4]
height = state[5]
x_left = state[0] - width/2 #左上角横坐标
y_left = state[1] - height/2 #左上角纵坐标
x_right = state[0] + width/2
y_right = state[1] + height/2
center_x = state[0]
center_y = state[1]
prePointCen.append((int(center_x),int(center_y)))
cv2.circle(res, (int(center_x),int(center_y)),2,(255,0,0),-1)
cv2.rectangle(res,(int(x_left),int(y_left)),(int(x_right),int(y_right)),(255,0,0),2)
#根据颜色二值化得到的位置
if ids is not None:
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, 0.05, mtx, dist)
(rvec - tvec).any()
for i in range(rvec.shape[0]):
cv2.drawFrameAxes(frame, mtx, dist, rvec[i, :, :], tvec[i, :, :], 0.03)
aruco.drawDetectedMarkers(frame, corners)
# 显示ID，rvec,tvec, 旋转向量和平移向量
cv2.putText(frame, "Id: " + str(ids), (0, 40), font, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(frame, "rvec: " + str(rvec[i, :, :]), (0, 60), font, 0.5, (0, 255, 0), 2, cv2.LINE_AA)
cv2.putText(frame, "tvec: " + str(tvec[i, :, :]), (0, 80), font, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
#检测轮廓，只检测最外围轮廓，保存物体边界上所有连续的轮廓点到contours向量内
balls = []
ballsBox = []
for i in range(len(corners)):
x, y, w, h = cv2.boundingRect(np.array(corners[i]))
ratio = float(w/h)
if(ratio > 1.0):
ratio = 1.0 / ratio
if(ratio > 0.75 and w*h>=400):
balls.append(corners[i])
ballsBox.append([x, y, w, h])
print( "found:", len(ballsBox))
print("\n")
for i in range(len(balls)):
# 绘制轮廓
#cv2.drawContours(res, balls, i, (20,150,20),1)
cv2.rectangle(res,(ballsBox[i][0],ballsBox[i][1]),(ballsBox[i][0]+ballsBox[i][2],ballsBox[i][1]+ballsBox[i][3]),(0,255,0),2) #二值化得到边界
center_x = ballsBox[i][0] + ballsBox[i][2] / 2
center_y = ballsBox[i][1] + ballsBox[i][3] / 2
meaPointCen.append((int(center_x),int(center_y)))
cv2.circle(res,(int(center_x),int(center_y)), 2, (20,150,20) ,-1)
name = "(" + str(center_x) + "," + str(center_y) + ")"
cv2.putText(res, name, (int(center_x) + 3, int(center_y) - 3), cv2.FONT_HERSHEY_COMPLEX, 0.5, (20,150,20), 2)
n = len(prePointCen)
for i in range(1, n):
print(i)
if prePointCen[i-1] is None or prePointCen[i] is None:
continue
# 注释掉的这块是为了绘制能够随时间先后慢慢消失的追踪轨迹，但是有一些小错误
# 计算所画小线段的粗细
# thickness = int(np.sqrt(64 / float(n - i + 1))*2.5)
# print(thickness)
# 画出小线段
# cv2.line(res, prePointCen[i-1], prePointCen[i], (0, 0, 255), thickness)
cv2.line(res, prePointCen[i-1], prePointCen[i], (0,0,255), 1, 4)
if(len(balls) == 0):
notFoundCount += 1
print("notFoundCount",notFoundCount)
print("\n")
if notFoundCount >= 100:
found = False
else:
#测量得到的物体位置
notFoundCount = 0
meas[0] = ballsBox[0][0] + ballsBox[0][2] / 2
meas[1] = ballsBox[0][1] + ballsBox[0][3] / 2
meas[2] = float(ballsBox[0][2])
meas[3] = float(ballsBox[0][3])
#第一次检测
if not found:
for i in range(len(kf.errorCovPre)):
kf.errorCovPre[i,i] = 1
state[0] = meas[0]
state[1] = meas[1]
state[2] = 0
state[3] = 0
state[4] = meas[2]
state[5] = meas[3]
kf.statePost = state
found = True
else:
kf.correct(meas) #Kalman修正
print('rr',res.shape)
print("Measure matrix:", meas)
cv2.imshow("Tracking", res)
cv2.waitKey(1)
videoWriter.write(res)
