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迭代次数为10
for 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;
end
M=-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
end
function [ 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_Z
Left_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;
end
end
Coordinate=zeros(size(UnknownPoint,1),4);
Coordinate(:,1)=UnknownPoint(:,1);
Coordinate(:,2:4)=XYZ;
end
