newall4.m

tsai经典标定程序 MATLAB语言

%摄像机标定新算法,用一个子函数计算出两组运动组的参数
%本程序用于调试摄像机标定新算法
%这个程序在原来的基础上改变了部分返回值，将标准离差改为均方值的计算
%加入标定整体误差计算，运动复原和三维复原整体误差计算
%应先用计算出的平移矢量进行三维复原，从得出的点对统计其z方向为正的个数，多者为正确的平移矢量
%已完成
%程序太复杂，简化，将输出参数只考虑整体复原误差,这样的运行时间与不简化时差不多，需要进一步简化程序

function [ans]=newall4();
%以下为本算法的输入参数,运动组的旋转阵和平移阵R,T1,T2
alpha=pi/6;
beta=pi/4;
gamma=pi/3;
% T11=[10 4 5],T12=[4 -12 1.6],T11*T12'=0，自己设计的正交数据
t11=0.842152;
t12=0.336861;
t13=0.421076;
t21=0.313728;
t22=-0.941184;
t23=0.125491;
%摄像机内参数理论值
fu=1000;
fv=1000;
u0=0;
v0=0;
s=0.02;
%以下对计算K阵的子函数调用100次
number=100;
i=1;
counter=0;
for j=1:number
    [p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11]=subfun(alpha,beta,gamma,t11,t12,t13,t21,t22,t23);
        error_K(i)=p1;
        error_R(i)=p2;
        error_T(i)=p3;
        error_H(i)=p4;
        recover(i)=p5;
        Snr1(i)=p6;
        Snr2(i)=p7;
        elapsed_tH(i)=p8;
        tK(i)=p9;
        trecover(i)=p10;
        elapsed_tall(i)=p11;
        i=i+1;
end
%以下部分程序用于返回各参数的误差，其中每两者为一组，前者表示绝对误差均值，后者表示均方根误差

ans(1)=number.\sum(error_K);
ans(2)=number.\sum(error_R);
ans(3)=number.\sum(error_T);
ans(4)=number.\sum(error_H);
ans(5)=number.\sum(recover);
ans(6)=number.\sum(Snr1);
ans(7)=number.\sum(Snr2);
ans(8)=number.\sum(elapsed_tH);
ans(9)=number.\sum(tK);
ans(10)=number.\sum(trecover);
ans(11)=number.\sum(elapsed_tall);

%以下部分为子函数对不同运动组，分别求相应的e,F，返回元胞数组
function [a]=subfunction(alpha,beta,gamma,t1,t2,t3);
u0=0;
v0=0;    
fu=1000;
fv=1000;
s=0.02;
K=[fu,s,u0;0,fv,v0;0,0,1];
%计算理论值旋转阵R,平移阵T,本征阵E0,极点p,规一化极点e0,e0的反对称阵ex0,单应性阵H0,基础阵F0
R(1,1)=cos(beta)*cos(gamma);
R(1,2)=cos(beta)*sin(gamma);
R(1,3)=-sin(beta);
R(2,1)=sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma);
R(2,2)=sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma);
R(2,3)=sin(alpha)*cos(beta);
R(3,1)=cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma);
R(3,2)=cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma);
R(3,3)=cos(alpha)*cos(beta);
T=[t1 t2 t3]';
p0=K*T;
%规一化的极点
e0=(p0./p0(3))./norm(p0./p0(3),'fro');
% ex0=[0 -e0(3) e0(2);e0(3) 0 -e0(1);-e0(2) e0(1) 0];
H0=K*R*inv(K);
% F0=ex0*H0;
%以下部分程序用于生成原始以及映射后的匹配点对，并计算相应的信号功率,由于数据的条件数太大
%在进行SVD之前，对数据进行正则化处理
min_x=-2;
max_x=2;
min_y=-2;
max_y=2;
min_z=1;
max_z=2;
xa=min_x+(max_x-min_x)*rand(1,50);
ya=min_y+(max_y-min_y)*rand(1,50);
za=min_z+(max_z-min_z)*rand(1,50);
P=[xa;ya;za];
[m,n]=size(P);
j=1;
for i=1:n
    P0(:,i)=R*P(:,i)+T;
        if P0(3,i)>0
        P1(:,j)=P(:,i);
        P2(:,j)=P0(:,i);
        j=j+1;
    end
end
[m,n]=size(P1);
%生成映射前后的齐次坐标对[rr_xx,rr_yy,ones(size(rr_xx,2))]，uorig,vorig为理论值，uorig=X3i'/X3i,vorig=(k*T)(3)/X3i
rr_x1=P1(3,:).\P1(1,:);
rr_y1=P1(3,:).\P1(2,:);
rr_x2=P2(3,:).\P2(1,:);
rr_y2=P2(3,:).\P2(2,:);
uorig=P1(3,:).\P2(3,:);
vorig=P1(3,:).\p0(3);
Pwx1=n\sum(rr_x1.^2);
Pwy1=n\sum(rr_y1.^2);
Pw1=2\(Pwx1+Pwy1);
Pwx2=n\sum(rr_x2.^2);
Pwy2=n\sum(rr_y2.^2);
Pw2=2\(Pwx2+Pwy2);
%以下部分程序用于形成噪化数据，其中A代表噪声强度,并计算相应的信噪比,A的值在0.0002和0.008之间变化，间距0.0002,每次改变参数A
A=0.001;                
nx1=A*(2*rand(1,n)-1);
ny1=A*(2*rand(1,n)-1);
nx2=A*(2*rand(1,n)-1);
ny2=A*(2*rand(1,n)-1);
mnx1=n\sum(nx1);
mny1=n\sum(ny1);
mnx2=n\sum(nx2);
mny2=n\sum(ny2);
sigma_nx1=n\sum((nx1-mnx1).^2);
sigma_ny1=n\sum((ny1-mny1).^2);
sigma_nx2=n\sum((nx2-mnx2).^2);
sigma_ny2=n\sum((ny2-mny2).^2);
sigma_n1=2\(sigma_nx1+sigma_ny1);
sigma_n2=2\(sigma_nx2+sigma_ny2);
snr_x1=10*log10(sigma_nx1\Pwx1);
snr_y1=10*log10(sigma_ny1\Pwy1);
snr_x2=10*log10(sigma_nx2\Pwx2);
snr_y2=10*log10(sigma_ny2\Pwy2);
snr1=10*log10(sigma_n1\Pw1);
snr2=10*log10(sigma_n2\Pw2);
%将信号进行加噪,生成加噪后的模拟齐次坐标对[r_x,r_y,ones(size(r_x,2))]
% nx1=0;
% ny1=0;
% nx2=0;
% ny2=0;
% snr1=0;
% snr2=0;
r_x1=rr_x1+nx1;
r_y1=rr_y1+ny1;
r_x2=rr_x2+nx2;
r_y2=rr_y2+ny2;
%生成数字齐次坐标对[xd,yd,ones(size(xd，2))]
M1=K*[r_x1;r_y1;ones(1,n)];
xd1=M1(1,:);
yd1=M1(2,:);
zd1=M1(3,:);
M2=K*[r_x2;r_y2;ones(1,n)];
xd2=M2(1,:);
yd2=M2(2,:);
zd2=M2(3,:);
xd1=fix(xd1+0.5);
yd1=fix(yd1+0.5);
xd2=fix(xd2+0.5);
yd2=fix(yd2+0.5);
Md1=[xd1;yd1;zd1]';
Md2=[xd2;yd2;zd2]';
%以下部分程序用于调用正则化算法对数据进行处理,生成规一化的数字齐次坐标[xdn,ydn,ones(size(xdn))]
mxd1=n\sum(xd1);
myd1=n\sum(yd1);
aver_r1=[mxd1;myd1];
mxd2=n\sum(xd2);
myd2=n\sum(yd2);
aver_r2=[mxd2;myd2];
cen_r1=[xd1-mxd1;yd1-myd1];
cen_r2=[xd2-mxd2;yd2-myd2];
sigma_r1=zeros(2,2);
for i=1:n
    sigma_r1=sigma_r1+n\cen_r1(:,i)*cen_r1(:,i)';
end
[RR1,p1]=chol(sigma_r1);
w1=inv(RR1');
W1=[w1 -w1*aver_r1;zeros(1,2) 1];
sigma_r2=zeros(2,2);
for i=1:n
    sigma_r2=sigma_r2+n\cen_r2(:,i)*cen_r2(:,i)';
end
[RR2,p2]=chol(sigma_r2);
w2=inv(RR2');
W2=[w2 -w2*aver_r2;zeros(1,2) 1];
rr1=W1*[xd1;yd1;ones(size(xd1))]; 
rr2=W2*[xd2;yd2;ones(size(xd2))];
xdn1=rr1(1,:);
ydn1=rr1(2,:);
xdn2=rr2(1,:);
ydn2=rr2(2,:);
Mdn1=[xdn1;ydn1;zd1]';
Mdn2=[xdn2;ydn2;zd2]';
%以下程序使用svd奇异值分解技术,求解Fw,然后转化为F
for i=1:n
        Xn(i,:)=[xdn1(i)*xdn2(i) xdn2(i)*ydn1(i) xdn2(i) ydn2(i)*xdn1(i) ydn2(i)*ydn1(i) ydn2(i) xdn1(i) ydn1(i) 1];
end
[uu,ss,vv]=svd(Xn);
th=vv(:,9);
Fw=[th(1) th(4) th(7)   
    th(2) th(5) th(8)
    th(3) th(6) th(9)]';
F=W2'*Fw*W1;
[u,s,v]=svd(F*F');
e=v(:,3);
%该函数返回的所有参数，用于与计算值比较和后面的计算
% a{1,1}=E0;
a{1,2}=p0;
a{1,3}=e0;
% a{1,4}=ex0;
a{2,1}=H0;
% a{2,2}=F0;
% a{2,3}=c0;       
% a{2,4}=L;  
a{3,1}=Md1;      %Md1,Md2 为数字坐标对
a{3,2}=Md2;
% a{3,3}=uorig;
% a{3,4}=vorig;
a{4,1}=snr1;
a{4,2}=snr2;
a{4,3}=F;
a{4,4}=e;   
% a{5,1}=h;
a{5,2}=Mdn1;
a{5,3}=Mdn2;
a{5,4}=R;
a{6,1}=T;
a{6,2}=P1;         %对应的初始三维点对，模拟坐标值
a{6,3}=P2;

function [error_K,error_R,error_T,error_H,recover,Snr1,Snr2,elapsed_tH,tK,trecover,elapsed_tall]=subfun(alpha,beta,gamma,t11,t12,t13,t21,t22,t23);
%内参数阵理论值
tall=cputime;
u0=0;
v0=0;    
fu=1000;
fv=1000;
s=0.02;
K=[fu,s,u0;0,fv,v0;0,0,1];
a1=subfunction(alpha,beta,gamma,t11,t12,t13);
a2=subfunction(alpha,beta,gamma,t21,t22,t23);
%返回的数据中经计算F1./F01中的（3,2）元素相差较大，而其他的元素相差小，此现象在其他的F02./F2中不明显
% e01=a1{1,3};
% e02=a2{1,3};
e1=a1{4,4};
e2=a2{4,4};
F1=a1{4,3};
F2=a2{4,3};
% h1=a1{5,1};
% h2=a2{5,1};
H01=a1{2,1};
% H02=a2{2,1};
% F01=a1{2,2};
% F02=a2{2,2};
R1=a1{5,4};
% R2=a2{5,4};
T1=a1{6,1};
T2=a2{6,1};
Snr11=a1{4,1};
Snr12=a1{4,2};
Snr21=a2{4,1};
Snr22=a2{4,2};
M11=a1{6,2};
M12=a1{6,3};
M21=a2{6,2};
M22=a2{6,3};
Data11=a1{3,1}';
Data12=a1{3,2}';
Data21=a2{3,1}';
Data22=a2{3,2}';
Snr1=(Snr11+Snr21)/2;
Snr2=(Snr12+Snr22)/2;

ex1=[0 -e1(3) e1(2);e1(3) 0 -e1(1);-e1(2) e1(1) 0];
ex2=[0 -e2(3) e2(2);e2(3) 0 -e2(1);-e2(2) e2(1) 0];
k=-(e1'*F2*F2'*e1).\(F1'*e2)'*(F2'*e1);
G=inv(ex1'*ex1+ex2'*ex2)*(ex1'*F1+k*ex2'*F2);
b=det(G).\1;  
%求方程x.^3+b=0的实根
c=[1 0 0 -b];
result=roots(c);
for i=1:3
    if isreal(result(i))==true;
        j=i;
    end
end
cal_H=result(j)*G;
elapsed_tH=cputime-tall;
error_H=norm(cal_H-H01,'fro');
tK=cputime;
w1=zeros(3,1);
w2=zeros(3,1);
w3=zeros(3,1);
%用本征分解技术求解cal_H的特征值和特征矢量
[v,d]=eig(cal_H,'nobalance');
for m=1:3
    if isreal(d(m,m))==true;
        p=m;
        w1=v(:,p);
    else
        if imag(d(m,m))>0;
            q=m;
            w2=real(v(:,q));
            w3=imag(v(:,q));
        end
    end
end
%计算s^2
Wr=[w1 w2 w3];
Wr1=inv(Wr');
for i=1:3
    wr1=Wr1(:,1);
    wr2=Wr1(:,2);
    wr3=Wr1(:,3);
end
S=-e1'*(wr2*wr2'+wr3*wr3')*e2\(e1'*wr1*wr1'*e2);     %S=s^2
%计算cal_C阵
cal_C=S*w1*w1'+w2*w2'+w3*w3';
%判断cal_C是否正定,对cal_C进行svd分解，考察特征值的正负个数
[vc,dc]=eig(cal_C,'nobalance');
order=0;
for i=1:3
    if dc(i,i)<0
        order=order+1;
    end
end       
sta2=0;
if order==3
    sta2=1;
    cal_C=-cal_C;
else 
    if order==0
        sta2=1;
    else
    sta2=0;
    error('C is not a positive matrix')
    end
end
[RR,p]=chol(cal_C);
cal_K=RR./RR(3,3);
tK=cputime-tall;
error_K=norm(cal_K-K,'fro');
%估计R,T1,T2
cal_R=inv(cal_K)*cal_H*cal_K;
error_R=norm(cal_R-R1,'fro');
%P1=a1*T1，P2=a2*T2 ，a1,a2用以上的算法无法得出，所以复原的平移矢量只能得到其方向
cal_P1=inv(cal_K)*e1;     
cal_P2=inv(cal_K)*e2;     
cal_T1=cal_P1/norm(cal_P1);
cal_T2=cal_P2/norm(cal_P2);
%下面要判断平移矢量的方向，分别用+/－T进行三维复原，统计得到点对第三维是正的个数，较多的是真值
san=cputime;
r11=inv(cal_K)*Data11;
r12=inv(cal_K)*Data12;
r21=inv(cal_K)*Data21;
r22=inv(cal_K)*Data22;
%以下是用函数recover进行的三维复原
[R11,R12,count1,count2]=recover(r11,r12,cal_R,cal_T1);
[R21,R22,count3,count4]=recover(r11,r12,cal_R,-cal_T1);
[R31,R32,count5,count6]=recover(r21,r22,cal_R,cal_T2);
[R41,R42,count7,count8]=recover(r21,r22,cal_R,-cal_T2);
%根据以上统计，判断T的方向
if count1==0 & count2==0 & count3~=0 & count4~=0
    cal_T1=-cal_T1;
    Point11=R21;
    Point12=R22;
else
    Point11=R11;
    Point12=R12;
end
if count5==0 & count6==0 & count7~=0 & count8~=0
    cal_T2=-cal_T2;
    Point21=R41;
    Point22=R42;
else
    Point21=R31;
    Point22=R32;
end
error_T1=norm(cal_T1-T1,'fro');
error_T2=norm(cal_T2-T2,'fro');
error_T=(error_T1+error_T2)/2;
%三维复原误差,因为上述方法没有去除加在数字像点上的噪声，因此复原后z的误差不大，但是x,y的误差较大，即应考虑进行数字滤波
%可以从试验中得出以下的复原误差经运动后的点对复原误差较大，因为有多个误差传递以及未滤波的影响
recover1=norm(Point11-M11,'fro');
recover2=norm(Point12-M12,'fro');
recover3=norm(Point21-M21,'fro');
recover4=norm(Point22-M22,'fro');
recover=(recover1+recover2+recover3+recover4)/4;
trecover=cputime-san;
elapsed_tall=cputime-tall;
%下面的函数进行三维复原，并判断平移矢量的方向
%参数说明，输入参数：由数字点坐标和复原出的R、T计算出的模拟齐次坐标以及变形，利用方程r2=R*r1+T,此处输入为r2和R*r1
%输出参数：复原的三维坐标，以及对第三维坐标正值的统计数
function [Point1,Point2,count1,count2]=recover(r1,r2,R,T);
[p,q]=size(r1);
count1=0;
count2=0;
rr1=R*r1;
j=1;
for i=1:q
    X(j:j+2,1)=T;
    X(j:j+2,2*i)=rr1(:,i);
    X(j:j+2,2*i+1)=-r2(:,i);
    j=j+3;
end
[u,s,v]=svd(X);
th=v(:,2*q+1);
th1=th(2:2*q+1)/th(1);
for i=1:q
    z1(1,i)=th1(2*i-1);
    if z1(1,i)>0
        count1=count1+1;
    end
    z2(1,i)=th1(2*i);
    if z2(1,i)>0
        count2=count2+1;
    end
end
Point1=[r1(1,:).*z1;r1(2,:).*z1;z1];
Point2=[r2(1,:).*z2;r2(2,:).*z2;z2];