computew.asv

摄像机圆球标定程序（包括SDP,DSDP,IDC,DIDC,ALG,DALG六种算法的实现）

function [w A pp]=ComputeW(C,vbar)
s12=compute_sij(C{1},C{2});
s13=compute_sij(C{1},C{3});
C{2}=s12*C{2};
C{3}=s13*C{3};
%A*p=0;
for i=1:3
    Ci=C{i};
    vi=vbar{i};
    a{i}=Ci(1,1);
    b{i}=Ci(1,2);
    c{i}=Ci(2,2);
    d{i}=Ci(1,3);
    e{i}=Ci(2,3);
    f{i}=Ci(3,3);
    x{i}=vi(1);
    y{i}=vi(2);
    z{i}=vi(3);
end

A=[a{1};b{1};c{1};d{1};e{1};f{1};zeros(6,1)];
A=[
    A [x{1}*x{1}; x{1}*y{1}; y{1}*y{1};x{1}*z{1};y{1}*z{1};z{1}*z{1};zeros(6,1)]
   ];
A=[
    A [-[a{2};b{2};c{2};d{2};e{2};f{2}];a{2};b{2};c{2};d{2};e{2};f{2}]
   ];
A=[
    A [
        -[x{2}*x{2}; x{2}*y{2}; y{2}*y{2};x{2}*z{2};y{2}*z{2};z{2}*z{2}];x{2}*x{2}; x{2}*y{2}; y{2}*y{2};x{2}*z{2};y{2}*z{2};z{2}*z{2}
       ]
   ];
A=[A [zeros(6,1);-[a{3};b{3};c{3};d{3};e{3};f{3}]]];
A=[A [zeros(6,1);-[x{3}*x{3}; x{3}*y{3}; y{3}*y{3};x{3}*z{3};y{3}*z{3};z{3}*z{3}]]];

[U S V]=svd(A);
pp=V(:,6);
p1=pp(1);
q1=pp(2);
p2=pp(3);
q2=pp(4);
p3=pp(5);
q3=pp(6);
w=(1/3)*(p1*C{1}+q1*vbar{1}*(vbar{1}')+p2*C{2}+q2*vbar{2}*(vbar{2}')+p3*C{3}+q3*vbar{3}*(vbar{3}'));
w=w/w(3,3);