MATLAB实现双目立体视觉定位

1. 相机标定

首先需要对双目相机进行标定，获取相机参数。

% 相机标定
[imagePoints, boardSize] = detectCheckerboardPoints('leftImage.jpg', 'rightImage.jpg');
worldPoints = generateCheckerboardPoints(boardSize, 25); % 25mm方格尺寸

% 左相机标定
paramsLeft = estimateCameraParameters(imagePoints(:,:,1), worldPoints);
% 右相机标定
paramsRight = estimateCameraParameters(imagePoints(:,:,2), worldPoints);

% 立体标定
stereoParams = estimateStereoBaseline(imagePoints, worldPoints, paramsLeft, paramsRight);

2. 图像校正

校正图像使对应点位于同一水平线上。

% 读取左右图像
I1 = imread('leftImage.jpg');
I2 = imread('rightImage.jpg');

% 校正图像
[J1, J2] = rectifyStereoImages(I1, I2, stereoParams);

% 显示校正后的图像
figure;
imshowpair(J1, J2, 'montage');
title('校正后的左右图像');

3. 视差图计算

计算视差图，这是立体视觉的核心步骤。

% 转换为灰度图像
J1_gray = rgb2gray(J1);
J2_gray = rgb2gray(J2);

% 设置视差范围
disparityRange = [0 64]; % 根据实际场景调整

% 计算视差图
disparityMap = disparitySGM(J1_gray, J2_gray, 'DisparityRange', disparityRange, 'UniquenessThreshold', 20);

% 显示视差图
figure;
imshow(disparityMap, disparityRange);
title('视差图');
colormap jet;
colorbar;

4. 三维重建

根据视差图重建三维点云。

% 计算三维点云
points3D = reconstructScene(disparityMap, stereoParams);

% 转换为米制单位
points3D = points3D ./ 1000; % 假设标定时使用毫米单位

% 显示三维点云
figure;
pcshow(points3D, J1, 'VerticalAxis', 'Y', 'VerticalAxisDir', 'Down');
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Z (m)');
title('三维点云重建');

5. 目标定位

从点云中提取特定目标并计算其位置。

% 假设我们要定位图像中心区域的目标
[h, w, ~] = size(J1);
centerX = round(w/2);
centerY = round(h/2);
windowSize = 50; % 选取的窗口大小

% 提取中心区域点云
roi = [centerX-windowSize, centerY-windowSize, windowSize*2, windowSize*2];
points3D_roi = points3D(roi(2):roi(2)+roi(4), roi(1):roi(1)+roi(3), :);

% 去除无效点(视差计算失败的点)
validPoints = ~isnan(points3D_roi(:,:,1)) & ~isnan(points3D_roi(:,:,2)) & ~isnan(points3D_roi(:,:,3));
points3D_valid = reshape(points3D_roi(repmat(validPoints, [1,1,3])), [], 3);

% 计算目标平均位置
targetPosition = mean(points3D_valid, 1);

disp(['目标三维位置 (X,Y,Z): ', num2str(targetPosition), ' 米']);

6. 完整流程封装

将上述步骤封装为一个完整函数：

function [targetPosition, points3D] = stereoVisionLocalization(leftImage, rightImage, stereoParams)
    % 图像校正
    [J1, J2] = rectifyStereoImages(leftImage, rightImage, stereoParams);
    
    % 视差计算
    J1_gray = rgb2gray(J1);
    J2_gray = rgb2gray(J2);
    disparityMap = disparitySGM(J1_gray, J2_gray, 'DisparityRange', [0 64]);
    
    % 三维重建
    points3D = reconstructScene(disparityMap, stereoParams) ./ 1000;
    
    % 目标定位(中心区域)
    [h, w, ~] = size(J1);
    roi = [round(w/2)-50, round(h/2)-50, 100, 100];
    points3D_roi = points3D(roi(2):roi(2)+roi(4), roi(1):roi(1)+roi(3), :);
    
    validPoints = ~isnan(points3D_roi(:,:,1)) & ~isnan(points3D_roi(:,:,2)) & ~isnan(points3D_roi(:,:,3));
    points3D_valid = reshape(points3D_roi(repmat(validPoints, [1,1,3])), [], 3);
    
    targetPosition = mean(points3D_valid, 1);
end

注意

1. 相机标定是基础，标定精度直接影响最终定位精度
2. 视差范围(DisparityRange)需要根据实际场景调整
3. 对于远距离目标，需要更高精度的相机和更大的基线距离
4. 光照条件、纹理丰富度会影响视差计算效果
5. 可以考虑加入滤波和后期处理提高视差图质量

参考代码 MATLAB程序实现双目立体视觉定位