%Shean Tokuta
%CAP 5415
%9/30/06

%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!   Homework 1  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

hold on

[v,f] = read_off('home1_shapes.off'); %read coordinates from off file
plot_mesh_edges(v,f);	%create a mesh plot of coordinates read from off file

cam1 = [25, -9, -15];
cam2 = [25, -6, -15];

f1 = 0.5; %focal length of cam 1
f2 = .25; %focal length of cam2

cam1x = [-0.25292875993863, -0.95550864865706, 0.15175725596318];
cam2x = [-0.17284822944879, -0.97947330020983, 0.10370893766928];

%calculate z coordinate for both cams
cam1z = -cam1 / norm(-cam1);
cam2z = -cam2 / norm(-cam2);

%calculate y coordinate for both cams by taking the cross product of x and z
cam1y = cross(cam1z, cam1x);
cam2y = cross(cam2z, cam2x);

%plot the axis for both of the cameras
plot3([cam1(1) cam1(1)+cam1x(1)], [cam1(2) cam1(2)+cam1x(2)], [cam1(3) cam1(3)+cam1x(3)]);
plot3([cam1(1) cam1(1)+cam1y(1)], [cam1(2) cam1(2)+cam1y(2)], [cam1(3) cam1(3)+cam1y(3)]);
plot3([cam1(1) cam1(1)+cam1z(1)], [cam1(2) cam1(2)+cam1z(2)], [cam1(3) cam1(3)+cam1z(3)]);

plot3([cam2(1) cam2(1)+cam2x(1)], [cam2(2) cam2(2)+cam2x(2)], [cam2(3) cam2(3)+cam2x(3)]);
plot3([cam2(1) cam2(1)+cam2y(1)], [cam2(2) cam2(2)+cam2y(2)], [cam2(3) cam2(3)+cam2y(3)]);
plot3([cam2(1) cam2(1)+cam2z(1)], [cam2(2) cam2(2)+cam2z(2)], [cam2(3) cam2(3)+cam2z(3)]);

%calculate the image plane for cam1
A1 = cam1 + f1*cam1z + .05*cam1x + 3.05*cam1y;
B1 = cam1 + f1*cam1z + .05*cam1x - 3.05*cam1y;
C1 = cam1 + f1*cam1z - .05*cam1x + 3.05*cam1y;
D1 = cam1 + f1*cam1z - .05*cam1x - 3.05*cam1y;

%plot the image plane for cam 1
%plot3([A1(1) B1(1)], [A1(2) B1(2)], [A1(3) B1(3)]);
%plot3([B1(1) D1(1)], [B1(2) D1(2)], [B1(3) D1(3)]);
%plot3([D1(1) C1(1)], [D1(2) C1(2)], [D1(3) C1(3)]);
%plot3([C1(1) A1(1)], [C1(2) A1(2)], [C1(3) A1(3)]);

%draw line from camera center to real world coordinate system center
plot3([0 cam1(1)], [0 cam1(2)], [0 cam1(3)]);
plot3([0 cam2(1)], [0 cam2(2)], [0 cam2(3)]);

%calculate the image plane for cam2
%A2 = cam2 + f2*cam2z + 3.05*cam2x + 3.05*cam2y;
%B2 = cam2 + f2*cam2z + 3.05*cam2x - 3.05*cam2y;
%C2 = cam2 + f2*cam2z - 3.05*cam2x + 3.05*cam2y;
%D2 = cam2 + f2*cam2z - 3.05*cam2x - 3.05*cam2y;

%plot the image plane for cam2
%plot3([A2(1) B2(1)], [A2(2) B2(2)], [A2(3) B2(3)]);
%plot3([B2(1) D2(1)], [B2(2) D2(2)], [B2(3) D2(3)]);
%plot3([D2(1) C2(1)], [D2(2) C2(2)], [D2(3) C2(3)]);
%plot3([C2(1) A2(1)], [C2(2) A2(2)], [C2(3) A2(3)]);

%p3D = points read in from real world [v] which is retreived via read_off.m
p3D = [v]

%Which of these are for the tetrahedron and which are for the cube-like object?
%Code to extract the points of the cube-like object and the tetrahedron points from 
%the 2D Homogeneous Matrix

%get real world points of cube
basePts_cube=[1 2 3 4 5 6 7 8; 9 10 11 12 13 14 15 16; 17 18 19 20 21 22 23 24];
x = 1;
y = 2;
z = 3;
for i = 1:8
	basePts_cube(x) = p3D(x);
	basePts_cube(y) = p3D(y);
	basePts_cube(z) = p3D(z);
	x = x + 3;
	y = y + 3;
	z = z + 3;
end
%now have the real world points for the cube 

%get real world points of tetrahedron
basePts_tetrahedron = [25 26 27 28; 29 30 31 32; 33 34 35 36];
x = 1;
y = 2;
z = 3;
for i = 1:4
	basePts_tetrahedron(x) = p3D(x+24);
	basePts_tetrahedron(y) = p3D(y+24);
	basePts_tetrahedron(z) = p3D(z+24);
	x = x + 3;
	y = y + 3;
	z = z + 3;
end
%now have the real world points for the tetrahedron 

%add ones to last row of real world points to make homogeneous
P3D =[p3D; ones(1,size(p3D,2))];
cube =[basePts_cube; ones(1,size(basePts_cube,2))];
tetrahedron =[basePts_tetrahedron; ones(1,size(basePts_tetrahedron,2))];


%P matrix of Perspective Projection Equation
pCamera1 = [.5 0 0 0; 0 .5 0 0; 0 0 1 0]; %P for cam 1
pCamera2 = [.25 0 0 0; 0 .25 0 0; 0 0 1 0]; % P for cam 2
scaleMatrix = [-10240 0 511.5; 0 -10240 511.5; 0 0 1]
%multipy the scale to get the true P matrix
%This is added by Xiuwen Liu
RCamera1=[cam1x; cam1y; cam1z]';
RCamera2=[cam2x; cam2y; cam2z]';

pCam1 = scaleMatrix * pCamera1 * [RCamera1',-RCamera1'*cam1';0 0 0 1];
pCam2 = scaleMatrix * pCamera2 * [RCamera2',-RCamera2'*cam2';0 0 0 1];

%Calculate Perspective Projection matrix
proj1 = pCam1 * P3D % Projection matrix for cam 1 a 3x12 matrix
proj2 = pCam2 * P3D % Projection matrix for cam2 a 3x12 matrix
cubePts1 = pCam1 * cube;
cubePts2 = pCam2 * cube;
tetraPts1 = pCam1 * tetrahedron;
tetraPts2 = pCam2 * tetrahedron;

%convert projection matrix to image coordinates (i.e. 2D homogeneous coordinates)
%convert points observed in cam1 to 2D homogeneous coordinates
x = 1;
y = 2;
z = 3;
for i = 1:12
	proj1(x) = proj1(x)/proj1(z);
	proj1(y) = proj1(y)/proj1(z);
	x = x + 3;
	y = y+ 3;
	z = z+3;
end
proj1(3,:)=1; %manually places ones in the last row

%must do the same for points in cam 2
x = 1;
y = 2;
z = 3;
for i = 1:12
	proj2(x) = proj2(x)/proj2(z);
	proj2(y) = proj2(y)/proj2(z);
	x = x + 3;
	y = y+ 3;
	z = z+3;
end
proj2(3,:) = 1; %manually place ones in the last row

%must do the same for points of cube in cam 1
x = 1;
y = 2;
z = 3;
for i = 1:8
	cubePts1(x) = cubePts1(x)/cubePts1(z);
	cubePts1(y) = cubePts1(y)/cubePts1(z);
	x = x + 3;
	y = y+ 3;
	z = z+3;
end
cubePts1(3,:) = 1; %manually place ones in the last row

%must do the same for points of cube in cam2
x = 1;
y = 2;
z = 3;
for i = 1:8
	cubePts2(x) = cubePts2(x)/cubePts2(z);
	cubePts2(y) = cubePts2(y)/cubePts2(z);
	x = x + 3;
	y = y+ 3;
	z = z+3;
end
cubePts2(3,:) = 1; %manually place ones in the last row


%code to plot the 2D Homogeneous image points for cam1 and cam2
%plot(proj1(1,:),proj1(2,:),'gx')
%plot(proj2(1,:),proj2(2,:),'b+')

%code creates a mesh of 2D homogeneous points previous plotted
%plot_mesh_edges(proj1(1:2,:),f);
%plot_mesh_edges(proj2(1:2,:),f);

%***********************************************Question 3 *********************************************
%----------------------------------------------------------
%Normalized 8pt Algorithm

%Step 1: Normalize 2D Homogeneous Image Coordinates
%Creating two transforms consisting of translations and scaling, that transforms each
%set of image points so their centroid is @ the origin and the RMS distnace from the origin is sqrt(2)
%-----------------normalize for cam1-----------------------------------
centroidCam1 = mean(proj1(1:2,:)')'; %Centroid for image points
%translate points so centroid is at the origin
newProj1(1,:) = proj1(1,:)-centroidCam1(1); 
newProj1(2,:) = proj1(2,:)-centroidCam1(2);
  
RMScam1 = mean(sqrt(newProj1(1,:).^2 + newProj1(2,:).^2)); %ensure RMS distance from origin is sqrt(2)
    
scalecam1 = sqrt(2)/RMScam1;

%create the scaling component with defined RMS
%Ttrans and Tscale combined into one
%Skipping the step of actually multiplying Ttrans x Tscale to get Tnorm    
TnormCam1 = [scalecam1   0   -scalecam1*centroidCam1(1)
     0     scalecam1 -scalecam1*centroidCam1(2)
     0       0      1      ]; 
newProj1 = TnormCam1*proj1
%------------------------End normalize for cam1-----------------------------------

%------------------------Normalize for cam2---------------------------------------------
centroidCam2 = mean(proj2(1:2,:)')';  %Centroid for image points
%trnslate points so centroid is at the origin
newProj2(1,:) = proj2(1,:)-centroidCam2(1); 
newProj2(2,:) = proj2(2,:)-centroidCam2(2);
  
RMScam2 = mean(sqrt(newProj2(1,:).^2 + newProj2(2,:).^2)); %ensure RMS distance from origin is sqrt(2)
    
scalecam2 = sqrt(2)/RMScam2;
   
%create the scaling component with defined RMS
%Ttrans and Tscale combined into one
%Skipping the step of actually multiplying Ttrans x Tscale to get Tnorm    
TnormCam2 = [scalecam2   0   -scalecam2*centroidCam2(1)
     0     scalecam2 -scalecam2*centroidCam2(2)
     0       0      1      ];
newproj2 = TnormCam2*proj2
%-----------------------------end normilize for cam2--------------------------------------
%------------------------------------------------------------------------------------------
%Need to determine F from normalized coordinates
%Have already normalized each set of ponts so that the origin is at centroid
%and mean distance from origin is sqrt(2)

%calculate the number of correspondence points should be 12
nptscam = size(newProj1,2);
%nptscam2 = size(newProj2,2); Just need one, the numbers should be the same
%That is there are the same number of marked points in both cams

%Matrix A which is computed from normalized image pts
Anorm = [newProj2(1,:)'.*newProj1(1,:)'   newProj2(1,:)'.*newProj1(2,:)'  newProj2(1,:)' ...
         newProj2(2,:)'.*newProj1(1,:)'   newProj2(2,:)'.*newProj1(2,:)'  newProj2(2,:)' ...
         newProj1(1,:)'                   newProj1(2,:)'                  ones(nptscam,1) ];   

%Need least squares solution.  Need a vector fnorm containing the elements of Fnorm
%I believe I need to use capital V, since v was used earlier when I pulled the coordinates in using
%read off
[u,s,V] =svd(Anorm) %u is Nx12 but calculations are the same......!!!!!!!!!!!!!!!!!!!!!!!!!!!
% or [u,s,V] = svd(Anorm,0) % u is Nx9

%Get fundamental matrix
%Fnorm is the last column of V
%will be a long vector, need to reshape then take transpose (')
%Fnorm = V(:,end);  or
Fnorm = reshape(V(:,9),3,3)'


%Enforcing Singularity
%Take another SVD of Fnorm =UDV' 
[U,D,B] = svd(Fnorm,0); %or [U,D,B] = svd(Fnorm)
F = U*diag([D(1,1) D(2,2) 0])*B'

%Denormalize, the fundamental matrix can be defined as:
F = TnormCam2' *F*TnormCam1
%**********************************End of Question 3*****************************************

%******************************************Question 4**************************************
%Use the computed fundamental matrix to calculate the epipolar lines of vertices
%of the tetrahedron in both images and the epipoles of both images
%Then use polt command to draw lines and epipoles on the output images

[U,D,B] = svd(F,0);

%Need only the points in the 2D Homogeneous Matrix that relate to the tetrahedron
temp_matrix1 = B(:,3); %coming out as a 3x1
temp_matrix2 = U(:,3); %coming out as a 3x1

epipole1= temp_matrix1;
epipole2 = temp_matrix2;

[rows1,npts1] = size(temp_matrix1);
[rows2,npts2] = size(temp_matrix2);

for r = 1:rows1-1
	epipole1(r,:) = temp_matrix1(r,:)./temp_matrix1(rows1,:);
end	
epipole1(rows1,:) = 1

for r = 1:rows2-1
	epipole2(r,:) = temp_matrix2(r,:)./temp_matrix2(rows2,:);
end	
epipole2(rows2,:) = 1
%****************************************End of Question 4********************************

%******************************************Extra Credit 5********************************
%First estimate the fundamental matrix using only the 8points on the cube-like object

%Normalized 8pt Algorithm

%Step 1: Normalize 2D Homogeneous Image Coordinates
%Creating two transforms consisting of translations and scaling, that transforms each
%set of image points so their centroid is @ the origin and the RMS distnace from the origin is sqrt(2)

cubeCam1 = cubePts1;
cubeCam2 = cubePts2;
%-----------------normalize for cube in cam1-----------------------------------
centroid_CubeCam1 = mean(cubeCam1(1:2,:)')';%Centroid of finite points
%translate points so centroid is at the origin
newcubeCam1(1,:) = cubeCam1(1,:)-centroid_CubeCam1(1); 
newcubeCam1(2,:) = cubeCam1(2,:)-centroid_CubeCam1(2);
  
RMScubeCam1 = mean(sqrt(newcubeCam1(1,:).^2 + newcubeCam1(2,:).^2)); %ensure RMS
%distance from origin is sqrt(2)
    
scaleCubeCam1 = sqrt(2)/RMScubeCam1;
%create the scaling component with defined RMS
%Ttrans and Tscale combined into one
%Skipping the step of actually multiplying Ttrans x Tscale to get Tnorm    
TCubeCam1 = [scaleCubeCam1   0   -scaleCubeCam1*centroid_CubeCam1(1)
     0     scaleCubeCam1 -scaleCubeCam1*centroid_CubeCam1(2)
     0       0      1      ];
newCubeCam1 = TCubeCam1*cubeCam1
%------------------------End normalize for cube in cam1-----------------------------------

%-----------------normalize for cube in cam2-----------------------------------
centroid_CubeCam2 = mean(cubeCam2(1:2,:)')'; %Centroid of finite points
newcubeCam2(1,:) = cubeCam2(1,:)-centroid_CubeCam2(1); 
newcubeCam2(2,:) = cubeCam2(2,:)-centroid_CubeCam2(2);
  
RMScubeCam2 = mean(sqrt(newcubeCam2(1,:).^2 + newcubeCam2(2,:).^2)); %ensure RMS
%distance from origin is sqrt(2)

    
scalecubeCam2 = sqrt(2)/RMScubeCam2;
%create the scaling component with defined RMS
%Ttrans and Tscale combined into one
%Skipping the step of actually multiplying Ttrans x Tscale to get Tnorm       
TCubeCam2 = [scalecubeCam2   0   -scalecubeCam2*centroid_CubeCam2(1)
     0     scalecubeCam2 -scalecubeCam2*centroid_CubeCam2(2)
     0       0      1      ];
newCubeCam2 = TCubeCam2*cubeCam2
%------------------------End normalize for cube in cam2-----------------------------------
%Need to determine F from normalized coordinates
%Have already normalized each set of ponts so that the origin is at centroid
%and mean distance from origin is sqrt(2)

%calculate the number of correspondence points should be 8
nptsCube = size(newCubeCam1,2)

%Matrix A which is computed from normalized image pts
Acube = [newCubeCam2(1,:)'.*newCubeCam1(1,:)'   newCubeCam2(1,:)'.*newCubeCam1(2,:)'  newCubeCam2(1,:)' ...
         newCubeCam2(2,:)'.*newCubeCam1(1,:)'   newCubeCam2(2,:)'.*newCubeCam1(2,:)'  newCubeCam2(2,:)' ...
         newCubeCam1(1,:)'             newCubeCam1(2,:)'            ones(nptsCube,1) ];   



%Need least squares solution.  Need a vector fnorm containing the elements of Fnorm
%I believe I need to use capital V, since v was used earlier when I pulled the coordinates in using
%read off
[u,s,V] = svd(Acube); 

%Get fundamental matrix
%Fnorm is the last column of V
%will be a long vector, need to reshape then take transpose (')
%Fnorm = V(:,end);  or
Fcube = reshape(V(:,9),3,3)'

%Enforce Singularity
%Take another SVD of Fnorm =UDV'
[U,D,B] = svd(Fcube);
F_cube = U*diag([D(1,1) D(2,2) 0])*B'

%Denormalize, the fundamental matrix can be defined as:
F_cube = TCubeCam2' *Fcube*TCubeCam1

%F_cube is the estimated fundamental matrix using only the eight points on the cube-like object
%---------------------------------End Computation For Fundamental Matrix--------------------------------

%------------------------------------------Part B of EC-----------------------------------------------
%Using a perfect cube instead of the cube-like object (i.e. change the first vertex to (1,-1,1).
Altered_Cube = basePts_cube;

%alter the cube, replacing the initial coordinates
Altered_Cube(1) = 1;
Altered_Cube(2) = -1;
Altered_Cube(3) = 1;

%add ones to the last row to make homogeneous
Altered_Cube1 = [Altered_Cube; ones(1,size(Altered_Cube,2))];
Altered_Cube2 = [Altered_Cube; ones(1,size(Altered_Cube,2))];

%Calculate Perspective Projection Matrix
Altered_Cube1 = pCam1 * Altered_Cube1; %Caculate Projection of perfect cube in cam1
Altered_Cube2 = pCam2 * Altered_Cube2; %Caculate Projection of perfect cube in cam2

%convert to image coords; 2D Homogeneous Coordinates
%must do for points of cube in cam 1
x = 1;
y = 2;
z = 3;
for i = 1:8
	Altered_Cube1(x) = Altered_Cube1(x)/Altered_Cube1(z);
	Altered_Cube1(y) = Altered_Cube1(y)/Altered_Cube1(z);
	x = x + 3;
	y = y+ 3;
	z = z+3;
end
Altered_Cube1(3,:) = 1; %manually place ones in the last row
%must do the same for points of cube in cam2
x = 1;
y = 2;
z = 3;
for i = 1:8
	Altered_Cube2(x) = Altered_Cube2(x)/Altered_Cube2(z);
	Altered_Cube2(y) = Altered_Cube2(y)/Altered_Cube2(z);
	x = x + 3;
	y = y+ 3;
	z = z+3;
end
Altered_Cube2(3,:) = 1; %manually place ones in the last row

%Normalized 8pt Algorithm

%Step 1: Normalize 2D Homogeneous Image Coordinates
%Creating two transforms consisting of translations and scaling, that transforms each
%set of image points so their centroid is @ the origin and the RMS distnace from the origin is sqrt(2)

%-------------------------------normalize for cam1----------------------------------------------------
centroid_a1 = mean(Altered_Cube1(1:2,:)')';            % Centroid of finite points
%translate points so centroid is at the origin
newAlterC1(1,:) = Altered_Cube1(1,:)-centroid_a1(1); 
newAlterC1(2,:) = Altered_Cube1(2,:)-centroid_a1(2);
  
RMSalter1 = mean(sqrt(newAlterC1(1,:).^2 + newAlterC1(2,:).^2)); %ensure RMS distance from origin is sqrt(2)
    
scalealter1 = sqrt(2)/RMSalter1;
%create the scaling component with defined RMS
%Ttrans and Tscale combined into one
%Skipping the step of actually multiplying Ttrans x Tscale to get Tnorm        
Talter1 = [scalealter1   0   -scalealter1*centroid_a1(1)
     0     scalealter1 -scalealter1*centroid_a1(2)
     0       0      1      ];
newAlter1 = Talter1*Altered_Cube1
%-----------------------------------------end normalize for cam1---------------------------------------
%-----------------------------------------normalize cam2---------------------------------------------
%normalize for cam2
%translate points so centroid is at the origin
centroid_a2 = mean(Altered_Cube2(1:2,:)')';            % Centroid of finite points
newAlterC2(1,:) = Altered_Cube2(1,:)-centroid_a2(1); 
newAlterC2(2,:) = Altered_Cube2(2,:)-centroid_a2(2);
  
RMSalter2 = mean(sqrt(newAlterC2(1,:).^2 + newAlterC2(2,:).^2));%ensure RMS distance from origin is sqrt(2)
    
scalealter2 = sqrt(2)/RMSalter2;
%create the scaling component with defined RMS
%Ttrans and Tscale combined into one
%Skipping the step of actually multiplying Ttrans x Tscale to get Tnorm        
Talter2 = [scalealter2   0   -scalealter2*centroid_a2(1)
     0     scalealter2 -scalealter2*centroid_a2(2)
     0       0      1      ];
newAlter2 = Talter2*Altered_Cube2
%------------------------------------------end normalize for cam2---------------------------------------
%Need to determine F from normalized coordinates
%Have already normalized each set of ponts so that the origin is at centroid
%and mean distance from origin is sqrt(2)

%calculate the number of correspondence points should be 8
nptscube = size(newAlter1,2);  %Should be 8

%Matrix A which is computed from normalized image pts
Aalter = [newAlter2(1,:)'.*newAlter1(1,:)'   newAlter2(1,:)'.*newAlter1(2,:)'  newAlter2(1,:)' ...
         newAlter2(2,:)'.*newAlter1(1,:)'   newAlter2(2,:)'.*newAlter1(2,:)'  newAlter2(2,:)' ...
         newAlter1(1,:)'             newAlter1(2,:)'            ones(nptscube,1) ];   

%Need least squares solution.  Need a vector fnorm containing the elements of Fnorm
[u,s,V] =svd(Aalter); 

%Get fundamental matrix
%Fnorm is the last column of V
%will be a long vector, need to reshape then take transpose (')
%Fnorm = V(:,end);  or
Fnormalter = reshape(V(:,9),3,3)'


%Enforce Singularity
%Take another SVD of Fnorm =UDV'
[U,D,B] = svd(Fnormalter); %old code used [U,D,B] = svd(Fnormalter,0)
Falter = U*diag([D(1,1) D(2,2) 0])*B'

%Denormalize
Falter = Talter2' *Falter*Talter1


%****************************************End of Extra Credit***********************************

%Comparisons
F
F_cube
Falter
