script2
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)
save extrinsics R1 T1 R2 T2
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);
check world origin -> camera
cam = T1/T1(3)
format shortg
img = cam*K
measured = cpts1(1,:)
format
cam =
-0.1882 -0.0591 1.0000
img =
1175.7 946.25 1
measured =
1175.8 946.25
find direction to project
for n=1:4
k1c = [cpts1(n,:) 1]*Kinv;
k2c = [cpts2(n,:) 1]*Kinv;
k1 = k1c*R1';
k2 = k2c*R2';
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('%11g \t %11g \t %11g \t world %g %g 0\n',pA,worldPoints(n,:));
end
4.35207e-14 -9.73444e-13 4.9738e-13 world 0 0 0
21.0273 -0.0128318 0.0342313 world 21 0 0
21.0185 20.8218 -0.043217 world 21 21 0
0.0568711 20.6607 0.0451562 world 0 21 0
show results
wpts = [ 0 0 0; 21 0 0; 21 21 0; 0 21 0];
ndx = [1:4 1];
plot3(wpts(ndx,1),-wpts(ndx,2),-wpts(ndx,3),'k','LineWidth',2);
hold on
plot3(wpts(1,1),-wpts(1,2),-wpts(1,3),'ko','MarkerSize',9);
plot3(C1(1),-C1(2),-C1(3),'ro','MarkerSize',9);
plot3(C2(1),-C2(2),-C2(3),'ro','MarkerSize',9);
clear dst
dst(1,:) = C1;
dst(2,:) = C1 + q(1)*k1;
plot3(dst(:,1),-dst(:,2),-dst(:,3),'k--','LineWidth',2);
dst(1,:) = C2;
dst(2,:) = C2 + q(2)*k2;
plot3(dst(:,1),-dst(:,2),-dst(:,3),'k--','LineWidth',2);
hold off
xlabel('x');
ylabel('y');
grid
axis equal
