script4c

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)

% w1 = (c1n-T1)*R1'
w1 = c1n*R1' + C1;


c2 = w*R2+T2;
c2n = c2/c2(3)
% w2 = (c2n-T2)*R2';
w2 = c2n*R2' + C2;

c1n =

   -0.1882   -0.0591    1.0000


c2n =

   -0.1413   -0.0837    1.0000

calculate essential matrix

% before (world reference)
% k1 = c1n*R1';
% k2 = c2n*R2';
% T = C2 - C1;

% now: image 2 reference
R = R1'*R2;
k1 = c1n*R;
k2 = c2n;
T = (C2 - C1)*R2;

Sk = [0 -T(3) T(2); T(3) 0 -T(1); -T(2) T(1) 0];
E = Sk*R'

test = c2n*E*c1n'

% dot1 = dot(k1,cross1)
% dot2 = dot(k2,cross1)

% cross3 = cross(k2,k1)
% dot3 = dot (cross3,T)

% xpts = [C1; C1+10*cross1/norm(cross1)];
% hold on
% plot3(xpts(:,1),-xpts(:,2),-xpts(:,3),'r');
% hold off

E =

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


test =

  -2.9976e-15

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 =

   6.2172e-15

for n=1:4
    xa = [cpts1(n,:) 1];
    xb = [cpts2(n,:) 1];
    test = xb*Fc*xa';
    fprintf('%d %g\n',n,test);
end

1 7.10543e-15
2 0.00669125
3 -0.0475347
4 -0.0655788

recover camera parameters

[U, S, V] = svd(E);
S
Ty = U(:,3);
Tz = Ty*S(1)
T
% norm(Tz)
W = [0 -1 0; 1 0 0; 0 0 1];
Rx =  U*W'*V'
Ry = -U*W*V'
Rt = R'

S =

   12.5035         0         0
         0   12.5035         0
         0         0    0.0000


Tz =

   10.0698
    3.7119
   -6.4154


T =

   10.0698    3.7119   -6.4154


Rx =

   -0.5034   -0.4529    0.7358
   -0.4146    0.8738    0.2542
    0.7580    0.1771    0.6277


Ry =

    0.9744   -0.1368    0.1785
    0.1301    0.9903    0.0487
   -0.1835   -0.0243    0.9827


Rt =

    0.9744   -0.1368    0.1785
    0.1301    0.9903    0.0487
   -0.1835   -0.0243    0.9827

Published with MATLAB® R2015a