- 标定，得到相机内参矩阵，或直接读取以前保留的标定结果，标定过程见https://blog.csdn.net/weixin_39266208/article/details/120955007
- 利用VideoCapture打开双目摄像头，读取并分割成左右两张图像。
- 通过stereoRectify initUndistortRectifyMap remap 得到校正后的图像，两张图像在同一坐标平面，y对齐。
- StereoSGBM_create(python) + compute得到视差。
- 然后通过reprojectImageTo3D映射到点云，需要注意SGBM算法生成的视差要/16.0，根据文档，整数中有4位代表小数部分。
- 利用官方示例文件提供的write_ply函数写到点云文件，ply格式。带有rgb信息，深度信息是和左侧的图相对应的，使用左侧图生成颜色。
- 通过meshlab软件或者pcl来可视化点云，注意可能看不到点云，因为标定的时候使用的单位是mm，数值过大，可能缩放半天才能出来，并且如果缩放方向错了，就不会出来了，可以在标定的时候使用m作为单位，或者保存成ply文件的时候除以1000。
- StereoSGBM_create参数，num_disp，官方给的举例的太小了，设置多大呢，看自己的测量范围，比如我的标定结果显示，焦距790，相机基线距离(两摄像头的距离)120，那么根据公式z=f*b/d (近似计算)，f使用标定的焦距fx或fy，fx和fy，以及两个相机之间的数值相差应该很小，标定得到的相机内参中的f是真实距离/像素间的距离的结果，即像素数，d是x方向的视差，也是像素数，b是带有单位的，比如我的情况是0.12m，因为基线距离近似120mm，如果测量距离是[0.2m, 10m],那么[790*0.12/10=9.48, 790*0.12/0.2=474]，按照文档要求，要16整数倍对齐，可见，越近，视差越大，这点画图也能看出来，如果视差取的比较小，则不能估计过近物体的深度，对于极限情况，1个像素，790*0.12/1=94.8，也就是说对于大约94.8m以后是无法区分的，根据ORB-SLAM2作者说的经验值，40倍相机基线的距离以内都可以进行三角测量，即精度足够高。关于相机内参和相关计算，参考《视觉SLAM14讲》103-104页，《OpenCV4快速入门》324-325页。不知道为什么，我的结果中总是含有比较小的视差，并且很多，所以导致点云中在一个比较远的距离上，存在极大量的点，差不多是整个图像，但是明明已经设置minDisparity = 16了，通过过滤掉小于16的，这些异常点都消失了，他们本来应该是无法计算的点，应该为0才对。uniquenessRatio，最优比次优强的百分比，官方推荐的比较小，如果生成点云中错误点很多，可以考虑增大，preFilterCap根据推荐，使用了63。其他参数都是按照文档和官方示例程序中的参数设置的。
如果有理解有错误，还望大佬指点。
下面的代码是试验参数和效果的时候的代码，胡乱写的，参考了官方的示例。在相机标定程序后面添加这部分代码，官方的python示例程序不带相机内参。
ply_header = '''ply
format ascii 1.0
element vertex %(vert_num)d
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
end_header
'''
# 官方代码的sample下面的python示例程序中的写ply文件的代码
def write_ply(fn, verts, colors):
verts = verts.reshape(-1, 3) / 1000.
colors = colors.reshape(-1, 3)
verts = np.hstack([verts, colors])
with open(fn, 'wb') as f:
f.write((ply_header % dict(vert_num=len(verts))).encode('utf-8'))
np.savetxt(f, verts, fmt='%f %f %f %d %d %d ')
cap = cv2.VideoCapture(2)
# 这个摄像头 设置宽高，帧率会自动跟着变
cap.set(cv2.CAP_PROP_frame_WIDTH,2560)
cap.set(cv2.CAP_PROP_frame_HEIGHT, 960)
fcc = cv2.VideoWriter_fourcc('M','J','P','G')
#cap.set(cv2.CAP_PROP_FOURCC, fcc)
while cap.isOpened():
ret, frame = cap.read()
if ret:
#frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
l = frame[:,0:1280,:]
r = frame[:,1280:,:]
img_left.append(l)
img_right.append(r)
imgsize = tuple(reversed(l.shape[0:2]))
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(cameraMatrix_l, distCoeffs_l, cameraMatrix_r, distCoeffs_r, imgsize, R, T)
maplx , maply = cv2.initUndistortRectifyMap(cameraMatrix_l, distCoeffs_l, R1, P1, imgsize, cv2.CV_16SC2)
maprx , mapry = cv2.initUndistortRectifyMap(cameraMatrix_r, distCoeffs_r, R2, P2, imgsize, cv2.CV_16SC2)
lr = cv2.remap(l, maplx, maply, cv2.INTER_LINEAR)
rr = cv2.remap(r, maprx, mapry, cv2.INTER_LINEAR)
lr_backup = lr
all = np.hstack((lr,rr))
print(all.shape)
cv2.line(all, (0, 300), (all.shape[1], 300), (0,0,255), 2)
window_size = 5
min_disp = 16
num_disp = 640-min_disp
#stereo = cv2.StereoBM_create()
stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
numDisparities = num_disp,
blockSize = window_size,
P1 = 8*3*window_size**2,
P2 = 32*3*window_size**2,
disp12MaxDiff = 1,
uniquenessRatio = 50,
speckleWindowSize = 100,
speckleRange = 32,
#mode = cv2.StereoSGBM_MODE_HH,
preFilterCap=63)
cv2.imshow('a', all)
c = cv2.waitKey()
#cv2.destroyAllWindows()
if c == 27:
break
disp = stereo.compute(lr, rr)
print('generating 3d point cloud...', disp.min(), disp.max(), disp.shape)
# guess for focal length
disp = disp.astype(np.float32) / 16.0 #* depth_mask
points = cv2.reprojectImageTo3D(disp, Q)
colors = cv2.cvtColor(lr_backup, cv2.COLOR_BGR2RGB)
mask = disp >15.
out_points = points[mask]
out_colors = colors[mask]
out_fn = 'out.ply'
c = cv2.waitKey()
if c == 27:
break
if c == 32:
write_ply(out_fn, out_points, out_colors)
print('%s saved' % out_fn)
else:
break
cv2.destroyAllWindows()
cap.release()
