uncondition_RZ

% uncondition Pose RZ

 Usage:
   RZc = unconditioned Pose
        RZc.RZ = [Rc,Zc]
        RZc.C  = D([drc;dZc])
  
   M   - 4x4 condition matrix from scene points
 
   RZ  - Pose, assuming P=[I3,0]*M(R,Z)
        RZ.RZ = [R,Z]
        RZ.C  = D([dr;dZ])

 Wolfgang Förstner 1/2018
 wfoerstn@uni-bonn.de 

 conditioning-matrix: Xec = (Xe - t)/s -> Xc = M * X 
 [Xch] = [eye(3)/s -t/s]   [Xh] = [Xh/s-t*X0/s]
 [Xc0]   [0'         1 ]   [X0]   [   X0      ]
 y = N(R*(X0/Xh-Z)) = N(R*((X0/Xh-t) -(Z-t))/s) = N(R*(Xc-Zc))
 Zc = (Z-t)/s  -> Z = s*Zc+t

0001 %% uncondition Pose RZ
0002 %
0003 % Usage:
0004 %   RZc = unconditioned Pose
0005 %        RZc.RZ = [Rc,Zc]
0006 %        RZc.C  = D([drc;dZc])
0007 %
0008 %   M   - 4x4 condition matrix from scene points
0009 %
0010 %   RZ  - Pose, assuming P=[I3,0]*M(R,Z)
0011 %        RZ.RZ = [R,Z]
0012 %        RZ.C  = D([dr;dZ])
0013 %
0014 % Wolfgang Förstner 1/2018
0015 % wfoerstn@uni-bonn.de
0016 %
0017 % conditioning-matrix: Xec = (Xe - t)/s -> Xc = M * X
0018 % [Xch] = [eye(3)/s -t/s]   [Xh] = [Xh/s-t*X0/s]
0019 % [Xc0]   [0'         1 ]   [X0]   [   X0      ]
0020 % y = N(R*(X0/Xh-Z)) = N(R*((X0/Xh-t) -(Z-t))/s) = N(R*(Xc-Zc))
0021 % Zc = (Z-t)/s  -> Z = s*Zc+t
0022 %
0023 
0024 function RZ = uncondition_RZ(RZc, M)
0025 % Parameters of conditioning
0026 s =  1/M(1,1);
0027 t = -M(1:3,4)*s;
0028 
0029 % Unonditioning of Z
0030 RZ.RZ = [RZc.RZ(:,1:3), s*RZc.RZ(:,4)+t];
0031 
0032 % variance propagation
0033 J = [eye(3) zeros(3); zeros(3) eye(3)*s];
0034 RZ.C = J*RZc.C*J';
0035 
0036 
0037 return
0038