f = Q[2][3]
Q_inv = np.linalg.inv(Q)
objpoints = np.matrix([[0], [0], [depth], [1]])
imgpointsmat = Q_inv * f / depth * np.matrix(objpoints)
imgpoints = imgpointsmat.tolist()
disValue = int(imgpoints[2][0])
return disValue
def getWidth_px(Q, realWidth, Z):
winX = getWinX(Q, 0, realWidth, Z)
width_px = int(winX[1] - winX[0])
return width_px
mapx1, mapy1, mapx2, mapy2, Q, roi1, roi2 = rectify.init()
# 得到检测到的最大距离对应的视差值
minDisparity = getDisparityValue(Q, maxDepth)
maxDisparity = getDisparityValue(Q, minDepth)
# 得到在maxDepth距离下实际车宽在像素图中对应的像素宽度，用于判断可容小车通过的空隙
car_width_px = getWidth_px(Q, carWidth, maxDepth)
winY = getWinY(Q, 0, capHeight, maxDepth)
winX = getWinX(Q, -carWidth / 2, carWidth / 2, maxDepth)
stereo = sm.readyStereoBM(roi1, roi2)
print 'Q:\n'.ljust(13), Q
print 'minDisparity:\n'.ljust(13), minDisparity
print 'maxDisparity:\n'.ljust(13), maxDisparity
print 'car_width_px:\n'.ljust(13), car_width_px
print 'winY:\n'.ljust(13), winY
print 'winX:\n'.ljust(13), winX
stereobm.setROI1(roi2)
return stereobm
def getDisparity(stereo, img1, img2, mapx1, mapy1, mapx2, mapy2):
dst1 = cv2.remap(img1, mapx1, mapy1, cv2.INTER_LINEAR)
dst2 = cv2.remap(img2, mapx2, mapy2, cv2.INTER_LINEAR)
gray1 = cv2.cvtColor(dst1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(dst2, cv2.COLOR_BGR2GRAY)
disparity = stereo.compute(gray1, gray2)/16
# disparity = cv2.medianBlur(disparity, 5)
return disparity
if __name__ == '__main__':
import rectify as re
mapx1, mapy1, mapx2, mapy2, Q, roi1, roi2= re.init()
stereo = readyStereoBM(roi1, roi2)
img1 = cv2.imread('data/img0/1.jpg')
img2 = cv2.imread('data/img1/1.jpg')
img1 = cv2.resize(img1, (BINIMG_W, BINIMG_H), interpolation=cv2.INTER_CUBIC)
img2 = cv2.resize(img2, (BINIMG_W, BINIMG_H), interpolation=cv2.INTER_CUBIC)
disparity = getDisparity(stereo, img1, img2, mapx1, mapy1, mapx2, mapy2)
disparity = cv2.medianBlur(disparity, 5)
cv2.imshow('disparity', np.uint8(disparity))
cv2.waitKey(0)
cv2.destroyAllWindows()
f = Q[2][3]
Q_inv = np.linalg.inv(Q)
objpoints = np.matrix([[0], [0], [depth], [1]])
imgpointsmat = Q_inv*f/depth*np.matrix(objpoints)
imgpoints = imgpointsmat.tolist()
disValue = int(imgpoints[2][0])
return disValue
def getWidth_px(Q, realWidth, Z):
winX = getWinX(Q, 0, realWidth, Z)
width_px = int(winX[1]-winX[0])
return width_px
mapx1, mapy1, mapx2, mapy2, Q, roi1, roi2 = rectify.init()
# 得到检测到的最大距离对应的视差值
minDisparity = getDisparityValue(Q, maxDepth)
maxDisparity = getDisparityValue(Q, minDepth)
# 得到在maxDepth距离下实际车宽在像素图中对应的像素宽度，用于判断可容小车通过的空隙
car_width_px = getWidth_px(Q, carWidth, maxDepth)
winY = getWinY(Q, 0, capHeight, maxDepth)
winX = getWinX(Q, -carWidth/2, carWidth/2, maxDepth)
stereo = sm.readyStereoBM(roi1, roi2)
print 'Q:\n'.ljust(13), Q
print 'minDisparity:\n'.ljust(13), minDisparity
print 'maxDisparity:\n'.ljust(13), maxDisparity
print 'car_width_px:\n'.ljust(13), car_width_px
print 'winY:\n'.ljust(13), winY
print 'winX:\n'.ljust(13), winX