demo_bR_from_UVuv

% demo: bR from (U,V,xs,xss)

 Check correctnes of basis and Rotation from 
 (U,V) or E and corresponding points (see Alg. 20)

 U,V      = from svd(E)
 xs,xss   = points in left and right camera system
 
 bR       = [b,R] matrix of relative orientation 

 Wolfgang Förstner 6/2017
 wfoerstn@uni-bonn.de

 last changes: Susanne Wenzel 06/18
 wenzel@igg.uni-bonn.de

0001 %% demo: bR from (U,V,xs,xss)
0002 %
0003 % Check correctnes of basis and Rotation from
0004 % (U,V) or E and corresponding points (see Alg. 20)
0005 %
0006 % U,V      = from svd(E)
0007 % xs,xss   = points in left and right camera system
0008 %
0009 % bR       = [b,R] matrix of relative orientation
0010 %
0011 % Wolfgang Förstner 6/2017
0012 % wfoerstn@uni-bonn.de
0013 %
0014 % last changes: Susanne Wenzel 06/18
0015 % wenzel@igg.uni-bonn.de
0016 
0017 clc
0018 clear vars
0019 
0020 addpath(genpath('../../General-Functions'))
0021 
0022 sugr_INIT
0023 
0024 disp('--------------------------------------------')
0025 disp(' bR from (U,V) = svd(E),x'',x'''' (Alg. 20) ')
0026 disp('--------------------------------------------')
0027 
0028 %% generate random E-matrix
0029 % choose rotation matrix
0030 Rs  = eye(3);
0031 Rss = calc_Rot_r(randn(3,1)); %diag([1,-1,-1]);
0032 % choose basis direction
0033 bh  = randn(3,1); 
0034 bh  = bh/norm(bh); %[-1,0,0]';
0035 % choose 3d point
0036 X = randn(3,1);
0037 % determine image coordinates
0038 xs  = Rs*X;
0039 xss = Rss*(X-bh);
0040 
0041 % choose E-matrix with random sign
0042 v = sign(randn(1));
0043 disp(strcat('random_sign of_E =  ',num2str(v)));
0044 E = Rs*calc_S(bh)*Rss'*v                                                     %#ok<*NOPTS>
0045 
0046 % provide SVD with positive U and V
0047 [U,D,V] = svd(E);
0048 U = U*det(U);
0049 V = V*det(V);
0050 
0051 % determine basis, Rotation and case for relative orientation
0052 % (arguments xs' and xss' must be a list of row vectors, here only one)
0053 
0054 %% from (E,xs,xss)
0055 disp('b and R from E using set of pairs of directions')
0056 % Alg. 20
0057 [bes,Res] = sugr_bR_from_E_uv(E,xs',xss')
0058 
0059 Es_check = sugr_E_Matrix(bes,Res);
0060 disp('check results, differences of given and estimated b and R should be zero' )
0061 diff_bRs_should_be_zero = [bh-bes,Rss-Res,v*E-Es_check.E]
0062 
0063 %% from (U,V,xs,xss)
0064 disp('b and R from U and V using set of pairs of directions')
0065 % similar to Alg. 20, but starting from (U,V) instead from E
0066 [be,Re] = sugr_select_bR_from_E_UV_uv(U,V,xs',xss')                              
0067 
0068 E_check = sugr_E_Matrix(be,Re);
0069 disp('check results, differences of given and estimated b and R should be zero' )
0070 diff_bR_should_be_zero = [bh-be,Rss-Re,v*E-E_check.E]                        
0071 
0072 
0073