【Matlab双目标定与立体匹配】

介绍

本文给出了一组主动散斑双目的仿真数据，在此基础上，用Maltab实现了双目相机的标定与立体匹配。数据和代码见Github仓库：https://github.com/zxcn/matlab_stereo_vision

仿真数据

仿真数据由Blender生成，双目模组参数值如下表。

参数	Width	Height	$f_x$	$f_y$	$c_x$	$c_y$	$k_1$	$$\begin{gathered} k_2,k_3 \\ {p}_1,p_2 \end{gathered}$$	Rotation (Euler XYZ)	Translation(mm)
左相机	1600	1200	718.66	718.66	799.5	599.5	-0.01	0	0°,0°,0°	0,0,0
右相机	1600	1200	718.66	718.66	799.5	599.5	-0.01	0	0.5°,0.1°,-0.2°	100,0.5,1

这里的主点位置是相对于图像左上角像素坐标为(0,0)的坐标。Matlab图像左上角像素坐标为(1,1)，标定结果中主点坐标需减1才能和此处的真值对应。

下图是标定板仿真结果示例，共有九组位姿。

下图是散斑图像仿真结果示例。

双目标定

clear
clc
close all

% 图像后缀
image_suffix = '.png';

% 图像路径
left_image_path = './Data/Left';
right_image_path = './Data/Right';

% 查找路径下所有指定格式的图像
left_image_list = dir([left_image_path,'/*',image_suffix]);
right_image_list = dir([right_image_path,'/*',image_suffix]);

% 结构体转换为元胞数组
imageFileNames1 = cellfun(@(c1,c2)[c1,'/',c2],{left_image_list.folder},{left_image_list.name},'UniformOutput',false);
imageFileNames2 = cellfun(@(c1,c2)[c1,'/',c2],{right_image_list.folder},{right_image_list.name},'UniformOutput',false);

% 棋盘格检测
detector = vision.calibration.stereo.CheckerboardDetector();
[imagePoints, imagesUsed] = detectPatternPoints(detector, imageFileNames1, imageFileNames2);

% 棋盘格角点的世界坐标
% Generate world coordinates for the planar patten keypoints
squareSize = 20;  % 单位mm
worldPoints = generateWorldPoints(detector, 'SquareSize', squareSize);

% 读取左相机的第一个图像，获得图像长宽
I1 = imread(imageFileNames1{1});
[mrows, ncols, ~] = size(I1);

% estimateCameraParameters：相机标定函数
% EstimateSkew：是否计算Skew参数
% EstimateTangentialDistortion：是否计算切向畸变参数p1,p2
% NumRadialDistortionCoefficients：径向畸变参数数量，为2则仅使用k1,k2，为3则使用k1,k2,k3
% WorldUnits：尺度单位
% InitialIntrinsicMatrix：内参矩阵初始值
% InitialRadialDistortion：径向畸变初始值
% ImageSize：图像长宽
[stereoParams, pairsUsed, estimationErrors] = estimateCameraParameters(imagePoints, worldPoints, ...
    'EstimateSkew', false, 'EstimateTangentialDistortion', true, ...
    'NumRadialDistortionCoefficients', 3, 'WorldUnits', 'millimeters', ...
    'InitialIntrinsicMatrix', [], 'InitialRadialDistortion', [], ...
    'ImageSize', [mrows, ncols]);

% 显示重投影误差
% View reprojection errors
h1=figure; showReprojectionErrors(stereoParams);

%可视化标定板和相机位姿
% Visualize pattern locations
h2=figure; showExtrinsics(stereoParams, 'CameraCentric');

% 显示相机标定结果和误差
% Display parameter estimation errors
displayErrors(estimationErrors, stereoParams);

% 保存标定结果
save stereoParams stereoParams

标定结果如下，与仿真参数十分接近。

			Standard Errors of Estimated Stereo Camera Parameters
			-----------------------------------------------------

Camera 1 Intrinsics
-------------------
Focal length (pixels):   [  718.6581 +/- 0.0170      718.6589 +/- 0.0170  ]
Principal point (pixels):[  800.4972 +/- 0.0135      600.5021 +/- 0.0104  ]
Radial distortion:       [   -0.0100 +/- 0.0000       -0.0001 +/- 0.0001        0.0001 +/- 0.0000  ]
Tangential distortion:   [    0.0000 +/- 0.0000       -0.0000 +/- 0.0000  ]

Camera 1 Extrinsics
-------------------
Rotation vectors:
                         [   -0.0000 +/- 0.0000        0.0000 +/- 0.0000       -0.0000 +/- 0.0000  ]
                         [    0.2618 +/- 0.0000        0.0000 +/- 0.0000       -0.0000 +/- 0.0000  ]
                         [   -0.2618 +/- 0.0000        0.0000 +/- 0.0000        0.0000 +/- 0.0000  ]
                         [    0.0000 +/- 0.0000        0.2618 +/- 0.0000        0.0000 +/- 0.0000  ]
                         [   -0.0000 +/- 0.0000       -0.2618 +/- 0.0000        0.0000 +/- 0.0000  ]
                         [    0.2603 +/- 0.0000       -0.2603 +/- 0.0000        0.0343 +/- 0.0000  ]
                         [    0.2603 +/- 0.0000        0.2603 +/- 0.0000       -0.0343 +/- 0.0000  ]
                         [   -0.2603 +/- 0.0000        0.2603 +/- 0.0000        0.0343 +/- 0.0000  ]
                         [   -0.2603 +/- 0.0000       -0.2603 +/- 0.0000       -0.0343 +/- 0.0000  ]

Translation vectors (millimeters):
                         [ -129.9996 +/- 0.0058     -130.0004 +/- 0.0044      300.0032 +/- 0.0076  ]
                         [ -129.9999 +/- 0.0053     -125.5723 +/- 0.0041      266.3559 +/- 0.0071  ]
                         [ -129.9984 +/- 0.0063     -125.5708 +/- 0.0047      333.6443 +/- 0.0079  ]
                         [ -123.8652 +/- 0.0065     -130.0015 +/- 0.0049      346.5868 +/- 0.0080  ]
                         [ -123.8676 +/- 0.0052     -130.0014 +/- 0.0039      253.4183 +/- 0.0069  ]
                         [ -115.1569 +/- 0.0047     -125.5707 +/- 0.0036      220.9121 +/- 0.0064  ]
                         [ -132.5749 +/- 0.0060     -125.5702 +/- 0.0046      314.0887 +/- 0.0074  ]
                         [ -115.1568 +/- 0.0070     -125.5710 +/- 0.0052      379.0872 +/- 0.0082  ]
                         [ -132.5746 +/- 0.0056     -125.5699 +/- 0.0042      285.9146 +/- 0.0072  ]

Camera 2 Intrinsics
-------------------
Focal length (pixels):   [  718.6726 +/- 0.0170      718.6702 +/- 0.0170  ]
Principal point (pixels):[  800.5148 +/- 0.0134      600.5037 +/- 0.0103  ]
Radial distortion:       [   -0.0100 +/- 0.0000       -0.0001 +/- 0.0001        0.0000 +/- 0.0000  ]
Tangential distortion:   [    0.0000 +/- 0.0000        0.0000 +/- 0.0000  ]

Position And Orientation of Camera 2 Relative to Camera 1
---------------------------------------------------------
Rotation of camera 2:         [   -0.0087 +/- 0.0000       -0.0017 +/- 0.0000        0.0035 +/- 0.0000  ]
Translation of camera 2 (millimeters):[  -99.9977 +/- 0.0030       -0.8583 +/- 0.0028       -1.1606 +/- 0.0061  ]

视差、点云计算

clear
clc
close all

% 加载标定参数
load stereoParams.mat

% 待计算图像路径
left_image_path = './Data/Scene/Left/Gray0001.png';
right_image_path = './Data/Scene/Right/Gray0001.png';

% 读取图片
I1 = double(imread(left_image_path));
I2 = double(imread(right_image_path));

% 极线校正
[J1, J2, reprojectionMatrix] = rectifyStereoImages(I1,I2,stereoParams,'OutputView','valid','FillValues',0);
J1 = mat2gray(J1,[0,1023]);
J2 = mat2gray(J2,[0,1023]);
figure,imshowpair(J1,J2),title('极线校正结果')

% disparityBM：BlockMatch计算视差
% DisparityRange：视差范围
% BlockSize：匹配块的大小
disparityMap = disparityBM(J1,J2,"DisparityRange",[0,128],"BlockSize",25);
figure,imshow(disparityMap,[0,128]),title('视差')

% 重建三维点云
xyzPoints = reconstructScene(disparityMap,reprojectionMatrix);

% 点云维度由HxWx3转换为(HxW)x3
xyzPoints = reshape(xyzPoints,[],3);
% 滤除z值大于2000的点
m = xyzPoints(:,3)<2000;
ind = find(m==1);
xyzPoints = xyzPoints(m,:);

% 显示点云
figure,pcshow(xyzPoints);