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 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92
| import cv2 as cv import numpy as np import time import os
depthWinTitle = 'Depth'
cv.namedWindow(depthWinTitle) cv.createTrackbar("num", depthWinTitle, 0, 10, lambda x: None) cv.createTrackbar("blockSize", depthWinTitle, 5, 255, lambda x: None) cv.namedWindow("3D Cam") cv.moveWindow("3D Cam", 100, 50) cv.moveWindow(depthWinTitle, 800, 50)
def callbackfunc(e, x, y, f, p): if e == cv.EVENT_LBUTTONDOWN: print(threeD[y][x])
cv.setMouseCallback(depthWinTitle, callbackfunc, None)
fs = cv.FileStorage(r'intrinsics.yml', cv.FILE_STORAGE_READ) M1 = fs.getNode('M1').mat() D1 = fs.getNode('D1').mat() M2 = fs.getNode('M2').mat() D2 = fs.getNode('D2').mat()
fs = cv.FileStorage(r'extrinsics.yml', cv.FILE_STORAGE_READ) R = fs.getNode('R').mat() T = fs.getNode('T').mat() R1 = fs.getNode('R1').mat() P1 = fs.getNode('P1').mat() R2 = fs.getNode('R2').mat() P2 = fs.getNode('P2').mat() Q = fs.getNode('Q').mat()
cap0 = cv.VideoCapture(0) cap1 = cv.VideoCapture(1) size = (640, 480) left_map1, left_map2 = cv.initUndistortRectifyMap( M1, D1, R1, P1, size, cv.CV_16SC2) right_map1, right_map2 = cv.initUndistortRectifyMap( M2, D2, R2, P2, size, cv.CV_16SC2)
while True: ret, frame1 = cap0.read() ret, frame2 = cap1.read() frame1 = cv.resize(frame1, (640, 480), interpolation=cv.CV_8SC1) frame2 = cv.resize(frame2, (640, 480), interpolation=cv.CV_8SC1) frame = np.concatenate([frame1, frame2], axis=1) frame_left = frame1 frame_right = frame2
left_rectified = cv.remap(frame_left, left_map1, left_map2, cv.INTER_LINEAR) right_rectified = cv.remap( frame_right, right_map1, right_map2, cv.INTER_LINEAR) rectified = np.concatenate([left_rectified, right_rectified], axis=1) image = np.concatenate([frame, rectified])
imgL = cv.cvtColor(left_rectified, cv.COLOR_BGR2GRAY) imgR = cv.cvtColor(right_rectified, cv.COLOR_BGR2GRAY)
num = cv.getTrackbarPos("num", depthWinTitle) blockSize = cv.getTrackbarPos("blockSize", depthWinTitle) if blockSize % 2 == 0: blockSize += 1 if blockSize < 5: blockSize = 5
stereo = cv.StereoBM_create(numDisparities=16 * num, blockSize=blockSize) disparity = stereo.compute(imgL, imgR)
disp = cv.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv.NORM_MINMAX, dtype=cv.CV_8U) threeD = cv.reprojectImageTo3D(disparity.astype(np.float32) / 16., Q) for i in range(1, 30): cv.line(image, (0, 16 * i), (640, 16 * i), (0, 255, 0), 1)
cv.imshow('3D Cam', image) cv.imshow(depthWinTitle, disp)
if cv.waitKey(1) & 0xFF == ord('q'): break
cap0.release() cap1.release() cv.destroyAllWindows()
|