双目视觉测距实现

一、双目测距核心原理

1.1 三角测量原理

% 核心公式
% 视差：d = x_left - x_right
% 深度：Z = f * B / d

% 其中：
% f  → 焦距（像素单位）
% B  → 基线距离（两相机间距，单位mm）
% d  → 视差（像素）
% Z  → 深度（mm）

视觉化理解：

左相机    右相机
  o----B----o
   \       /
    \     /  ← 同一点P
     \   /
      \ /
       P（物体）

---

：

相机内参、外参、畸变系数）和立体标定（获取两相机相对位置）。

% MATLAB 标定步骤
clear; clc;

% 1. 准备标定板图片
imageDir = 'stereoImages/';
imageFiles1 = dir(fullfile(imageDir, 'left_*.jpg'));
imageFiles2 = dir(fullfile(imageDir, 'right_*.jpg'));

% 2. 设置棋盘格参数
squareSize = 25; % 毫米
boardSize = [9, 6]; % 内角点数
worldPoints = generateCheckerboardPoints(boardSize, squareSize);

% 3. 检测角点
[imagePoints1, boardSize1] = detectCheckerboardPoints(imageFiles1);
[imagePoints2, boardSize2] = detectCheckerboardPoints(imageFiles2);

% 4. 单目标定
[params1, ~] = estimateCameraParameters(imagePoints1, worldPoints);
[params2, ~] = estimateCameraParameters(imagePoints2, worldPoints);

% 5. 立体标定
[stereoParams, ~] = estimateStereoParameters(...
    imagePoints1, imagePoints2, params1, params2);

% 6. 保存标定结果
save('stereoParams.mat', 'stereoParams');

标定结果关键参数：

% 内参矩阵
K_left = stereoParams.CameraParameters1.IntrinsicMatrix;
K_right = stereoParams.CameraParameters2.IntrinsicMatrix;

% 旋转矩阵 R 和平移向量 T
R = stereoParams.RotationOfCamera2;
T = stereoParams.TranslationOfCamera2;

% 焦距 f
f = K_left(1,1); % 假设fx = fy

---

三、极线校正（立体校正）

将两幅图像变换到同一平面上，使匹配点位于同一水平线。

function [left_rect, right_rect, H1, H2] = stereo_rectify(left, right, stereoParams)
    % 读取参数
    K1 = stereoParams.CameraParameters1.IntrinsicMatrix';
    D1 = stereoParams.CameraParameters1.RadialDistortion;
    K2 = stereoParams.CameraParameters2.IntrinsicMatrix';
    D2 = stereoParams.CameraParameters2.RadialDistortion;
    R = stereoParams.RotationOfCamera2;
    T = stereoParams.TranslationOfCamera2;
    
    % 计算校正变换
    [R1, R2, P1, P2, Q] = cv.stereoRectify(K1, D1, K2, D2, ...
        size(left), R, T, 'Alpha', 0);
    
    % 计算映射
    [map1, map2] = cv.init();
    [map1(1), map1(2)] = cv.initUndistortRectifyMap(K1, D1, R1, P1, size(left));
    [map2(1), map2(2)] = cv.ini```

Python OpenCV版本（更常用）：

import cv2
import numpy as np

def stereo_rectify(left_img, right_img, stereo_params):
    # 内参
    K1 = stereo_params['K1']
    D1 = stereo_params['D1']
    K2 = stereo_params['K2']
    D2 = stereo_params['D2']
    R = stereo_params['R']
    T = stereo_params['T']
    
    # 计算校正参数
    R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
        K1, D1, K2, D2, left_img.shape[:2][::-1], R, T, alpha=0
    )
    
    # 计算映射表
    map1x, map1y = cv2.initUndistortRectifyMap(K1, D1, R1, P1, 
                                               left_img.shape[:2][::-1], cv2.CV_32FC1)
    map2x, map2y = cv2.initUndistortRectifyMap(K2, D2, R2, P2,
                                               right_img.shape[:2][::-1], cv2.CV_32FC1)
    
    # 应用校正
    left_rect = cv2.remap(left_img, map1x, map1y, cv2.INTER_LINEAR)
    right_rect = cv2.remap(right_img, map2x, map2y, cv2.INTER_LINEAR)
    
    return left_rect, right_rect, Q

---

四、立体匹配算法（核心）

4.1 块匹配（Block Matching）------ 最常用

import numpy as np
import cv2

def compute_disparity_BM(left, right, block_size=15, num_disparities=64):
    """
    使用SGBM算法计算视差图
    block_size: 匹配块大小（奇数）
    num_disparities: 最大视差搜索范围（16的倍数）
    """
    # 转换为灰度
    left_gray = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
    right_gray = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)
    
    # 创建StereoSGBM对象
    stereo = cv2.StereoSGBM_create(
        minDisparity=0,
        numDisparities=num_disparities,  # 16的倍数
        blockSize=block_size,
        P1=8 * 3 * block_size**2,       # 通常为8*通道数*SAD窗口大小^2
        P2=32 * 3 * block_size**2,       # 通常为32*通道数*SAD窗口大小^2
        disp12MaxDiff=1,
        uniquenessRatio=15,
        speckleWindowSize=100,
        speckleRange=32,
        mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY
    )
    
    # 计算视差
    disparity = stereo.compute(left_gray, right_gray).astype(np.float32) / 16.0
    
    return disparity

4.2 半全局匹配（SGBM）------ 效果更好

def compute_disparity_SGBM(left, right, num_disparities=128):
    """改进的SGBM算法"""
    # 创建左右SGBM对象
    window_size = 3
    
    left_matcher = cv2.StereoSGBM_create(
        minDisparity=0,
        numDisparities=num_disparities,
        blockSize=5,
        P1=8 * 3 * window_size**2,
        P2=32 * 3 * window_size**2,
        disp12MaxDiff=1,
        uniquenessRatio=10,
        speckleWindowSize=100,
        speckleRange=32,
        mode=cv22.STEREO_SGBM_M
    )
    
    right_matcher = cv2.xim2.StereoRightMatcher(left_matcher)
    
    # 计算左右视差
    left_disp = left_matcher.compute(left, right)
    right_disp = right_matcher.compute(right, left)
    
    # WLS滤波
    wls_filter = cv2.xim2.createDisparityWLSFilter(left_matcher)
    filtered_disp = wls_filter.filter(left_disp, left, disparity_map_right=right_disp)
    
    return filtered_disp

---

五、视差 → 深度转换

def disparity_to_depth(disparity, Q, baseline=120.0, f=718.856):
    """
    将视差图转换为深度图
    Q: 重投影矩阵
    baseline: 基线长度（mm）
    f: 焦距（像素）
    """
    # 方法1：使用Q矩阵重投影
    points_3D = cv2.reprojectImageTo3D(disparity, Q)
    
    # 方法2：直接公式计算
    depth = np.zeros(disparity.shape, dtype=np.float32)
    mask = disparity > 0
    
    # 深度 = 焦距 * 基线 / 视差
    depth[mask] = (f * baseline) / (disparity[mask] + 1e-6)
    
    # 单位转换（mm → m）
    depth = depth / 1000.0
    
    return depth, points_3D

---

六、完整的MATLAB实现

%% 双目测距主程序
clear; clc; close all;

%% 1. 参数设置
baseline = 120;          % 基线长度 (mm)
focal_length = 718.856;  % 焦距 (像素)
min_disparity = 0;        % 最小视差
max_disparity = 64;       % 最大视差
block_size = 15;         % 匹配块大小

%% 2. 加载标定参数
load('stereoParams.mat');  % 包含stereoParams结构体

%% 3. 读取左右图像
left_img = imread('left_01.jpg');
right_img = imread('right_01.jpg');

%% 4. 极线校正
[left_rect, right_rect] = rectifyStereoImages(left_img, right_img, stereoParams);

%% 5. 立体匹配（块匹配）
left_gray = rgb2gray(left_rect);
right_gray = rgb2gray(right_rect);

% 使用MATLAB的disparity函数
disparity_map = disparity(left_gray, right_gray, ...
    'Method', 'SemiGlobal', ...
    'DisparityRange', [min_disparity, max_disparity], ...
    'BlockSize', block_size, ...
    'ContrastThreshold', 0.5, ...
    'UniquenessThreshold', 15);

%% 6. 计算深度
disparity_map(disparity_map <= 0) = NaN;  % 去除无效点
depth_map = (focal_length * baseline) ./ disparity_map;

% 单位转换：mm -> m
depth_map = depth_map / 1000;

%% 7. 可视化结果
figure;
subplot(2, 3, 1); imshow(left_img); title('原始左图');
subplot(2, 3, 2); imshow(right_img); title('原始右图');
subplot(2, 3, 3); imshow(left_rect); title('校正后左图');
subplot(2, 3, 4); imshow(anaglyph(left_rect, right_rect)); title('红蓝立体图');
subplot(2, 3, 5); 
imagesc(disparity_map); colormap jet; colorbar;
title('视差图'); axis image;

subplot(2, 3, 6);
imagesc(depth_map); colormap jet; colorbar;
caxis([0, 10]);  % 限制深度显示范围0-10m
title('深度图 (m)'); axis image;

%% 8. 测量特定点深度
figure;
imshow(left_rect);
title('点击选择测量点');
[x, y] = ginput(1);
point_depth = depth_map(round(y), round(x));
fprintf('点(%d, %d)深度: %.2f m\n', round(x), round(y), point_depth);

参考代码 双目测距程序 www.youwenfan.com/contentcsv/103658.html

七、Python完整实现（OpenCV）

import cv2
import numpy as np
import matplotlib.pyplot as plt

class StereoDepth:
    def __init__(self, baseline, focal_length, K_left, K_right, D_left, D_right, R, T):
        """初始化双目相机参数"""
        self.baseline = baseline
        self.focal_length = focal_length
        
        # 计算校正参数
        self.R1, self.R2, self.P1, self.P2, self.Q, _, _ = cv2.stereoRectify(
            K_left, D_left, K_right, D_right, (640, 480), R, T, alpha=0
        )
        
        # 计算映射表
        self.map1x, self.map1y = cv2.initUndistortRectifyMap(
            K_left, D_left, self.R1, self.P1, (640, 480), cv2.CV_32FC1
        )
        self.map2x, self.map2y = cv2.initUndistortRectifyMap(
            K_right, D_right, self.R2, self.P2, (640, 480), cv2.CV_32FC1
        )
        
    def rectify_images(self, left_img, right_img):
        """极线校正"""
        left_rect = cv2.remap(left_img, self.map1x, self.map1y, cv2.INTER_LINEAR)
        right_rect = cv2.remap(right_img, self.map2x, self.map2y, cv2.INTER_LINEAR)
        return left_rect, right_rect
    
    def compute_disparity(self, left_img, right_img, method='SGBM'):
        """计算视差图"""
        left_gray = cv2.cvtColor(left_img, cv2.COLOR_BGR2GRAY)
        right_gray = cv2.cvtColor(right_img, cv2.COLOR_BGR2GRAY)
        
        if method == 'BM':
            stereo = cv2.StereoBM_create(numDisparities=64, blockSize=15)
        else:  # SGBM
            stereo = cv2.StereoSGBM_create(
                minDisparity=0,
                numDisparities=64,
                blockSize=11,
                P1=8*3*11**2,
                P2=32*3*11**2,
                disp12MaxDiff=1,
                uniquenessRatio=10,
                speckleWindowSize=100,
                speckleRange=32
            )
        
        disparity = stereo.compute(left_gray, right_gray).astype(np.float32) / 16.0
        
        # 中值滤波去除噪声
        disparity = cv2.medianBlur(disparity, 5)
        
        return disparity
    
    def compute_depth(self, disparity):
        """视差转深度"""
        mask = disparity > 0
        depth = np.zeros(disparity.shape, dtype=np.float32)
        depth[mask] = (self.focal_length * self.baseline) / (disparity[mask] + 1e-6)
        
        # 单位转换：mm -> m
        depth = depth / 1000.0
        
        return depth
    
    def visualize_results(self, left_img, right_img, disparity, depth):
        """可视化结果"""
        fig, axes = plt.subplots(2, 3, figsize=(15, 10))
        
        # 原始图像
        axes[0, 0].imshow(cv2.cvtColor(left_img, cv2.COLOR_BGR2RGB))
        axes[0, 0].set_title('原始左图')
        axes[0, 0].axis('off')
        
        axes[0, 1].imshow(cv2.cvtColor(right_img, cv2.COLOR_BGR2RGB))
        axes[0, 1].set_title('原始右图')
        axes[0, 1].axis('off')
        
        # 校正后图像
        left_rect, right_rect = self.rectify_images(left_img, right_img)
        axes[0, 2].imshow(cv2.cvtColor(left_rect, cv2.COLOR_BGR2RGB))
        axes[0, 2].set_title('校正后左图')
        axes[0, 

---

八、实际应用：距离测量

def measure_distance_at_point(depth_map, x, y, pixel_size=0.004):
    """
    测量特定点的距离和物理尺寸
    depth_map: 深度图 (m)
    x, y: 像素坐标
    pixel_size: 单个像素物理尺寸 (mm)
    """
    depth = depth_map[y, x]
    
    if depth <= 0 or depth > 50:  # 有效范围检查
        return None, None
    
    # 物体实际尺寸（假设物体在图像中占N个像素）
    # 实际尺寸 = 像素数量 * 像素物理尺寸 * 距离 / 焦距
    return depth

---

九、性能优化技巧

1. GPU加速（CUDA）

# 使用CUDA加速SGBM
stereo = cv2.cuda.StereoSGBM_create(
    minDisparity=0,
    numDisparities=128,
    blockSize=11
)
left_gpu = cv2.cuda_GpuMat(left_gray)
right_gpu = cv2.cuda_GpuMat(right_gray)
disparity_gpu = stereo.compute(left_gpu, right_gpu)

2. 多分辨率金字塔（加速大图）

def multi_scale_disparity(left, right, scale=2):
    """金字塔下采样加速"""
    left_small = cv2.resize(left, None, fx=1/scale, fy=1/scale)
    right_small = cv2.resize(right, None, fx=1/scale, fy=1/scale)
    
    # 在小图上计算
    disparity_small = compute_disparity(left_small, right_small)
    
    # 上采样回原尺寸
    disparity = cv2.resize(disparity_small, left.shape[:2][::-1])
    return disparity

---

十、误差分析与调试

误差来源	影响	解决方法
标定误差	系统误差，影响精度	多次标定取平均，使用高精度标定板
图像噪声	视差图噪声	中值滤波、双边滤波
纹理缺失	匹配困难	增加SAD窗口，纹理投影
遮挡区域	无匹配点	左右一致性检查（LRC）
光照变化	匹配失败	直方图均衡化，归一化互相关

调试技巧：

# 1. 检查标定重投影误差
reprojection_error = cv2.stereoCalibrate(...)[1]
print(f"重投影误差: {reprojection_error:.3f}像素")  # 应小于0.5像素

# 2. 检查极线对齐
plt.figure()
for i in range(0, height, 50):
    plt.axhline(y=i, color='r', linestyle='--', alpha=0.5)
plt.imshow(cv2.hconcat([left_rect, right_rect]))
plt.title('极线检查：红线应在同一水平')

---

十一、完整项目结构建议

stereo_vision_project/
├── calibration/          # 标定相关
│   ├── left/            # 左相机标定图
│   ├── right/           # 右相机标定图
│   └── calib.py         # 标定脚本
├── data/               # 测试数据
├── scripts/
│   ├── stereo_match.py  # 立体匹配
│   ├── disparity.py     # 视差计算
│   ├── depth.py         # 深度计算
│   └── visualize.py     # 可视化
├── config/
│   �