Adding matlab simulations

This is an experiment script for learning about camera calibration. The
camera poses and intrinsic matrices are roughly reality. It would be
nice to be able to somehow iteratively tweek these parameters using real
data...

Author: bredfeldt

matlab/CameraCalibrationExp.m

@@ -0,0 +1,175 @@
+%Camera Calibration Experiments:
+% Jeremy Bredfeldt - Morgridge Institutes for Research
+% Sept 2013
+
+%Trying to derive the camera calibration matrices from first principles
+%If we know the pose of the camera, then what are the camera matrices?
+%With this, we can ROM check the calibrations generated by the calibration routines
+
+%Then can we use these matrices to triangulate points properly in 3D, based on 2 camera poses?
+
+%Place the origin in the south, west, floor level corner of the atrium
+%Positive x = points east
+%Positive y = points north
+%Positive z = up
+%Rotations: Cameras start pointing in positive z direction, and rotated from there
+% Bottom of camera is pointing in neg y direction
+% Use right hand rule for direction of rotation
+
+%Atrium dimensions
+%39624 mm in x direction
+ax = 39624;
+%10947 mm in y direction
+ay = 10947;
+%15240 mm in z direction
+az = 15240;
+
+%Camera sensor and lens
+%Focal length = 4.2 mm
+fx = 4.2;
+fy = fx;
+%Sensor = 4.54 mm X 3.42 mm
+sx = 4.54/2;
+sy = 3.42/2;
+%Image size = 1280 X 720 pixels
+ix = 1280;
+iy = 720;
+
+%These are in the world's reference frame, to actually get camera matrices, these must be inverted.
+%--Translation--
+%Camera 1 (west side)
+Cx1 = 0;
+Cy1 = ay/2; %centered in y dim
+Cz1 = az;
+C1 = [Cx1; Cy1; Cz1];
+
+%Camera 2 (east side)
+Cx2 = ax;
+Cy2 = ay/2;
+Cz2 = az;
+C2 = [Cx2; Cy2; Cz2];
+
+%--Rotation--
+%Camera 1
+thx1 = 0;
+thy1 = pi/1.5; %point 45 deg down
+thz1 = 0;
+Rcx1 = [1 0 0; 0 cos(thx1) -sin(thx1); 0 sin(thx1) cos(thx1)];
+Rcy1 = [cos(thy1) 0 sin(thy1); 0 1 0; -sin(thy1) 0 cos(thy1)];
+Rcz1 = [cos(thz1) -sin(thz1) 0; sin(thz1) cos(thz1) 0; 0 0 1];
+Rc1 = Rcx1*Rcy1*Rcz1;
+
+%Camera 2
+thx2 = 0;
+thy2 = -pi/1.5; %point 45 deg down
+thz2 = pi;
+Rcx2 = [1 0 0; 0 cos(thx2) -sin(thx2); 0 sin(thx2) cos(thx2)];
+Rcy2 = [cos(thy2) 0 sin(thy2); 0 1 0; -sin(thy2) 0 cos(thy2)];
+Rcz2 = [cos(thz2) -sin(thz2) 0; sin(thz2) cos(thz2) 0; 0 0 1];
+Rc2 = Rcx2*Rcy2*Rcz2;
+
+%Get the actual camera matrices (inverse of the previously written matrices):
+R1 = Rc1';
+R2 = Rc2';
+t1 = -R1*C1; %origin in cam 1 ref frame
+t2 = -R2*C2; %origin in cam 2 ref frame
+
+%--Extrinsic--
+E1 = [R1 t1];
+E2 = [R2 t2];
+
+%--Intrinsic--
+%Assume no skew or translation, and fx and fy are same
+%Assume both cameras are the same
+K = [fx 0 0; 0 fy 0; 0 0 1];
+
+%--Final Matrices--
+P1 = K*E1;
+P2 = K*E2;
+
+%--Tests--
+
+%3D point, middle of the atrium floor:
+x1 = [ax/1.5; ay/3; 6000; 1];
+%x1 = [C2; 1];
+
+%Project these points onto the camera image planes
+y11 = P1*x1;
+y12 = P2*x1;
+
+c1x = y11(1)/y11(3)
+c1y = y11(2)/y11(3)
+c2x = y12(1)/y12(3)
+c2y = y12(2)/y12(3)
+
+%Draw the spots on the images
+figure(2); clf;
+plot(c1x,c1y,'r*');
+axis equal;
+xlim([-sx sx]);
+ylim([-sy sy]);
+title('Camera 1');
+figure(3); clf;
+plot(c2x,c2y,'k*');
+axis equal;
+xlim([-sx sx]);
+ylim([-sy sy]);
+title('Camera 2');
+
+%Create plots of the results
+figure(1);
+clf;
+%draw atrium floor
+plot3([0 ax ax 0 0],[0 0 ay ay 0],[0 0 0 0 0]);
+hold on;
+%draw camera centers
+%plot3(C1(1),C1(2),C1(3),'r*');
+%plot3(C2(1),C2(2),C2(3),'k*');
+%draw camera directions
+zd = 2000;
+d = [0; 0; zd; 1];
+D1 = [Rc1 C1]*d;
+D2 = [Rc2 C2]*d;
+plot3([C1(1) D1(1)],[C1(2) D1(2)],[C1(3) D1(3)],'r');
+plot3([C2(1) D2(1)],[C2(2) D2(2)],[C2(3) D2(3)],'k');
+
+%draw camera frustrum
+%create rays in camera frame, then transform to world frame
+%4 rays
+f1 = [sx*zd/fx; sy*zd/fy; zd; 1];
+f2 = [-sx*zd/fx; sy*zd/fy; zd; 1];
+f3 = [-sx*zd/fx; -sy*zd/fy; zd; 1];
+f4 = [sx*zd/fx; -sy*zd/fy; zd; 1];
+%rays for first camera
+F11 = [Rc1 C1]*f1;
+F12 = [Rc1 C1]*f2;
+F13 = [Rc1 C1]*f3;
+F14 = [Rc1 C1]*f4;
+plot3([C1(1) F11(1)],[C1(2) F11(2)],[C1(3) F11(3)],'r');
+plot3([C1(1) F12(1)],[C1(2) F12(2)],[C1(3) F12(3)],'r');
+plot3([C1(1) F13(1)],[C1(2) F13(2)],[C1(3) F13(3)],'r');
+plot3([C1(1) F14(1)],[C1(2) F14(2)],[C1(3) F14(3)],'r');
+
+%rays for second camera
+F21 = [Rc2 C2]*f1;
+F22 = [Rc2 C2]*f2;
+F23 = [Rc2 C2]*f3;
+F24 = [Rc2 C2]*f4;
+plot3([C2(1) F21(1)],[C2(2) F21(2)],[C2(3) F21(3)],'k');
+plot3([C2(1) F22(1)],[C2(2) F22(2)],[C2(3) F22(3)],'k');
+plot3([C2(1) F23(1)],[C2(2) F23(2)],[C2(3) F23(3)],'k');
+plot3([C2(1) F24(1)],[C2(2) F24(2)],[C2(3) F24(3)],'k');
+
+%draw test point
+plot3(x1(1),x1(2),x1(3),'b*');
+%draw projection result
+
+xlabel('South Wall (x, mm)');
+ylabel('West Wall (y, mm)');
+zlabel('(z, mm)');
+axis equal;
+
+na = 0.01;
+%now triangulate to find the 3d location again, using the camera matrices
+M = triangulateJYB(P1,[c1x+na*randn c1y+na*randn],P2,[c2x+na*randn c2y+na*randn]);
+plot3(M(1),M(2),M(3),'ro');

matlab/triangulateJYB.m

@@ -0,0 +1,40 @@
+function M = triangulateJYB(P1, m1, P2, m2)
+% TRIANGULATE computes the 3D point location using 2D camera views
+% P1: camera matrix of the first camera.
+% m1: pixel location (x1, y1) on the first view. Row vector.
+% P2: camera matrix of the second camera
+% m2: pixel location (x2, y2) on the second view. Row vector.
+% M: the (x, y, z) coordinate of the reconstructed 3D point. Row vector.
+% Camera one
+C1 = inv(P1(1:3, 1:3)) * (-P1(:,4));
+x0 = C1(1);
+y0 = C1(2);
+z0 = C1(3);
+m1 = [m1'; 1];
+M1 = pinv(P1) * m1;
+x = M1(1)/M1(4);
+y = M1(2)/M1(4);
+z = M1(3)/M1(4);
+a = x-x0;
+b = y-y0;
+c = z-z0;
+% Camera Two
+C2 = inv(P2(1:3, 1:3)) * (-P2(:,4));
+x1 = C2(1);
+y1 = C2(2);
+z1 = C2(3);
+m2 = [m2'; 1];
+M2 = pinv(P2) * m2;
+x = M2(1)/M2(4);
+y = M2(2)/M2(4);
+z = M2(3)/M2(4);
+d = x-x1;
+e = y-y1;
+f = z-z1;
+% Solve u and v
+A = [a^2 + b^2 + c^2, -(a*d + e*b + f*c);...
+-(a*d + e*b + f*c), d^2 + e^2 + f^2];
+v = [ (x1-x0)*a + (y1-y0)*b + (z1-z0)*c;...
+(x0-x1)*d + (y0-y1)*e + (z0-z1)*f];
+r = inv(A) * v;
+M = [x0+a*r(1) y0+b*r(1) z0+c*r(1)];