%读取控制点像点坐标和物方坐标,格式:点号 左片x 左片y 右片x 右片y 物方坐标X 物方坐标Y 物方坐标Z
LeftRight_image=fopen('C:\Users\岳文鹏\Desktop\空间后方交会-前方交会\控制点坐标4.txt');
control_pointsL=textscan(LeftRight_image,'%f%f%f%f%f%f%f%f');
control_points=[control_pointsL{1} control_pointsL{2} control_pointsL{3} control_pointsL{4} control_pointsL{5} control_pointsL{6} control_pointsL{7} control_pointsL{8}];
%读取未知点的像点坐标,格式:点号 左片x 左片y 右片x 右片y
image_points=fopen('C:\Users\岳文鹏\Desktop\空间后方交会-前方交会\未知点像点坐标1.txt');
unknow_pointsL=textscan(image_points,'%f%f%f%f%f');
unknow_points=[unknow_pointsL{1} unknow_pointsL{2} unknow_pointsL{3} unknow_pointsL{4} unknow_pointsL{5}];
%单位换算,换算成米
for i=1:size(control_points,1)
control_points(i,2:5)=control_points(i,2:5)/1000;
end
for i=1:size(unknow_points,1)
unknow_points(i,2:5)=unknow_points(i,2:5)/1000;
end
%计算左片的外方位元素和旋转矩阵
[Left_Xs0,Left_Ys0,Left_Zs0,Left_p,Left_w,Left_k,Left_R] = Calculate_Left( control_points );
%计算右片的外方位元素和旋转矩阵
[Right_Xs0,Right_Ys0,Right_Zs0,Right_p,Right_w,Right_k,Right_R] = Calculate_Right( control_points );
%计算未知点的物方坐标,并以矩阵形式存储,格式:物方坐标X 物方坐标Y 物方坐标Z
Coordinate=Calculate_unknow_points( unknow_points,Left_Xs0,Left_Ys0,Left_Zs0,Left_R ,Right_Xs0,Right_Ys0,Right_Zs0,Right_R);
%计算左片外方位元素
function [ Xs0,Ys0,Zs0,p0,w0,k0,R] = Calculate_Left( control_points )
f=0.15;%f为主距
m=9943;%已知数据比例尺
n=0;
Xs0=0;
Ys0=0;
Zs0=0;
p0=0;
w0=0;
k0=0;
Gaizheng_4=1;
Gaizheng_5=1;
Gaizheng_6=1;
R=zeros(3,3);
A=zeros(8,6);
L=zeros(8,1);
%计算外方位线元素初始值
for i=1:size(control_points,1)
Xs0=Xs0+control_points(i,6);
Ys0=Ys0+control_points(i,7);
Zs0=Zs0+control_points(i,8);
end
Xs0=Xs0/size(control_points,1);
Ys0=Ys0/size(control_points,1);
Zs0=Zs0/size(control_points,1)+m*f;
while abs(Gaizheng_5)>0.00000001||abs(Gaizheng_4)>0.00000001||abs(Gaizheng_6)>0.00000001
%计算旋转矩阵
R(1,1)=cos(p0)*cos(k0)-sin(p0)*sin(w0)*sin(k0);
R(1,2)=(-1)*cos(p0)*sin(k0)-sin(p0)*sin(w0)*cos(k0);
R(1,3)=(-1)*sin(p0)*cos(w0);
R(2,1)=cos(w0)*sin(k0);
R(2,2)=cos(w0)*cos(k0);
R(2,3)=(-1)*sin(w0);
R(3,1)=sin(p0)*cos(k0)+cos(p0)*sin(w0)*sin(k0);
R(3,2)=(-1)*sin(p0)*sin(k0)+cos(p0)*sin(w0)*cos(k0);