computew_d.asv

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

function [w A p]=ComputeW_D(C)

s12=compute_sij(C{1},C{2})
s13=compute_sij(C{1},C{3})

for i=1:3
    Ci=C{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);
end

    a1=a{1};
    b1=b{1};
    c1=c{1};
    d1=d{1};
    e1=e{1};
    f1=f{1};

    a2=a{2};
    b2=b{2};
    c2=c{2};
    d2=d{2};
    e2=e{2};
    f2=f{2};

    a3=a{3};
    b3=b{3};
    c3=c{3};
    d3=d{3};
    e3=e{3};
    f3=f{3};

    A = [0  0  -f1+s12*f2  0  2*e1-2*s12*e2   -c1+s12*c2   c1*f1-e1*e1-s12*s12*c2*f2+s12*s12*e2*e2 ; 
         -f1+s12*f2  0  0  2*d1-2*s12*d2  0  -a1+s12*a2  a1*f1-d1*d1-s12*s12*a2*f2+s12*s12*d2*d2;
         -c1+s12*c2  2*b1-2*s12*b2  -a1+s12*a2  0  0  0  a1*c1-b1*b1-s12*s12*a2*c2+s12*s12*b2*b2;
         0  -f1+s12*f2  0  e1-s12*e2  d1-s12*d2  -b1+s12*b2  b1*f1-d1*e1-s12*s12*b2*f2+s12*s12*d2*e2;
         0  -e1+s12*e2  d1-s12*d2  c1-s12*c2  -b1+s12*b2  0  b1*e1-c1*d1-s12*s12*b2*e2+s12*s12*c2*d2;
         -e1+s12*e2  d1-s12*d2  0  b1-s12*b2  -a1+s12*a2  0  a1*e1-b1*d1-s12*s12*a2*e2+s12*s12*b2*d2;
         0  0  -f1+s13*f3  0  2*e1-2*s13*e3  -c1+s13*c3  c1*f1-e1*e1-s13*s13*c3*f3+s13*s13*e3*e3;
         -f1+s13*f3  0  0  2*d1-2*s13*d3  0  -a1+s13*a3  a1*f1-d1*d1-s13*s13*a3*f3+s13*s13*d3*d3;
         -c1+s13*c3  2*b1-2*s13*b3  -a1+s13*a3  0  0  0  a1*c1-b1*b1-s13*s13*a3*c3+s13*s13*b3*b3;
         0  -f1+s13*f3  0  e1-s13*e3  d1-s13*d3   -b1+s13*b3   b1*f1-d1*e1-s13*s13*b3*f3+s13*s13*d3*e3;
         0  -e1+s13*e3  d1-s13*d3  c1-s13*c3  -b1+s13*b3 0   b1*e1-c1*d1-s13*s13*b3*e3+s13*s13*c3*d3;
         -e1+s13*e3  d1-s13*d3  0  b1-s13*b3  -a1+s13*a3  0  a1*e1-b1*d1-s13*s13*a3*e3+s13*s13*b3*d3];

    [U S V]=svd(A);
    p=V(:,7);
    w=[p(1) p(2) p(4);p(2) p(3) p(5);p(4) p(5) p(6)];
    w=w/w(3,3);