www.gusucode.com > IPCV_Eval_Kit_R2019a_0ce6858工具箱matlab程序源码 > IPCV_Eval_Kit_R2019a_0ce6858/code/demo_files/I4_09_2_myStereoCalibAndSceneReconst.m

    %% Stereo Calibration and Scene Reconstruction 
% 1: 僗僥儗僆僇儊儔偺僉儍儕僽儗乕僔儑儞
% 2: 僗僥儗僆暯峴壔 (Rectification)
% 3: 帇嵎(Disparity)偺寁嶼
% 4: Reconstruct the 3-D point cloud
clc;close all;imtool close all;clear;

%% 1: 僗僥儗僆僇儊儔偺僉儍儕僽儗乕僔儑儞 %%%%%%%%%%%%%%%
% 僼傽僀儖柤偺巜掕乮嵍塃10枃偢偮乯
leftImages = imageDatastore(fullfile(toolboxdir('vision'),'visiondata', ...
    'calibration','stereo','left'));
rightImages = imageDatastore(fullfile(toolboxdir('vision'),'visiondata', ...
    'calibration','stereo','right'));
imageFileNames1 = leftImages.Files;
imageFileNames2 = rightImages.Files;

%% 堦枃傪椺偲偟偰戝偒偔昞帵
image1 = imread(imageFileNames1{1});
figure;imshow(image1);

%% 夋憸偺昞帵: 忋抜偑嵍夋憸丒壓抜偑塃夋憸
figure;montage([imageFileNames1 imageFileNames2], 'Size', [2 10]);truesize;   

%% 嵍塃偺慡夋憸僙僢僩偐傜丄僷僞乕儞偺僐乕僫乕揰傪専弌陽嚃趥虊鎽渹虝\帵
[imagePoints, boardSize, pairsUsed] = ...
                 detectCheckerboardPoints(imageFileNames1, imageFileNames2);
figure;imshow(insertMarker(image1, imagePoints(:,:,1,1), 'o', 'Color', 'green', 'Size', 5));  %侾枃栚偺嵍夋憸

% 僐乕僫乕揰偺 幚悽奅偱偺埵抲(world coordinates) 傪寁嶼
squareSize = 108; % 扨埵丗mm
worldPoints = generateCheckerboardPoints(boardSize, squareSize);

%% 僗僥儗僆僇儊儔僷儔儊乕僞偺悇掕(Apps)
[leftFolder,~,~] = fileparts(leftImages.Files{1});
[rightFolder,~,~] = fileparts(rightImages.Files{1});
stereoCameraCalibrator(leftFolder,rightFolder,squareSize);

%% 僗僥儗僆僇儊儔僷儔儊乕僞偺悇掕
stereoParams = estimateCameraParameters(imagePoints, worldPoints);

%% 僉儍儕僽儗乕僔儑儞岆嵎偺昞帵
figure; showReprojectionErrors(stereoParams);

%% 僇儊儔偺奜晹僷儔儊乕僞偺壜帇壔乮僇儊儔傪屌掕乯
figure; showExtrinsics(stereoParams);

%% 僇儊儔偺奜晹僷儔儊乕僞偺壜帇壔乮僠僃僢僇乕僷僞乕儞傪屌掕乯
figure; showExtrinsics(stereoParams, 'patternCentric');
 
%% 2: 僗僥儗僆暯峴壔 (Rectification) %%%%%%%%%%%%%%%%%%%%%%
% 嵍塃儁傾偺夋憸傪撉崬傒丒昞帵
I1 = imread(imageFileNames1{1});
I2 = imread(imageFileNames2{1});
figure;imshowpair(I1, I2, 'montage');
%% 廳偹偰昞帵
figure; imshow(stereoAnaglyph(I1, I2), 'InitialMagnification', 50);

%% 僉儍儕僽儗乕僔儑儞僨乕僞傪梡偄丄僗僥儗僆暯峴壔丒昞帵
[J1, J2] = rectifyStereoImages(I1, I2, stereoParams);
% 昞帵
figure; imshow(stereoAnaglyph(J1, J2), 'InitialMagnification', 50);

%% 帇嵎(僇儊儔偐傜偺嫍棧偵敿斾椺)偺寁嶼丒帇嵎儅僢僾偺昞帵乮嬤偔偑敀乯
%     disparityMap偼丄J1偺Pixel埵抲偵懳墳丅Single僞僀僾攝楍丅嵟彫扨埵:1/16 Pixels
disparityMap = disparity(J1, J2);
figure; imshow(disparityMap, [0, 64], 'InitialMagnification', 50);

%% 夋憸偺丄3師尦嵗昗宯傊偺嵞峔抸丒昞帵
%  帇嵎儅僢僾忋偺奺揰傪3師尦 world 嵗昗宯偺揰傊儅僢僺儞僌
%  I1偺岝妛拞怱偑world嵗昗宯偺尨揰
pointCloud = reconstructScene(disparityMap, stereoParams);      %    % 799x1122x3 single: disparityMap偺奺僺僋僙儖偵懳偟丄World嵗昗宯偺[x,y.z]傪寁嶼偟3師尦晹暘偵憓擖
pointCloud = pointCloud / 1000;  %扨埵傪 mm 偐傜 m 傊曄姺

% 僇儊儔偐傜0乣4m棧傟偨揰偺傒僾儘僢僩
z = pointCloud(:, :, 3);    % 嫍棧偺傒拪弌
zdisp = z;
zdisp(z < 0 | z > 4) = NaN;   % 4m傛傝嬤偄丒0m傛傝棧傟偰偄傞揰傪彍嫀
pointCloud(:,:,3) = zdisp;
figure;showPointCloud(pointCloud, J1, 'VerticalAxis', 'Y',...
    'VerticalAxisDir', 'Down' );
xlabel('X');ylabel('Y');zlabel('Z');
xlim([-1 3]); ylim([-2 1]);
box on;

%% 暿夋憸偵懳偟偰偺嵞峔惉椺

%% 夋憸撉傒崬傒
I1 = imread('sceneReconstructionLeft.jpg');
I2 = imread('sceneReconstructionRight.jpg');
figure, imshowpair(I1,I2,'montage');

%% 僉儍儕僽儗乕僔儑儞僨乕僞傪梡偄丄僗僥儗僆暯峴壔丒昞帵
load('webcamsSceneReconstruction.mat')    %曐懚偟偰偁傞僉儍儕僽儗乕僔儑儞僨乕僞梡偄傞偲偒
[J1, J2] = rectifyStereoImages(I1, I2, stereoParams);
% 昞帵
figure; imshow(stereoAnaglyph(J1, J2), 'InitialMagnification', 50);

%% 帇嵎(僇儊儔偐傜偺嫍棧偵敿斾椺)偺寁嶼丒帇嵎儅僢僾偺昞帵乮嬤偔偑敀乯
%     disparityMap偼丄J1偺Pixel埵抲偵懳墳丅Single僞僀僾攝楍丅嵟彫扨埵:1/16 Pixels
disparityMap = disparity(rgb2gray(J1), rgb2gray(J2));
figure; imshow(disparityMap, [0, 64], 'InitialMagnification', 50);



%% 夋憸偺丄3師尦嵗昗宯傊偺嵞峔抸丒昞帵
%  帇嵎儅僢僾忋偺奺揰傪3師尦 world 嵗昗宯偺揰傊儅僢僺儞僌
%  I1偺岝妛拞怱偑world嵗昗宯偺尨揰
pointCloud = reconstructScene(disparityMap, stereoParams);      %    % 799x1122x3 single: disparityMap偺奺僺僋僙儖偵懳偟丄World嵗昗宯偺[x,y.z]傪寁嶼偟3師尦晹暘偵憓擖
pointCloud = pointCloud / 1000;  %扨埵傪 mm 偐傜 m 傊曄姺

% 僇儊儔偐傜3乣7m棧傟偨揰偺傒僾儘僢僩
z = pointCloud(:, :, 3);    % 嫍棧偺傒拪弌
zdisp = z;
zdisp(z < 3 | z > 7) = NaN;   % 3m傛傝嬤偄丒7m傛傝棧傟偰偄傞揰傪彍嫀
pointCloud(:,:,3) = zdisp;
figure;showPointCloud(pointCloud, J1, 'VerticalAxis', 'Y',...
    'VerticalAxisDir', 'Down' );
xlabel('X');ylabel('Y');zlabel('Z');
xlim([-1 3]); ylim([-2 1]);
box on;
%%
% Copyright 2014 The MathWorks, Inc.