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

    %% 婄傑偱偺嫍棧偺應掕
clc;clear;close all;imtool close all;

%% 僇儊儔1丒2偺丄撪晹丒奜晹(暲恑丒夞揮)僷儔儊乕僞偺撉崬傒
load('I4_09_4b_params.mat');

%% 僇儊儔1丒2偺丄僇儊儔峴楍乮儚乕儖僪嵗昗偲夋憸忋僺僋僙儖嵗昗偺娭學乯偺寁嶼
camMatrix1 = cameraMatrix(cameraParams1, cam1R, cam1T);
camMatrix2 = cameraMatrix(cameraParams2, cam2R, cam2T);

%% 嵍塃偺僇儊儔偺夋憸傪撉崬傒\帵
I1 = imread('sceneReconstructionLeft.jpg');
I2 = imread('sceneReconstructionRight.jpg');
figure; imshowpair(I1, I2, 'montage'); truesize;

%% 儗儞僘榗偺彍嫀
I1u = undistortImage(I1, cameraParams1);
I2u = undistortImage(I2, cameraParams2);
figure; imshowpair(I1u, I2u, 'montage'); truesize;

%% 椉曽偺夋憸偐傜丄惓柺傪岦偄偰偄傞婄偺専弌丒昞帵
faceDetector = vision.CascadeObjectDetector;
face1 = step(faceDetector,I1)           % (x0, y0, 暆, 崅偝)
face2 = step(faceDetector,I2)
I1i = insertShape(I1u,'Rectangle',face1, 'LineWidth', 5);
I2i = insertShape(I2u,'Rectangle',face2, 'LineWidth', 5);
imshowpair(I1i, I2i, 'montage'); shg;

%% 婄偺拞怱揰嵗昗偺寁嶼
center1 = face1(1:2) + face1(3:4)/2
center2 = face2(1:2) + face2(3:4)/2

%% 奜晹僷儔儊乕僞偱梡偄偨儚乕儖僪嵗昗尨揰偐傜丄
%              婄偺拞怱傊偺嫍棧傪寁嶼 (mm扨埵)丗 X丒Y丒Z
point3d = triangulate(center1, center2, camMatrix1, camMatrix2)

%% 僇儊儔1偺岝妛拞怱偺儚乕儖僪嵗昗抣
x0 = -1 * cam1T * cam1R'

%% 僇儊儔1偺岝妛拞怱偐傜丄婄偺拞怱傊偺嫍棧傪寁嶼 (mm扨埵)丗 X丒Y丒Z
point3d = point3d - x0

%% 儊乕僩儖扨埵偺捈慄嫍棧傊曄姺
distanceInMeters = norm(point3d)/1000

%% 廔椆

















%% (嶲峫)偙偙偱梡偄偨僇儊儔1丒2偺僇儊儔撪晹丒奜晹僷儔儊乕僞偺惗惉
load('webcamsSceneReconstruction.mat');   % 僗僥儗僆僇儊儔僷儔儊乕僞偺撉崬傒
cameraParams1 = stereoParams.CameraParameters1;
cameraParams2 = stereoParams.CameraParameters2;
cam1T = cameraParams1.TranslationVectors(1,:)
cam2T = stereoParams.CameraParameters2.TranslationVectors(1,:)
cam1R = stereoParams.CameraParameters1.RotationMatrices(:,:,1)
cam2R = stereoParams.CameraParameters2.RotationMatrices(:,:,1)

save('I4_09_4b_params.mat', 'cameraParams1', 'cameraParams2', 'cam1T', 'cam2T', 'cam1R', 'cam2R');


%% 僇儊儔1?偺埵抲p惃偺娭學偐傜丄
%  偦傟偧傟偺僇儊儔峴楍(World嵗昗偲夋憸偺嵗昗偺娭學)傪寁嶼偡傞応崌
%  乮偙偙偱偼World嵗昗偺尨揰傪夋憸1偺僇儊儔埵抲丄僇儊儔偺曽岦傪Z幉偺惓偵偡傞乯
t1 = [0 0 0];      % 僇儊儔1偼World嵗昗尨揰偲愝掕乮暲恑堏摦側偟乯
R1 = eye(3);       % 僇儊儔1偼World嵗昗Z幉偺惓曽岦偲愝掕乮夞揮側偟乯
t2 = stereoParams.TranslationOfCamera2    % L=[-95.6895 1.1788 -6.8476]
R2 = stereoParams.RotationOfCamera2
camMatrix1 = cameraMatrix(stereoParams.CameraParameters1, R1, t1);
camMatrix2 = cameraMatrix(stereoParams.CameraParameters2, R2, t2);

%% Copyright 2015 The MathWorks, Inc.