simon_hhh.m

目前常用的张正友的摄像机标定方法

%
% ***********************************************************************************
% *******          A Flexible New Technique for Camera Calibration            *******
% ***********************************************************************************
%                            7/2004    Simon Wan
%                            [e-mail]
%
% REF:	   "A Flexible New Technique for Camera Calibration"
%           - Zhengyou Zhang 
%           - Microsoft Research 
%
function f = simon_HHH(params, m, M)
% unpack the params
  num=size(m,3);
  R=[];
  for i=1:num
      R_new=params( [(i-1)*6+1 : (i-1)*6+6] );
      Q1=R_new(1);
      Q2=R_new(2);
      Q3=R_new(3);
      TL=R_new([4:6])';
      RL=[cos(Q2)*cos(Q1)   sin(Q2)*cos(Q1)   -sin(Q1) ; -sin(Q2)*cos(Q3)+cos(Q2)*sin(Q1)*sin(Q3)    cos(Q2)*cos(Q3)+sin(Q2)*sin(Q1)*sin(Q3)  cos(Q1)*sin(Q3) ; sin(Q2)*sin(Q3)+cos(Q2)*sin(Q1)*cos(Q3)    -cos(Q2)*sin(Q3)+sin(Q2)*sin(Q1)*cos(Q3)  cos(Q1)*cos(Q3)];
      RT=[RL(:,1:2) , TL];
      R=[R;RT];
  end
  k1=params(num*6+1);
  k2=params(num*6+2);
  k3=params(num*6+3);%
  k4=params(num*6+4);%
%   p1=params(num*6+5);%
%   p2=params(num*6+6);%
%   s1=params(num*6+7);%
%   s2=params(num*6+8);%
  %A=[params(num*6+3) params(num*6+4) params(num*6+5); 0 params(num*6+6) params(num*6+7); 0,0,1];%
  %A=[params(num*6+6) params(num*6+7) params(num*6+8); 0 params(num*6+9) params(num*6+10); 0,0,1];%
  A=[params(num*6+5) params(num*6+6) params(num*6+7); 0 params(num*6+8) params(num*6+9); 0,0,1];
  %A=[params(num*6+9) params(num*6+10) params(num*6+11); 0 params(num*6+12) params(num*6+13); 0,0,1];
  u0=A(1,3);
  v0=A(2,3);
D=[];
d=[];
npts=size(m,2);
for flag=1:num
    RT=R([(flag-1)*3+1 : (flag-1)*3+3],:);
    XY=RT*M;
    UV=A*XY;
    UV=[UV(1,:)./UV(3,:); UV(2,:)./UV(3,:); UV(3,:)./UV(3,:)];
    XY=[XY(1,:)./XY(3,:); XY(2,:)./XY(3,:); XY(3,:)./XY(3,:)];
    %k1,k2
   % D=[D; UV(1,:)+((UV(1,:)-u0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ))*k1 + ((UV(1,:)-u0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^2)*k2 ; UV(2,:) + ((UV(2,:)-v0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ))*k1 + ((UV(2,:)-v0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^2)*k2 ];
   %这个畸变方程误差比较大
   % D=[D; UV(1,:)+((UV(1,:)-u0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ))*k1 + ((UV(1,:)-u0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ).^2)*k2 ; 
   %       UV(2,:)+((UV(2,:)-v0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ))*k1 + ((UV(2,:)-v0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ).^2)*k2 ];
   
    %D=[D;UV(1,:)+((UV(1,:)-u0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ))*k1 + ((UV(1,:)-u0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ).^2)*k2 ;
   %      UV(2,:)+((UV(2,:)-v0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ))*k3 + ((UV(2,:)-v0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ).^2)*k4 ];
   %D1=UV(1,:)+((UV(1,:)-u0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ))*k1 + ((UV(1,:)-u0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ).^2)*k2 +p1*(3*(UV(1,:)-u0).^2+(UV(2,:)-v0).^2)+2*p2*(UV(1,:)-u0).*(UV(2,:)-v0)+s1*((UV(1,:)-u0).^2+(UV(2,:)-v0).^2);
   %D2=UV(2,:)+((UV(2,:)-v0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ))*k3 + ((UV(2,:)-v0).*( (UV(1,:)-u0).^2 + (UV(2,:)-v0).^2 ).^2)*k4 +p2*(3*(UV(1,:)-u0).^2+(UV(2,:)-v0).^2)+2*p1*(UV(1,:)-u0).*(UV(2,:)-v0)+s2*((UV(1,:)-u0).^2+(UV(2,:)-v0).^2);
   %D=[D;D1;D2];
   
   %k1,k2,k3,k4,k5
   %D1=UV(1,:)+((UV(1,:)-u0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ))*k1 + ((UV(1,:)-u0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^2)*k2+((UV(1,:)-u0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^3)*k5+k3*2*XY(1,:).*XY(2,:)+k4*(3*(XY(1,:)).^2 +(XY(2,:)).^2 );
   %D2=UV(2,:)+((UV(2,:)-v0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ))*k1 + ((UV(2,:)-v0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^2)*k2+((UV(2,:)-v0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^3)*k5+k4*2*XY(1,:).*XY(2,:)+k3*(3*(XY(2,:)).^2 +(XY(1,:)).^2 );
  
   %k1,k2,k3,k4
   %%-----注意-----注意-----注意-----注意-----注意-----注意-----注意-----注意
   D1=UV(1,:)+((UV(1,:)-u0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ))*k1 + ((UV(1,:)-u0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^2)*k2+k3*2*XY(1,:).*XY(2,:)*A(1,1)+k4*(3*(XY(1,:)).^2 +(XY(2,:)).^2 *A(1,1));%530 try 
   D2=UV(2,:)+((UV(2,:)-v0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ))*k1 + ((UV(2,:)-v0).*( (XY(1,:)).^2 + (XY(2,:)).^2 ).^2)*k2+k4*2*XY(1,:).*XY(2,:)*A(2,2)+k3*(3*(XY(2,:)).^2 +(XY(1,:)).^2*A(2,2) );%530 try
   D=[D;D1;D2];
   d=[d; m(1,:,flag); m(2,:,flag)];
end
f=d-D;