At present, I am trying to do some work on depth estimation. The following is my code
import cv2
import numpy as np
import math
left_camera_matrix = np.array([
[379.631915328262, -0.102220945295059, 315.958769543110],
[ 0, 379.732222668215, 203.885845031288],
[ 0, 0, 1]
])
left_distortion = np.array([-0.417587221276122, 0.197712476970145, 0.000411591628002515, 0.000387147224501379, 0])
right_camera_matrix = np.array([
[381.634497453356, -0.288819419456584, 317.646637211302],
[ 0, 381.808417017535, 201.083906323572],
[ 0, 0, 1]
])
right_distortion = np.array([-0.411726209080630, 0.177029424249621, -9.24380995973130e-05, -0.000341428776614118, 0])
R = np.array([
[ 0.999991875121332, 0.000530694118664997, -0.00399600488918945],
[-0.000511655123048737, 0.999988521078969, 0.00476402343943493],
[0.00399848725858538, - 0.00476194015594924, 0.999980667825931]
]) # 使用Rodrigues变换将om变换为R
T = np.array([
[-80.0808305021553],
[ 0.104412532264216],
[ -0.0774106064433224]
]) # 平移关系向量
size = (640, 400) # 图像尺寸
# 进行立体更正
(R1, R2, P1, P2, Q, validPixROI1, validPixROI2) = \
cv2.stereoRectify(
left_camera_matrix, left_distortion, right_camera_matrix, right_distortion, size, R, T)
# 计算更正map
left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)
def callbackFunc(e, x, y, f, p):
if e == cv2.EVENT_LBUTTONDOWN:
print(threeD[y][x])
def onmouse_pick_points(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
threeD = param
print('\n像素坐标 x = %d, y = %d' % (x, y))
# print("世界坐标是：", threeD[y][x][0], threeD[y][x][1], threeD[y][x][2], "mm")
print("世界坐标xyz 是：", threeD[y][x][0] / 1000.0, threeD[y][x][1] / 1000.0, threeD[y][x][2] / 1000.0, "m")
distance = math.sqrt(threeD[y][x][0] ** 2 + threeD[y][x][1] ** 2 + threeD[y][x][2] ** 2)
distance = distance / 1000.0 # mm -> m
print("距离是：", distance, "m")
cap = cv2.VideoCapture(0)
(ret,frame) = cap.read()
img_channels = 3
stereo = cv2.StereoSGBM_create()
while ret:
(ret, frame) = cap.read()
left_frame = cv2.imread('./left.png')
right_frame = cv2.imread('./right.png')
img1_rectified = cv2.remap(left_frame, left_map1, left_map2, cv2.INTER_LINEAR)
img2_rectified = cv2.remap(right_frame, right_map1, right_map2, cv2.INTER_LINEAR)
imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY)
imgR = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY)
blockSize=10
stereo.setMinDisparity(0)
stereo.setNumDisparities(6*16)
stereo.setBlockSize(10)
stereo.setDisp12MaxDiff(1)
stereo.setPreFilterCap(0)
stereo.setUniquenessRatio(0)
stereo.setSpeckleRange(0)
stereo.setSpeckleWindowSize(0)
stereo.setP1(8 * 3 * blockSize**2)
stereo.setP2(32 * 3 * blockSize**2)
stereo.setMode(cv2.STEREO_SGBM_MODE_SGBM_3WAY)
left_disparity = stereo.compute(imgL,imgR)
disp = cv2.normalize(left_disparity,dst=None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
threeD = cv2.reprojectImageTo3D(left_disparity, Q, handleMissingValues=True)
threeD = threeD * 16
cv2.imshow("show",disp)
cv2.setMouseCallback("show", onmouse_pick_points, threeD)
if cv2.waitKey(1)&0xff==ord('q'):
break
cv2.destroyAllWindows()
cap.release()
the problem is that it can't work out when I want to know the object close to the camera by disparity map, the map becomes a mess. [1]: https://i.stack.imgur.com/JZHFK.png However, when the object is far away from the camera, it becomes a little better, but still not ideal [2]:https://i.stack.imgur.com/FnlAK.png I have tuned the parameters, but still, I can't get the ideal map.
here is the left frame[3]:https://i.stack.imgur.com/ZsaPx.png and the right frame [4]:https://i.stack.imgur.com/4Ruq6.png
