www.gusucode.com > 基于matlab的三维重建代码，提取两幅图片的匹配点源码程序 > 基于matlab的三维重建代码，提取两幅图片的匹配点源码程序/code/code/TwoViewDemo.m

    % two view example of calibrated structure from motion 
% recovery
close all; clear;

im0 = rgb2gray(imread('boxes1.bmp','bmp'));
im1 = rgb2gray(imread('boxes8.bmp', 'bmp'));
[ydim, xdim] = size(im0);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% intrinsic parameter matrix
Khat = [653.91 0 320.52; 0 649.72 220.40; 0 0 1];

%%%%%%%%%%%%%%%%%%%
% undo distortion
im0 = distortion(im0,0.03); 
im1 = distortion(im1,0.03);

%%%%%%%%%%%%%%%%%%%%%
load boxpoints  % 13 points seen in two frames frames

NPOINTS =  size(xim(:,:,1),2);   

figure(1); imagesc(im0); colormap gray; hold on; axis equal; axis off;
plot(xim(1,:,1), xim(2,:,1),'w.');
figure(2); imagesc(im1); colormap gray; hold on; axis equal; axis off;
plot(xim(1,:,2), xim(2,:,2),'w.');


FRAMES = 2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute retinal coordinates
for i = 1:FRAMES
  xn(:,:,i) = inv(Khat)*xim(:,:,i);
end

%%%%%%%%%%%%%%%%%%%%
% initialize motion
[T0, R0] = dessential(xn(:,:,1), xn(:,:,2));

lambda = zeros(1,NPOINTS);
XE = zeros(3,NPOINTS,FRAMES);
[X,lambda]=compute3DStructure(xn(:,:,1),xn(:,:,2), R0, T0);
figure(FRAMES + 1);
XE(:,:,1) = X(:,:,1);
XE(:,:,2) = X(:,:,2);
plot3(XE(1,:,1),XE(2,:,1),XE(3,:,1),'r.'); hold on;

% pause
[l3d, inc]  = boxseqlines(XE(1:3,:,1));
lnum = size(l3d,2);
 for i=1:lnum
   line([l3d(1,i) l3d(4,i)], [l3d(2,i) l3d(5,i)], [l3d(3,i) l3d(6,i)]);
end;
xlabel('x'); ylabel('y'); zlabel('z'); % axis equal;
title('Euclidean reconstruction'); view(220,20); axis equal; box on;
hold on;
pause;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute residuals and reproject them
xres0=Khat*[X(1,:,1)./X(3,:,1); X(2,:,1)./X(3,:,1); ones(1,NPOINTS)];
xres1=Khat*[X(1,:,2)./X(3,:,2); X(2,:,2)./X(3,:,2); ones(1,NPOINTS)];
figure(1); hold on; plot(xres0(1,:), xres0(2,:),'y.');  
figure(FRAMES); hold on; plot(xres1(1,:), xres1(2,:),'y.'); 
pause;

xd0 = xim(1:2,:,1) - xres0(1:2,:);
xd1 = xim(1:2,:,2) - xres1(1:2,:);
res = [xd0'; xd1'];
f = [norm(res'*res)/2];
% pause