标题: 张正友Matlab相机标定完整版原代码，亲测可用 [打印本页]
作者: zhaotk    时间: 2018-11-19 16:58
标题: 张正友Matlab相机标定完整版原代码，亲测可用
张正友的Matlab相机标定的完整版原代码，亲测可行！！！


Matlab源程序如下:
  1. % function Zhang(M,m)
  2. %
  3. % ***********************************************************************************
  4. % *******          A Flexible New Technique for Camera Calibration            *******
  5. % ***********************************************************************************
  6. %
  7. % Note:    M:2*N  m:2*N
  8. % M        point on the model plane, when using M=[X,Y]' ---> M=[X,Y,1]'
  9. % m        M's image, when using                m=[u,v]' ---> m=[u,v,1]' , so that
  10. %          s*m = H*M , with H=A*[r1,r2,t];                  (2)
  11. % H        homography matrix
  12. %
  13. % REF:           "A Flexible New Technique for Camera Calibration"
  14. %           - Zhengyou Zhang
  15. %           - Microsoft Research
  16. %
  17. function Zhang(M,m)

  19. %  M=[X,Y]' ---> M=[X,Y,1]'  ;   m=[u,v]' ---> m=[u,v,1]'
  20.     [rows,npts]=size(M);
  21.     matrixone=ones(1,npts);
  22.     M=[M;matrixone];
  23.     num=size(m,3);
  24.     for i=1:num
  25.         m(3,:,i)=matrixone;
  26.     end
  27. % Estimate the H
  28.     for i=1:num
  29.         H(:,:,i)=homography2d1(M,m(:,:,i))';
  30.     end
  31. % solve the intrinsic parameters matrix A
  32. % A=[alpha_u skewness u0
  33. %    0       alpha_v  v0
  34. %    0       0        1]
  35. % see Appendix B "Extraction of the Intrisic Parameters from Matrix B", P18
  36.     V=[];
  37.     for flag=1:num
  38.         v12(:,:,flag)=[H(1,1,flag)*H(2,1,flag), H(1,1,flag)*H(2,2,flag)+H(1,2,flag)*H(2,1,flag), H(1,2,flag)*H(2,2,flag), H(1,3,flag)*H(2,1,flag)+H(1,1,flag)*H(2,3,flag), H(1,3,flag)*H(2,2,flag)+H(1,2,flag)*H(2,3,flag), H(1,3,flag)*H(2,3,flag)];
  39.         v11(:,:,flag)=[H(1,1,flag)*H(1,1,flag), H(1,1,flag)*H(1,2,flag)+H(1,2,flag)*H(1,1,flag), H(1,2,flag)*H(1,2,flag), H(1,3,flag)*H(1,1,flag)+H(1,1,flag)*H(1,3,flag), H(1,3,flag)*H(1,2,flag)+H(1,2,flag)*H(1,3,flag), H(1,3,flag)*H(1,3,flag)];
  40.         v22(:,:,flag)=[H(2,1,flag)*H(2,1,flag), H(2,1,flag)*H(2,2,flag)+H(2,2,flag)*H(2,1,flag), H(2,2,flag)*H(2,2,flag), H(2,3,flag)*H(2,1,flag)+H(2,1,flag)*H(2,3,flag), H(2,3,flag)*H(2,2,flag)+H(2,2,flag)*H(2,3,flag), H(2,3,flag)*H(2,3,flag)];
  41.         V=[V;v12(:,:,flag);v11(:,:,flag)-v22(:,:,flag)];
  42.     end
  43.     k=V'*V;      
  44.     [u,v,d]=svd(k);
  45.     [e,d2]=eig(k);
  46.     b=d(:,6);
  47.     v0=(b(2)*b(4)-b(1)*b(5))/(b(1)*b(3)-b(2)^2);
  48.     s=b(6)-(b(4)^2+v0*(b(2)*b(4)-b(1)*b(5)))/b(1);
  49.     alpha_u=sqrt(s/b(1));
  50.     alpha_v=sqrt(s*b(1)/(b(1)*b(3)-b(2)^2));
  51.     skewness=-b(2)*alpha_u*alpha_u*alpha_v/s;
  52.     u0=skewness*v0/alpha_u-b(4)*alpha_u*alpha_u/s;
  53.     A=[alpha_u skewness u0
  54.         0      alpha_v  v0
  55.         0      0        1];
  56. % solve k1 k1 and all the extrisic parameters, P6
  57.     D=[];
  58.     d=[];
  59.     Rm=[];
  60.     for flag=1:num
  61.         s=(1/norm(inv(A)*H(1,:,flag)')+1/norm(inv(A)*H(2,:,flag)'))/2;
  62.         rl1=s*inv(A)*H(1,:,flag)';
  63.         rl2=s*inv(A)*H(2,:,flag)';
  64.         rl3=cross(rl1,rl2);
  65.         RL=[rl1,rl2,rl3];
  66.         %%%%%%%%%%%%%%%%%%%%
  67.         % see Appendix C "Approximating a 3*3 matrix by a Rotation Matrix", P19
  68.         [U,S,V] = svd(RL);
  69.         RL=U*V';
  70.         %%%%%%%%%%%%%%%%%%%%
  71.         TL=s*inv(A)*H(3,:,flag)';
  72.         RT=[rl1,rl2,TL];
  73.         XY=RT*M;
  74.         UV=A*XY;
  75.         UV=[UV(1,:)./UV(3,:); UV(2,:)./UV(3,:); UV(3,:)./UV(3,:)];
  76.         XY=[XY(1,:)./XY(3,:); XY(2,:)./XY(3,:); XY(3,:)./XY(3,:)];
  77.         for j=1:npts
  78.             D=[D; ((UV(1,j)-u0)*( (XY(1,j))^2 + (XY(2,j))^2 )) , ((UV(1,j)-u0)*( (XY(1,j))^2 + (XY(2,j))^2 )^2) ; ((UV(2,j)-v0)*( (XY(1,j))^2 + (XY(2,j))^2 )) , ((UV(2,j)-v0)*( (XY(1,j))^2 + (XY(2,j))^2 )^2) ];
  79.             d=[d; (m(1,j,flag)-UV(1,j)) ; (m(2,j,flag)-UV(2,j))];
  80.         end
  81.         r13=RL(1,3);
  82.         r12=RL(1,2);
  83.         r23=RL(2,3);
  84.         Q1=-asin(r13);
  85.         Q2=asin(r12/cos(Q1));
  86.         Q3=asin(r23/cos(Q1));
  87.         [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)];
  88.         R_new=[Q1,Q2,Q3,TL'];
  89.         Rm=[Rm , R_new];
  90.     end
  91. % using function (13), P8
  92.     k=inv(D'*D)*D'*d;
  93. % Complete Maximun Likelihood Estimation, using function (14), P8
  94.     para=[Rm,k(1),k(2),alpha_u,skewness,u0,alpha_v,v0];
  95.     options = optimset('LargeScale','off','LevenbergMarquardt','on');
  96.     [x,resnorm,residual,exitflag,output]  = lsqnonlin( @simon_HHH, para, [],[],options, m, M);
  97. % display the result
  98.     k1=x(num*6+1)
  99.     k2=x(num*6+2)
  100.     A=[x(num*6+3) x(num*6+4) x(num*6+5); 0 x(num*6+6) x(num*6+7); 0,0,1]
复制代码

所有资料51hei提供下载:
zhang's+calibration.zip (7.25 MB, 下载次数: 30)


作者: Wangqq    时间: 2019-4-25 14:19
请问我运行为啥报错。。。

出错 homography2d1 (line 78)
    options = optimset('LargeScale','off','LevenbergMarquardt','on');

出错 Zhang (line 31)
        H(:,:,i)=homography2d1(M,m(:,:,i))';

出错 test (line 28)
Zhang(M,m)
作者: chuanzz    时间: 2022-3-19 20:12
Wangqq 发表于 2019-4-25 14:19
请问我运行为啥报错。。。

出错 homography2d1 (line 78)

我也是这个问题，请问解决了吗？



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1