后方交会.zip

使用matlab进行角锥体法后方交会，线元素迭代求解，角元素直接求解

%% 基于“以投影中心为顶点的两根构像光线的像方角应与其物方角相等”原理
%  即cos（ij）=cos（IJ） 
clear;
clc;
%% ******************读取dat文件并获取数据**************************
filein=‘Lor50.dat‘;           
fid=fopen（filein‘rt‘）;   %%打开数据文件                            
tline = fgetl（fid）;
%%读取初始参数
disp（tline）;   %%显示出每行读取的文本
tline=str2num（tline）;
org_data=tline;
%%读取坐标数据
base_data=[];  
while 1
    tline=fgetl（fid）;
    if ~ischar（tline）
         break;
    end
    disp（tline）;  %%显示出每行读取的文本
    tline=str2num（tline）;
    base_data=[base_data;tline];
end
fclose（fid）;  %%关闭文件
%% ********************角锥体法后方交会****************************
%% 初始参数
dpi = 0.498; %% 像素每mm
m   = 5000 ; %% 比例尺分母
n_points = org_data（12）;  %% 控制点数目
l_limit  = org_data（13）;   %% 线元素限差
a_limit  = org_data（14）;   %% 角元素限差
f = org_data（15） * dpi;    %% 焦距
x0=org_data（16）; %% 像主点横坐标
y0=org_data（17）; %% 像主点纵坐标
X0=mean（base_data（:2））; %% 投影中心X初始值
Y0=mean（base_data（:3））; %% 投影中心Y初始值
Z0=m*f/1000; %% 投影中心Z初始值
XX=[X0;Y0;Z0];
%% 图像坐标系与像平面坐标系的转换
%像点
img_x（:1） = base_data（:5）*dpi-x0*dpi;
img_y（:1） = y0*dpi-base_data（:6）*dpi;
%地面控制点
obj_X（:1） = base_data（:2）;
obj_Y（:1） = base_data（:3）;
obj_Z（:1） = base_data（:4）;
x0=0;y0=0;
%% 求解线元素***迭代求解***
iter=0;flag=1;
while flag==1
    for i=1:n_points
        img_s（i） = sqrt（（img_x（i）-x0）^2+（img_y（i）-y0）^2+f*f）;%S（i）  
        obj_s（i） = sqrt（（obj_X（i）-X0）^2+（obj_Y（i）-Y0）^2++（obj_Z（i）-Z0）^2）;%S（I）  
    end
    m=0;
    for i=1:n_points
        for j=i+1:n_points
            m=m+1; 
            %系数阵和常数阵求解
            img_cos（m）= （（x0-img_x（i））*（x0-img_x（j））+（y0-img_y（i））*（y0-img_y（j））+f*f）/（img_s（i）*img_s（j））;%cos（ij）         
            obj_cos（m）= （（X0-obj_X（i））*（X0-obj_X（j））+（Y0-obj_Y（i））*（Y0-obj_Y（j））+（Z0-obj_Z（i））*（Z0-obj_Z（j）））/（obj_s（i）*obj_s（j））;%cos（IJ）
            T0（m） = （1-obj_s（i）*obj_cos（m）/obj_s（j））/（obj_s（i）*obj_s（j））;
            T1（m） = （1-obj_s（j）*obj_cos（m）/obj_s（i））/（obj_s（i）*obj_s（j））;
            a（m） = （X0-obj_X（i））*T1（m）+（X0-obj_X（j））*T0（m）;
            b（m） = （Y0-obj_Y（i））*T1（m）+（Y0-obj_Y（j））*T0（m）;
            c（m） = （Z0-obj_Z（i））*T1（m）+（Z0-obj_Z（j））*T0（m）;
            l（m） = img_cos（m）-obj_cos（m）;
        end
    end
    %系数阵
    A=[a;b;c]‘;
    L=l‘;
    Xnew=（A‘*A）\A‘*L;
    if norm（Xnew）        flag=0;
    end
    iter=iter+1;
    X0=X0+Xnew（1）;Y0=Y0+Xnew（2）;Z0=Z0+Xnew（3）;
    Xs=X0;Ys=Y0;Zs=Z0;
end 
%% 求解角元素***直接求解***
% 共线条件方程
for i=1:n_points
    SP（i1）=sqrt（（obj_X（i）-Xs）*（obj_X（i）-Xs）+（obj_Y（i）-Ys）*（obj_Y（i）-Ys）+（obj_Z（i）-Zs）*（obj_Z（i）-Zs））;%S（I）  
    Sp（i1）=sqrt（（img_x（i）-x0）*（img_x（i）-x0）+（img_y（i）-y0）*（img_y（i）-y0）+f*f）;%S（i）  
    aa（i1）=（obj_X（i）-Xs）/SP（i1）;
    bb（i1）=（obj_Y（i）-Ys）/SP（i1）;
    cc（i1）=（obj_Z（i）-Zs）/SP（i1）;
    l1（i1）=（img_x（i）-x0）/Sp（i1）;
    l2（i1）=（img_y（i）-y0）/Sp（i1）;
    l3（i1）=-f/Sp（i1）;
end
A=[aabbcc];
R1=（A‘*A）\A‘*l1;R2=（A‘*A）\A‘*l2;R3=（A‘*A）\A‘*l3;
R=[R1R2R3]‘; 
phi=atan（-R3（11）/R3（31））;
omeg

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-06-12 14:43  后方交会\
     文件         936  2008-03-17 11:55  后方交会\Lor49-说明.dat
     文件         585  2005-03-18 22:33  后方交会\Lor49.dat
     文件      214788  1999-06-17 09:57  后方交会\LOR49.tif
     文件         357  2006-05-15 16:13  后方交会\lor49后交结果
     文件         510  2005-04-07 08:54  后方交会\lor50.dat
     文件      210871  1999-08-11 11:58  后方交会\LOR50.tif
     文件         356  2005-03-20 14:38  后方交会\lor50后交结果
     文件        4252  2019-06-11 18:55  后方交会\resection.m
     文件        4725  2019-06-11 18:47  后方交会\resection_method.m
     文件         289  2019-06-11 20:02  后方交会\ResultF.txt