%you摄像机标定用数据
format long;
u1=123;
v1=62;
Xw1=0;
Yw1=0;
Zw1=34;
u2=288;
v2=67;
Xw2=40;
Yw2=0;
Zw2=26;
u3=443
v3=73;
Xw3=80;
Yw3=0;
Zw3=18;
u4=606;
v4=73;
Xw4=120;
Yw4=0;
Zw4=26;
u5=124;
v5=224;
Xw5=0;
Yw5=40;
Zw5=34;
u6=286;
v6=228;
Xw6=40;
Yw6=40;
Zw6=30;
u7=445;
v7=231;
Xw7=80;
Yw7=40;
Zw7=22;
u8=593;
v8=233;
Xw8=120;
Yw8=40;
Zw8=10;
u9=121;
v9=390;
Xw9=0;
Yw9=80;
Zw9=26;
u10=287;
v10=391;
Xw10=40;
Yw10=80;
Zw10=30;
u11=443;
v11=388;
Xw11=80;
Yw11=80;
Zw11=22;
u12=591;
v12=387;
Xw12=120;
Yw12=80;
Zw12=10;
u13=123;
v13=541;
Xw13=0;
Yw13=120;
Zw13=14;
u14=281;
v14=541;
Xw14=40;
Yw14=120;
Zw14=14;
u15=443;
v15=546;
Xw15=80;
Yw15=120;
Zw15=26;
Xw16=80;
Yw16=120;
Zw16=26;
A=[Xw1, Yw1, Zw1, 1, 0, 0, 0, 0, -u1*Xw1, -u1*Yw1, -u1*Zw1;
0, 0, 0, 0, Xw1, Yw1, Zw1, 1, -v1*Xw1, -v1*Yw1, -v1*Zw1;
Xw2, Yw2, Zw2, 1, 0, 0, 0, 0, -u2*Xw2, -u2*Yw2, -u2*Zw2;
0, 0, 0, 0, Xw2, Yw2, Zw2, 1, -v2*Xw2, -v2*Yw2, -v2*Zw2;
Xw3, Yw3, Zw3, 1, 0, 0, 0, 0, -u3*Xw3, -u3*Yw3, -u3*Zw3;
0, 0, 0, 0, Xw3, Yw3, Zw3, 1, -v3*Xw3, -v3*Yw3, -v3*Zw3;
Xw4, Yw4, Zw4, 1, 0, 0, 0, 0, -u4*Xw4, -u4*Yw4, -u4*Zw4;
0, 0, 0, 0, Xw4, Yw4, Zw4, 1, -v4*Xw4, -v4*Yw4, -v4*Zw4;
Xw5, Yw5, Zw5, 1, 0, 0, 0, 0, -u5*Xw5, -u5*Yw5, -u5*Zw5;
0, 0, 0, 0, Xw5, Yw5, Zw5, 1, -v5*Xw5, -v5*Yw5, -v5*Zw5;
Xw6, Yw6, Zw6, 1, 0, 0, 0, 0, -u6*Xw6, -u6*Yw6, -u6*Zw6;
0, 0, 0, 0, Xw6, Yw6, Zw6, 1, -v6*Xw6, -v6*Yw6, -v6*Zw6;
Xw7, Yw7, Zw7, 1, 0, 0, 0, 0, -u7*Xw7, -u7*Yw7, -u7*Zw7;
0, 0, 0, 0, Xw7, Yw7, Zw7, 1, -v7*Xw7, -v7*Yw7, -v7*Zw7;
Xw8, Yw8, Zw8, 1, 0, 0, 0, 0, -u8*Xw8, -u8*Yw8, -u8*Zw8;
0, 0, 0, 0, Xw8, Yw8, Zw8, 1, -v8*Xw8, -v8*Yw8, -v8*Zw8;
Xw9, Yw9, Zw9, 1, 0, 0, 0, 0, -u9*Xw9, -u9*Yw9, -u9*Zw9;
0, 0, 0, 0, Xw9, Yw9, Zw9, 1, -v9*Xw9, -v9*Yw9, -v9*Zw9;
Xw10, Yw10, Zw10, 1, 0, 0, 0, 0, -u10*Xw10, -u10*Yw10, -u10*Zw10;
0, 0, 0, 0, Xw10, Yw10, Zw10, 1, -v10*Xw10, -v10*Yw10, -v10*Zw10;
Xw11, Yw11, Zw11, 1, 0, 0, 0, 0, -u11*Xw11, -u11*Yw11, -u11*Zw11;
0, 0, 0, 0, Xw11, Yw11, Zw11, 1, -v11*Xw11, -v11*Yw11, -v11*Zw11;
Xw12, Yw12, Zw12, 1, 0, 0, 0, 0, -u12*Xw12, -u12*Yw12, -u12*Zw12;
0, 0, 0, 0, Xw12, Yw12, Zw12, 1, -v12*Xw12, -v12*Yw12, -v12*Zw12;
Xw13, Yw13, Zw13, 1, 0, 0, 0, 0, -u13*Xw13, -u13*Yw13, -u13*Zw13;
0, 0, 0, 0, Xw13, Yw13, Zw13, 1, -v13*Xw13, -v13*Yw13, -v13*Zw13;
Xw14, Yw14, Zw14, 1, 0, 0, 0, 0, -u14*Xw14, -u14*Yw14, -u14*Zw14;
0, 0, 0, 0, Xw14, Yw14, Zw14, 1, -v14*Xw14, -v14*Yw14, -v14*Zw14;
Xw15, Yw15, Zw15, 1, 0, 0, 0, 0, -u15*Xw15, -u15*Yw15, -u15*Zw15;
0, 0, 0, 0, Xw15, Yw15, Zw15, 1, -v15*Xw15, -v15*Yw15, -v15*Zw15;
];
B=[u1,v1,u2,v2,u3,v3,u4,v4,u5,v5,u6,v6,u7,v7,u8,v8,u9,v9, u10,v10,u11,v11,u12,v12,u13,v13,u14,v14,u15,v15]
B=B';
% 运用广义逆求解A*M1=B
M1=(inv(A'*A)*A'*B)';
% 将M1扩展为12列
M1(:,12)=1;
% 将M1转换为3*4的矩阵
M2=reshape(M1,4,3);
MB=M2';
%求解m34
d=[MB(3,1),MB(3,2),MB(3,3)];
m34=sqrt(d*d');
%求解M
N=MB/m34;
u16=(MB(1,1)*Xw16+MB(1,2)*Yw16+MB(1,3)*Zw16+MB(1,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
v16=(MB(2,1)*Xw16+MB(2,2)*Yw16+MB(2,3)*Zw16+MB(2,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
%*********************************************8
%左摄像机标定用数据
format long;
u1=237;
v1=95;
Xw1=0;
Yw1=0;
Zw1=34;
u2=360;
v2=96;
Xw2=40;
Yw2=0;
Zw2=26;
u3=482
v3=100;
Xw3=80;
Yw3=0;
Zw3=18;
u4=602;
v4=97;
Xw4=120;
Yw4=0;
Zw4=26;
u5=236;
v5=213;
Xw5=0;
Yw5=40;
Zw5=34;
u6=356;
v6=215;
Xw6=40;
Yw6=40;
Zw6=30;
u7=480;
v7=218;
Xw7=80;
Yw7=40;
Zw7=22;
u8=603;
v8=220;
Xw8=120;
Yw8=40;
Zw8=10;
u9=239;
v9=333;
Xw9=0;
Yw9=80;
Zw9=26;
u10=355;
v10=337;
Xw10=40;
Yw10=80;
Zw10=30;
u11=477;
v11=338;
Xw11=80;
Yw11=80;
Zw11=22;
u12=600;
v12=340;
Xw12=120;
Yw12=80;
Zw12=10;
u13=248;
v13=444;
Xw13=0;
Yw13=120;
Zw13=14;
u14=362;
v14=449;
Xw14=40;
Yw14=120;
Zw14=14;
u15=473;
v15=458;
Xw15=80;
Yw15=120;
Zw15=26;
Xw16=80;
Yw16=120;
Zw16=26;
A=[Xw1, Yw1, Zw1, 1, 0, 0, 0, 0, -u1*Xw1, -u1*Yw1, -u1*Zw1;
0, 0, 0, 0, Xw1, Yw1, Zw1, 1, -v1*Xw1, -v1*Yw1, -v1*Zw1;
Xw2, Yw2, Zw2, 1, 0, 0, 0, 0, -u2*Xw2, -u2*Yw2, -u2*Zw2;
0, 0, 0, 0, Xw2, Yw2, Zw2, 1, -v2*Xw2, -v2*Yw2, -v2*Zw2;
Xw3, Yw3, Zw3, 1, 0, 0, 0, 0, -u3*Xw3, -u3*Yw3, -u3*Zw3;
0, 0, 0, 0, Xw3, Yw3, Zw3, 1, -v3*Xw3, -v3*Yw3, -v3*Zw3;
Xw4, Yw4, Zw4, 1, 0, 0, 0, 0, -u4*Xw4, -u4*Yw4, -u4*Zw4;
0, 0, 0, 0, Xw4, Yw4, Zw4, 1, -v4*Xw4, -v4*Yw4, -v4*Zw4;
Xw5, Yw5, Zw5, 1, 0, 0, 0, 0, -u5*Xw5, -u5*Yw5, -u5*Zw5;
0, 0, 0, 0, Xw5, Yw5, Zw5, 1, -v5*Xw5, -v5*Yw5, -v5*Zw5;
Xw6, Yw6, Zw6, 1, 0, 0, 0, 0, -u6*Xw6, -u6*Yw6, -u6*Zw6;
0, 0, 0, 0, Xw6, Yw6, Zw6, 1, -v6*Xw6, -v6*Yw6, -v6*Zw6;
Xw7, Yw7, Zw7, 1, 0, 0, 0, 0, -u7*Xw7, -u7*Yw7, -u7*Zw7;
0, 0, 0, 0, Xw7, Yw7, Zw7, 1, -v7*Xw7, -v7*Yw7, -v7*Zw7;
Xw8, Yw8, Zw8, 1, 0, 0, 0, 0, -u8*Xw8, -u8*Yw8, -u8*Zw8;
0, 0, 0, 0, Xw8, Yw8, Zw8, 1, -v8*Xw8, -v8*Yw8, -v8*Zw8;
Xw9, Yw9, Zw9, 1, 0, 0, 0, 0, -u9*Xw9, -u9*Yw9, -u9*Zw9;
0, 0, 0, 0, Xw9, Yw9, Zw9, 1, -v9*Xw9, -v9*Yw9, -v9*Zw9;
Xw10, Yw10, Zw10, 1, 0, 0, 0, 0, -u10*Xw10, -u10*Yw10, -u10*Zw10;
0, 0, 0, 0, Xw10, Yw10, Zw10, 1, -v10*Xw10, -v10*Yw10, -v10*Zw10;
Xw11, Yw11, Zw11, 1, 0, 0, 0, 0, -u11*Xw11, -u11*Yw11, -u11*Zw11;
0, 0, 0, 0, Xw11, Yw11, Zw11, 1, -v11*Xw11, -v11*Yw11, -v11*Zw11;
Xw12, Yw12, Zw12, 1, 0, 0, 0, 0, -u12*Xw12, -u12*Yw12, -u12*Zw12;
0, 0, 0, 0, Xw12, Yw12, Zw12, 1, -v12*Xw12, -v12*Yw12, -v12*Zw12;
Xw13, Yw13, Zw13, 1, 0, 0, 0, 0, -u13*Xw13, -u13*Yw13, -u13*Zw13;
0, 0, 0, 0, Xw13, Yw13, Zw13, 1, -v13*Xw13, -v13*Yw13, -v13*Zw13;
Xw14, Yw14, Zw14, 1, 0, 0, 0, 0, -u14*Xw14, -u14*Yw14, -u14*Zw14;
0, 0, 0, 0, Xw14, Yw14, Zw14, 1, -v14*Xw14, -v14*Yw14, -v14*Zw14;
Xw15, Yw15, Zw15, 1, 0, 0, 0, 0, -u15*Xw15, -u15*Yw15, -u15*Zw15;
0, 0, 0, 0, Xw15, Yw15, Zw15, 1, -v15*Xw15, -v15*Yw15, -v15*Zw15;
];
B=[u1,v1,u2,v2,u3,v3,u4,v4,u5,v5,u6,v6,u7,v7,u8,v8,u9,v9, u10,v10,u11,v11,u12,v12,u13,v13,u14,v14,u15,v15]
B=B';
% 运用广义逆求解A*M1=B
M1=(inv(A'*A)*A'*B)';
% 将M1扩展为12列
M1(:,12)=1;
% 将M1转换为3*4的矩阵
M2=reshape(M1,4,3);
MB=M2';
%求解m34
d=[MB(3,1),MB(3,2),MB(3,3)];
m34=sqrt(d*d');
%求解M
M=MB/m34;
u16=(MB(1,1)*Xw16+MB(1,2)*Yw16+MB(1,3)*Zw16+MB(1,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
v16=(MB(2,1)*Xw16+MB(2,2)*Yw16+MB(2,3)*Zw16+MB(2,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
%*****************************************
ul=482
vl=100
ur=443
vr=73
E=[M(3,1:3)*ul-M(1,1:3);M(3,1:3)*vl-M(2,1:3);N(3,1:3)*ur-N(1,1:3);N(3,1:3)*vr-N(2,1:3);];
F=[M(1,4)-M(3,4)*ul;M(2,4)-M(3,4)*vl;N(1,4)-N(3,4)*ur;N(2,4)-N(3,4)*vr;];
P=pinv(E)*F;
%********************************
% 理论测定的值
% Xw16=80.0
% Yw16=0.0
% Zw16=18.0
%经过Matlab运算得到的试验值
%X'w16=79.77508164506581
%Y'w16=0.08068881938200
%Z'w16=17.76543369869931
% 精确度计算
% 绝对误差r1=|80.0-79.78|=0.22 r2=|0.0-0.08|=0.08 r3=|18.0-17.77|=0.23
% 平均误差计算
% 平均绝对误差 r=(r1+r2+r3)/3=0.18
% 总的来说理论值和实验值符合得相当好
