先放上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