computew_sdp.m

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

function [w]=ComputeW_SDP(Cstar,vbar)

Cstar1=Cstar{1};
Cstar2=Cstar{2};
Cstar3=Cstar{3};

V1=vbar{1}*vbar{1}';
V2=vbar{2}*vbar{2}';
V3=vbar{3}*vbar{3}';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%SDP计算,使用的SDPT3-4.0
n=6;

%
%[s1*I     A(x)]
%[             ] >=0    
%[A(x)'    s1*I]
%
blk{1,1}='s';blk{1,2}=n;
AA{1}=-eye(6,6);
AA{2}=zeros(6,6);
AA{3}=zeros(6,6);
AA{4}=[zeros(3,3) -V1;-V1' zeros(3,3)];
AA{5}=[zeros(3,3) V2;V2' zeros(3,3)];
AA{6}=zeros(6,6);
Atemp=svec(blk(1,:),AA);
At{1,1}=Atemp{1};
C{1,1}=[zeros(3,3) Cstar1-Cstar2;Cstar1'-Cstar2' zeros(3,3)];
%
%[s2*I     B(x)]
%[             ] >=0    
%[B(x)'    s2*I]
%
blk{2,1}='s';blk{2,2}=n;
AA{1}=zeros(6,6);
AA{2}=-eye(6,6);
AA{3}=zeros(6,6);
AA{4}=[zeros(3,3) -V1;-V1' zeros(3,3)];
AA{5}=zeros(6,6);
AA{6}=[zeros(3,3) V3;V3' zeros(3,3)];
Atemp=svec(blk(2,:),AA);
At{2,1}=Atemp{1};
C{2,1}=[zeros(3,3) Cstar1-Cstar3;Cstar1'-Cstar3' zeros(3,3)];
%
%[s3*I     C(x)]
%[             ] >=0    
%[C(x)'    s3*I]
%
blk{3,1}='s';blk{3,2}=n;
AA{1}=zeros(6,6);
AA{2}=zeros(6,6);
AA{3}=-eye(6,6);
AA{4}=zeros(6,6);
AA{5}=[zeros(3,3) -V2;-V2' zeros(3,3)];
AA{6}=[zeros(3,3) V3;V3' zeros(3,3)];
Atemp=svec(blk(3,:),AA);
At{3,1}=Atemp{1};
C{3,1}=[zeros(3,3) Cstar2-Cstar3;Cstar2'-Cstar3' zeros(3,3)];
%
%Cstar1+d1*V1>=0
%
n=3;
blk{4,1}='s';blk{4,2}=n;
AA{1}=zeros(3,3);
AA{2}=zeros(3,3);
AA{3}=zeros(3,3);
AA{4}=-V1;
AA{5}=zeros(3,3);
AA{6}=zeros(3,3);
Atemp=svec(blk(4,:),AA);
At{4,1}=Atemp{1};
C{4,1}=Cstar1;
%
%Cstar2+d2*V2>=0
%
blk{5,1}='s';blk{5,2}=n;
AA{1}=zeros(3,3);
AA{2}=zeros(3,3);
AA{3}=zeros(3,3);
AA{4}=zeros(3,3);
AA{5}=-V2;
AA{6}=zeros(3,3);
Atemp=svec(blk(5,:),AA);
At{5,1}=Atemp{1};
C{5,1}=Cstar2;
%
%Cstar3+d3*V3>=0
%
blk{6,1}='s';blk{6,2}=n;
AA{1}=zeros(3,3);
AA{2}=zeros(3,3);
AA{3}=zeros(3,3);
AA{4}=zeros(3,3);
AA{5}=zeros(3,3);
AA{6}=-V3;
Atemp=svec(blk(6,:),AA);
At{6,1}=Atemp{1};
C{6,1}=Cstar3;
%
%diag(d1,d2,d3)>=0
%
blk{7,1}='s';blk{7,2}=n;
AA{1}=zeros(3,3);
AA{2}=zeros(3,3);
AA{3}=zeros(3,3);
AA{4}=-[1 0 0;0 0 0;0 0 0];
AA{5}=-[0 0 0;0 1 0;0 0 0];
AA{6}=-[0 0 0;0 0 0;0 0 1];
Atemp=svec(blk(7,:),AA);
At{7,1}=Atemp{1};
C{7,1}=zeros(3,3);

b=-[1 1 1 0 0 0];

[obj,X,y,Z] = sqlp(blk,At,C,b);


%A*A'
format long
w=(Cstar1+y(4)*V1+Cstar2+y(5)*V2+Cstar3+y(6)*V3)/3;