% determines constraint values and Jacobians for 2D similarity Model: cg(x,l) = zr - s*zl - t = 0 [l(3)] - [x(1) -x(2)] [l(1)] - [x(3)] = [0] [l(4)] - [x(2) x(1)] [l(2)] - [x(4)] [0] l = 4x1-vector [xl yl,xr yr] = [zl, zr] x = 4x1-vector [ a b, c d] = [ s, t] Wolfgang Förstner wfoerstn@uni-bonn.de
0001 %% determines constraint values and Jacobians for 2D similarity 0002 % 0003 % Model: 0004 % cg(x,l) = zr - s*zl - t = 0 0005 % 0006 % [l(3)] - [x(1) -x(2)] [l(1)] - [x(3)] = [0] 0007 % [l(4)] - [x(2) x(1)] [l(2)] - [x(4)] [0] 0008 % 0009 % l = 4x1-vector [xl yl,xr yr] = [zl, zr] 0010 % x = 4x1-vector [ a b, c d] = [ s, t] 0011 % 0012 % Wolfgang Förstner 0013 % wfoerstn@uni-bonn.de 0014 0015 function [cg,At,Bt] = cg_similarity_2D(l,la,xa) 0016 0017 At = [-la(1) la(2) -1 0 ;... 0018 -la(2) -la(1) 0 -1]; 0019 Bt = [-xa(1) xa(2) 1 0;... 0020 -xa(2) -xa(1) 0 1]; 0021 0022 cg = -( [la(3);... 0023 la(4)] - ... 0024 [xa(1) -xa(2); 0025 xa(2) xa(1)] * ... 0026 [la(1);... 0027 la(2)] - ... 0028 [xa(3); ... 0029 xa(4)])... 0030 + Bt * (la-l);