clear
sul=350.983032;
svl=307.299408;
sur=139.903656;
svr=76.789368;
load('Calib_Results_stereo.mat');
flx=fc_left(1,1);
fly=fc_left(2,1);
frx=fc_right(1,1);
fry=fc_right(2,1);
uol=cc_left(1,1);
vol=cc_left(2,1);
uor=cc_right(1,1);
vor=cc_right(2,1);
tx=T(1,1);
ty=T(2,1);
tz=T(3,1);

k1=kc_left(1,1);
k2=kc_left(2,1);
k3=kc_left(3,1);
k4=kc_left(4,1);

rk1=kc_right(1,1);
rk2=kc_right(2,1);
rk3=kc_right(3,1);
rk4=kc_right(4,1);

cxl=(sul-uol)*(k1*(((sul-uol)/flx)^2+((svl-vol)/fly)^2)+k2*(((sul-uol)/flx)^2+((svl-vol)/fly)^2)^2)/flx+2*k3*((sul-uol)/flx)*((svl-vol)/fly)+k4*((((sul-uol)/flx)^2+((svl-vol)/fly)^2)+2*((sul-uol)/flx)^2);
cyl=(svl-vol)*(k1*(((sul-uol)/flx)^2+((svl-vol)/fly)^2)+k2*(((sul-uol)/flx)^2+((svl-vol)/fly)^2)^2)/fly+2*k4*((sul-uol)/flx)*((svl-vol)/fly)+k3*((((sul-uol)/fly)^2+((svl-vol)/fly)^2)+2*((svl-vol)/flx)^2);
cxr=(sur-uor)*(k1*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)+k2*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)^2)/frx+2*k3*((sur-uor)/flx)*((svr-vor)/fly)+k4*((((sur-uor)/flx)^2+((svr-vor)/fly)^2)+2*((sur-uor)/flx)^2);
cyr=(svr-vor)*(k1*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)+k2*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)^2)/fry+2*k4*((sur-uor)/flx)*((svr-vor)/fly)+k3*((((sur-uor)/fly)^2+((svr-vor)/fly)^2)+2*((svr-vor)/flx)^2);

%cyl=(svl-vol)*(k1*(((sul-uol)/flx)^2+((svl-vol)/fly)^2)+k2*(((sul-uol)/flx)^2+((svl-vol)/fly)^2)^2)+2*k3*((sul-uol)/flx)*((svl-vol)/fly)+k4*((((sul-uol)/fly)^2+((svl-vol)/fly)^2)+2*((svl-vol)/flx)^2)/fly;
%cxr=(sur-uor)*(k1*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)+k2*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)^2)+2*k3*((sur-uor)/flx)*((svr-vor)/fly)+k4*((((sur-uor)/flx)^2+((svr-vor)/fly)^2)+2*((sur-uor)/flx)^2)/frx;
%cyr=(svr-vor)*(k1*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)+k2*(((sur-uor)/flx)^2+((svr-vor)/fly)^2)^2)+2*k3*((sur-uor)/flx)*((svr-vor)/fly)+k4*((((sur-uor)/fly)^2+((svr-vor)/fly)^2)+2*((svr-vor)/flx)^2)/fry;
%cyl=(svl-vol)*(k1*((sul-uol)^2+(svl-vol)^2)+k2*((sul-uol)^2+(svl-vol)^2)^2)+2*k3*(sul-uol)*(svl-vol)+k4*(((sul-uol)^2+(svl-vol)^2)+2*(svl-vol)^2);

%cxr=(sur-uor)*(rk1*((sur-uor)^2+(svr-vor)^2)+rk2*((sur-uor)^2+(svr-vor)^2)^2)+2*rk3*(sur-uor)*(svr-vor)+rk4*(((sur-uor)^2+(svr-vor)^2)+2*(sur-uor)^2);
%cyr=(svr-vor)*(rk1*((sur-uor)^2+(svr-vor)^2)+rk2*((sur-uor)^2+(svr-vor)^2)^2)+2*rk3*(sur-uor)*(svr-vor)+rk4*(((sur-uor)^2+(svr-vor)^2)+2*(svr-vor)^2);

ul=sul-cxl*flx;
vl=svl-cyl*fly;
ur=sur-cxr*frx;
vr=svr-cyr*fry;

M=cat(2,R,T);
r1=M(1,1);
r2=M(1,2);
r3=M(1,3);
r4=M(2,1);
r5=M(2,2);
r6=M(2,3);
r7=M(3,1);
r8=M(3,2);
r9=M(3,3);
tx=M(1,4);
ty=M(2,4);
tz=M(3,4);
%MM=M;
m11=r1*frx;
m12=r2*frx;
m13=r3*frx;
m14=tx*frx;
m21=r4*fry;
m22=r5*fry;
m23=r6*fry;
m24=ty*fry;
m31=r7;
m32=r8;
m33=r9;
m34=tz;

zx=(m14-m34*(ur-uor))/((m31*(ul-uol)/flx+m32*(vl-vol)/fly+m33)*(ur-uor)-(m11*(ul-uol)/flx+m12*(vl-vol)/fly+m13));
zy=(m24-m34*(vr-vor))/((m31*(ul-uol)/flx+m32*(vl-vol)/fly+m33)*(vr-vor)-(m21*(ul-uol)/flx+m22*(vl-vol)/fly+m23));
%zx=flx*(m14-m34*(ur-uor))/((m31*(ul-uol)+m32*(vl-vol)+m33*flx)*(ur-uor)-(m11*(ul-uol)+m12*(vl-vol)+m13*flx));
%zy=fly*(m24-m34*(vr-vor))/((m31*(ul-uol)+m32*(vl-vol)+m33*fly)*(vr-vor)-(m21*(ul-uol)+m22*(vl-vol)+m23*fly));