function hz(M, W, C);
  printf("Hartley & Zisserman Method\n");
  r = 1:3;

  theta = atan(-M(3,2)/M(3,3));
  c = cos(theta);
  s = sin(theta);
  Qx = [1 0 0; 0 c -s; 0 s c];
  R = M(:,r)*Qx;
 
  theta = atan(M(3,1)/M(3,3));
  c = cos(theta);
  s = sin(theta);
  Qy = [c 0 s; 0 1 0; -s 0 c];
  R = R*Qy;
 
  theta = atan(-M(2,1)/M(2,2));
  c = cos(theta);
  s = sin(theta);
  Qz = [c -s 0; s c 0; 0 0 1];
 
  R = R*Qz;
  gamma = R(3,3);
  R = R ./ gamma;
  Q = Qz'*Qy'*Qx';

  ox =R(1,3);
  oy =R(2,3);
  printf("  (ox,oy): %f %f\n",ox,oy);

  fx=R(1,1);
  fy=R(2,2);

  T = zeros(3,1);
  T(3) = M(3,4);
  T(1) = (M(1,4) - ox*T(3))/fx;
  T(2) = (M(2,4) - oy*T(3))/fy;
  T = -Q * T ./ gamma; % Q is the rotation here
  Q
  
  xy = fx/fy;
  k = fmins( 'radial', [fy T(3) 0], [], 0, W, C, R, ox, oy, T(2) );
  fx = xy*k(1);
  fy = k(1);
  T(3) = k(2);
  T
  printf("  (fx,fy): %f %f\n",fx,fy);
  printf("  (k1): %f\n\n",k(3));