From ed7b11ffe25f9217ef5ba175f9de5f376513cee3 Mon Sep 17 00:00:00 2001 From: lisb20 <2692465509@qq.com> Date: Fri, 28 Jul 2023 14:53:57 +0800 Subject: [PATCH] add_real_shot --- real_shot_code/cam.py | 52 ++++ real_shot_code/det.py | 256 ++++++++++++++++++ real_shot_code/findframe.py | 42 +++ real_shot_code/make_data.py | 105 +++++++ ...77\347\224\250\350\257\264\346\230\216.md" | 14 + 5 files changed, 469 insertions(+) create mode 100644 real_shot_code/cam.py create mode 100644 real_shot_code/det.py create mode 100644 real_shot_code/findframe.py create mode 100644 real_shot_code/make_data.py create mode 100644 "real_shot_code/\344\275\277\347\224\250\350\257\264\346\230\216.md" diff --git a/real_shot_code/cam.py b/real_shot_code/cam.py new file mode 100644 index 0000000..144139c --- /dev/null +++ b/real_shot_code/cam.py @@ -0,0 +1,52 @@ +import cv2 +import os +## 同时开启两个摄像头并录像 +# cap1 = cv2.VideoCapture(1, cv2.CAP_DSHOW) +cap1 = cv2.VideoCapture(1) +# cap2 = cv2.VideoCapture(2, cv2.CAP_DSHOW) +cap2 = cv2.VideoCapture(2) + +print(cap1.isOpened()) +print(cap2.isOpened()) +print(cap1.get(cv2.CAP_PROP_FPS)) +print(cap2.get(cv2.CAP_PROP_FPS)) + +cap1.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) +cap1.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) + +cap2.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) +cap2.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) + +# fourcc = cv2.VideoWriter_fourcc(*'XVID') +# out1 = cv2.VideoWriter('output1.avi', fourcc, 30.0, (1920, 1080)) +# out2 = cv2.VideoWriter('output2.avi', fourcc, 30.0, (1920, 1080)) +fp = "./1102/005/" +if os.path.exists(fp): + dirs = os.listdir(fp) + assert len(dirs) == 0 ## 确保文件夹为空 +else: + os.makedirs(fp) + +fourcc = cv2.VideoWriter_fourcc(*'mp4v') +out1 = cv2.VideoWriter(fp + 'output1.mp4', fourcc, 30.0, (1920, 1080)) +out2 = cv2.VideoWriter(fp + 'output2.mp4', fourcc, 30.0, (1920, 1080)) + +cnt = 0 +while (cap1.isOpened() and cap2.isOpened()): + cnt += 1 + if cnt % 100 == 0: + print(cnt) + ret1, frame1 = cap1.read() + ret2, frame2 = cap2.read() + if ret1 == True and ret2 == True: + out1.write(frame1) + out2.write(frame2) + cv2.imshow('frame1', frame1) + cv2.imshow('frame2', frame2) + if cv2.waitKey(1) & 0xFF == ord('q'): + break + else: + break + +cap1.release() +cap2.release() diff --git a/real_shot_code/det.py b/real_shot_code/det.py new file mode 100644 index 0000000..f8e2156 --- /dev/null +++ b/real_shot_code/det.py @@ -0,0 +1,256 @@ +import matplotlib.pyplot as plt +import pupil_apriltags as apriltag +import cv2 +import numpy as np +from tqdm import tqdm +import pickle +from collections import defaultdict +import os + +# Imat = [[1338.56729845250, 0, 0], +# [0, 1323.97312268293, 0], +# [942.273221383391, 473.805089526029, 1]] +# Imat = np.array(Imat).T + +Imat = [[1131.57817294016,0,0], + [0,1131.25028255906,0], + [962.248182523716,543.533259657236,1]] +Imat = np.array(Imat).T + + + + +# Dmat = [-0.346167416278953, 0.110363006733750, 0, 0] +Dmat = [0.117006120213299,-0.126019080006247,0,0] +Dmat = np.array(Dmat) +# cam1_loc = [2960,6930] +cam1_loc = [1730,4340] ## 1102: x为横向，y为纵向 + +Imat2 = [[1124.57531450059,0,0], +[0,1126.18079389101,0], +[968.764721434572,537.149978277261,1]] +Imat2 = np.array(Imat2).T + +Dmat2 = [0.110755756890598,-0.121416360832669,0,0] +Dmat2 = np.array(Dmat2) +# cam2_loc = [3150,160] ### 有海康标的那个 +cam2_loc = [1730,110] + + +npeo = 5 + +at_detector = apriltag.Detector(decode_sharpening=0.5, quad_decimate=1) + + +def pnp(obj, corners, C_M, d): + ''' + obj: tag四个角的物理坐标, + corners： tag四个角的像素坐标 + c_m:内参 + d:畸变矩阵 + 注意：这四个都要是np.float_64 + + 输出： p_o：机器人物理坐标 + dir：机器人面对的方向 + ''' + s, r, t = cv2.solvePnP(obj, corners, C_M, d) + r_m, ja = cv2.Rodrigues(r) + + robot_pos = np.array([0, 0, 0]).reshape((3, 1)) + robot_dir = np.array([0, 0, 100]).reshape((3, 1)) + + p_o = np.linalg.pinv(r_m) @ (robot_pos - t) + + return p_o, t + +halflen = 80.5 ##tag边长 +obj = np.array([[0, halflen, -halflen], [0, halflen, halflen], [0, -halflen, halflen], + [0, -halflen, -halflen]], dtype=np.float64) + + +# read demo.mp4 +folder = './1102/004/' +dirs = os.listdir(folder) +if ("loc1.pkl" in dirs) or ("loc2.pkl" in dirs): + print("Warning overwrite loc1.pkl or loc2.pkl") + # assert False + + +cap1 = cv2.VideoCapture(folder + 'output1.mp4') ## cap1 约定为可以照到拍手的摄像头 +cap2 = cv2.VideoCapture(folder + 'output2.mp4') + + + +def get_location(cap,I,D): + frame = 0 + fail_cnt = 0 + location = [] + for _ in range(npeo): + location.append([]) + + while(cap.isOpened()): + ret, img = cap.read() + frame += 1 + if frame % 100 == 0: + print(frame) + # cv2.imshow('img',img) + # cv2.waitKey(0) + + if ret == False: + break + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + + tags = at_detector.detect(gray) + + tag_num = len(tags) + if(tag_num == 0): + for p in location: + p.append([None, None]) + fail_cnt += 1 + continue + else: + x = defaultdict(list) + z = defaultdict(list) + for tag in tags: + peo_idx = tag.tag_id // 4 + corners = np.array(tag.corners, dtype=np.float64) + p1, p2 = pnp(obj, corners, I, D) + x[peo_idx].append(p2[0]) + z[peo_idx].append(p2[2]) + + for i in range(npeo): + if len(x[i]) == 0: + location[i].append([None, None]) + else: + location[i].append([np.mean(x[i]), np.mean(z[i])]) + + if frame < 300: + print(frame,location[0][frame-1]) + + print('frame:', frame) + print('fail_cnt:', fail_cnt) + location = np.array(location) + return location + +def fix_frame(l1, l2): + ## 补足缺失的帧 + loc = [] + print(l1.shape, l2.shape) + for i in range(min(len(l1), len(l2))): + if l1[i,0] is None and l2[i,0] is None: + loc.append(None) + elif l1[i,0] is None: + loc.append(l2[i]) + elif l2[i,0] is None: + loc.append(l1[i]) + else: + loc.append([(l1[i,0]+l2[i,0])/2, (l1[i,1]+l2[i,1])/2]) + + + for idx, p in enumerate(loc): + if p is None: + front_id = -1 + back_id = -1 + for i in range(5): + if idx-i < 0: + break + if loc[idx-i] is not None: + front_id = idx-i + break + for i in range(5): + if idx+i >= len(loc): + break + if loc[idx+i] is not None: + back_id = idx+i + break + + if front_id != -1 and back_id != -1: + front = loc[front_id] + back = loc[back_id] + loc[idx] = [(front[0] * (back_id - idx) + back[0] * (idx - front_id)) / (back_id - front_id), + (front[1] * (back_id - idx) + back[1] * (idx - front_id)) / (back_id - front_id)] + elif front_id != -1: + loc[idx] = loc[front_id] + elif back_id != -1: + loc[idx] = loc[back_id] + elif idx > 100 and idx < len(loc) - 100: + print("critical error: too many missing frames") + assert False + else: + loc[idx] = [0, 0] + else: + pass + + loc = np.array(loc) + # print('frame:', len(loc)) + + x = loc[:, 0] + y = loc[:, 1] + ## x,y 通过低通滤波 + + x = np.array(x) + y = np.array(y) + x = np.convolve(x, np.ones(10)/10, mode='same') + y = np.convolve(y, np.ones(10)/10, mode='same') + return x, y + +### main + + +# loc1 = get_location(cap1,Imat,Dmat) +loc1 = pickle.load(open(folder + 'loc1.pkl', 'rb')) +pickle.dump(loc1, open(folder + 'loc1.pkl', 'wb')) +print('loc1:', loc1.shape) +for i in range(loc1.shape[1]): + for peo in range(npeo): + if loc1[peo,i,0] is not None: + loc1[peo,i,0] = cam1_loc[0] - loc1[peo,i,0] ## 正负号由位置定 + loc1[peo,i,1] = cam1_loc[1] - loc1[peo,i,1] + +# loc2 = get_location(cap2,Imat2,Dmat2) +loc2 = pickle.load(open(folder + 'loc2.pkl', 'rb')) +pickle.dump(loc2, open(folder + 'loc2.pkl', 'wb')) +print('loc2:', loc2.shape) +for i in range(loc2.shape[1]): + for peo in range(npeo): + if loc2[peo,i,0] is not None: + loc2[peo,i,0] = cam2_loc[0] + loc2[peo,i,0] + loc2[peo,i,1] = cam2_loc[1] + loc2[peo,i,1] + +plt.plot(loc1[0,:,0], loc1[0,:,1],"r") +plt.plot(loc2[0,:,0], loc2[0,:,1],"g") +plt.show() +# exit() + + +x = [] +y = [] +for i in range(npeo): + if not all(loc1[i,:,0] == None) and not all(loc2[i,:,0] == None): + xx,yy = fix_frame(loc1[i,:,:], loc2[i,:,:]) + x.append(xx) + y.append(yy) + +x = np.array(x) +y = np.array(y) + +print(x.shape, y.shape) +from scipy.io import savemat +savemat(folder + 'loc.mat', {'x':x, 'y':y}) + +npeo = x.shape[0] +# print(x.shape, y.shape) +print("valid people:", npeo) +for i in range(npeo): + plt.plot(x[i,10:-10], y[i,10:-10]) +## x,y分度值250 +plt.xticks(np.arange(0, 5500, 250)) +plt.yticks(np.arange(0, 5500, 250)) +plt.grid() +plt.show() + + +# # plt.show() +# plt.savefig('demo.png') + + diff --git a/real_shot_code/findframe.py b/real_shot_code/findframe.py new file mode 100644 index 0000000..7b2c5fe --- /dev/null +++ b/real_shot_code/findframe.py @@ -0,0 +1,42 @@ +import cv2 + +## p1: 109-957 camera: 133-1994 +fp = "./1102/005/output1.mp4" +#./1/001/MVI_0075.mp4 cam：220-9873 output1: 315-4810 +#./1/002/MVI_0076.mp4 cam：74-19697 output1: 254-9219 +#./1/003/MVI_0078.mp4 cam：68-27497 p1: 169-12686 +#./1/004/MVI_0079.mp4 cam：77-12295 p1: 200-5188 +#./1/005/MVI_0080.mp4 cam：117-44790 p1: 344-20081 +#./1/006/MVI_0081.mp4 cam：57-38461 p1: 250-17276 + +#./1104/003/MVI_0085.mp4 cam：- p1: 284-16050 作废 +#./1104/007/MVI_0092.mp4 cam：58-40848 p1: 172-19907 +#./1104/008/MVI_0093.mp4 cam：45-41784 p1: 215-20626 +#./1104/009/MVI_0094.mp4 cam：74-42000 p1: 131-20816 + +#./1102/001/MVI_0095.mp4 cam：46-39995 p1: 204-19514 +#./1102/003/MVI_0096.mp4 cam：37-41545 p1: 207-34292 +#./1102/004/MVI_0097.mp4 cam：100-41688 p1: 241-35427 +#./1102/005/MVI_0098.mp4 cam：94-41023 p1: 377-38379 + +cap = cv2.VideoCapture(fp) +cnt = 0 +while (cap.isOpened()): + ret, frame = cap.read() + if ret == True: + + cnt += 1 + if cnt % 100 == 0: + print(cnt) + if cnt > 38300 : + cv2.imshow('frame', frame) + cv2.waitKey(0) + print(cnt) + else: + pass + # cv2.waitKey(1) + # print(cnt) + else: + print(cnt) + assert False + break \ No newline at end of file diff --git a/real_shot_code/make_data.py b/real_shot_code/make_data.py new file mode 100644 index 0000000..8938256 --- /dev/null +++ b/real_shot_code/make_data.py @@ -0,0 +1,105 @@ +import cv2 +import numpy as np +import matplotlib.pyplot as plt +import pickle +import os + +project_folder = './data_1102/' +folder = './1102/001/' +map_size = [3.384,5] +cap = cv2.VideoCapture(folder + "MVI_0095.mp4") +cnt = 0 +start = 46 +end = 39995 + +st_trace = 204 +ed_trace = 19514 + +from scipy.io import loadmat +mat = loadmat(folder + 'loc.mat') +x = mat['x'].T.squeeze() +y = mat['y'].T.squeeze() + +x = x[st_trace:ed_trace] +xx = x +print(x.shape) +x = x.astype(np.float32) +## 插值使得x为end-start+1个元素 +x = np.interp(np.arange(end-start+1)/(end-start+1), np.arange(ed_trace-st_trace)/(ed_trace-st_trace), x) + + +y = y[st_trace:ed_trace] +y = y.astype(np.float32) +yy = y +y = np.interp(np.arange(end-start+1)/(end-start+1), np.arange(ed_trace-st_trace)/(ed_trace-st_trace), y) + + +x = x/map_size[0]/1000 +y = y/map_size[1]/1000 + +assert all(x >= 0) and all(x <= 1.05) +assert all(y >= 0) and all(y <= 1.05) +# plt.plot(x[:100] * 1000 * map_size[0],y[:100] * 1000 * map_size[1]) +# plt.plot(xx[:100],yy[:100]) +# plt.show() + +# exit() + +vid_len = 256 +idx = 0 +nowvid = [] + +offset = len(os.listdir(project_folder)) + +frames = [] +while (cap.isOpened()): + ret, frame = cap.read() + # frames.append(frame) + if ret == True: + cnt += 1 + else: + # assert False + break + if cnt > start + 10 and cnt < end - 10: + frames.append(frame) + if len(frames) == vid_len: + for i in frames: + f = i + f = cv2.resize(f, (128,128)) + # cv2.imshow('frame', f) + # cv2.waitKey(0) + + ## c,h,w -> h,w,c + f = f.transpose(2,0,1) + nowvid.append(f) + + if len(nowvid) == vid_len: + out_folder = os.path.join(project_folder, str(idx + offset)) + if not os.path.exists(out_folder): + os.makedirs(out_folder) + nowvid = np.array(nowvid) + nowvid = np.transpose(nowvid, (1,0,2,3)) + # print(nowvid.shape) + assert nowvid.shape == (3, vid_len, 128, 128) + np.save(out_folder + '/video_128.npy', nowvid) + + nowvid = [] + idxstart = cnt - vid_len - start + idxend = cnt - start + ## cnt:相机开始后的绝对帧数 + ## x,y: 长度为start-end + + pos = [x[idxstart:idxend], y[idxstart:idxend]] + v = [x[idxstart+1:idxend+1] - x[idxstart:idxend], y[idxstart+1:idxend+1] - y[idxstart:idxend]] + pos = np.array(pos).T + v = np.array(v).T + assert pos.shape == (vid_len, 2) + assert v.shape == (vid_len, 2) + # print(pos) + from scipy.io import savemat + savemat(out_folder + "/route.mat", {"map_size":map_size,'route':pos, 'velocities':v}) + + idx += 1 + print(idx) + + frames = [] \ No newline at end of file diff --git "a/real_shot_code/\344\275\277\347\224\250\350\257\264\346\230\216.md" "b/real_shot_code/\344\275\277\347\224\250\350\257\264\346\230\216.md" new file mode 100644 index 0000000..885b65b --- /dev/null +++ "b/real_shot_code/\344\275\277\347\224\250\350\257\264\346\230\216.md" @@ -0,0 +1,14 @@ +## 1. 调用cam.py拍照 +连接摄像头、设置输出文件夹即可。使用拍手的方式可以对齐不同摄像头的帧数。两个摄像头的帧数是严格对齐的 + +## 使用det.py检测 +检测原理为利用相机成像原理，将相机坐标系的点与图片坐标系的进行pnp操作，即可得到tag在相机坐标系下的位置。tag在图片坐标系的位置由调用的apriltag库完成。 + +需要准备：相机内参、相机坐标、tag的大小。（这里简化为相机坐标系的x、z轴与现实的xy轴平行，从相机坐标系到现实转换简化了。） +检测时，两个摄像头联合完成，随后取均值。经补帧得到最后的xy坐标 + +## 使用findframe.py得到起始帧数 +播放视频，找到拍手的帧数。 + +## 使用make_data.py得到最终数据： +注意本程序不覆盖之前的，而是append \ No newline at end of file