san.m

经典的张正友的摄像机的两步标定程序

clear
ul=350.983032;
vl=307.299408;
ur=139.903656;
vr=297.644897;
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);

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));