MATLAB实现DLT(Direct Linear Transform)算法

转载备用
作者：My_CSDN_Bo_Ke 
来源：CSDN 
原文：https://blog.csdn.net/weixin_40327927/article/details/81020798 

% DLT算法主程序
Left_image=load('left_image.txt');
[Left_L,Left_x0,Left_y0]=Computer_L(Left_image);
Right_image=load('right_image.txt');
[Right_L,Right_x0,Right_y0]=Computer_L(Right_image);
UnknownPoint=load('test2.txt');
Coordinate=Computer_UnknownPoint( UnknownPoint,Left_L,Left_x0,Left_y0,Right_L,Right_x0,Right_y0 );function [ L,x0,y0] = Computer_L( Points )
%功能：计算L系数的近似值和精确值
%输入：Points为控制点的像点坐标及物方坐标%格式为：点号 x y X Y Z
%输出：L系数的精确值，像主点坐标x0,y0%计算L系数近似值
X=zeros(2*size(Points,1),11);
B=zeros(2*size(Points,1),1);
for i=1:size(Points,1);X(2*i-1,1:3)=Points(i,4:6);X(2*i-1,4)=1;X(2*i-1,9:11)=Points(i,2)*Points(i,4:6);X(2*i,5:8)=X(2*i-1,1:4);X(2*i,9:11)=Points(i,3)*Points(i,4:6);B(2*i-1,1)=Points(i,2);B(2*i,1)=Points(i,3);
end
L=X(1:11,:)\(-B(1:11,1));
%计算像主点近似值x0,y0
x0=-(L(1)*L(9)+L(2)*L(10)+L(3)*L(11))/(L(9)*L(9)+L(10)*L(10)+L(11)*L(11));
y0=-(L(5)*L(9)+L(6)*L(10)+L(7)*L(11))/(L(9)*L(9)+L(10)*L(10)+L(11)*L(11));
%计算L系数精确值
M=zeros(2*size(Points,1),12);
W=zeros(2*size(Points,1),1);
n=0;                     %迭代次数统计
while n<10;              %设置L迭代次数为10for i=1:6;A=L(9)*Points(i,4)+L(10)*Points(i,5)+L(11)*Points(i,6)+1;M(2*i-1,1:4)=[Points(i,4:6) 1]/A;M(2*i-1,9:11)=Points(i,2)* M(2*i-1,1:3);R=(Points(i,2)-x0)^2+(Points(i,3)-y0)^2;M(2*i-1,12)=(Points(i,2)-x0)*R;M(2*i,5:8)=M(2*i-1,1:4);M(2*i,9:11)=Points(i,3)* M(2*i-1,1:3);M(2*i,12)=(Points(i,3)-y0)*R;W(2*i-1,1)=Points(i,2)/A;W(2*i,1)=Points(i,3)/A;endM=-M;L=(M'*M)\M'*W;x0=-(L(1)*L(9)+L(2)*L(10)+L(3)*L(11))/(L(9)*L(9)+L(10)*L(10)+L(11)*L(11));y0=-(L(5)*L(9)+L(6)*L(10)+L(7)*L(11))/(L(9)*L(9)+L(10)*L(10)+L(11)*L(11));n=n+1;
end
endfunction [ Coordinate ] = Computer_UnknownPoint( UnknownPoint,Left_L,Left_x0,Left_y0,Right_L,Right_x0,Right_y0 )
%功能：计算待定点物方坐标
%输入：UnknownPoint为待定点在左右片的像点坐标%格式为：点号 左片坐标x y 右片坐标x y%Left_L、Left_x0、Left_y0为左片L系数及像主点坐标%Right_L、Right_x0、Right_y0为右片L系数及像主点坐标
%输出：Coordinate为待定点物方坐标%格式为：点号 X Y Z%Error为计算出的待定点坐标与真实坐标之差%格式为：点号 delta_X delta_Y delta_ZLeft_R=(UnknownPoint(:,2)-Left_x0).^2+(UnknownPoint(:,3)-Left_y0).^2;Left_xx=UnknownPoint(:,2)+(UnknownPoint(:,2)-Left_x0).*Left_R*Left_L(12);Left_yy=UnknownPoint(:,3)+(UnknownPoint(:,3)-Left_y0).*Left_R*Left_L(12);Right_R=(UnknownPoint(:,4)-Right_x0).^2+(UnknownPoint(:,5)-Right_y0).^2;Right_xx=UnknownPoint(:,4)+(UnknownPoint(:,4)-Right_x0).*Right_R*Right_L(12);Right_yy=UnknownPoint(:,5)+(UnknownPoint(:,5)-Right_y0).*Right_R*Right_L(12);%计算待定点物方坐标近似值B=zeros(3,3);C=zeros(3,1);XYZ=zeros(size(UnknownPoint,1),3);for i=1:size(UnknownPoint,1);B(1,1)=Left_L(1)+Left_xx(i)*Left_L(9);B(1,2)=Left_L(2)+Left_xx(i)*Left_L(10);B(1,3)=Left_L(3)+Left_xx(i)*Left_L(11);B(2,1)=Left_L(5)+Left_yy(i)*Left_L(9);B(2,2)=Left_L(6)+Left_yy(i)*Left_L(10);B(2,3)=Left_L(7)+Left_yy(i)*Left_L(11);B(3,1)=Right_L(1)+Right_xx(i)*Right_L(9);B(3,2)=Right_L(2)+Right_xx(i)*Right_L(10);B(3,3)=Right_L(3)+Right_xx(i)*Right_L(11);C(1,1)=Left_L(4)+Left_xx(i);C(2,1)=Left_L(8)+Left_yy(i);C(3,1)=Right_L(4)+Right_xx(i);XYZ(i,:)=(B\(-C))';end%计算待定点物方坐标精确值N=zeros(4,3);Q=zeros(4,1);delta_XYZ=100*ones(1,3);            %相邻两次迭代差值for i=1:size(UnknownPoint,1);n=0;                            %统计迭代次数while max(abs(delta_XYZ))>0.000001;A_L=Left_L(9)*XYZ(i,1)+Left_L(10)*XYZ(i,2)+Left_L(11)*XYZ(i,3)+1;N(1,1)=-(Left_L(1)+Left_xx(i)*Left_L(9))/A_L;N(1,2)=-(Left_L(2)+Left_xx(i)*Left_L(10))/A_L;N(1,3)=-(Left_L(3)+Left_xx(i)*Left_L(11))/A_L;N(2,1)=-(Left_L(5)+Left_yy(i)*Left_L(9))/A_L;N(2,2)=-(Left_L(6)+Left_yy(i)*Left_L(10))/A_L;N(2,3)=-(Left_L(7)+Left_yy(i)*Left_L(11))/A_L;A_R=Right_L(9)*XYZ(i,1)+Right_L(10)*XYZ(i,2)+Right_L(11)*XYZ(i,3)+1;N(3,1)=-(Right_L(1)+Right_xx(i)*Right_L(9))/A_R;N(3,2)=-(Right_L(2)+Right_xx(i)*Right_L(10))/A_R;N(3,3)=-(Right_L(3)+Right_xx(i)*Right_L(11))/A_R;N(4,1)=-(Right_L(5)+Right_yy(i)*Right_L(9))/A_R;N(4,2)=-(Right_L(6)+Right_yy(i)*Right_L(10))/A_R;N(4,3)=-(Right_L(7)+Right_yy(i)*Right_L(11))/A_R;Q(1,1)=(Left_L(4)+Left_xx(i))/A_L;Q(2,1)=(Left_L(8)+Left_yy(i))/A_L;Q(3,1)=(Right_L(4)+Right_xx(i))/A_R;Q(4,1)=(Right_L(8)+Right_yy(i))/A_R;XYZ_new=((N'*N)\N'*Q)';delta_XYZ=XYZ_new-XYZ(i,:);XYZ(i,:)=XYZ_new;n=n+1;endendCoordinate=zeros(size(UnknownPoint,1),4);Coordinate(:,1)=UnknownPoint(:,1);Coordinate(:,2:4)=XYZ;
end