script4b

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)
img = K*c1n';
format shortg
img1 = img'/img(3)
cpts1(1,:)
format

% w1 = (c1n-T1)*R1'
w1 = c1n*R1' + C1;
xpts = [ C1; w1; w];

plot3(xpts(:,1),-xpts(:,2),-xpts(:,3),'ko--');
hold on
plot3(w(1),-w(2),-w(3),'ko');
hold off
axis equal
grid

c2 = w*R2+T2;
c2n = c2/c2(3)
% w2 = (c2n-T2)*R2';
w2 = 10*c2n*R2' + C2;
xpts = [ C2; w2; w];
hold on
plot3(xpts(:,1),-xpts(:,2),-xpts(:,3),'bo--');
hold off

c1n =

   -0.1882   -0.0591    1.0000


img1 =

       1175.7       946.25            1


ans =

       1175.8       946.25


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];
cross1 = cross(T,k1)
cross2 = Sk*k1'
E = R2'*Sk*R1

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

cross1 =

    3.4477   -4.6354   -9.2245


cross2 =

    3.4477
   -4.6354
   -9.2245


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

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

1 8.88178e-16
2 0.00669125
3 -0.0475347
4 -0.0655788

recover camera parameters

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

E =

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


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.9744   -0.1368    0.1785
    0.1301    0.9903    0.0487
   -0.1835   -0.0243    0.9827


Ry =

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


Rt =

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

compare to fitted F

test3 = x2*F*x1'

E2 = K'*F*K

[U, S, V] = svd(E2);
S
Tx = -U(:,3)
Ty % from before
% norm(Tx)
W = [0 -1 0; 1 0 0; 0 0 1];
Rx = U*W'*V'
Ry = -U*W*V'
% check if rotation matrix
% det(R)
% R*R'

test3 =

    0.0053


E2 =

   -3.1321  -38.9746  -14.0862
   34.8922   -9.0300   32.5972
    9.3937  -31.8071    0.5356


S =

   53.0277         0         0
         0   48.7557         0
         0         0    0.0000


Tx =

    0.5735
    0.2606
   -0.7767


Ty =

    0.8054
    0.2969
   -0.5131


Rx =

    0.1827   -0.3791    0.9071
   -0.2112    0.8860    0.4128
    0.9602    0.2670   -0.0818


Ry =

    0.9810   -0.1567    0.1142
    0.1516    0.9870    0.0526
   -0.1210   -0.0342    0.9921

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

1 0.00526258
2 0.0606759
3 0.0147397
4 0.0115282

Published with MATLAB® R2015a