MATLAB实现DLT(Direct Linear Transform)算法

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