function K = paracalib1(imgpoint_pair, angle, cxy0)

siz = size(imgpoint_pair);
%imgnum = siz(3);
pnum = siz(1);

C = cos(angle);


%%
%% Compute initial estimation of f_x
%%
x1 = imgpoint_pair(:,1) - cxy0(1);
y1 = imgpoint_pair(:,2) - cxy0(2);
x2 = imgpoint_pair(:,3) - cxy0(1);
y2 = imgpoint_pair(:,4) - cxy0(2);

C2 = C.^2;
x1_2 = x1.^2;
y1_2 = y1.^2;
x2_2 = x2.^2;
y2_2 = y2.^2;

c(1) = sum(-C2 + ones(pnum,1));
c(2) = sum(-C2.*x1_2-C2.*x2_2+2.*x2.*x1+2.*y2.*y1-C2.*y2_2-C2.*y1_2);
c(3) = sum(2.*x2.*x1.*y2.*y1-C2.*x2_2.*x1_2-C2.*y2_2.*x1_2 ...
             -C2.*y2_2.*y1_2+y2_2.*y1_2-C2.*x2_2.*y1_2+x2_2.*x1_2);

f2 = roots(c);
tmpf = sqrt(f2);
for k = 1:2
  if isreal(tmpf(k))
    f = tmpf(k);
  end
end

%Initial intrinsic parameters
K = [f 0 cxy0(1);
     0 f cxy0(2);
     0 0 1];
invK = inv(K);

%%
%% Find optimal intrinsic parameters by nonlinear minimization
%%
param = [invK(1,1) invK(2,2) invK(1,3) invK(2,3)];
ip1 = imgpoint_pair(:,1:2)';
ip2 = imgpoint_pair(:,3:4)';
ip1(3,:) = 1;
ip2(3,:) = 1;

options = optimset('fminsearch');
options.MaxFunEvals = 100000;
options.MaxIter = 100000;

param = fminsearch(@comp_angle, param, options);

invK = [param(1) 0 param(3);
        0 param(2) param(4);
        0 0 1];
K = inv(invK);

function e = comp_angle(param)

invK = [param(1) 0 param(3);
        0 param(2) param(4);
        0 0 1];

wp1 = invK * ip1;
wp2 = invK * ip2;

len1 = sqrt(wp1(1,:).^2 + wp1(2,:).^2 + wp1(3,:).^2);
len2 = sqrt(wp2(1,:).^2 + wp2(2,:).^2 + wp2(3,:).^2);

for k = 1:3
  wp1(k,:) = wp1(k,:) ./ len1;
  wp2(k,:) = wp2(k,:) ./ len2;
end

d = sum(wp1 .* wp2) - C';
e = sum(d.^2);

end

end
