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

    %% 2偮偺夋憸偐傜偺丄Structure From Motion
%     僉儍儕僽儗乕僔儑儞嵪傒偺僇儊儔偱嶣塭偟偨2枃偺2師尦夋憸偐傜丄
%     3D峔憿峔抸偲丄偦傟偧傟偺夋憸偵懳墳偡傞僇儊儔埵抲p惃悇掕
% [僼儘乕]
%  [偦傟偧傟偺夋憸偵懳偡傞丄僇儊儔帺屓埵抲p惃悇掕]
%   1: 2偮偺夋憸偱慳側懳墳揰傪扵嶕
%      乮夋憸1偐傜摿挜揰専弌偟丄夋憸2傊億僀儞僩僩儔僢僋乯
%      乮摿挜揰専弌偲摿挜検儅僢僠傿儞僌偺曽朄傕偁傝乯
%   2: 婎杮峴楍偺悇掕乮2偮偺夋憸偺懳墳娭學乯偲丄岆懳墳揰偺彍嫀
%   3: 婎杮峴楍偲懳墳揰嵗昗傪梡偄丄僇儊儔1偵懳偡傞丄僇儊儔2偺埵抲p惃傪悇掕
%  [3師尦嵞峔抸]
%   4: 僇儊儔1?偺埵抲p惃娭學偐傜丄偦傟偧傟偺僇儊儔峴楍(World嵗昗偲夋憸偺嵗昗偺娭學)傪寁嶼
%   5: 2偮偺夋憸偱枾側懳墳揰傪嵞扵嶕
%   6: 偦傟偧傟偺僇儊儔峴楍傪梡偄丄懳墳揰偺3師尦埵抲傪寁嶼
%  [幚僗働乕儖傊曗惓]
%   7: 戝偒偝偑婛抦偺暔懱傪揰孮偵僼傿僥傿儞僌偟丄揰孮偺僗働乕儖傪寁嶼
%   8: 媮傔偨僗働乕儖傪梡偄偰丄愨懳僗働乕儖偺3師尦揰孮傊曄姺

clc;clear;close all;imtool close all; rng('default');

%% 2枃偺夋憸偺撉崬傒丒昞帵
imgDir = [matlabroot '\toolbox\vision\visiondata\upToScaleReconstructionImages\'];
I1 = imread([imgDir 'globe1.jpg']);
I2 = imread([imgDir 'globe2.jpg']);
figure; imshowpair(I1, I2, 'montage'); title('Original Images');truesize;

%% 偦傟偧傟偺夋憸偐傜儗儞僘榗偺彍嫀
load upToScaleReconstructionCameraParameters.mat  % 僇儊儔僷儔儊乕僞偺撉崬傒乮撪晹僷儔儊乕僞丒榗學悢乯
I1u = undistortImage(I1, cameraParams);
I2u = undistortImage(I2, cameraParams);
imshowpair(I1u, I2u, 'montage'); title('Undistorted Images');truesize;shg;

%% [偦傟偧傟偺夋憸偵懳偡傞丄僇儊儔帺屓埵抲p惃悇掕] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 2偮偺夋憸娫偱懳墳偡傞揰傪尒偮偗傞
% 夋憸1撪偺慳側僐乕僫乕揰傪専弌丒昞帵 (栺400揰)
%      夋柺慡懱偵枮曊側偔専弌偡傞偨傔丄MinQuality愝掕偱丄専弌偝傟傞僐乕僫乕揰悢傪掅尭
imagePoints1 = detectMinEigenFeatures(rgb2gray(I1u), 'MinQuality', 0.1);
figure; imshow(I1u); truesize;
hold on; plot(imagePoints1);

%%
% 堏摦偑彫偝偄偺偱丄億僀儞僩僩儔僢僇乕傪梡偄夋憸娫偺懳墳揰傪扵嶕
% 愭偢億僀儞僩僩儔僢僇乕偺僆僽僕僃僋僩傪惗惉
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);
% 夋憸1忋偺僐乕僫乕揰偺嵗昗偱億僀儞僩僩儔僢僇乕傪弶婜壔
initialize(tracker, imagePoints1.Location, I1u);
% 億僀儞僩僩儔僢僇乕偱丄夋憸2忋偺懳墳揰傪専嶕
[imagePointsLoc2, validIdx] = step(tracker, I2u);
% 寢壥偺昞帵
matchedPoints1 = imagePoints1.Location(validIdx, :);
matchedPoints2 = imagePointsLoc2(validIdx, :);
figure; showMatchedFeatures(I1u, I2u, matchedPoints1, matchedPoints2);  % Figure撪偱奼戝偟偰妋擣

%% 婎杮峴楍偺悇掕丒岆懳墳揰(outlier)偺彍嫀媺蕚虝\帵
%  乮2偮偺夋憸忋偺揰偺懳墳娭學傪媮傔丄僄僺億乕儔峉懇傪枮偨偝側偄揰傪彍嫀乯
[E, epipolarInliers] = estimateEssentialMatrix(...
      matchedPoints1, matchedPoints2, cameraParams, 'Confidence', 99.99);

inlierPoints1 = matchedPoints1(epipolarInliers, :);
inlierPoints2 = matchedPoints2(epipolarInliers, :);
figure;
showMatchedFeatures(I1u, I2u, inlierPoints1, inlierPoints2); title('Epipolar Inliers');

%% 夋憸1偵懳偡傞丄夋憸2偺僇儊儔埵抲p惃(Orientation)傪寁嶼
%    僗働乕儖晄掕偺偨傔丄L(埵抲)偼扨埵儀僋僩儖偲壖掕
%    O : Orientation丄L丗Location
[O, L] = relativeCameraPose(E, cameraParams, inlierPoints1, inlierPoints2)

%% 2戜偺僇儊儔埵抲娭學傪壜帇壔
figure; grid on; hold on
plotCamera('Size',0.1,'Color','b','Label','start');  % 1斣栚偺僇儊儔傪尨揰偵昤夋
plotCamera('Location',L, 'Orientation',O, 'Size',0.1, ...
                      'Color','r','Label','finish');
xlabel('X'); ylabel('Y'); zlabel('Z'); view(3);
xlim([-0.4 1.4]);ylim([-0.3 0.3]);zlim([-0.2 1.6]);box on;
axis equal

%% [3師尦嵞峔抸] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 僇儊儔1?偺埵抲p惃偺娭學偐傜丄偦傟偧傟偺僇儊儔峴楍(World嵗昗偲夋憸偺嵗昗偺娭學)傪寁嶼
%      World嵗昗偺尨揰傪夋憸1偺僇儊儔埵抲丄僇儊儔偺曽岦傪Z幉偺惓偵偡傞
t1 = [0 0 0];      % 僇儊儔1偼World嵗昗尨揰偲愝掕乮暲恑堏摦側偟乯
R1 = eye(3);       % 僇儊儔1偼World嵗昗Z幉偺惓曽岦偲愝掕乮夞揮側偟乯
%t2 = -L*O';       % 僇儊儔2傊偺暲恑儀僋僩儖乮奜晹僷儔儊乕僞乯
%R2 = O';          % 僇儊儔2傊偺夞揮峴楍乮奜晹僷儔儊乕僞乯
[R2, t2] = cameraPoseToExtrinsics(O, L);     % R2016b
camMatrix1 = cameraMatrix(cameraParams, R1, t1);
camMatrix2 = cameraMatrix(cameraParams, R2, t2);

%% 2偮偺夋憸偱丄枾側懳墳揰傪嵞専嶕
% MinQuality傪壓偘偰丄懡偔偺(枾側)摿挜揰傪嵟専弌
roi = [30, 30, size(I1u, 2) - 30, size(I1u, 1) - 30];   % 夋憸抂傪彍嫀
imagePoints1 = detectMinEigenFeatures(rgb2gray(I1u), 'ROI',roi, 'MinQuality', 0.001);

% 億僀儞僩僩儔僢僇乕偺僆僽僕僃僋僩傪嵞搙惗惉
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 5);
% 億僀儞僩僩儔僢僇乕傪丄夋憸1忋偺枾側摿挜揰嵗昗偱弶婜壔
initialize(tracker, imagePoints1.Location, I1u);

% 億僀儞僩僩儔僢僇乕偱丄夋憸2忋偺懳墳揰傪専嶕
[imagePointsLoc2, validIdx] = step(tracker, I2u);
matchedPoints1 = imagePoints1.Location(validIdx, :);
matchedPoints2 = imagePointsLoc2(validIdx, :);

%% 懳墳揰偦傟偧傟偺丄3師尦埵抲傪寁嶼
points3D = triangulate(matchedPoints1, matchedPoints2, camMatrix1, camMatrix2);         % Direct Linear Transformation (DLT) algorithm

% 夋憸1偐傜偦傟偧傟偺揰偺怓忣曬傪拪弌
numPixels = size(I1u, 1) * size(I1u, 2);
allColors = reshape(I1u, [numPixels, 3]);
colorIdx = sub2ind([size(I1u, 1), size(I1u, 2)], round(matchedPoints1(:,2)), ...
    round(matchedPoints1(:, 1)));
color = allColors(colorIdx, :);

% 3師尦揰孮偺惗惉 (pointCloud僋儔僗)
ptCloud = pointCloud(points3D, 'Color', color)

%% 嵞峔抸偝傟偨3師尦揰孮偺昞帵
figure; grid on; hold on
plotCamera('Size', 0.3, 'Color', 'r', 'Label', '1', 'Opacity', 0);
plotCamera('Location', L, 'Orientation', O, 'Size', 0.3, ...
                  'Color', 'b', 'Label', '2', 'Opacity', 0);
pcshow(ptCloud, 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', ...
              'MarkerSize', 45);
camorbit(0, -30);     % 尒傞妏搙傪夞揮
xlabel('X'); ylabel('Y'); zlabel('Z');
xlim([-6 6]);ylim([-5 5]);zlim([-1 15]);box on;title('擟堄僗働乕儖')

%% [幚僗働乕儖傊曗惓] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 揰孮偵丄媴傪僼傿僢僥傿儞僌偟偰抧媴媀偺埵抲傪専弌\帵
globe = pcfitsphere(ptCloud, 0.1)    % 媴偺僼傿僢僥傿儞僌乮嵟戝嫍棧岆嵎0.1乯
plot(globe);      % 媴傪廳偹彂偒
hold off; shg;

%% 抧媴媀偺婛抦偺戝偒偝傪梡偄丄愨懳嫍棧偺揰孮偵曄姺
scaleFactor = 10 / globe.Radius;    % 抧媴媀偺敿宎:10cm
ptCloud = pointCloud(points3D * scaleFactor, 'Color', color);
Ls = L * scaleFactor;        % 僇儊儔埵抲2偺嵗昗傕愨懳嫍棧傊曄姺

% 愨懳嫍棧偱丄嵞壜帇壔
figure; grid on; hold on;
plotCamera('Size', 2, 'Color', 'r', 'Label', '1', 'Opacity', 0);
plotCamera('Location', Ls, 'Orientation', O, 'Size', 2, ...
                'Color', 'b', 'Label', '2', 'Opacity', 0);
pcshow(ptCloud, 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', ...
                'MarkerSize', 45);
camorbit(0, -30);
xlabel('X (cm)'); ylabel('Y (cm)'); zlabel('Z (cm)');
xlim([-40 40]);ylim([-30 30]);zlim([-5 85]);box on;

%% Copyright 2016 The MathWorks, Inc.