MATLAB实现空间前方交会-后方交会计算

OtterYu

于 2021-12-12 03:48:26 发布

阅读量4.8k

点赞数 5

分类专栏： MATLAB 摄影测量文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/Yu_yunlan/article/details/121882669

版权

MATLAB 同时被 2 个专栏收录

3 篇文章 1 订阅

订阅专栏

摄影测量

3 篇文章 1 订阅

订阅专栏

先放上MATLAB代码，之后会对计算原理进行更新。

空间后方交会函数设计

function [ExteriorOrientationElement,InteriorOrientationCoefficient] = Space_Resection(DataStruct)
%% Data Preparation
f = DataStruct.PrincipalDistance;
FrameCoordinate = DataStruct.FrameCoordinate;
ImageSize = DataStruct.ImageSize;
ControlPointsCoordinate = DataStruct.ControlPointsCoordinate;
x = ControlPointsCoordinate(:,1);y = ControlPointsCoordinate(:,2);
X = ControlPointsCoordinate(:,3);Y = ControlPointsCoordinate(:,4);Z = ControlPointsCoordinate(:,5);
%% Internal Orientation and Anti Internal Orientation
% Internal Orientation Element
InteriorOrientationCoefficient = pinv([ones(4,1),FrameCoordinate(1:4,1:2)])*FrameCoordinate(1:4,3:4);
%{
a0 = InteriorOrientationCoefficient(1,1);b0 = InteriorOrientationCoefficient(1:2);
a1 = InteriorOrientationCoefficient(2,1);b1 = InteriorOrientationCoefficient(2,2);
a2 = InteriorOrientationCoefficient(3,1);b2 = InteriorOrientationCoefficient(3,2);
%}
% Anti Internal Orientation Element
AntiInteriorOrientationCoefficient = pinv([ones(4,1),FrameCoordinate(1:4,3:4)])*FrameCoordinate(1:4,1:2);
%{
c0 = AntiInteriorOrientationCoefficient(1,1);d0 = AntiInteriorOrientationCoefficient(1,2);
c1 = AntiInteriorOrientationCoefficient(2,1);d1 = AntiInteriorOrientationCoefficient(2,2);
c2 = AntiInteriorOrientationCoefficient(3,1);d2 = AntiInteriorOrientationCoefficient(3,2);
%}
% Internal Orientation Element
% InternalOrientationElement = [0;0;f];
%% Mean Height
ScaleofPixel = sum(ImageSize(:,1))/sum(ImageSize(:,2));
ControlPointsPixelCoordinatie = [ones(length(ControlPointsCoordinate),1),ControlPointsCoordinate(:,1:2)]*AntiInteriorOrientationCoefficient;
ScaleofImage = 0;
for i = 1:length(ControlPointsCoordinate)-1
    for j = i+1:length(ControlPointsCoordinate)
        ScaleofImage = ScaleofImage+...
            sqrt(((ControlPointsPixelCoordinatie(i,1)-ControlPointsPixelCoordinatie(j,1))/ScaleofPixel)^2+...
                 ((ControlPointsPixelCoordinatie(i,2)-ControlPointsPixelCoordinatie(j,2))/ScaleofPixel)^2)/...
            sqrt((X(i,1)-X(j,1))^2+...
                 (Y(i,1)-Y(j,1))^2);
    end
end
ScaleofImage = ScaleofImage/length(ControlPointsCoordinate);
MeanHeight = f/ScaleofImage+sum(Z)/length(ControlPointsCoordinate);
%% Exterior Orientation Element
Xs = sum(X)/length(ControlPointsCoordinate);
Ys = sum(Y)/length(ControlPointsCoordinate);
Zs = MeanHeight;
phi = 0;omega = 0;kappa = 0;
Tolerance = 0.1*pi/(180*60*60);
while 1
    R = [cos(phi),0,-sin(phi);0,1,0;sin(phi),0,cos(phi)]*...
        [1,0,0;0,cos(omega),-sin(omega);0,sin(omega),cos(omega)]*...
        [cos(kappa),-sin(kappa),0;sin(kappa),cos(kappa),0;0,0,1];
    Approximate_x = -f*...
                    (([X,Y,Z]-[Xs,Ys,Zs].*ones(length(ControlPointsCoordinate),3))*R(1:3,1))./...
                    (([X,Y,Z]-[Xs,Ys,Zs].*ones(length(ControlPointsCoordinate),3))*R(1:3,3));
    Approximate_y = -f*...
                    (([X,Y,Z]-[Xs,Ys,Zs].*ones(length(ControlPointsCoordinate),3))*R(1:3,2))./...
                    (([X,Y,Z]-[Xs,Ys,Zs].*ones(length(ControlPointsCoordinate),3))*R(1:3,3));
    l = [x-Approximate_x;y-Approximate_y];
    Z_Auxiliary = ([X,Y,Z]-[Xs,Ys,Zs].*ones(length(ControlPointsCoordinate),3))*R(1:3,3);
    B = [R(1,1)*f*ones(length(ControlPointsCoordinate),1)+R(1,3)*(x-Approximate_x)./(Z_Auxiliary),...
         R(2,1)*f*ones(length(ControlPointsCoordinate),1)+R(2,3)*(x-Approximate_x)./(Z_Auxiliary),...
         R(3,1)*f*ones(length(ControlPointsCoordinate),1)+R(3,3)*(x-Approximate_x)./(Z_Auxiliary),...
         (y-Approximate_y)*sin(omega)-(((x-Approximate_x)/f).*((x-Approximate_x)*cos(kappa)-(y-Approximate_y)*sin(kappa))+f*cos(kappa))*cos(omega),...
         -f*sin(kappa)*ones(length(ControlPointsCoordinate),1)-((x-Approximate_x)/f).*((x-Approximate_x)*sin(kappa)+(y-Approximate_y)*cos(kappa)),...
         y-Approximate_y;...
         R(1,2)*f*ones(length(ControlPointsCoordinate),1)+R(1,3)*(y-Approximate_y)./(Z_Auxiliary),...
         R(2,2)*f*ones(length(ControlPointsCoordinate),1)+R(2,3)*(y-Approximate_y)./(Z_Auxiliary),...
         R(3,2)*f*ones(length(ControlPointsCoordinate),1)+R(3,3)*(y-Approximate_y)./(Z_Auxiliary),...
         (x-Approximate_x)*sin(omega)-(((y-Approximate_y)/f).*((x-Approximate_x)*cos(kappa)-(y-Approximate_y)*sin(kappa))-f*sin(kappa))*cos(omega),...
         -f*cos(kappa)*ones(length(ControlPointsCoordinate),1)-((y-Approximate_y)/f).*((x-Approximate_x)*sin(kappa)+(y-Approximate_y)*cos(kappa)),...
         -(x-Approximate_x)];
    P = eye(2*length(ControlPointsCoordinate));
    ExteriorOrientationElement_Correction = inv(B'*P*B)*B'*P*l;
    Xs = Xs+ExteriorOrientationElement_Correction(1,1);
    Ys = Ys+ExteriorOrientationElement_Correction(2,1);
    Zs = Zs+ExteriorOrientationElement_Correction(3,1);
    phi = phi+ExteriorOrientationElement_Correction(4,1);
    omega = omega+ExteriorOrientationElement_Correction(5,1);
    kappa = kappa+ExteriorOrientationElement_Correction(6,1);
    if ExteriorOrientationElement_Correction(4,1)<=Tolerance && ExteriorOrientationElement_Correction(5,1)<=Tolerance && ExteriorOrientationElement_Correction(6,1)<=Tolerance
        break
    end
end
ExteriorOrientationElement = [Xs;Ys;Zs;phi;omega;kappa];
end

利用空间后方交会程序，可以实现单幅影像的外方位元素的计算，此处同时将内定向参数解出一并输出。

空间前方交会函数设计

function [ModelCoordinate] = Space_Intersection(LeftImageDataStruct,RightImageDataStruct)
%% Left Image Preparation
[ExteriorOrientationElement_Left,InteriorOrientationCoefficient_Left] = Space_Resection(LeftImageDataStruct);
HomonymousPointsCoordinate_Left = [ones(length(LeftImageDataStruct.HomonymousPointsPixel),1),LeftImageDataStruct.HomonymousPointsPixel]*InteriorOrientationCoefficient_Left;
R_Left = [cos(ExteriorOrientationElement_Left(4,1)),0,-sin(ExteriorOrientationElement_Left(4,1));...
          0,1,0;...
          sin(ExteriorOrientationElement_Left(4,1)),0,cos(ExteriorOrientationElement_Left(4,1))]*...
         [1,0,0;...
          0,cos(ExteriorOrientationElement_Left(5,1)),-sin(ExteriorOrientationElement_Left(5,1));...
          0,sin(ExteriorOrientationElement_Left(5,1)),cos(ExteriorOrientationElement_Left(5,1))]*...
         [cos(ExteriorOrientationElement_Left(6,1)),-sin(ExteriorOrientationElement_Left(6,1)),0;...
          sin(ExteriorOrientationElement_Left(6,1)),cos(ExteriorOrientationElement_Left(6,1)),0;...
          0,0,1];
S_XYZ_Left = [HomonymousPointsCoordinate_Left,-LeftImageDataStruct.PrincipalDistance*ones(length(LeftImageDataStruct.HomonymousPointsPixel),1)]*R_Left';
%% Right Image Preparation
[ExteriorOrientationElement_Right,InteriorOrientationCoefficient_Right] = Space_Resection(RightImageDataStruct);
HomonymousPointsCoordinate_Right = [ones(length(RightImageDataStruct.HomonymousPointsPixel),1),RightImageDataStruct.HomonymousPointsPixel]*InteriorOrientationCoefficient_Right;
R_Right = [cos(ExteriorOrientationElement_Right(4,1)),0,-sin(ExteriorOrientationElement_Right(4,1));...
          0,1,0;...
          sin(ExteriorOrientationElement_Right(4,1)),0,cos(ExteriorOrientationElement_Right(4,1))]*...
         [1,0,0;...
          0,cos(ExteriorOrientationElement_Right(5,1)),-sin(ExteriorOrientationElement_Right(5,1));...
          0,sin(ExteriorOrientationElement_Right(5,1)),cos(ExteriorOrientationElement_Right(5,1))]*...
         [cos(ExteriorOrientationElement_Right(6,1)),-sin(ExteriorOrientationElement_Right(6,1)),0;...
          sin(ExteriorOrientationElement_Right(6,1)),cos(ExteriorOrientationElement_Right(6,1)),0;...
          0,0,1];
S_XYZ_Right = [HomonymousPointsCoordinate_Right,-RightImageDataStruct.PrincipalDistance*ones(length(RightImageDataStruct.HomonymousPointsPixel),1)]*R_Right';
%% Model Points Coordinate
Bx = ExteriorOrientationElement_Right(1,1)-ExteriorOrientationElement_Left(1,1);
By = ExteriorOrientationElement_Right(2,1)-ExteriorOrientationElement_Left(2,1);
Bz = ExteriorOrientationElement_Right(3,1)-ExteriorOrientationElement_Left(3,1);
N1 = (Bx*S_XYZ_Right(:,3)-Bz*S_XYZ_Right(:,1))./(S_XYZ_Left(:,1).*S_XYZ_Right(:,3)-S_XYZ_Left(:,3).*S_XYZ_Right(:,1));
N2 = (Bx*S_XYZ_Left(:,3)-Bz*S_XYZ_Left(:,1))./(S_XYZ_Left(:,1).*S_XYZ_Right(:,3)-S_XYZ_Left(:,3).*S_XYZ_Right(:,1));
X = ExteriorOrientationElement_Left(1,1)*ones(length(LeftImageDataStruct.HomonymousPointsPixel),1)+N1.*S_XYZ_Left(:,1);
Y = ExteriorOrientationElement_Left(2,1)*ones(length(LeftImageDataStruct.HomonymousPointsPixel),1)+0.5*(N1.*S_XYZ_Left(:,2)+N2.*S_XYZ_Right(:,2)+By);
Z = ExteriorOrientationElement_Left(3,1)*ones(length(LeftImageDataStruct.HomonymousPointsPixel),1)+N1.*S_XYZ_Left(:,3);
ModelCoordinate = [X,Y,Z];
end