tsai.m

摄像机标定的matlab算法

%没有考虑摄像头畸变的标定程序
i=5;%数据的个数
fp=fopen('F:\data1.dat','r+');
s=fscanf(fp,'%f',[5,i]);
fclose(fp);
A=zeros(i,5);
B=zeros(i,1);
R=zeros(3,3);
X=zeros(i,2);
Y=zeros(i,1);
for(i=1:5)
   A(i,:)=[s(5,i)*s(1,i) s(5,i)*s(2,i) -s(4,i)*s(1,i) -s(4,i)*s(2,i) s(5,i)];
   B(i,1)=[s(4,i)]';%构造Axu＝B的矩阵；
end
xu=A\B; %求解矩阵的解xu
U=0;
for (i=1:4)
    U=U+xu(i,1)*xu(i,1);%计算U＝u1^2＋u2^2+u3^2+u4^2;
end
t1=xu(1,1)*xu(4,1)-xu(2,1)*xu(3,1);
t1=t1.^2;
t2=xu(1,1).^2+xu(2,1).^2;
t3=xu(3,1).^2+xu(4,1).^2;
C=[xu(1,1),xu(2,1);xu(3,1),xu(4,1)];
if rank(C)==2
    Ty2=(U-((U.^2-4*t1).^0.5))./(2*t1);%计算平移参数ty^2,ty＝ty^2;求解平移参数ty＝ty^0.5;
else 
    z=C(abs(C)>0);
    Ty2=1.0/(z(1).^2+z(2).^2);
end
Ty=sqrt(Ty2);                     %求解ty的值；
r11=xu(1,1)*Ty;
r12=xu(2,1)*Ty;
r21=xu(3,1).*Ty;
r22=xu(4,1).*Ty;
Tx=xu(5,1).*Ty;
x=r11*s(1,4)+r12*s(2,4)+Tx;
y=r21*s(1,4)+r22*s(2,4)+Ty;
if(sign(x)==sign(s(1,4)))&(sign(y)==sign(s(2,4)))
    Ty=Ty;
else
    Ty=-Ty;                %完成Ty的符号的选择
end
r11=xu(1,1)*Ty;       %重新求解矩阵的解
r12=xu(2,1)*Ty;
r21=xu(3,1).*Ty;
r22=xu(4,1).*Ty;
Tx=xu(5,1).*Ty;       %至此完成部分矩阵的求解
t=-sign(r11*r21+r12*r22);
%求解其余旋转矩阵的解
R=[r11,r12,(1-r11.^2-r12.^2).^0.5;r21,r22,t*((1-r21.^2-r22.^2).^0.5)];
R=[R(1:2,:);cross(R(1,:),R(2,:))];
r13=R(1,3);
r23=R(2,3);
r31=R(3,1);
r32=R(3,2);
r33=R(3,3);
%至此取出全部矩阵的解
for(i=1:5)
   X(i,:)=[r21*s(1,i)+r22*s(2,i)+Ty s(5,i)];
   Y(i,:)=[(r31*s(1,i)+r32*s(2,i))*s(5,i)];%构造AX＝Y的矩阵；
end
V=X\Y;
f=V(1);
Tz=V(2);
if f<0
    r13=-r13;
    r23=-r23;
    r31=-r31;
    r32=-r32;
    Tz=-Tz;
end