function rms = paracalib1_compute_error(K, imgpoint_pair, angle)
% Compute RMS error of reprojection in pixels

siz = size(imgpoint_pair);
pnum = siz(1);

invK = inv(K);

rms = 0;
num = 0;
for pn1 = 1:pnum
  % relative angle is known
  cos1 = cos(angle(pn1));
    
  mm1 = imgpoint_pair(pn1, 1:2)';
  mm2 = imgpoint_pair(pn1, 3:4)';
  mm1(3) = 1;
  mm2(3) = 1;
  MM1 = invK * mm1;
  MM1 = MM1 / norm(MM1);

  C = MM1 * transpose(MM1) - eye(3,3) * cos1^2;
  CC = transpose(invK) * C * invK;
          
  a = CC(1,1); b = CC(1,2); c = CC(1,3); d = CC(2,2);
  e = CC(2,3); f = CC(3,3);
  px = mm2(1); py = mm2(2); pz = mm2(3);

  tmpcoef = [-(d*a-b^2)*(-f*d*a+a*e^2+d*c^2-2*b*e*c+f*b^2)
             -2*(a+d)*(-f*d*a+a*e^2+d*c^2-2*b*e*c+f*b^2)
             2*px*b^2*c+6*b*e*c-px^2*b^2*d-4*a*e^2-2*a*d*px*b*py+a*d^2*px^2+2*b^2*py*e-4*d*c^2-2*b*py*d*c+2*px*b^3*py-2*b*c*a*py-2*a*px*b*e+2*c*d^2*px-2*f*b^2-e^2*d+f*a^2+d*a^2*py^2-a*c^2-b^2*py^2*a+f*d^2+2*e*a^2*py-2*b*e*d*px+4*f*d*a
             2*f*d-2*e^2-4*b*e*px+2*a*px^2*d+2*f*a+2*d*py^2*a-4*b*py*c-2*c^2+4*e*a*py-2*px^2*b^2-2*b^2*py^2+4*c*d*px
             2*b*py*px+d*py^2+2*e*py+2*c*px+f+a*px^2];

  r = roots(tmpcoef);
          
  mind = Inf;
  for n = 1:4
    lambda = r(n);
    q(1) = (-lambda*b*py+px+lambda^2*b*e+lambda*d*px-lambda*c-lambda^2*d*c)/(-lambda^2*b^2+1+lambda*a+lambda*d+lambda^2*d*a);
    q(2) = -(-py+lambda*e-lambda*a*py+lambda^2*a*e+px*lambda*b-lambda^2*c*b)/(-lambda^2*b^2+1+lambda*a+lambda*d+lambda^2*d*a);
            
    if imag(q(1)) < 0.001 & imag(q(2)) < 0.001
      dd = real(q)' - mm2(1:2);
      if mind > norm(dd);
        mind = norm(dd);
        minq = real(q);
      end
    end
  end
  rms = rms + mind^2;
  num = num + 1;
end

rms = sqrt(rms / num);
