sugr_estimation_algebraic_Point_2D_from_Lines

% Algebraic estimate of intersection Point_2D from N Lines_2D

 [x_est,sigma_0_est_D,sigma_0_est_O] =
         sugr_algebraic_estimation_Point_2D_from_lines(lines,spherically)

 lines = struct
         lines.h    = N x 3 matrix of sherically normalized homogeneous coordinates
         lines.Crr  = N x 2 x 2 array of reduced covariance matrices
         lines.type = N x 1 vector of 2's
 spherically = 1 use spherically normalized lines
             = 0 use Euclideanly normalized lines

 x_est       = structure of algebarically estimated intersection point |x_est|=1
 sigma_0_est_O = estimated variance factor sqrt(v' Sigma^{-1} v /(N-2))
 sigma_0_est_D = estimated variance factor sqrt(D33^2 /(N-2))
                 from SVD of points

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

 See also sugr_Point_2D, sugr_Line_2D,  sugr_estimation_ml_Point_2D_from_Lines
 sugr_estimation_geometric_Point_2D_from_Lines

0001 %% Algebraic estimate of intersection Point_2D from N Lines_2D
0002 %
0003 % [x_est,sigma_0_est_D,sigma_0_est_O] =
0004 %         sugr_algebraic_estimation_Point_2D_from_lines(lines,spherically)
0005 %
0006 % lines = struct
0007 %         lines.h    = N x 3 matrix of sherically normalized homogeneous coordinates
0008 %         lines.Crr  = N x 2 x 2 array of reduced covariance matrices
0009 %         lines.type = N x 1 vector of 2's
0010 % spherically = 1 use spherically normalized lines
0011 %             = 0 use Euclideanly normalized lines
0012 %
0013 % x_est       = structure of algebarically estimated intersection point |x_est|=1
0014 % sigma_0_est_O = estimated variance factor sqrt(v' Sigma^{-1} v /(N-2))
0015 % sigma_0_est_D = estimated variance factor sqrt(D33^2 /(N-2))
0016 %                 from SVD of points
0017 %
0018 % Wolfgang Förstner 2/2011
0019 % wfoerstn@uni-bonn.de
0020 %
0021 % See also sugr_Point_2D, sugr_Line_2D,  sugr_estimation_ml_Point_2D_from_Lines
0022 % sugr_estimation_geometric_Point_2D_from_Lines
0023 
0024 
0025 function [ex,sigma_0_est_D,sigma_0_est_O] = sugr_estimation_algebraic_Point_2D_from_Lines(lines,sph)
0026 
0027 lh   = lines.h;
0028 lCrr = lines.Crr;
0029 
0030 [N,~] = size(lh);
0031 Red = N-2;
0032 
0033 % algebraic estimation
0034 % Euclidean normalization changes
0035 if sph==0
0036     lh=lh./(sqrt(lh(:,1).^2+lh(:,2).^2)*[1,1,1]);
0037 end
0038 [U,D,V]=svd(lh,'econ');
0039 est_x = V(:,3);
0040 
0041 % Covariance matrix: from c+v = A x, A = (l_n'), c = (c_n) = (l_n' x)
0042 % dx = (A' A)^-1 A' c = A^+ c --> Cxx = A^+ Ccc A^+', Ccc = (x' Clnn x)
0043 
0044 % Pseudoinverse of D
0045 invD = zeros(3);
0046 for i=1:2
0047     invD(i,i) =1/D(i,i);
0048 end
0049 %pseudoinvers of A
0050 Ap = V * invD * U';                              % pseudo inverse (N x 3)
0051 
0052 Omega = 0;
0053 Cxx   = zeros(3);
0054 for n=1:N
0055     Jln      = null(lh(n,:));                       % Jacobian l -> l_r
0056     Clnn     = Jln * squeeze(lCrr(n,:,:)) * Jln';    % Cl_nl_h
0057     cgn      = est_x' * lh(n,:)';                    % cgn
0058     var_cgn  = est_x' * Clnn * est_x;               % var(cgn)
0059 %     c_sc =[cgn/sqrt(var_cgn)];
0060     Omega    = Omega + cgn^2/var_cgn;               %
0061     Cxx      = Cxx + var_cgn * Ap(:,n) * Ap(:,n)';  % Cxx update
0062 end
0063 if Red > 0
0064     sigma_0_est_D = sqrt(D(3,3)^2/Red);
0065     sigma_0_est_O = sqrt(Omega/Red);
0066 else
0067     sigma_0_est_D = 1;
0068     sigma_0_est_O = 1;
0069 end
0070 
0071 ex = sugr_Point_2D(est_x,Cxx);
0072