example_p_579_relative_orientation_mirror

% test mirror image
 example according to Figure 13.11 page 579

 Wolfgang Förstner
 wfoerstn@uni-bonn.de

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

0001 %% test mirror image
0002 % example according to Figure 13.11 page 579
0003 %
0004 % Wolfgang Förstner
0005 % wfoerstn@uni-bonn.de
0006 %
0007 % last changes: Susanne Wenzel 06/18
0008 % wenzel@igg.uni-bonn.de
0009 
0010 addpath(genpath('../General-Functions'))
0011 
0012 close all
0013 clc
0014 
0015 % three points: X=right, Y=depth, Z=heigth
0016 disp('----------- mirror image -----------')
0017 N = 3;
0018 
0019 %% in original position, will be rotated later
0020 % points
0021 disp('Create points and plane in generic situation ...')
0022 disp('original points')
0023 X0 = [1.0,1.0,1.2;
0024       1.2,1.0,1.5;
0025       1.1,3.0,2.0]                                                         %#ok<*NOPTS>
0026 
0027 % mirror plane
0028 disp('mirrow plane')
0029 A0 = [1,0,0,-2]'
0030 
0031 % mirrored points
0032 disp('mirrowed points')
0033 Y0 = [4-X0(:,1),X0(:,2),X0(:,3)]
0034 
0035 %% now rotate to obtain generic situation
0036 % rotation
0037 disp('rotation to obtain generic situation')
0038 Rp = calc_Rot_rod([0,0,0.05]')                                             
0039 
0040 % rotated points, plane
0041 disp('rotated points (see Tab. 13.4)')
0042 X = X0*Rp'
0043 Y = Y0*Rp'
0044 disp('transformation matrix for plane')
0045 M = [Rp, zeros(3,1);zeros(1,3),1]
0046 Ma = adjunctMatrix(M);
0047 disp('rotated mirrow plane')
0048 A = Ma' * A0
0049 
0050 % check mirroring of points X at A to yield Y
0051 disp('check mirroring of points X at A to yield Y ...')
0052 A = A/norm(A(1:3));
0053 Normal = A(1:3);
0054 S = -A(4);
0055 H = [eye(3)-2*(Normal*Normal'), 2*S*Normal;[0,0,0,1]]
0056 Xs = (H*[X,ones(3,1)]')';
0057 disp('difference should be 0:')
0058 check_mirroring = Xs(:,1:3)-Y
0059 
0060 % projection
0061 disp('projection ...')
0062 disp('rotation matrix') % freely chosen
0063 R = calc_Rot_r([0.1,0.2,-0.3])
0064 disp('projection matrix') % translation freely chosen
0065 P = R'*[eye(3), [2,2,-1.0]']
0066 
0067 % image points
0068 disp('projected image points')
0069 x = (P*[X,ones(N,1)]')'
0070 y = (P*[Y,ones(N,1)]')'
0071 
0072 figure('Color','w')
0073 plot(x(:,1)/x(:,3),x(:,2)/x(:,3),'or')
0074 hold on
0075 plot(y(:,1)/y(:,3),y(:,2)/y(:,3),'xb')
0076 axis equal
0077 title('Original points (red), mirrowed points (blue)')
0078 
0079 disp(' ')
0080 disp('Given image points, solve for E-matrix ... ')
0081 %% solve for rotations
0082 disp('solve for rotations ...')
0083 % normal
0084 n = cross(cross(x(2,:),y(2,:)),cross(x(3,:),y(3,:)))';
0085 disp('normal')
0086 n = n/norm(n)
0087 disp('|[x1 cross y1, x2 cross y2, x3 cross y3]|, should be 0')
0088 det_p = det([cross(x(1,:),y(1,:))',cross(x(2,:),y(2,:))',cross(x(3,:),y(3,:))'])
0089 
0090 % Parameters
0091 %% I. Solution
0092 disp('1. Solution ....')
0093 s = 1/(n(2)^2+n(3)^2)*(1-n(1))
0094 q2 = -n(3)*s
0095 q3 = n(2)*s
0096 
0097 % Rotations
0098 disp('rotations')
0099 Rl = calc_Rot_q([1, 0,  q2,  q3])
0100 Rr = calc_Rot_q([1, 0, -q2, -q3])
0101 disp('Essential matrix')
0102 E  = Rl*calc_S([1,0,0]')*Rr'
0103 
0104 % check coplanarity constraints of point pairs
0105 disp('check coplanarity constraints of point pairs ...')
0106 disp('contradictions w_i = x_i^T E x_i')
0107 w1 = x(1,:)*E*[-y(1,1),y(1,2),y(1,3)]'                                     %#ok<*NASGU>
0108 w2 = x(2,:)*E*[-y(2,1),y(2,2),y(2,3)]'
0109 w3 = x(3,:)*E*[-y(3,1),y(3,2),y(3,3)]'
0110 
0111 % check whether points are in front of the cameras
0112 % see lines 11-13 in Alg. 20, p. 583
0113 N = cross(cross([1,0,0]',Rl'*x(1,:)'),[1,0,0]');
0114 M = calc_S(N)* cross((Rl'*x(1,:)'),Rr'*[-y(1,1),y(1,2),y(1,3)]');
0115 ssa = sign([1,0,0] * M);
0116 sra = ssa.* sign((Rr'*[-y(1,1),y(1,2),y(1,3)]')'* N);
0117 % if positive: in front
0118 disp('positive, if points in front of camera (here: correct solution)')
0119 [sra,ssa]
0120 
0121 %% II. solution
0122 disp(' ')
0123 disp('2. Solution ....')
0124 s = 1/(n(2)^2+n(3)^2)*(-1-n(1))
0125 q2 = -n(3)*s
0126 q3 = n(2)*s
0127 
0128 % Rotations
0129 disp('rotations')
0130 Rl2 = calc_Rot_q([1, 0,  q2,  q3])
0131 Rr2 = calc_Rot_q([1, 0, -q2, -q3])
0132 disp('Essential matrix')
0133 E  = Rl2*calc_S([1,0,0]')*Rr2'
0134 
0135 % check coplanarity constraints of point pairs
0136 disp('check coplanarity constraints of point pairs ...')
0137 disp('contardictions w_i = x_i^T E x_i')
0138 w1 = x(1,:)*E*[-y(1,1),y(1,2),y(1,3)]'
0139 w2 = x(2,:)*E*[-y(2,1),y(2,2),y(2,3)]'
0140 w3 = x(3,:)*E*[-y(3,1),y(3,2),y(3,3)]'
0141 
0142 % check whether points are in front of the cameras
0143 % see lines 11-13 in Alg. 20, p. 583
0144 N = cross(cross([1,0,0]',Rl2'*x(1,:)'),[1,0,0]');
0145 M = calc_S(N)* cross((Rl2'*x(1,:)'),Rr2'*[-y(1,1),y(1,2),y(1,3)]');
0146 ssa = sign([1,0,0] * M);
0147 sra = ssa.* sign((Rr2'*[-y(1,1),y(1,2),y(1,3)]')'* N);
0148 % if positive: in front
0149 disp('positive, if points in front of camera (here: wrong solution)')
0150 [sra,ssa]
0151 
0152 Rl'*Rl2