script3

Contents

load data and calc extrinsics

clear
close all
load cpts
load mpts
load CalibResults

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

camera origins

C1 = -T1*R1'

C2 = -T2*R2'

C1 =

   -2.3130   56.3234  -29.1674


C2 =

    7.5710   63.9797  -29.3205

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);

K =

   1.0e+03 *

    6.5846         0         0
         0    6.5845         0
    2.4149    1.3354    0.0010

find projections

fprintf('\n');
npts = size(mpts1,1);
for n=1:24
    k1c = [mpts1(n,:) 1]*Kinv;
    k2c = [mpts2(n,:) 1]*Kinv;

    k1 = k1c*R1';
    k2 = k2c*R2';

    % find projection distance

    b = [ dot(C2-C1,k1); -dot(C2-C1,k2)];
    A = [dot(k1,k1) -dot(k1,k2); -dot(k1,k2) dot(k2,k2)];
    q = A\b;

    pA = C1+k1*q(1);
    pB = C2+k2*q(2);
    fprintf('%d \t %8.4f \t %8.4f \t %8.4f \t   %10.6f\n',n,pA,norm(pA-pB));
end

1 	   0.0376 	   0.0964 	  -0.0445 	     0.016820
2 	  12.0349 	  -0.1212 	   0.0089 	     0.037869
3 	  18.0212 	   0.0188 	  -0.0394 	     0.045440
4 	  21.0029 	   0.0430 	  -0.0117 	     0.036953
5 	  16.1952 	   9.5454 	  -4.3385 	     0.060603
6 	  18.2264 	  11.3593 	  -4.2453 	     0.101709
7 	  17.2113 	  12.4221 	  -4.2053 	     0.172738
8 	  15.1509 	  10.6362 	  -4.2190 	     0.103872
9 	  20.9907 	   6.0088 	  -0.0576 	     0.008074
10 	  14.9952 	   3.0495 	  -0.0935 	     0.014603
11 	   3.6613 	  10.4836 	  -8.4072 	     0.145389
12 	   9.2969 	   7.9869 	  -8.4556 	     0.075145
13 	   8.5654 	   5.3840 	  -8.2224 	     0.066653
14 	   2.6853 	   8.1530 	  -8.2529 	     0.094310
15 	   3.8368 	  10.6246 	   0.0571 	     0.077957
16 	   6.0456 	  11.6619 	   0.0692 	     0.047538
17 	  17.1893 	  12.5572 	  -0.1299 	     0.068494
18 	  15.0576 	  11.7822 	   0.0384 	     0.057565
19 	  18.0613 	  11.8249 	   0.0021 	     0.062212
20 	  20.9720 	  20.8766 	  -0.0762 	     0.104300
21 	  18.0468 	  20.8451 	  -0.0576 	     0.118973
22 	  15.0862 	  20.6588 	   0.0546 	     0.123564
23 	   3.0491 	  20.7005 	  -0.0031 	     0.151537
24 	   0.0571 	  20.5331 	   0.1215 	     0.140688

figure(1)
rgb = imread(files{1});
show_points(rgb,mpts1);

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

figure(2)
rgb = imread(files{2});
show_points(rgb,mpts2);

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

Published with MATLAB® R2016a