www.gusucode.com > IPCV_Eval_Kit_R2019a_0ce6858工具箱matlab程序源码 > IPCV_Eval_Kit_R2019a_0ce6858/code/demo_files/I4_09_6_2_mySfM_multiViews1_R2016b.m
%% 暋悢偺2師尦夋憸偐傜偺丄Structure From Motion
% 僉儍儕僽儗乕僔儑儞嵪傒僇儊儔偱嶣塭偝傟偨暋悢偺帪宯楍2師尦夋憸偐傜丄
% 3師尦峔憿偺嵞峔抸丄奺夋憸偵懳墳偡傞僇儊儔埵抲p惃悇掕乮僗働乕儖偼晄掕乯
% 僙僋僔儑儞#1丗僇儊儔堏摦偺悇掕乮夋憸忋偺揰偺廤崌偼慳側傕偺傪巊梡乯
% 僙僋僔儑儞#2丗枾側3師尦揰孮偺嵞峔惉
clc;clear;close all;imtool close all; rng('default');
%% 帪宯楍偺暋悢夋憸偺僼傽僀儖巜掕乮imageSet僋儔僗傪巊梡乯
imageDir = [matlabroot '\toolbox\vision\visiondata\structureFromMotion'];
imds = imageDatastore(imageDir)
%% 夋憸偺昞帵 (僇儊儔偑嵍曽岦傊夞揮)
figure; montage(imds.Files, 'Size', [1,5]); truesize;
%% 慡夋憸偺撉崬傒
images = readall(imds); % 5x1
%% [僙僋僔儑儞#1丗僇儊儔堏摦(埵抲p惃)偺悇掕] %%%%%%%%%%%%%%%%%%%%
% 仏viewSet僆僽僕僃僋僩傪巊梡丗
% 奺夋憸偱偺摿挜揰嵗昗偲僇儊儔偺埵抲p惃傪奿擺 => vSet.Views View悢x[ViewId, Points, Orientation, Location]
% 奺夋憸娫偺摿挜揰偺懳墳傪奿擺 => vSet.Connections View悢-1 x[ViewId1, ViewId2, Matches, RelativeOrientation, RelativeLocation]
% 壓婰1乣9傪丄慡View偵懳偟丄View偺弴偵幚峴
% 1: 儗儞僘榗彍嫀XS
% 2: 摿挜揰偺専弌丄摿挜検拪弌丄
% 3: 堦偮慜偺夋憸偲丄2偮偺夋憸娫偱摿挜検偺儅僢僠儞僌乮億僀儞僩僩儔僢僇乕偱傕壜擻乯
% 4: 婎杮峴楍(2偮偺惓婯壔夋憸嵗昗忋偺揰偺懳墳娭學)偺悇掕
% 5: 2偮偺夋憸娫偺揰偺懳墳娭學偐傜丄堦偮慜偺僇儊儔埵抲p惃偵懳偡傞埵抲p惃傪悇掕
% 6: 嵟弶偺僇儊儔埵抲p惃偵懳偡傞丄尰僇儊儔埵抲p惃傪寁嶼
% 7: 僩儔僢僋(暋悢View偵傑偨偑傞丄揰偺懳墳娭學)偺拪弌
% 8: 尰View傑偱偺慡僇儊儔埵抲p惃忣曬偲慡揰懳墳娭學傪梡偄丄奺揰偺3師尦埵抲(儅僢僾)傪悇掕
% 9: 僶儞僪儖挷惍偵傛傝丄偦傟傑偱偺慡揰孮嵗昗偲丄僇儊儔埵抲p惃傪廋惓
%% 1枃栚偺夋憸傪張棟
% 1枃栚偺夋憸偺儗儞僘榗彍嫀\帵
load([imageDir, '\cameraParams.mat']); % 僇儊儔僷儔儊乕僞偺撉崬傒
I = undistortImage(images{1}, cameraParams);
figure; imshowpair(images{1}, I, 'montage');truesize;
%%
% 廃埻50僺僋僙儖偼彍偄偰丄摿挜揰拪弌乮夋憸僄僢僕偱偺摿挜揰傪攔彍乯
border = 50;
roi = [border, border, size(I, 2)- 2*border, size(I, 1)- 2*border];
% 摿挜揰専弌丗戝嬊揑摿挿傪庢摼偡傞堊偵丄NumOctaves傪戝偒偔愝掕
G = rgb2gray(I); % 僌儗乕僗働乕儖傊曄姺
prevPoints = detectSURFFeatures(G, 'NumOctaves', 8, 'ROI', roi);
% 摿挜検拪弌丗夞揮偑彮側偄偨傔Upright傪巜掕(Orientation傪忋岦偒偵屌掕)
prevFeatures = extractFeatures(G, prevPoints, 'Upright', true);
% 1枃栚偺夋憸偺僨乕僞(摿挜揰嵗昗丄僇儊儔埵抲p惃)傪丄viewID=1偲偟偰搊榐
% 偙偺偲偒偺僇儊儔埵抲(岝妛拞怱)偑World嵗昗偺尨揰
% 僇儊儔偺岦偒偑World嵗昗宯偺Z幉偺惓曽岦
vSet = viewSet % 嬻偺viewSet僆僽僕僃僋僩傪惗惉
viewID = 1;
vSet = addView(vSet, viewID, 'Points', prevPoints, 'Orientation', eye(3,'single'),...
'Location', single([0 0 0])); % Point偺宆偵懙偊傞
vSet.Views % ViewId=1 偑搊榐偝傟偰偄傞
%% 巆傝偺夋憸偺僨乕僞傕丄儖乕僾張棟偱vSet僆僽僕僃僋僩傊搊榐
for viewID = 2:numel(images)
% 儗儞僘榗彍嫀
I = undistortImage(images{viewID}, cameraParams);
% 摿挜揰専弌翏蕭姀o陚聭O偺夋憸偺摿挜検偲儅僢僠儞僌
G = rgb2gray(I); % 僌儗乕僗働乕儖傊曄姺
currPoints = detectSURFFeatures(G, 'NumOctaves', 8, 'ROI', roi);
currFeatures = extractFeatures(G, currPoints, 'Upright', true);
indexPairs = matchFeatures(prevFeatures, currFeatures, ...
'MaxRatio', .7, 'Unique', true);
% 懳墳娭學偑尒偮偐偭偨摿挜揰傪拪弌
matchedPoints1 = prevPoints(indexPairs(:, 1));
matchedPoints2 = currPoints(indexPairs(:, 2));
% 懳墳揰偺忣曬偐傜
% 堦偮慜偺僇儊儔埵抲p惃(僇儊儔嵗昗)偵懳偡傞埵抲p惃傪悇掕乮嫍棧偼1偲壖掕乯
% 屻偺Bundle Adjustment偱丄嫍棧偺岆嵎摍偼曗惓偝傟傞
for i = 1:100
% 婎杮峴楍(2偮偺夋憸偺懳墳娭學)偺悇掕
[E, inlierIdx] = estimateEssentialMatrix( ...
matchedPoints1, matchedPoints2, cameraParams);
% inlier偑彮側偄応崌偼丄嵞搙悇掕
if sum(inlierIdx) / numel(inlierIdx) < .3
continue;
end
% 僄僺億乕儔峉懇傪枮偨偝側偄傕偺(岆懳墳揰)偺彍嫀
inlierPoints1 = matchedPoints1(inlierIdx, :);
inlierPoints2 = matchedPoints2(inlierIdx, :);
% 婎杮峴楍偐傜丄堦偮慜偺僇儊儔埵抲p惃偵懳偡傞丄尰僇儊儔埵抲(扨埵儀僋僩儖)丒巔惃傪悇掕
[relativeOrient, relativeLoc, validPointFraction] = ...
relativeCameraPose(E, cameraParams, inlierPoints1, inlierPoints2); % 億僀儞僩傪敿暘偵娫堷偔偙偲偱張棟偺崅懍壔傕壜擻 (1:2:end, :)
% 桳岠側揰偺妱崌偑崅偔側傞傑偱孞傝曉偟丄婎杮峴楍偺悇掕傪峴偆
if validPointFraction > .8
break;
elseif i == 100;
% 100夞斀暅偟偰傕validPointFraction偑掅偄応崌偼丄僄儔乕偵偡傞
error('Unable to compute the Essental matrix');
end
end
% 1偮慜偺僇儊儔埵抲p惃(儚乕儖僪嵗昗宯)傪庢摼 (addView 偱僙僢僩偟偨傕偺)
prevPose = poses(vSet, viewID-1);
prevOrientation = prevPose.Orientation{1};
prevLocation = prevPose.Location{1};
% 堦斣栚偺View偵懳偡傞(儚乕儖僪嵗昗宯)丄尰僇儊儔埵抲p惃傪媮傔傞.
orientation = relativeOrient * prevOrientation;
location = relativeLoc * prevOrientation + prevLocation;
% 摿挜揰偺嵗昗丄僇儊儔埵抲p惃傪丄vSet傊搊榐
vSet = addView(vSet, viewID, 'Points', currPoints, 'Orientation', orientation, ...
'Location', location);
% 堦偮慜偺夋憸偲偺摿挜揰偺懳墳娭學傪丄vSet傊搊榐
vSet = addConnection(vSet, viewID-1, viewID, 'Matches', indexPairs(inlierIdx,:));
% 僩儔僢僋丗暋悢View偵傑偨偑傞丄揰偺慡懳墳娭學忣曬乮堦晹偺僩儔僢僋偼丄慡View偵傑偨偑偭偰偄傞乯
tracks1 = findTracks(vSet); % 尰嵼傑偱偺慡View娫偺僩儔僢僋忣曬拪弌
% 尰嵼傑偱偺慡View偺丄僇儊儔埵抲p惃傪庢摼
camPoses1 = poses(vSet);
% 暋悢夋憸忋偺揰懳墳娭學偐傜丄奺揰偺3師尦埵抲傪悇掕
xyzPoints1 = triangulateMultiview(tracks1, camPoses1, cameraParams);
% Bundle Adjustment偱丄揰孮偺埵抲偲僇儊儔埵抲p惃傪嵟揔壔偡傞
[xyzPoints1, camPoses1, reprojectionErrors] = bundleAdjustment(xyzPoints1, ...
tracks1, camPoses1, cameraParams, 'FixedViewId', 1, ...
'PointsUndistorted', true);
% 僶儞僪儖挷惍偱旝廋惓偟偨僇儊儔埵抲p惃傪搊榐
vSet = updateView(vSet, camPoses1); % 僥乕僽儖丗camPoses1 偺忣曬乮ViewID, Orientation, Location乯偱傾僢僾僨乕僩
prevFeatures = currFeatures;
prevPoints = currPoints;
end
%% 寢壥偺妋擣
vSet.Views % 奺夋憸(View)偱偺丄摿挜揰嵗昗丄僇儊儔巔惃丒埵抲
vSet.Connections % 奺夋憸娫偺揰偺懳墳娭學
figure; % 嵟屻偺2枃偺夋憸娫偺懳墳娭學乮墦曽偺傕偺偼丄堏摦検戝乯
showMatchedFeatures(undistortImage(images{viewID-1}, cameraParams), I, inlierPoints1, inlierPoints2); truesize;
%% 僾儘僢僩丗3師尦揰孮丄僇儊儔埵抲p惃
camPoses1 = poses(vSet);
figure; plotCamera(camPoses1, 'Size', 0.2); % 僇儊儔傪Figure忋偵僾儘僢僩
hold on;
goodIdx = (reprojectionErrors < 5); % 僄儔乕抣偺戝偒偄揰傪彍嫀
pcshow(xyzPoints1(goodIdx, :), 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', 'MarkerSize', 45);
xlim([-6 4]); ylim([-4 5]); zlim([-1 21]);
xlabel('X');ylabel('Y');zlabel('Z'); camorbit(0, -30); grid on; box on; hold off
title('Refined Camera Poses');
%% [僙僋僔儑儞#2丗嵞搙慡偰偺夋憸傪張棟偟丄枾側3師尦揰孮傪嵞峔惉] %%%%%%%
% 堦枃栚偺夋憸偱枾側僐乕僫乕揰専弌丄偦偺屻億僀儞僩僩儔僢僇乕偱慡View忋偱懳墳埵抲傪専弌
% 暋悢夋憸忋偺懳墳揰忣曬傪梡偄3師尦嵞峔抸
% 嵞搙僶儞僪儖挷惍偱丄3師尦揰孮嵗昗偲丄僇儊儔埵抲p惃傪旝挷惍
% 堦枃栚偺夋憸偺儗儞僘榗彍嫀
I = undistortImage(images{1}, cameraParams);
% 僐乕僫乕揰専弌乮MinQuality傪壓偘偰丄悢傪憹傗偡乯
G = rgb2gray(I);
initPoints = detectMinEigenFeatures(G, 'MinQuality', 0.001);
% 僩儔僢僉儞僌梡僆僽僕僃僋僩傪惗惉乮僩儔僢僉儞僌偱丄師偺夋憸忋偺懳墳揰傪専弌)
tracker = vision.PointTracker('MaxBidirectionalError', 1, 'NumPyramidLevels', 6);
% 専弌偝傟偨僐乕僫乕揰偱丄億僀儞僩僩儔僢僇乕傪弶婜壔
initPoints = initPoints.Location;
initialize(tracker, initPoints, I);
% 揰懳墳娭學傪弶婜壔屻丄枾側僐乕僫乕揰偱摿挜揰嵗昗傪峏怴丄
viewID = 1;
vSet = updateConnection(vSet, viewID, viewID+1, 'Matches', zeros(0, 2));
vSet = updateView(vSet, viewID, 'Points', initPoints);
%% 2枃栚埲崀偺夋憸忋偺懳墳揰嵗昗傪丄億僀儞僩僩儔僢僇乕偱悇掕
% vSet偺丄View偲Connection忣曬傪傾僢僾僨乕僩
for viewID = 2:numel(images)
% 尰夋憸偐傜丄儗儞僘榗偺彍嫀
I = undistortImage(images{viewID}, cameraParams);
% 尰夋憸忋偺懳墳揰傪専弌
[currPoints, validIdx] = step(tracker, I);
% 揰懳墳娭學傪弶婜壔屻丄枾側僐乕僫乕揰偱摿挜揰嵗昗忣曬傪峏怴丄
if viewID < numel(images)
vSet = updateConnection(vSet, viewID, viewID+1, 'Matches', zeros(0, 2));
end
vSet = updateView(vSet, viewID, 'Points', currPoints);
% 揰懳墳娭學忣曬傪峏怴
matches = repmat((1:size(initPoints, 1))', [1, 2]);
matches = matches(validIdx, :);
vSet = updateConnection(vSet, viewID-1, viewID, 'Matches', matches);
end
% 寢壥偺妋擣
vSet.Connections % 桳岠揰偑彊乆偵尭彮丗億僀儞僩僩儔僢僇乕傪嵟弶偺夋憸偱弶婜壔偟偰偄傞偨傔
% 僩儔僢僋丗View娫偺揰偺懳墳娭學忣曬丄堦晹偺僩儔僢僋偼丄慡View偵傑偨偑偭偰偄傞
tracks2 = findTracks(vSet); % 慡View娫偺僩儔僢僋忣曬偺拪弌: 奺僩儔僢僋:懚嵼偡傞View(夋憸)斣崋偲丄奺夋憸忋偺嵗昗
% 慡View偺丄僇儊儔埵抲p惃傪庢摼
camPoses2 = poses(vSet); % viewSet偺儊僜僢僪
% 暋悢夋憸忋偺懳墳揰慻乮僩儔僢僋乯偵懳墳偡傞丄奺揰偺3師尦儚乕儖僪嵗昗傪寁嶼
xyzPoints2 = triangulateMultiview(tracks2, camPoses2, cameraParams);
% 慡View偺丄3師尦揰孮嵗昗偲僇儊儔埵抲p惃傪僶儞僪儖挷惍偱旝廋惓
[xyzPoints2, camPoses2, reprojectionErrors] = bundleAdjustment(...
xyzPoints2, tracks2, camPoses2, cameraParams, 'FixedViewId', 1, 'PointsUndistorted', true);
%% 枾側3師尦嵞峔惉寢壥偺昞帵
figure; plotCamera(camPoses2, 'Size',0.2); % 僇儊儔埵抲p惃傪僾儘僢僩
hold on;
% 奺僩儔僢僋偺怓傪拪弌
color = zeros(numel(tracks2),3,'uint8');
for k = 1:numel(tracks2) % 慡僩儔僢僋傪儖乕僾張棟
Itracked = images{tracks2(k).ViewIds(1)}; % 奺僩儔僢僋偺嵟弶偺ViewId偺夋憸傪慖戰 => RGB抣傪拪弌
x = round(tracks2(k).Points(1,1));
y = round(tracks2(k).Points(1,2));
x = min(size(Itracked,2),x);
y = min(size(Itracked,1),y);
color(k,:) = Itracked(y,x,:);
end
goodIdx = (reprojectionErrors < 5); % 僄儔乕抣偑戝偒偄揰傪彍嫀
pcshow(xyzPoints2(goodIdx, :), color(goodIdx, :), 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', 'MarkerSize', 20);
xlim([-6 4]); ylim([-4 5]); zlim([-1 21]);
xlabel('X');ylabel('Y');zlabel('Z');camorbit(0, -30); grid on; box on;
title('Dense Reconstruction');
%% 廔椆
%% 僇儊儔堏摦検偑婛抦偺応崌丄愨懳嫍棧偺揰孮偵曄姺
camPoses2.Location{1} % 僇儊儔埵抲1偺嵗昗偼丄[0 0 0]
p5 = camPoses2.Location{5} % 僇儊儔埵抲5偺嵗昗乮僇儊儔1偐傜偺憡懳嵗昗乯
scaleFactor = 2.1 / norm(p5) % [椺] 僇儊儔埵抲1偲僇儊儔埵抲5偺嫍棧偑丄2.1m偺偲偒
xyzPointsS = xyzPoints2 * scaleFactor; % 揰孮偺僗働乕儖傪曄峏
camPosesS = camPoses2;
locS =cell2mat(camPosesS.Location) * scaleFactor
camPosesS.Location = num2cell(locS, 2)
% 昞帵
figure; plotCamera(camPosesS, 'Size',0.2); % 僇儊儔埵抲p惃傪僾儘僢僩
hold on;
pcshow(xyzPointsS(goodIdx, :), color(goodIdx, :), 'VerticalAxis', 'y', 'VerticalAxisDir', 'down', 'MarkerSize', 20);
xlim([-6 4]); ylim([-4 5]); zlim([-1 21]);
xlabel('X');ylabel('Y');zlabel('Z');camorbit(0, -30); grid on; box on;
title('Dense Reconstruction (Actual Scale)');
%% 僇儊儔僷儔儊乕僞傪丄悢抣偱巜掕偡傞応崌 %%%%%%%%%%%%
%load([imageDir, '\cameraParams.mat']) 偺戙傢傝偵
% 撪晹僇儊儔僷儔儊乕僞偺愝掕
intrinsicMatrix = [1037.575214664696 0 0;
0 1043.315752317925 0;
642.231583031218 387.835775096238 1];
% 儗儞僘榗僷儔儊乕僞偺愝掕
radialDistortion = [0.146911684283474 -0.214389634520344];
% cameraParameters 僆僽僕僃僋僩偺惗惉
cameraParams = cameraParameters('IntrinsicMatrix',intrinsicMatrix, 'RadialDistortion',radialDistortion);
%% Copyright 2016 The MathWorks, Inc.