condition_RZ

% condition Pose RZ

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


 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)/s -(Z-t)/s) = N(R*(Xc-Zc))
 Zc = (Z-t)/s

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