%B. Vandeportaële & D. Maestro 2017
close all
clear all
%example for the inversion of P1 to P1 scale and offset
pixperm=sym('pixperm','real');
pud=sym('pud','real');
Kd=[pixperm  pud; 0 1]
a=sym('a','real');
b=sym('b','real');
c=sym('c','real');
d=sym('d','real');
Kdinv=[a b;c d ]
eqn= reshape(Kdinv*Kd-eye(2),4,1)
S=solve(eqn,a,b,c,d)
Kdinv=[S.a S.b;S.c S.d]
jacobian(reshape(Kdinv,4,1),[pixperm,pud])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tcx=sym('tcx','real');
tcy=sym('tcy','real');
r11=sym('r11','real');
r12=sym('r12','real');
r21=sym('r21','real');
r22=sym('r22','real');
M=[r11,r12,tcx;r21,r22,tcy ;0 0 1]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fuc=sym('fuc','real');
puc=sym('puc','real');
K=[fuc  puc 0;
    0  1 0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
E=[1 0;0 0 ;0 1]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
H=K*M*E*Kdinv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tdx=sym('tdx','real');
tdy=sym('tdy','real');
thetad=sym('thetad','real');
Md=[cos(thetad),-sin(thetad), tdx;sin(thetad),cos(thetad),tdy; 0, 0, 1]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%