sugr_CovM_E_matrix

% Covariance matrix of parameters of essential matrix

 CovM = sugr_CovM_E_matrix(X, Cpp, B, R)

 model
        xl' * E * xr = 0
        E = S(B) R'
        xl = [I |  0] X
        xr = [R | -B] X

 X   = N x 4 matrix of homogeneous 3D points, N >= 5
 Cpp = N x 4 x 4 Covariance matrix of reduced image coordinate pairs
 R   = 3 x 3 rotation matrix
 B   = 3 x 1 direction of base line, 1-->2

 Checked in sugr_estimation_ml_E_Matrix_from_point_pairs.m

  see: PCV 13.3.5.1

 Wolfgang Förstner 2016-09-01
 wfoerstn@uni-bonn.de

 See also sugr_E_Matrix

0001 %% Covariance matrix of parameters of essential matrix
0002 %
0003 % CovM = sugr_CovM_E_matrix(X, Cpp, B, R)
0004 %
0005 % model
0006 %        xl' * E * xr = 0
0007 %        E = S(B) R'
0008 %        xl = [I |  0] X
0009 %        xr = [R | -B] X
0010 %
0011 % X   = N x 4 matrix of homogeneous 3D points, N >= 5
0012 % Cpp = N x 4 x 4 Covariance matrix of reduced image coordinate pairs
0013 % R   = 3 x 3 rotation matrix
0014 % B   = 3 x 1 direction of base line, 1-->2
0015 %
0016 % Checked in sugr_estimation_ml_E_Matrix_from_point_pairs.m
0017 %
0018 %  see: PCV 13.3.5.1
0019 %
0020 % Wolfgang Förstner 2016-09-01
0021 % wfoerstn@uni-bonn.de
0022 %
0023 % See also sugr_E_Matrix
0024 
0025 function CovM = sugr_CovM_E_matrix(X, Cpp, B, R)
0026 
0027 % number of points
0028 NN = size(X, 1);
0029 
0030 % essential matrix
0031 E = calc_S(B) * R';
0032 
0033 % projection matrix right
0034 Pr = R * [eye(3), - B];
0035 
0036 n = 0;
0037 % fitted image directions
0038 for nn = 1:NN
0039     if norm(X(nn, :)) > 10 ^ (- 10)
0040         n = n + 1;
0041         xl(n, :) = X(nn, 1:3) / norm(X(nn, 1:3));                          %#ok<*AGROW>
0042         xr(n, :) = X(nn, :) * Pr';
0043         xr(n, :) = xr(n, :) / norm(xr(n, :));
0044     end
0045 end
0046 N = n;
0047 
0048 % ancillary vectors
0049 ll = xr * E';
0050 lr = xl * E;
0051 x1 = xr * R;
0052 
0053 at = zeros(1, 5);
0054 bt = zeros(1, 4);
0055 
0056 % dtermine variances of constraints
0057 btCb = zeros(N, 1);
0058 for n = 1:N
0059     bt(1:2) = ll(n, :) * null(xl(n, :));
0060     bt(3:4) = lr(n, :) * null(xr(n, :));
0061     C = reshape(Cpp(n, :, :), 4, 4);
0062     btCb(n) = bt * C * bt';
0063 end
0064 
0065 % determine information matrix
0066 Nm = zeros(5);
0067 for n = 1:N
0068     at(1:2) = (calc_S(x1(n, :)') * xl(n,:)')' * null(B');
0069     at(3:5) = (calc_S(lr(n, :)') * xr(n,:)')';
0070     Nm = Nm + at' * at / btCb(n);
0071 end
0072 
0073 CovM = inv(Nm);
0074 
0075 
0076