function ps6(k)
% Main function for problem set 6.

f = 1;
% Focal length will always be 1.

fp1 = [0;0;0];
fp1a = [3;1;2];
zvec1 = [0;0;1];
xvec1 = [1;0;0];
yvec1 = [0;1;0];

fp2 = [1;0;3];
fp2a = [2; 4; -1];
ang = pi/8;
zvec2 = [sin(ang); 0; cos(ang)];
xvec2 = [cos(ang); 0; -sin(ang)];
yvec2 = [0;1;0];

fp3 = [0; 10; 0];
ang3 = pi/4;
xvec3 = [1;0;0];
yvec3 = [0; sin(ang3); -cos(ang3)];
zvec3 = [0; cos(ang3); sin(ang3)];

switch k
    case 1
        Ps = test_points(1,8);
        ps = project_standard(Ps, f)
        % Project 3D scene points into the image using the simplest camera
        % paramaters.
        display_points(ps);
        
        Ps = test_points(2,8);
        ps = project_standard(Ps, f)
    case 2
        Ps = [10;9;8];
        change_coordinates(fp2, xvec2, yvec2, zvec2, Ps)
        
        Ps = [-2;7;4];
        change_coordinates(fp3, xvec3, yvec3, zvec3, Ps)
    case 3
        Ps = test_points(1,8);
        qs = project_general(fp1a, xvec1, yvec1, zvec1, f, Ps)
        % Test a more general routine for projecting points into the image.
        display_points(qs);
        
        Ps = test_points(2,8);
        qs = project_general(fp1a, xvec2, yvec2, zvec2, f, Ps)        
    case 4
        Ps = test_points(1,8);
        ps = project_standard(Ps, f);
        qs = project_general(fp1a, xvec1, yvec1, zvec1, f, Ps);
        E = eight_point(ps, qs)
        % Use the eight point algorithm to compute the Essential matrix.
        % Note that from this essential matrix you can infer the
        % translation, up to a scale factor.  This should match the
        % difference in the focal points used to generate ps and qs.
        
        Ps = test_points(2,8);
        ps = project_general(fp2a, xvec2, yvec2, zvec2, f, Ps);
        qs = project_general(fp1a, xvec1, yvec1, zvec1, f, Ps);
        E = eight_point(ps, qs) 
        % It's not so easy to infer translation from this E.
    case 5
        Ps = test_points(1,12);
        % Here we generate 12 points.  The first 8 will be used to compute
        % the essential matrix.  We will compute the epipolar lines for the
        % remaining 4.
        ps = project_standard(Ps, f);
        qs = project_general(fp1a, xvec1, yvec1, zvec1, f, Ps); 
        % Generate the same points as in case 2.
        E = eight_point(ps(:,1:8), qs(:,1:8));
        % And the same Essential matrix.
        lines = epipolar_lines(qs(:,9:12), E)
        % Use it to generate the epipolar lines for points 9-12 in the
        % first image.
        display_points_lines(ps, lines);
        % Notice that the epipolar lines all intersect at a single point.
        
        Ps = test_points(2,12);
        ps = project_general(fp2a, xvec2, yvec2, zvec2, f, Ps);
        qs = project_general(fp1a, xvec1, yvec1, zvec1, f, Ps);
        E = eight_point(ps(:,1:8), qs(:,1:8));
        lines = epipolar_lines(qs(:,9:12), E)
    case 6
        Ps = test_points(1,12, 7);
        % Generate points that are closer to the camera.  This produces 
        % more interesting epipolar lines.
        ps = project_standard(Ps, f);
        qs = project_general(fp2, xvec2, yvec2, zvec2, f, Ps);
        % We also use a different camera for the second image. 
        E = eight_point(ps(:,1:8), qs(:,1:8));
        lines = epipolar_lines(qs(:,9:12), E);
        epipole = find_epipole(lines)
        % Find the epipole by intersecting the epipolar lines.
        display_points_lines(ps, lines, epipole);
        
        Ps = test_points(2,12, 7);
        ps = project_general(fp1a, xvec1, yvec1, zvec1, f, Ps)
        qs = project_general(fp2, xvec2, yvec2, zvec2, f, Ps);
        E = eight_point(ps(:,1:8), qs(:,1:8));
        lines = epipolar_lines(qs(:,9:12), E);
        epipole = find_epipole(lines)
    case 7
        Ps = test_points(1, 12, 7);
        ps = project_standard(Ps, f);
        qs = project_general(fp2, xvec2, yvec2, zvec2, f, Ps);
        E = eight_point(ps(:,1:8), qs(:,1:8))
        lines = epipolar_lines(qs(:,9:12), E);
        epipole = find_epipole(lines); 
        % Compute the same epipole as in Case 5.
        find_translation(epipole, fp1, xvec1, yvec1, zvec1, f) 
        fp2
        % Using the epipole, we can compute the direction of translation from
        % the first camera to the second one.  This matches the true
        % translation up to a scale factor.
    case 8
        R = compute_rotation(xvec2, yvec2, zvec2, xvec3, yvec3, zvec3);
        % R gives the rotation from the second coordinate system to the
        % first.
        pt = [7;3;2];
        % A test point in 3D.
        q1 = change_coordinates([0;0;0], xvec2, yvec2, zvec2, pt)
        % Compute the coordinates of pt using a new coordinate system with
        % [0;0;0] as the origin, and xvec2, yvec2, zvec2 giving the new
        % directions of the x,y and z axes.  This is not perspective
        % projection, just a change in the coordinate system.
        q2 = change_coordinates([0;0;0], xvec3, yvec3, zvec3, pt);
        % Do the same with these directions for the axes.
        R*q2
        % Now see that R rotates points from the second coordinate system
        % to the first.  R*q2 should be the same as q1.
    case 9
        Ps = test_points(1, 12, 7);
        ps = project_general(fp1, xvec1, yvec1, zvec1, f, Ps);
        qs = project_general(fp3, xvec3, yvec3, zvec3, f, Ps);
        E1 = eight_point(ps(:,1:8), qs(:,1:8))
        E2 = compute_essential(fp1, xvec1, yvec1, zvec1, fp3, xvec3, yvec3, zvec3)
        % Using the rotation and translation between the two cameras, when
        % we know their positions, we can also compute the essential
        % matrix.  Verify that this is the same result we got when we used
        % the eight point algorithm, in which we knew corresponding points
        % but did not know the cameras' parameters.
    case 10
        Ps = test_points(1, 12, 7);
        ps = project_general(fp2, xvec2, yvec2, zvec2, f, Ps);
        qs = project_general(fp3, xvec3, yvec3, zvec3, f, Ps);
        E1 = eight_point(ps(:,1:8), qs(:,1:8));
        E2 = compute_essential(fp2, xvec2, yvec2, zvec2, fp3, xvec3, yvec3, zvec3);
        (E1+eps)./(E2+eps)
        % This is the same as case 9, but a little harder test of our code,
        % since neither of these cameras is at the origin using the
        % standard coordinate system.  One might well write a version of
        % compute_essential that works for case 8 but not for case 9.
end

function E = compute_essential(fp1, xvec1, yvec1, zvec1, fp2, xvec2, yvec2, zvec2)
% Given two camera focal points, and the corresponding x, y and z
% directions for these cameras, compute the Essential matrix that relates
% the cameras.


function R = compute_rotation(xvec1, yvec1, zvec1, xvec2, yvec2, zvec2)
% Compute a 3x3 rotation matrix, R, that relates the two input coordinate
% systems by taking any vector in the second coordinate system and mapping
% it to the corresponding vector in the first coordinate system.
% Specifically, we should have xvec1 = R*xvec2, yvec1=R*yvec2,
% zvec1=R*zvec2.


function trans = find_translation(epipole, fp, xvec, yvec, zvec, f)
% Given the epipole for an image, relative to a second image, and the 
% parameters that describe the camera that took the image, find the 
% translation from the image's focal point to the second image's focal point.  

function epipole = find_epipole(lines)
% A set of lines intersect in the epipole.  Given the lines, find the
% epipole.
% lines is a matrix.  Each column describes a 2D line.  For example, line =
% lines(:,1) is the first line.  line(1)*x + line(2)*y + line(3) = 0 is the
% equation for the line.


function lines = epipolar_lines(qs, E)
% Given the essential matrix, and a set of points in the second image,
% compute the epipolar lines in the first matrix.  The first column of
% lines should contain an epipolar line for the point that is in the first
% column of q.

function qs = project_standard(Ps, f)
% Given a camera with standard configuration of focal point at (0,0,0),
% pointing in the z direction, with a focal length of 1, find the image
% positions, qs, of a set of 3d points, ps.
% Every column of qs is a 2D point.  So qs has two rows, with x and y coordinates. 

function Qs = change_coordinates(origin, xvec, yvec, zvec, Ps)
% This takes a set of 3D points, Ps, as input.  It rewrites the coordinates 
% of these points in a new coordinates system.  origin gives the origin 
% xvec, yvec, and zvec are unit vectors that give the x, y, and z
% directions for this new coordinate system.  
% Qs is the new set of 3D points; each column of Qs corresponds to the
% matching column of Ps.  

function qs = project_general(fp, xvec, yvec, zvec, f, Ps)
% This performs perspective projection for a general camera, described by
% the focal point, fp, and the coordinate system.  

function E = eight_point(ps, qs)
% Given points ps in the first image and points qs in the second image, use
% the eight point algorithm to compute the essential matrix, E.


function display_points(ps)
% This function is given to the students.  It just displays a set of
% points.
% 
figure;
plot(ps(1,:), ps(2,:), '.', 'MarkerSize', 35);

function ps = test_points(seed, n, dist)
% This function is given to the students.  It generates test points.
if nargin < 3
    dist = 19;
end
rng(seed-1);
ps = [rand(1,n)*2-1; rand(1,n)*2-1; randn(1,n)+dist];

function display_points_lines(qs, lines, epipole)
% This function is given to the students.
% This function displays points, lines and the epipole.  The epipole is an
% optional argument.  
% Each column of qs gives the x and y coordinates of a point.  
% lines is a matrix.  Each column describes a 2D line.  For example, line =
% lines(:,1) is the first line.  line(1)*x + line(2)*y + line(3) = 0 is the
% equation for the line.
if nargin > 2
    maxval = 1.1*max(max([qs(1:2, :), epipole]));
    minval = 1.1*min(0, min(min([qs(1:2, :), epipole])));
else
    maxval = 1.1*max(max(qs(1:2, :)));
    minval = 1.1*min(0, min(min(qs(1:2, :))));
end

figure;
hold on
xlim([minval, maxval]);
ylim([minval, maxval]);
plot(qs(1,1:8), qs(2,1:8), '.k', 'MarkerSize', 35);
plot(qs(1,9:end), qs(2,9:end), '.r', 'MarkerSize', 35);
for i = 1:size(lines,2)
    line = lines(:,i);
    x1 = minval;
    y1 = -(line(1)*x1 + line(3))/line(2);
    x2 = maxval;
    y2 = -(line(1)*x2 + line(3))/line(2);
    plot([x1, x2], [y1, y2], '-r', 'LineWidth', 2);
end
if nargin > 2
    plot(epipole(1), epipole(2), 'ob', 'MarkerSize', 10);
end


