% Determines residuals etc. for 2D point from passing lines using line's Hessian representation [va,C,cg,atr,btr] = sugr_ghm_cg_2d_point_from_lines_Hessian(l,le,xe,Cee) with Hessian representation * l = 2 x 1 vector, observed line * le = 2 x 1 vector, approximated fitted line * xe = 2 x 1 vector, approximated point * Cee = 2 x 2 matrix, covariance matrix of observations * va = = 2 x 1 vector, approximate residual * cg = scalar, residual of constraint * atr = 1 x 2 vector, transposed Jacobian for xe * btr = 1 x 2 vector, transposed jacobian for le Wolfgang Förstner 07/2012, adapted for sugr 6/2010 wfoerstn@uni-bonn.de See also sugr_Point_2D, sugr_Line_2D, sugr_estimation_ml_Point_2D_from_Lines_Hessian
0001 %% Determines residuals etc. for 2D point from passing lines 0002 % using line's Hessian representation 0003 % 0004 % [va,C,cg,atr,btr] = sugr_ghm_cg_2d_point_from_lines_Hessian(l,le,xe,Cee) 0005 % 0006 % with Hessian representation 0007 % 0008 % * l = 2 x 1 vector, observed line 0009 % * le = 2 x 1 vector, approximated fitted line 0010 % * xe = 2 x 1 vector, approximated point 0011 % * Cee = 2 x 2 matrix, covariance matrix of observations 0012 % 0013 % * va = = 2 x 1 vector, approximate residual 0014 % * cg = scalar, residual of constraint 0015 % * atr = 1 x 2 vector, transposed Jacobian for xe 0016 % * btr = 1 x 2 vector, transposed jacobian for le 0017 % 0018 % Wolfgang Förstner 07/2012, adapted for sugr 6/2010 0019 % wfoerstn@uni-bonn.de 0020 % 0021 % See also sugr_Point_2D, sugr_Line_2D, sugr_estimation_ml_Point_2D_from_Lines_Hessian 0022 0023 function [va,C,cg,atr,btr] = sugr_ghm_cg_Point_2D_from_Lines_Hessian(l,le,xe,C) 0024 0025 % g(l,x): x1 cos(l1) + x2 sin(l1) - l2 = 0 0026 0027 % approximate residual 0028 va = le-l; 0029 % Jacobian for x: 0030 atr = [cos(le(1)),sin(le(1))]; % row 2-vector 0031 0032 % Jacobain for l 0033 btr = [-xe(1)*sin(le(1))+xe(2)*cos(le(1)),-1]; % row 2-vector 0034 0035 % residual of constraint 0036 %cg = - l' * xe ; % scalar 0037 cg = -(xe'*[cos(l(1));sin(l(1))]-l(2));