% determines constraint value and Jacobians for 2D line estimation g= ^l2 - a ^l1 -b = 0 (== y_i-ax_i-b=0,i=1,...,N) lv = 2*G vector of observations [x;y] lnu = 2*G vector of approximate fitted observations [x^a;y^a] xa = 2x1 vector, approximate values for parameters xa=[a;b] cg = Gx1 vector of residual of constraints A = GxU Jacobian dg/dx B = NxG Jacobian (dg/dl)' Wolfgang Förstner wfoerstn@uni-bonn.de
0001 %% determines constraint value and Jacobians for 2D line estimation 0002 % 0003 % g= ^l2 - a ^l1 -b = 0 (== y_i-ax_i-b=0,i=1,...,N) 0004 % 0005 % lv = 2*G vector of observations [x;y] 0006 % lnu = 2*G vector of approximate fitted observations [x^a;y^a] 0007 % xa = 2x1 vector, approximate values for parameters xa=[a;b] 0008 % 0009 % cg = Gx1 vector of residual of constraints 0010 % A = GxU Jacobian dg/dx 0011 % B = NxG Jacobian (dg/dl)' 0012 % 0013 % Wolfgang Förstner 0014 % wfoerstn@uni-bonn.de 0015 0016 function [cg,A,B] = cg_2D_line(lv,lnu,xa) 0017 0018 % number of constraints 0019 G2 = size(lv,1); 0020 G = G2/2; % == N 0021 0022 % Jacobians 0023 A = - [lv(1:G) ones(G,1)]; % G x U 0024 B = [-xa(1)*eye(G) eye(G)]'; % N x G 0025 0026 % residual of constraints 0027 cg = -(lv(G+1:2*G) - xa(1) * lv(1:G) - xa(2)) + B'*(lnu-lv); % G x 1 0028 0029