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

    %% 婄傑偱偺嫍棧偺應掕
%    僗僥儗僆僷儔儊乕僞丄嵍塃偺夋憸偺懳墳揰偑婛抦偺応崌
clc;clear;close all;imtool close all;

%%  僗僥儗僆僇儊儔僷儔儊乕僞偺撉崬傒
load('webcamsSceneReconstruction.mat');

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

%% 儗儞僘榗偺彍嫀
I1u = undistortImage(I1, stereoParams.CameraParameters1);     % 僨僼僅儖僩愝掕偱偼newOrigin=[0 0], i.e., 拞怱偼晄曄
I2u = undistortImage(I2, stereoParams.CameraParameters2);     % 僨僼僅儖僩愝掕偱偼newOrigin=[0 0], i.e., 拞怱偼晄曄
figure; imshowpair(I1u, I2u, 'montage'); truesize;

%% 椉曽偺夋憸偐傜丄惓柺傪岦偄偰偄傞婄偺専弌丒昞帵
faceDetector = vision.CascadeObjectDetector;
face1 = step(faceDetector,I1u)           % (x0, y0, 暆, 崅偝)
face2 = step(faceDetector,I2u)
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

%% 僇儊儔侾偺岝妛拞怱偐傜丄婄偺拞怱偺X曽岦偺嫍棧傪寁嶼 (mm扨埵)
point3d = triangulate(center1, center2, stereoParams)

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

%% Copyright 2015 The MathWorks, Inc.