clc;
clear all;
m=[0 0 0 1;
0 0 0 0;
0 0 0 0];
%标定点物理坐标采集数据(2)
Xw(1,:)=[772 121 90 1];
Xw(2,:)=[772 121 100 1];
Xw(3,:)=[765 114 90 1];
Xw(4,:)=[757 107 80 1];
Xw(5,:)=[764 84 80 1];
Xw(6,:)=[757 90 90 1];
Xw(7,:)=[771 77 90 1];
Xw(8,:)=[771 77 70 1];
Xw(9,:)=[785.3 121 108.5 1];
Xw(10,:)=[807 100 108.5 1];
Xw(11,:)=[778.3 86 108.5 1];
Xw(12,:)=[778.3 100 108.5 1];
%标定点图像坐标采集数据(2)
u(1,:)=[360 285];
u(2,:)=[354 222];
u(3,:)=[398 302];
u(4,:)=[441 376];
u(5,:)=[587 359];
u(6,:)=[532 316];
u(7,:)=[632 280];
u(8,:)=[639 399];
u(9,:)=[361 146];
u(10,:)=[512 92];
u(11,:)=[583 153];
u(12,:)=[494 158];
%%%%%%%%%
[size_mR,size_mC]=size(m);
for i=1:size_mR
for j=1:size_mC
M((i-1)*size_mC+j)=m(i,j);
end
end
[size_XwR,size_XwC]=size(Xw);
j=1;
for i=1:12
K(j,:)=[Xw(i,1), Xw(i,2), Xw(i,3),1, 0, 0, 0, 0 ,-u(i,1)*Xw(i,1),-u(i,1)*Xw(i,2),-u(i,1)*Xw(i,3)];
K(j+1,:)=[0, 0, 0, 0 ,Xw(i,1), Xw(i,2), Xw(i,3),1,-u(i,2)*Xw(i,1),-u(i,2)*Xw(i,2),-u(i,2)*Xw(i,3)];
j=j+2;
end
[size_uR,size_uC]=size(u);
j=1;
for i=1:size_uR
U(j)=u(i,1);
U(j+1)=u(i,2);
j=j+2;
end
U=U';
m_Temp=inv(K'*K)*K'*U;
m=[m_Temp(1),m_Temp(2),m_Temp(3),m_Temp(4);
m_Temp(5),m_Temp(6),m_Temp(7),m_Temp(8);
m_Temp(9),m_Temp(10),m_Temp(11),1]
M=[m_Temp;1];
M=reshape(M,4,3)';
M34=M;
M(:,4)=[];
M33=M;
%
% %%计算参数
m34=1/norm(M33(3,:)')
r3=m34.*M33(3,:)'
u0=m34^2.*(M33(1,:)*M33(3,:)')
v0=m34^2.*(M33(2,:)*M33(3,:)')
ax=m34^2*norm(cross(M33(1,:)',M33(3,:)'))
ay=m34^2*norm(cross(M33(2,:)',M33(3,:)'))
r1=m34/ax.*(M33(1,:)'-u0*M33(3,:)')
r2=m34/ay.*(M33(2,:)'-u0*M33(3,:)')
tz=m34
tx=(m34/ax)*(M34(1,4)-u0)
ty=(m34/ay)*(M34(2,4)-v0)
