用Python和OpenCV从零搭建一个完整的双目视觉系统（五）

本系列文章旨在系统性地阐述如何利用 Python 与 OpenCV 库，从零开始构建一个完整的双目立体视觉系统。

本项目github地址：https://github.com/present-cjn/stereo-vision-python.git

三维重建与可视化

经过相机标定与立体匹配，我们已经成功获取了包含场景深度信息的“视差图”。现在，我们来到了从2D到3D的最后一次飞跃——三维重建 (3D Reconstruction)。

本篇文章将详细阐述如何利用视差图和相机几何参数，通过数学变换，计算出每个像素点在真实世界中的三维坐标，最终生成一个可交互的点云 (Point Cloud)。同时，我们还将实现一个交互式的2D深度图，以另一种直观的方式来展示和验证我们的测量结果。

1. 从视差到3D：reprojectImageTo3D 的作用

三维重建的根本原理是三角测量 (Triangulation)。在已知两个相机的位置（由平移向量T确定）、它们的朝向（由旋转矩阵R确定）以及它们的成像模型（由内参矩阵K确定）后，一个在左右图像上都被观察到的点，其在三维空间中的位置是唯一确定的。

幸运的是，我们不需要手动去实现复杂的三角测量公式。OpenCV的 stereoRectify 函数已经为我们计算好了一个关键的矩阵——4x4的重投影矩阵 Q。

这个 Q 矩阵封装了所有必要的几何信息（焦距、主点、基线距等）。我们只需要将像素坐标 (x, y) 和在该点上测得的视差 d 提供给它，就能通过一个简单的矩阵乘法，反解出该点的三维坐标 (X, Y, Z)。

cv2.reprojectImageTo3D 函数正是高效地为整张视差图执行这个矩阵运算的工具。

关键的尺度修正

我们在前文提到，SGBM算法输出的视差值被放大了16倍。官方文档指出，当向 reprojectImageTo3D 传入 CV_16S 格式的视差图时，它会将其视为没有小数位的整数。这会导致计算出的深度被精确地缩小16倍。因此，在调用该函数前，我们必须先将视差图转换为浮点数格式并除以16.0，以恢复其真实的像素单位。

2. 代码实现 (processing/reconstructor.py)

我们将所有重建逻辑封装在 Reconstructor 类中。

# processing/reconstructor.py
import cv2
import numpy as np
import config

class Reconstructor:
    def __init__(self):
        print("Initializing Reconstructor...")

    def reconstruct(self, disparity_map, left_rectified_img, Q_matrix):
        """
        返回两种形式的点云数据：
        1. 原始的、与图像对应的HxWx3的3D矩阵（用于交互式查找）。
        2. 经过过滤和清理的点列表（用于保存和3D可视化）。
        """
        # 1. 核心修正：将视差图从 CV_16S 转换为 CV_32F 并除以16
        true_disparity_map = disparity_map.astype(np.float32) / 16.0

        # 2. 使用修正后的真实视差图进行三维重建
        points_3D_matrix = cv2.reprojectImageTo3D(true_disparity_map, Q_matrix)

        colors_matrix = cv2.cvtColor(left_rectified_img, cv2.COLOR_BGR2RGB)

        # 3. 过滤无效点，生成干净的点列表
        #    使用 true_disparity_map 来创建掩码
        mask = true_disparity_map > true_disparity_map.min()
        points_3D_filtered = points_3D_matrix[mask]
        colors_filtered = colors_matrix[mask]

        # 4. 进一步进行深度过滤，移除无效的负值和过远的点
        positive_z_mask = points_3D_filtered[:, 2] > 0
        z_max_threshold = 10000.0 # 10米
        far_points_mask = points_3D_filtered[:, 2] < z_max_threshold
        final_mask = np.logical_and(positive_z_mask, far_points_mask)

        points_3D_filtered = points_3D_filtered[final_mask]
        colors_filtered = colors_filtered[final_mask]

        return points_3D_matrix, (points_3D_filtered, colors_filtered)

这个方法非常健壮，它一次性产出了两种我们需要的数据：用于精确像素索引的原始3D矩阵，和用于高效保存与显示的过滤后点列表。

3. 点云的保存与显示

对于3D数据的处理和可视化，我们引入了强大的 Open3D 库。

保存点云 (utils/file_utils.py)

我们将点云数据保存为 .ply 格式，这是一种通用的三维模型文件格式，可以被 MeshLab、Blender 等多种软件打开。

# utils/file_utils.py
def save_point_cloud(path, points_3D, colors):
    """使用 Open3D 将点云保存为 .ply 文件。"""
    try:
        import open3d as o3d
    except ImportError:
        # ... 错误处理 ...
        return

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points_3D)
    # Open3D 需要的颜色值是 0-1 范围的浮点数
    pcd.colors = o3d.utility.Vector3dVector(colors / 255.0)

    o3d.io.write_point_cloud(path, pcd)
    print(f"Point cloud saved to {path}")

显示点云 (visualization/visualizer.py)

Open3D 提供了一个非常易于使用的交互式窗口来显示点云。

# visualization/visualizer.py
def show_point_cloud(ply_file_path):
    """加载并显示 .ply 格式的点云文件。"""
    try:
        import open3d as o3d
    except ImportError:
        # ... 错误处理 ...
        return
    
    pcd = o3d.io.read_point_cloud(ply_file_path)
    if not pcd.has_points():
        # ... 错误处理 ...
        return
    
    # 创建一个可视化窗口并显示点云
    o3d.visualization.draw_geometries([pcd])

当用户在命令行使用 --view-3d 标志时，我们就会调用这个函数，弹出一个允许用户自由旋转、缩放和平移三维场景的窗口。

使用项目测试图片可以得到如下点云图

4. 打造交互式深度图

除了3D点云，我们还实现了一种更直接的2D可视化方式，让用户可以通过鼠标实时查询深度。这是通过 OpenCV 的鼠标事件回调机制实现的。

代码实现 (visualization/visualizer.py)

# visualization/visualizer.py
def show_interactive_depth_map(disparity_map, left_image_for_display, points_3D, ...):
    # ...
    # 定义一个在内部更新鼠标坐标的回调函数
    def on_mouse(event, x, y, flags, param):
        if event == cv2.EVENT_MOUSEMOVE:
            param['x'] = x
            param['y'] = y

    # 将回调函数绑定到窗口
    cv2.setMouseCallback(window_name, on_mouse, mouse_params)

    # 在主显示循环中
    while True:
        # ...
        # 获取当前鼠标坐标
        x, y = mouse_params['x'], mouse_params['y']

        # 如果鼠标在视差图区域内
        if w < x < w * 2 and 0 < y < h:
            # 从原始的 HxWx3 3D矩阵中，通过像素坐标直接索引到三维坐标
            point_3d = points_3D[y, x - w]
            pz = point_3d[2] # 这就是深度值Z

            # 过滤无效值并格式化文本
            if 0 < pz < 100000:
                distance_text = f"Distance: {int(pz)} mm"
            else:
                distance_text = "Distance: N/A"

            # 将文本绘制在图像上
            cv2.putText(...)

        cv2.imshow(window_name, display_copy)
        # ... (等待按键和窗口关闭的逻辑)

这个功能的核心在于，我们将 reconstruct 方法返回的、未经任何过滤的 points_3D_matrix 传递给了它。这保证了图像上的每一个 (x, y) 像素，都能在这个3D矩阵中找到一个与之对应的 (X, Y, Z) 坐标，从而实现了精确的实时深度查询。

使用项目测试图片可以得到如下深度图，此时鼠标放置的位置会显示该点的深度值

总结

通过本篇文章的步骤，我们成功地完成了从2D视差信息到3D空间数据的转换。我们不仅生成了可用于后续分析的三维点云文件，还实现了两种可视化工具，让我们的测量结果能够被直观地呈现和验证。

在下一篇，也是本系列的最后一篇文章中，我们将回顾整个项目的架构，并详细介绍如何使用我们最终打造出的这个命令行工具。