calc_KRZ_from_P

% K, R, Z, K33 from P (without covariance propagation)
 see Algorithm 18, PCV, p. 500, without covariance propagation

 Usage:
   [K,R,Z,K33] = calc_KRZ_from_P(P,sc)
  
   P   - 4 x 3-projection matrix P=K*R*[I|-Z]
   sc  - sign of the principle distance +1, -1, optional, default sc = -1;
 
   K   - calibration matrix 
   R   - rotation matrix
   Z   - projection centre
   K33 - sign of K33 from the decomposition

 Wolfgang Förstner
 wfoerstn@uni-bonn.de 

 See also calc_P_from_KRZ, calc_Q_from_P, calc_viewing_direction

0001 %% K, R, Z, K33 from P (without covariance propagation)
0002 % see Algorithm 18, PCV, p. 500, without covariance propagation
0003 %
0004 % Usage:
0005 %   [K,R,Z,K33] = calc_KRZ_from_P(P,sc)
0006 %
0007 %   P   - 4 x 3-projection matrix P=K*R*[I|-Z]
0008 %   sc  - sign of the principle distance +1, -1, optional, default sc = -1;
0009 %
0010 %   K   - calibration matrix
0011 %   R   - rotation matrix
0012 %   Z   - projection centre
0013 %   K33 - sign of K33 from the decomposition
0014 %
0015 % Wolfgang Förstner
0016 % wfoerstn@uni-bonn.de
0017 %
0018 % See also calc_P_from_KRZ, calc_Q_from_P, calc_viewing_direction
0019 
0020 function [K,R,Z,K33] = calc_KRZ_from_P(P,sc)
0021 
0022 if nargin == 1
0023     sc = -1;
0024 end
0025 
0026 % partitioning
0027 A = P(1:3,1:3);
0028 a = P(1:3,4);
0029 Ainv = inv(A);
0030 
0031 % projection centre
0032 Z = -Ainv*a;
0033 
0034 % normalize P
0035 sdetA = sign(det(A));
0036 A = A*sdetA;
0037 
0038 % calibration and rotation matrix
0039 [Rinv,Kinv] = qr(inv(A));
0040 R  = Rinv';
0041 K0 = inv(Kinv);
0042 
0043 % change chirality abd signs in K if necessary
0044 Di = diag(sign(diag(K0)))*diag([sc,sc,1]);
0045 R  = Di*R;
0046 K0 = K0*Di;                                                                %#ok<MINV>
0047 
0048 % nomalize K
0049 K33 = K0(3,3); %*sign(sc);
0050 K   = K0/K33;