script5

Contents

clear
close all
load cpts
load mpts
load CalibResults
load Fmatrix

intrinsic matrix

fc = cameraParams.FocalLength;
cc = cameraParams.PrincipalPoint;
K = [fc(1) 0 cc(1); 0 fc(2) cc(2); 0 0 1];
Kinv = inv(K);

extrinsic matrices

worldPoints = [ 0 0 ; 21 0 ; 21 21 ; 0 21 ];

[R1, T1] = extrinsics(cpts1(1:4,:),worldPoints,cameraParams)

[R2, T2] = extrinsics(cpts2(1:4,:),worldPoints,cameraParams)

R1 =

    0.9689   -0.1280    0.2119
    0.2475    0.5028   -0.8282
   -0.0006    0.8549    0.5188


T1 =

  -11.7182   -3.6801   62.2700


R2 =

    0.9994    0.0097    0.0336
    0.0245    0.4898   -0.8715
   -0.0249    0.8718    0.4892


T2 =

   -9.8666   -5.8465   69.8490

verify basic equations

C1 = -T1*R1';
C2 = -T2*R2';

w = [worldPoints(1,:) 0];
c1 = w*R1+T1;
c1n = c1/c1(3)
c2 = w*R2+T2;
c2n = c2/c2(3)

c1n =

   -0.1882   -0.0591    1.0000


c2n =

   -0.1413   -0.0837    1.0000

calculate essential matrix

k1 = c1n*R1';
k2 = c2n*R2';

T = C2 - C1;
Sk = [0 -T(3) T(2); T(3) 0 -T(1); -T(2) T(1) 0];
E = R2'*Sk*R1

test = c2n*E*c1n'

E =

    0.1537    6.2632    3.9604
   -4.4035    1.1219  -11.0412
   -2.3066   10.4799   -0.1720


test =

   1.6653e-16

calculate fundamental matrix

x1 = c1n*K'
x2 = c2n*K'
Fc = inv(K')*E*inv(K);
Fc = Fc/Fc(3,3)
test2 = x2*Fc*x1'

x1 =

   1.0e+03 *

    1.1757    0.9463    0.0010


x2 =

   1.0e+03 *

    1.4847    0.7842    0.0010


Fc =

   -0.0000   -0.0000   -0.0009
    0.0000   -0.0000    0.0032
    0.0005   -0.0026    1.0000


test2 =

   4.4409e-16

figure(1)
rgb1 = imread(files{1});
show_points(rgb1,mpts1);
% imshow(rgb1);
%
% hold on
% plot(mpts1(:,1),mpts1(:,2),'yx','MarkerSize',9,'LineWidth',2);
% for n=1:24
%     text(mpts1(n,1)+20,mpts1(n,2),num2str(n),'Color','w');
% end
% hold off

Warning: Image is too big to fit on screen; displaying at 33% 

xnull = null(F')


figure(2)
rgb2 = imread(files{2});
imshow(rgb2);
sz = size(rgb2);

m = 0;
for n=[11 23]
    xA = [mpts1(n,:) 1];
    J = F*xA';
    m = m+1;
    M(m,:) = J;
    k = [-J(2) J(1)];
    k = k/norm(k);
    hold on
    x = [500 sz(2)-500];
    y = (-x*J(1)-J(3))/J(2);
    plot(x,y,'m','LineWidth',2);
    plot(mpts2(n,1),mpts2(n,2),'rx','MarkerSize',21);
    hold off
end

xnull =

    0.9125
    0.4090
   -0.0002

Warning: Image is too big to fit on screen; displaying at 33% 

% calculate epipole three different ways
format shortg
pt = -M(1:2,1:2)\M(1:2,3);
fprintf('epipole from intersection of two lines %g %g\n',pt);
pn = xnull/xnull(3);
fprintf('epipole from null matrix %g %g\n',pn(1:2));

% use MATLAB toolbox function
 [isIn,epipole] = isEpipoleInImage(F', size(rgb1))
 format

epipole from intersection of two lines -4695.19 -2104.4
epipole from null matrix -4695.19 -2104.4

isIn =

     0


epipole =

      -4695.2      -2104.4

% use MATLAB toolbox function
epiLines = epipolarLine(F, mpts1(11,:));
points = lineToBorderPoints(epiLines, size(rgb2));
imshow(rgb2);
hold on
plot(mpts2(11,1),mpts2(11,2),'yo','MarkerSize',11);
line(points(:, [1,3])', points(:, [2,4])');
hold off

Published with MATLAB® R2016a