双目视觉相机标定

%选择表格中离散程度较大的六个点进行直接线性变换，得到M矩阵，并反向求解图像坐标，与给定图像坐标比较，并分解M矩阵，得到内外方位参数。 clc clear all; % 清除所有变量 format long g WC = xlsread("E:..\**.xlsx");%加载世界坐标系 IC = load("E:..\**.txt");%加载图像坐标1 d=1:16:92; X= WC(d,2); Y= WC(d,3); Z= WC(d,4); u= IC(d,2); v= IC(d,3); a=size(d); U=[u(1,1);v(1,1);u(2,1);v(2,1);u(3,1);v(3,1);u(4,1);v(4,1);u(5,1);v(5,1);u(6,1);v(6,1);]; for i=1:6 K1(i,:)=[X(i,1),Y(i,1),Z(i,1),1,0,0,0,0,-u(i,1)*X(i,1),-u(i,1)*Y(i,1),-u(i,1)*Z(i,1)]; K2(i,:)=[0,0,0,0,X(i,1),Y(i,1),Z(i,1),1,-v(i,1)*X(i,1),-v(i,1)*Y(i,1),-v(i,1)*Z(i,1)]; end K=zeros(12,11); K(1:2:end,:)=K1; K(2:2:end,:)=K2; m=inv(K'*K)*K'*U m=[m;1]; M=reshape(m,4,3); M=M' % %反向求解图像坐标 %给定图像坐标 RIC=[u,v] for i=1:6 s(i)=M(3,:)*[X(i,1);Y(i,1);Z(i,1);1]; su(i)=M(1,:)*[X(i,1);Y(i,1);Z(i,1);1]; sv(i)=M(2,:)*[X(i,1);Y(i,1);Z(i,1);1]; u(i)=su(i)/s(i); v(i)=sv(i)/s(i); end IC=[u,v] % 计算误差 for i=1:6 dist(i)=sqrt((RIC(i,1)-IC(i,1))^2+((RIC(i,2)-IC(i,2))^2)); end error=dist' %%计算内外参数。 m1=m(1:3,1); m2=m(5:7,1); m3=m(9:11,1); m14=m(4,1); m24=m(8,1); m34=1/norm(m3); tz=m34; r3=m34.*m3 u0=m34.^2*m1'*m3; v0=m34.^2*m2'*m3; ax=m34.^2.*norm(cross(m1,m2)); ay=m34.^2.*norm(cross(m1,m2)); r1=(m34.*(m1-u0.*m3))./ax r2=(m34.*(m2-v0.*m3))./ay tx=(m34.*(m14'-u0))./ax ty=(m34.*(m24'-v0))./ay