MATLAB实现DLT(DirectLinearTransform)算法

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

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