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

    %% 儗儞僘榗偺曗惓
clear;clc;close all;close all force

%% 僇儊儔僉儍儕僽儗乕僔儑儞梡偺夋憸偺妋擣
imageFolder = fullfile(toolboxdir('vision'), 'visiondata', ...
    'calibration', 'mono');
winopen(imageFolder);

%% 傾僾儕働乕僔儑儞偺婲摦
squareSize = 29;  % 彙栚堦偮偺僒僀僘乮扨埵丗mm乯
cameraCalibrator(imageFolder,squareSize);

%% 悇掕偟偨僇儊儔僷儔儊乕僞偺撉崬
%   (傕偟偔偼 傾僾儕働乕僔儑儞偺僉儍儕僽儗乕僔儑儞寢壥傪巊梡)
load('I2_02_calibration_cameraParams');

%% 擟堄偺懳徾夋憸偺撉崬
imds = imageDatastore(imageFolder);
origI = imds.readimage(1);
figure;imshow(origI)

%% 夋憸偺榗曗惓\帵 :僨僼僅儖僩偱偼丄"擖弌椡夋憸偼摨僒僀僘,尨揰Origin偼晄曄, i.e., newOrigin=(0,0)"
[undistI, ~] = undistortImage(origI, cameraParams);
figure;imshowpair(origI,undistI);truesize;

%% 廔椆















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

%% 夋憸忋偺揰嵗昗偵懳偟偰儗儞僘榗曗惓
points = detectCheckerboardPoints(origI)
figure; imshow(origI);
hold on;
plot(points(:, 1), points(:, 2), 'g*');
hold off;

% 夋憸偵懳偟偰丄儗儞僘榗彍嫀
[undistI, newOrigin] = undistortImage(origI, cameraParams, 'OutputView','full');
% 揰嵗昗偵懳偟偰丄儗儞僘榗彍嫀
undistPoints = undistortPoints(points, cameraParams);
% 揰嵗昗偺傾儔僀儊儞僩
undistPoints = [undistPoints(:,1) - newOrigin(1), undistPoints(:,2) - newOrigin(2)];

% 寢壥偺昞帵
figure; imshow(undistI);
hold on;
plot(undistPoints(:, 1), undistPoints(:, 2), 'r*');
hold off;

%% 夋憸偺榗曗惓(慡懱傪巆偡)\帵
[undistI, newOrigin] = undistortImage(origI, cameraParams, 'OutputView', 'full');
figure;imshowpair(origI, undistI, 'montage');

%% 夋憸忋偺揰嵗昗偑梌偊傜傟偨偲偒偺榗曗惓丒昞帵
[points, boardSize] = detectCheckerboardPoints(origI)   % points偵擖偭偰偄傞揰嵗昗傪偙偺偁偲偵榗曗惓
undistPoints = undistortPoints(points, cameraParams);   % 榗曗惓
figure;imshow(origI);hold on;
plot(undistPoints(:,1), undistPoints(:,2), 'g*');hold off;
%% (榗曗惓偟偨夋憸偵廳偹崌傢偣)
undistPoints = [undistPoints(:,1) - newOrigin(1), undistPoints(:,2) - newOrigin(2)];   % undistortImage偱丄夋憸慡懱偑擖傞傛偆偵夋憸僒僀僘偑曄傢傞愝掕偵偟偨偨傔丄廳偹傞偵偼揰偺嵗昗偺堏摦偑昁梫
figure;imshow(undistI);hold on;
plot(undistPoints(:,1), undistPoints(:,2), 'g*');hold off;

%% 僠僃僢僇乕儃乕僪傪屌掕偟偰奜晹僷儔儊乕僞偺昞帵 %%%%%%%%%%%%%%%%%%
figure; showExtrinsics(cameraParams, 'PatternCentric');

%% 夋憸偑僠僃僢僇乕僷僞乕儞傪娷傓偲偒偼埲壓偑壜擻
%    儗儞僘榗曗惓屻丄偦偺僠僃僢僇乕僷僞乕儞傪World嵗昗宯偺尨揰偵偟偰丄
%    偦傟偵懳偡傞僇儊儔偺埵抲乮奜晹僷儔儊乕僞乯悇掕
squareSize = 150;   % 扨埵丗mm
worldPoints = generateCheckerboardPoints(boardSize, squareSize);  %World嵗昗宯偱偺棟憐僐乕僫乕揰嵗昗傪寁嶼丅嵍忋偺僐乕僫乕揰偑(0,0)
% 榗曗惓偟偨夋憸偐傜偺岎揰嵗昗丒World嵗昗宯偱偺岎揰嵗昗偲丄cameraParams撪偺intrinsics偐傜丄
%    夞揮(R)偲暲恑(T)偺峴楍傪寁嶼乮僇儊儔偺埵抲丗奜晹僷儔儊乕僞傪悇掕乯
%    worldPoint偑X,Y偺傒偺応崌偼丄Z=0傪巊梡
%       [x y z] = [X Y Z]R + T         % [x y z]:僇儊儔嵗昗丄[X Y Z]:World嵗昗
[rotationMatrix, translationVector] = extrinsics(flipud(undistPoints), worldPoints, cameraParams)

%% 儗儞僘榗曗惓屻偺夋憸忋偺嵍忋僠僃僢僇乕僐乕僫乕揰傪丄World嵗昗宯偺XY傊曄姺 (World嵗昗宯偱岎揰偼z=0)
%     cameraParams.WorldUnits丗mm
worldPoint1 = pointsToWorld(cameraParams, rotationMatrix, translationVector, undistPoints(1,:))

%% 儗儞僘榗曗惓夋憸忋偺(2,2)僠僃僢僇乕僐乕僫乕揰傪丄World嵗昗宯偺XY傊曄姺
worldPoint2 = pointsToWorld(cameraParams, rotationMatrix, translationVector, undistPoints(8,:))

%% World嵗昗乮忋婰 worldPoint2丄z=0)傪丄媡偵夋憸忋偺嵗昗傊曄姺
%     w [x y 1] = [X Y Z 1] * [R;t] * K        : w偼擟堄偺學悢丄[X Y Z]偼World嵗昗宯丄K偼僇儊儔撪晹僷儔儊乕僞
%    camMatrix = [rotationMatrix; translationVector] 亊 K
camMatrix = cameraMatrix(cameraParams, rotationMatrix, translationVector)   % Projection Matrix (4x3)
imagePointsP = [worldPoint2, 0, 1] * camMatrix
imagePoints = imagePointsP / imagePointsP(3)

%% 僠僃僢僇乕儃乕僪偺尨揰偵懳偡傞乮World嵗昗宯偱偺乯丄僇儊儔偺埵抲
%    僇儊儔嵗昗偺尨揰偵懳墳偡傞丄World嵗昗
orientation = rotationMatrix'
location = -translationVector * orientation

%% 僉儍儕僽儗乕僔儑儞偵梡偄偨4斣栚偺丄僠僃僢僇乕儃乕僪偺尨揰偵懳偡傞僇儊儔偺埵抲
rotationMatrix = cameraParams.RotationMatrices(:,:,6)
translationVector = cameraParams.TranslationVectors(6,:)
orientation = rotationMatrix'
location = -translationVector * orientation



%% [pointsToWorld娭悢傪巊傢側偄応崌丗R2014a埲慜] World嵗昗宯偐傜夋憸嵗昗宯傊偺曄姺峴楍傪寁嶼
R = rotationMatrix;
t = translationVector;
T = [R(1, :); R(2, :); t] * cameraParams.IntrinsicMatrix
tform2 = projective2d(T)      % 媮傔偨曄姺峴楍偐傜丄2師尦幩塭婔壗妛曄姺梡偺僆僽僕僃僋僩傪嶌惉

%% 夋憸忋偺"揰" undistPoints1傪丄World嵗昗宯傊曄姺偡傞応崌偵偼丄嶌惉偟偨tform偺媡曄姺傪揔梡
% 夋憸忋偺揰 undistPoints傪丄world coordinate嵗昗乮幚埵抲乯傊曄姺偡傞偲偒偼師峴
worldPoints1a = transformPointsInverse(tform2, undistPoints(8,:))

%% 2師尦僨乕僞(夋憸)傪World嵗昗宯(僇儊儔偵岦偒崌偆巔惃丄僇儊儔偺拞怱偑尨揰)傊曄姺乮1pixel = 1mm乯
birdsEyeView1 = imwarp(undistI, invert(tform2));   %Create the Bird's-Eye View
imtool(birdsEyeView1);
%% World嵗昗宯偺尨揰傪丄夋憸偺嵍忋嬿偵偟偰嵞昞帵乮1pixel = 1mm乯
birdsEyeView2 = imwarp(undistI, invert(tform2), 'OutputView', imref2d([750 900]));   %Create the Bird's-Eye View
imtool(birdsEyeView2);

% Copyright 2014 The MathWorks, Inc.