%% 本算法想写一个RPC正算/反算的算法
fclose all
clear all
clc
%要是用RPC影像，及其RPB文件，DEM文件
img_file='E:\GF_test\GF_2022_4_19\test_bundle_adjust\pa2_021_010\pm2_010_6100_1.tif';
rpb_file='E:\GF_test\GF_2022_4_19\test_bundle_adjust\pa2_021_010\pm2_010_6100_1.rpb';
dem_file='E:\GF_test\GF_2022_4_19\test_bundle_adjust\pa2_021_010\srtm_n50_50_e124_125_tif_asp-adj.tif';

%要求取的像素的行列号
pixel_line=13010;
pixel_sample=7506.09;

% 读取SRTM的相关geotiff信息
info = geotiffinfo(dem_file);
[A,bbox] = geotiffread(dem_file);


lsh=[pixel_line,pixel_sample,0];%高程给定 0 为初值
%value_bijin=[50.7240,124.9591];
% 以下两个是阈值相关的参数
old_ll=[0,0];
end_yuzhi=100;
% 以提取处的经纬度坐标，读取DEM影像，获取其高程值
while( end_yuzhi > 1.5e-4)
    pixel_ll=RS_backward_compute_rpc(rpb_file,lsh);
    %pixel_ll=RS_backward_compute_rpc(f_in_name,lsh,value_bijin);
    [row, col] = latlon2pix(bbox, pixel_ll(1),pixel_ll(2));
    value_dem=A(int32(row),int32(col));
    % 第一次，由像素坐标推测出的【纬度，经度，高程值】
    new_llh=[pixel_ll(1),pixel_ll(2),double(value_dem)];
    %value_bijin=[pixel_ll(1)+1,pixel_ll(2)+1];
    value_bijin=[pixel_ll(1),pixel_ll(2)];
    pixel_position_read=RS_forward_compute_rpc(rpb_file,new_llh);
    %end_yuzhi=sum(abs(pixel_position_read-[pixel_line,pixel_sample]));
    end_yuzhi=sum(abs(pixel_ll-old_ll));
    lsh=[pixel_line,pixel_sample,value_dem];
    old_ll=pixel_ll;
end

%最终优化后的像素的经纬度
disp([num2str(pixel_line),'行',num2str(pixel_sample),'列','像素的经纬度,高程值为：'])
disp(new_llh(1))
disp(new_llh(2))
disp(new_llh(3))