% Write points
compute_angle;

% Read input points
data = load('point_angle.txt');
siz = size(data);
point = data(:,1:4);
% Angles between 2 points in radian
angle = data(:,5);

% Initial parameter of principal point
cxy0 = [320 240];

% Compute intrinsic parameter K
K = paracalib1(point, angle, cxy0);
fprintf(1, 'Intrinsic parameter K:\n');
disp(K);

% Compute RMS error of reprojection in pixels
rms = paracalib1_compute_error(K, point, angle);
fprintf(1, 'Fitting Error: RMS = %3.6f pixels\n', rms);

% Visualization of error and degeneracy
% Symbolic Math Toolbox is required for visualization

% imgsize = [ysize xsize fmin fmax] is used for the range of plotting
imgsize = [480 640 700 900];
if license('checkout', 'symbolic_toolbox')
  compute_surface;
else
  disp('Symbolic Math Toolbox is required for visualization');
end
