sugr_estimation_geometric_Point_2D_from_Lines

% Geometric estimate of intersection Point_2D from N Lines_2D

 [x_est, sigma_0, x] = sugr_estimation_geometric_Point_2D_from_lines(lines)

 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

 x_est   = structure of geometricall estimated intersection point |x_est|=1 
 sigma_0 = estimated variance factor sqrt(v' Sigma^{-1} v /(N-2))
 x       = 2x1 estimated point

 Wolfgang Förstner 7/2012
 wfoerstn@uni-bonn.de

 See also sugr_Point_2D, sugr_Line_2D,  sugr_estimation_ml_Point_2D_from_Lines
 sugr_estimation_algebraic_Point_2D_from_Lines

0001 %% Geometric estimate of intersection Point_2D from N Lines_2D
0002 %
0003 % [x_est, sigma_0, x] = sugr_estimation_geometric_Point_2D_from_lines(lines)
0004 %
0005 % lines = struct
0006 %         lines.h    = N x 3 matrix of sherically normalized homogeneous coordinates
0007 %         lines.Crr  = N x 2 x 2 array of reduced covariance matrices
0008 %         lines.type = N x 1 vector of 2's
0009 %
0010 % x_est   = structure of geometricall estimated intersection point |x_est|=1
0011 % sigma_0 = estimated variance factor sqrt(v' Sigma^{-1} v /(N-2))
0012 % x       = 2x1 estimated point
0013 %
0014 % Wolfgang Förstner 7/2012
0015 % wfoerstn@uni-bonn.de
0016 %
0017 % See also sugr_Point_2D, sugr_Line_2D,  sugr_estimation_ml_Point_2D_from_Lines
0018 % sugr_estimation_algebraic_Point_2D_from_Lines
0019 
0020 function [xe,sigma_0,x] = sugr_estimation_geometric_Point_2D_from_Lines(lines)
0021 
0022 % homogeneous coordinates
0023 lh   = lines.h;
0024 % number of edges
0025 [N,~] = size(lh);
0026 % redundancy
0027 R = N-2;
0028 % initiate normals
0029 nv = zeros(2,1);
0030 Nm = zeros(2);
0031 di=zeros(N,2);
0032 x0=zeros(N,2);
0033 % convert to centroid from and build Normals
0034 w = zeros(N,1);
0035 for n=1:N
0036     sugr_l.h    = lh(n,:)';
0037     sugr_l.Crr  = reshape(lines.Crr(n,:,:),2,2);
0038     sugr_l.type = 2;
0039     [e,Cee] = sugr_Line_2D_hom2Hes(sugr_l);
0040     [x0n,pn,~,~]= sugr_Line_2D_Hes2cen(e,Cee);
0041    
0042     dirn =[cos(pn-pi/2),sin(pn-pi/2)]';
0043     wn   = 1;%1/sqn^2;
0044     w(n) = wn;
0045     Wn   = wn*(eye(2)-dirn*dirn');
0046     nv = nv + Wn*x0n;
0047     Nm = Nm + Wn;
0048     x0(n,:)=x0n';
0049     di(n,:)=dirn';
0050 end
0051 % covariance matrix
0052 Cxx   = inv(Nm);
0053 % estimated point
0054 x     = Cxx * nv;                                                          %#ok<*MINV>
0055 % sum of weighted squared residuals
0056 % res   = [di(:,2),-di(:,1)].*(x0-ones(N,1)*x');
0057 Omega = 0;
0058 for n=1:N
0059     Omega=Omega+(x0(n,:)-x')*w(n)*(eye(2)-di(n,:)'*di(n,:))*(x0(n,:)-x')';
0060 end
0061 
0062 if R > 1
0063     sigma_0 = sqrt(Omega/(N-2));
0064 end
0065 xe=sugr_Point_2D(x,Cxx);