以下是将双目视觉三维重建的完整Python代码整合到单个文件中的实现。此代码包含了双目标定、立体校正、立体匹配、视差计算以及点云生成等功能模块。 --- ### 双目视觉三维重建 Python 完整代码 ```python import numpy as np import cv2 from matplotlib import pyplot as plt def load_images(): """加载左右图像""" img_left = cv2.imread("left.jpg", 0) # 左侧灰度图像 img_right = cv2.imread("right.jpg", 0) # 右侧灰度图像 return img_left, img_right def calibrate_cameras(): """ 相机标定部分，假设已经通过标定工具获得了相机内参矩阵K和畸变系数D。 如果未标定，则可以运行mono_camera_calibration.py或stereo_camera_calibration.py脚本来完成标定[^2]。 """ K = np.array([[1.42137032e+03, 0.00000000e+00, 6.04816620e+02], [0.00000000e+00, 1.42203259e+03, 3.72745263e+02], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) D = np.array([[-0.36287734, 0.1288602, 0.00080295, 0.0021035, -0.05076447]]) return K, D def rectify_images(img_left, img_right, K, D): """对图像进行去畸变处理并完成极线矫正""" h, w = img_left.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K, D, (w, h), 1, (w, h)) undistorted_img_left = cv2.undistort(img_left, K, D, None, newcameramtx) undistorted_img_right = cv2.undistort(img_right, K, D, None, newcameramtx) return undistorted_img_left, undistorted_img_right def compute_disparity_map(undistorted_img_left, undistorted_img_right): """使用StereoBM算法计算视差图""" stereo = cv2.StereoBM_create(numDisparities=16 * 8, blockSize=15) disparity = stereo.compute(undistorted_img_left, undistorted_img_right).astype(np.float32) / 16 normalized_disparity = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) return disparity, normalized_disparity def generate_point_cloud(disparity, K): """根据视差图生成三维点云""" focal_length = K[0][0] baseline = 0.1 # 基线距离 (单位：米) cx = K[0][2] cy = K[1][2] Q = np.array([ [1, 0, 0, -cx], [0, 1, 0, -cy], [0, 0, 0, focal_length], [0, 0, baseline, 0] ]) points_3d = cv2.reprojectImageTo3D(disparity, Q) mask = disparity > disparity.min() colors = cv2.cvtColor(cv2.imread("left.jpg"), cv2.COLOR_BGR2RGB) out_points = points_3d[mask] out_colors = colors[mask] return out_points, out_colors def save_point_cloud(out_points, out_colors, filename="output.ply"): """保存点云至PLY文件""" def write_ply(fn, points, colors): header = '''ply format ascii 1.0 element vertex %d property float x property float y property float z property uchar red property uchar green property uchar blue end_header ''' with open(fn, 'wb') as f: f.write((header % len(points)).encode()) np.savetxt(f, np.c_[points, colors], fmt='%f %f %f %d %d %d ') write_ply(filename, out_points, out_colors) print(f"{filename} saved") if __name__ == "__main__": # Step 1: Load images img_left, img_right = load_images() # Step 2: Calibrate cameras and rectify images K, D = calibrate_cameras() undistorted_img_left, undistorted_img_right = rectify_images(img_left, img_right, K, D) # Display undistorted images cv2.imshow('Undistorted Left Image', undistorted_img_left) cv2.imshow('Undistorted Right Image', undistorted_img_right) cv2.waitKey(0) cv2.destroyAllWindows() # Step 3: Compute disparity map disparity, normalized_disparity = compute_disparity_map(undistorted_img_left, undistorted_img_right) # Display disparity map plt.imshow(normalized_disparity, cmap='gray') plt.title("Normalized Disparity Map") plt.show() # Step 4: Generate point cloud out_points, out_colors = generate_point_cloud(disparity, K) # Step 5: Save point cloud to PLY file save_point_cloud(out_points, out_colors) ``` --- ### 功能说明 1. **图像加载**: 使用`load_images()`函数读取左右摄像头拍摄的图片。 2. **相机标定**: 调用`calibrate_cameras()`返回相机内参矩阵`K`和畸变系数`D`。如果尚未标定，可参考提供的标定工具[^2]。 3. **图像校正**: 利用`rectify_images()`去除镜头畸变并对齐极线。 4. **视差计算**: 使用`compute_disparity_map()`基于StereoBM算法生成视差图。 5. **点云生成**: 结合重投影矩阵`Q`调用`generate_point_cloud()`生成三维点云。 6. **点云存储**: 将点云数据保存为`.ply`格式文件以便后续可视化。 --- ###