import cv2
import numpy as np
import matplotlib.pyplot as plt
class stereoCamera(object):
def __init__(self):
self.cam_matrix_l = np.array([[1.2498849e+03, 0.00000000e+00, 6.829243e+02],
[0.00000000e+00, 1.2044291e+03, 5.188187e+02],
[0.00000000e+00, 0.00000000e+00, 1.0000000e+00]])
self.cam_matrix_r = np.array([[1.2445653e+03, 0.00000000e+00, 7.0047636e+02],
[0.00000000e+00, 1.1918681e+03, 5.4966224e+02],
[0.00000000e+00, 0.00000000e+00, 1.0000000e+00]])
self.distortion_l = np.array([-0.18717783, 0.56100237, 0.00552916, 0.0119844, -0.81453985])
self.distortion_r = np.array(
[3.95667206e-03, -4.37876986e-01, 8.43310080e-04, 2.29506074e-02, 1.05390445e+00])
self.R = np.array([[0.99671652, -0.01553918, -0.07946511],
[0.01456501, 0.99981168, -0.01282404],
[0.07964942, 0.01162452, 0.99675516]])
self.T = np.array([-109.64067213, 0.71621923, -12.1202964])
self.doffs = 0.0
self.isRectified = True
def getRectifyTransform(high, wide, config):
left_K = config.cam_matrix_l
right_K = config.cam_matrix_r
left_distortion = config.distortion_l
right_distortion = config.distortion_r
R = config.R
T = config.T
# 计算校正变换
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion,
(wide, high), R, T, alpha=0)
map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (wide, high), cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (wide, high), cv2.CV_32FC1)
return map1x, map1y, map2x, map2y, Q
def rectifyImage(img1, img2, map1x, map1y, map2x, map2y):
rectify_img1 = cv2.remap(img1, map1x, map1y, cv2.INTER_AREA)
rectify_img2 = cv2.remap(img2, map2x, map2y, cv2.INTER_AREA)
return rectify_img1, rectify_img2
def draw_line(img1, img2):
height = max(img1.shape[0], img2.shape[0])
width = img1.shape[1] + img2.shape[1]
output = np.zeros((height, width, 3), dtype=np.uint8)
output[0:img1.shape[0], 0:img1.shape[1]] = img1
output[0:img2.shape[0], img1.shape[1]:] = img2
line_interval = 50 # 直线间隔
for k in range(height // line_interval):
cv2.line(output, (0, line_interval * (k + 1)),
(2 * width, line_interval * (k + 1)),
(0, 255, 0), thickness=2, lineType=cv2.LINE_AA)
plt.imshow(output, 'gray')
plt.show()
# cv.imwrite('./lineOut.jpg',output)
return output
def opencv_SGBM(left_img, right_img, use_wls=False):
blockSize = 11
paramL = {"minDisparity": 0, #表示可能的最小视差值。通常为0,但有时校正算法会移动图像,所以参数值也要相应调整
"numDisparities": 170, #表示最大的视差值与最小的视差值之差,这个差值总是大于0。在当前的实现中,这个值必须要能被16整除,越大黑色边缘越多,表示不能计算视差的区域
"blockSize": blockSize,
"P1": 8 * 3 * blockSize * blockSize, #控制视差图平滑度的第一个参数
"P2": 32 * 3 * blockSize * blockSize, #控制视差图平滑度的第二个参数,值越大,视差图越平滑。P1是邻近像素间视差值变化为1时的惩罚值,
#p2是邻近像素间视差值变化大于1时的惩罚值。算法要求P2>P1,stereo_match.cpp样例中给出一些p1和p2的合理取值。
"disp12MaxDiff": 1, #表示在左右视图检查中最大允许的偏差(整数像素单位)。设为非正值将不做检查。
"uniquenessRatio": 10, #表示由代价函数计算得到的最好(最小)结果值比第二好的值小多少(用百分比表示)才被认为是正确的。通常在5-15之间。
"speckleWindowSize": 50, #表示平滑视差区域的最大窗口尺寸,以考虑噪声斑点或无效性。将它设为0就不会进行斑点过滤,否则应取50-200之间的某个值。
"speckleRange": 1, #指每个已连接部分的最大视差变化,如果进行斑点过滤,则该参数取正值,函数会自动乘以16、一般情况下取1或2就足够了。
"preFilterCap": 31,
"mode": cv2.STEREO_SGBM_MODE_SGBM_3WAY
}
matcherL = cv2.StereoSGBM_create(**paramL)
# 计算视差图
dispL = matcherL.compute(left_img, right_img)
# WLS滤波平滑优化图像
if use_wls:
paramR = paramL
paramR['minDisparity'] = -paramL['numDisparities']
matcherR = cv2.StereoSGBM_create(**paramR)
dispR = matcherR.compute(right_img, left_img)
# dispR = np.int16(dispR)
lmbda = 80000
sigma = 1.5
filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=matcherL)
filter.setLambda(lmbda)
filter.setSigmaColor(sigma)
dispL = filter.filter(dispL, left_img, None, dispR)
#双边滤波
dispL = cv2.bilateralFilter(dispL.astype(np.float32), d=9, sigmaColor=75, sigmaSpace=75)
# 除以16得到真实视差(因为SGBM算法得到的视差是×16的)
dispL[dispL < 0] = 1e-6
dispL = dispL.astype(np.int16)
dispL = dispL / 16.0 #float 64 必须转为uint才能正确显示,如果不使用空洞填充的话
dispL_nor = cv2.normalize(dispL,None,0,255,cv2.NORM_MINMAX)
dispL_nor = dispL_nor.astype(np.uint8)
cv2.imshow('dispL',cv2.resize(dispL_nor,(960,540)))
cv2.waitKey(0)
return dispL
def create_point_cloud(dispL, img, Q):
# step = 2 # 取样间隔
# dispL = dispL[::step, ::step]
# img = img[::step, ::step]
points = cv2.reprojectImageTo3D(dispL, Q)
colors = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
# mask = dispL > dispL.min()
mask = (dispL > 0)
points = points.astype(np.float16)
out_points = points[mask]
out_colors = colors[mask]
return out_points, out_colors
def save_point_cloud(points, colors, filename, depth_range):
colors = colors.reshape(-1, 3)
points = points.reshape(-1, 3)
# 只要规定区域的点
print("Depth range in point cloud:", points[:, 2].min(), points[:, 2].max())
valid_indices = np.where((points[:, 2] > depth_range[0]) & (points[:, 2] < depth_range[1]))[0]
filtered_points = points[valid_indices]
# 带颜色的
filtered_colors = colors[valid_indices]
point_cloud = np.hstack([filtered_points, filtered_colors])
np.savetxt(filename, point_cloud, fmt='%0.4f %0.4f %0.4f %d %d %d')
# # 不带颜色的
# point_cloud = filtered_points
# np.savetxt(filename, point_cloud, fmt='%0.4f %0.4f %0.4f')
def creatDepthView(dispL = None,focal_length=1733.74,baseline = 0.53662): #focal_length=1733.74,baseline = 0.53662
dispL[dispL < 55] = 55 #根据视差图中的直方图确定,也可以根据数据集中提供的视差范围确定
dispL[dispL > 142] = 142
depth_map = (baseline * focal_length) / (dispL.astype(np.float32))
print(np.max(depth_map))
print(np.min(depth_map))
# 归一化深度图到 0-255 范围
depth_normalized = cv2.normalize(depth_map, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX)
depth_normalized = depth_normalized.astype(np.uint8)
# 使用伪彩色图显示深度
depth_color_map = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_JET)
return depth_color_map
if __name__ == '__main__':
imgl = cv2.imread('TEST_LEFT.png')
imgr = cv2.imread('TEST_RIGHT.png')
high, wide = imgl.shape[0:2]
# # 读取相机参数
config = stereoCamera()
# # 消除图像畸变
# imgl_qb = cv.undistort(imgl, config.cam_matrix_l, config.distortion_l)
# imgr_qb = cv.undistort(imgr, config.cam_matrix_r, config.distortion_r)
# 数据集中图像是矫正之后的,
# # 极线校正 获取Q矩阵备用
map1x, map1y, map2x, map2y, Q = getRectifyTransform(high, wide, config)
# imgl_jx, imgr_jx = rectifyImage(imgl_qb, imgr_qb, map1x, map1y, map2x, map2y)
# print("Print Q!")
# print(Q)
# # 绘制等间距平行线,检查效果
# line = draw_line(imgl_jx, imgr_jx)
# line = draw_line(imgl_qb, imgr_qb)
imgl_hd = cv2.cvtColor(imgl, cv2.COLOR_BGR2GRAY)
imgr_hd = cv2.cvtColor(imgr, cv2.COLOR_BGR2GRAY)
imgl_hd = cv2.equalizeHist(imgl_hd)
imgr_hd = cv2.equalizeHist(imgr_hd)
# 立体匹配——SBGM算法
dispL = opencv_SGBM(imgl_hd, imgr_hd, True)
# 视差图空洞填充
def fill_disparity_map(disp):
# 创建掩码:标记无效区域(值为0的位置)
mask = np.uint8(disp == 0)
# 使用中值滤波填充空洞
filled_disp = cv2.medianBlur(disp.astype(np.float32), 5)
# 仅替换原始视差图中的空洞区域
disp[mask] = filled_disp[mask]
return disp
# # 计算视差图中每个值出现的次数
# unique, counts = np.unique(dispL, return_counts=True)
# # 绘制直方图
# plt.figure(figsize=(10, 6))
# plt.bar(unique, counts, width=1, color='b', alpha=0.7)
# plt.xlabel('视差值')
# plt.ylabel('出现次数')
# plt.title('视差图直方图')
# plt.show()
#深度图
depthmap = creatDepthView(dispL)
# 深度图空洞填充
# depthmap = insert_depth_filled(depthmap)
#show original depthMap distanceMap
cv2.imshow('image',cv2.resize(imgl,(960,540)))
cv2.imshow('dispL',cv2.resize(dispL,(960,540)))
cv2.imshow('depthmap',cv2.resize(depthmap,(960,540)))
# cv.imwrite('depth.png',depthmap)
# cv.imwrite('displ.png',dispL)
cv2.waitKey(0)
# 生成三维点云
assert imgl.shape[:2] == dispL.shape[:2], "Image and disparity map must have the same resolution!"
# points, colors = create_point_cloud(dispL, imgl_jx, Q)
points, colors = create_point_cloud(dispL, imgl, Q)
# 保存点云(带颜色.xyz;不带颜色.ply)
depth_range = [5000, 20000] #深度范围
save_point_cloud(points, colors, 'point_cloud1.xyz', depth_range)
在保证可以继续运行的条件下,帮我修改前面这段代码,使得深度图和视差图和点云图更清晰更细致
最新发布