%攝像機(jī)標(biāo)定新算法,用一個子函數(shù)計算出兩組運(yùn)動組的參數(shù)
%本程序用于調(diào)試攝像機(jī)標(biāo)定新算法
%這個程序在原來的基礎(chǔ)上改變了部分返回值，將標(biāo)準(zhǔn)離差改為均方值的計算
%加入標(biāo)定整體誤差計算，運(yùn)動復(fù)原和三維復(fù)原整體誤差計算
%應(yīng)先用計算出的平移矢量進(jìn)行三維復(fù)原，從得出的點(diǎn)對統(tǒng)計其z方向?yàn)檎膫€數(shù)，多者為正確的平移矢量
%已完成
%程序太復(fù)雜，簡化，將輸出參數(shù)只考慮整體復(fù)原誤差,這樣的運(yùn)行時間與不簡化時差不多，需要進(jìn)一步簡化程序

function [ans]=newall4();
%以下為本算法的輸入?yún)?shù),運(yùn)動組的旋轉(zhuǎn)陣和平移陣R,T1,T2
alpha=pi/6;
beta=pi/4;
gamma=pi/3;
% T11=[10 4 5],T12=[4 -12 1.6],T11*T12'=0，自己設(shè)計的正交數(shù)據(jù)
t11=0.842152;
t12=0.336861;
t13=0.421076;
t21=0.313728;
t22=-0.941184;
t23=0.125491;
%攝像機(jī)內(nèi)參數(shù)理論值
fu=1000;
fv=1000;
u0=0;
v0=0;
s=0.02;
%以下對計算K陣的子函數(shù)調(diào)用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
%以下部分程序用于返回各參數(shù)的誤差，其中每兩者為一組，前者表示絕對誤差均值，后者表示均方根誤差

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

%以下部分為子函數(shù)對不同運(yùn)動組，分別求相應(yīng)的e,F，返回元胞數(shù)組
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];
%計算理論值旋轉(zhuǎn)陣R,平移陣T,本征陣E0,極點(diǎn)p,規(guī)一化極點(diǎn)e0,e0的反對稱陣ex0,單應(yīng)性陣H0,基礎(chǔ)陣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;
%規(guī)一化的極點(diǎn)
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;
%以下部分程序用于生成原始以及映射后的匹配點(diǎn)對，并計算相應(yīng)的信號功率,由于數(shù)據(jù)的條件數(shù)太大
%在進(jìn)行SVD之前，對數(shù)據(jù)進(jìn)行正則化處理
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);
%生成映射前后的齊次坐標(biāo)對[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);
%以下部分程序用于形成噪化數(shù)據(jù)，其中A代表噪聲強(qiáng)度,并計算相應(yīng)的信噪比,A的值在0.0002和0.008之間變化，間距0.0002,每次改變參數(shù)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);
%將信號進(jìn)行加噪,生成加噪后的模擬齊次坐標(biāo)對[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;
%生成數(shù)字齊次坐標(biāo)對[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]';
%以下部分程序用于調(diào)用正則化算法對數(shù)據(jù)進(jìn)行處理,生成規(guī)一化的數(shù)字齊次坐標(biāo)[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奇異值分解技術(shù),求解Fw,然后轉(zhuǎn)化為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);
%該函數(shù)返回的所有參數(shù)，用于與計算值比較和后面的計算
% 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 為數(shù)字坐標(biāo)對
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;         %對應(yīng)的初始三維點(diǎn)對，模擬坐標(biāo)值
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);
%內(nèi)參數(shù)陣?yán)碚撝?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);
%返回的數(shù)據(jù)中經(jīng)計算F1./F01中的（3,2）元素相差較大，而其他的元素相差小，此現(xiàn)象在其他的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的實(shí)根
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);
%用本征分解技術(shù)求解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進(jìn)行svd分解，考察特征值的正負(fù)個數(shù)
[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用以上的算法無法得出，所以復(fù)原的平移矢量只能得到其方向
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進(jìn)行三維復(fù)原，統(tǒng)計得到點(diǎn)對第三維是正的個數(shù)，較多的是真值
san=cputime;
r11=inv(cal_K)*Data11;
r12=inv(cal_K)*Data12;
r21=inv(cal_K)*Data21;
r22=inv(cal_K)*Data22;
%以下是用函數(shù)recover進(jìn)行的三維復(fù)原
[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);
%根據(jù)以上統(tǒng)計，判斷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;
%三維復(fù)原誤差,因?yàn)樯鲜龇椒]有去除加在數(shù)字像點(diǎn)上的噪聲，因此復(fù)原后z的誤差不大，但是x,y的誤差較大，即應(yīng)考慮進(jìn)行數(shù)字濾波
%可以從試驗(yàn)中得出以下的復(fù)原誤差經(jīng)運(yùn)動后的點(diǎn)對復(fù)原誤差較大，因?yàn)橛卸鄠€誤差傳遞以及未濾波的影響
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;
%下面的函數(shù)進(jìn)行三維復(fù)原，并判斷平移矢量的方向
%參數(shù)說明，輸入?yún)?shù)：由數(shù)字點(diǎn)坐標(biāo)和復(fù)原出的R、T計算出的模擬齊次坐標(biāo)以及變形，利用方程r2=R*r1+T,此處輸入為r2和R*r1
%輸出參數(shù)：復(fù)原的三維坐標(biāo)，以及對第三維坐標(biāo)正值的統(tǒng)計數(shù)
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];