function Triangulation
%% Data loading
load('triangulation.mat');

% (C1, R1) and (C2, R2) are camera center and orientation of camera 1 and 2, respectively.
% u and v are Nx2 correspondences

%% Image loading
im1 = imread('left.bmp');
im2 = imread('right.bmp');
im1 = imresize(im1, 0.5);
im2 = imresize(im2, 0.5);
im = im1;
im(:,end+1:end+size(im2,2),:) = im2;

figure(1)
clf;
imshow(im);
hold on
plot(u(:,1), u(:,2), 'g.', 'MarkerSize', 18);
hold on
plot(size(im1,2)+v(:,1), v(:,2), 'g.', 'MarkerSize', 18);

%% Camera matrix build
K = [700/2 0 960/2;
     0 700/2 540/2;
     0 0 1];
% Build camera matrix 1 and 2
% P1
% P2

%% Triangulation
% Go to each correspondence and compute the 3D point X (3xN) matrix
for i = 1 : size(u,1)
    % Construct A matrix
    % Solve linear least squares to get 3D point
    % X(:,i) = point_3d;
end

figure(2)
clf;
plot3(X(1,:), X(2,:), X(3,:), 'b.');
hold on
DisplayCamera(C1, R1, 2);
hold on
DisplayCamera(C2, R2, 2);