sugr_bR_from_UVuv

% Determine bR from (U,V,u,v
 select one of the four cases;

 [be,Re] = sugr_bR_from_UVuv(U,V,u,v)

 U, V    = from [U,D,V]=svd(E), with positive determinants enforced
 u,v     = Nx3 matrices for corresponding points

 Wolfgang Förstner 9/2011
 wfoerstn@uni-bonn.de

 See also sugr_E_Matrix

0001 %% Determine bR from (U,V,u,v
0002 % select one of the four cases;
0003 %
0004 % [be,Re] = sugr_bR_from_UVuv(U,V,u,v)
0005 %
0006 % U, V    = from [U,D,V]=svd(E), with positive determinants enforced
0007 % u,v     = Nx3 matrices for corresponding points
0008 %
0009 % Wolfgang Förstner 9/2011
0010 % wfoerstn@uni-bonn.de
0011 %
0012 % See also sugr_E_Matrix
0013 
0014 function [be,Re] = sugr_bR_from_UVuv(U,V,u,v)
0015 
0016 % auxiliary matrices
0017 W  = [0,1,0;-1,0,0;0,0,1];
0018 u3 = U(:,3);
0019 
0020 % number of corresponding directions
0021 N = size(u,1);
0022 
0023 % initialize
0024 solution = 0;
0025 
0026 be       = [1,0,0]';
0027 Re       = eye(3);
0028 
0029 % determine alternatives (PCV p. 583, Alg. 20)
0030 for t=0:3
0031     switch t
0032 %         case 0
0033 %             R  = V*W*U';     % R'=U*W*V' (HZ) -> R = V*W'*U'
0034 %             b  = u3;
0035 %         case 1
0036 %             R   = V*W*U';
0037 %             b  = -u3;
0038 %         case 2
0039 %             R   = V*W'*U';
0040 %             b  = u3;
0041 %         case 3
0042 %             R   = V*W'*U';
0043 %             b  = -u3;
0044     case 0
0045         R  = V*W'*U';     % R'=U*W*V' (HZ) -> R = V*W'*U'
0046         b  = u3;
0047     case 1
0048         R   = V*W'*U';   
0049         b  = -u3;
0050     case 2
0051         R   = V*W*U';
0052         b  = u3;
0053     case 3
0054         R   = V*W*U';
0055         b  = -u3;
0056     end   
0057     S = calc_S(b);
0058 %% check for positiveness (not yet included: check for direction || basis )
0059     % m-vectors: 3 x N Matrix, PCV (13.165)
0060     Nm = -S*S*u(:,1:3)';   
0061     % m x (u x R'v) - vectors, 3 x M-matrix, part of denom., PCV (13.164)
0062     Mm = (calc_S( Nm(1:3,:)) * cross(u(:,1:3)', R'*v(:,1:3)'))';
0063     %
0064     ssn = sign((ones(N,1) * b' .* Mm) * [1 1 1]');
0065     srn = ssn.* sign(((R'*v(:,1:3)')' .* Nm(1:3,:)')*[1 1 1]'); 
0066     if mean(srn) > 0 && mean(ssn) > 0 
0067         solution = 1;
0068         be       = calc_v_from_S(S);
0069         Re       = R;
0070     end
0071 end
0072 if solution == 0
0073     be = zeros(3,1);
0074 end