摄影测量——单像后方交会代码matlab

摊饼子

已于 2024-03-26 20:09:31 修改

阅读量1k

点赞数 8

文章标签： matlab 算法

于 2024-03-25 21:39:40 首次发布

本文链接：https://blog.csdn.net/2301_79719287/article/details/137026183

版权

%模拟数据给出4个控制点，将4个控制点数据保存进txt文件，命名为data.txt

%具体的数学理论以及公式推导这里不给出。

%% Initialization
clear
clc
format shortG
load('data.txt');
f=153.24*1e-3;%unit:m
delta_fai = 1;delta_omega = 1;delta_kalpa = 1;
Ps = [mean(data(:,3)) mean(data(:,4)) mean(data(:,5))+f*40000 0 0 0];%初始值
count = 0;
while ~(abs(delta_fai*206265) < 0.1 && abs(delta_omega*206265) < 0.1 && abs(delta_kalpa*206265) < 0.1)
Xs = Ps(1);Ys = Ps(2);Zs = Ps(3);
fai = Ps(4);omega = Ps(5);kalpa = Ps(6);
B = [];
%% RotateMatrix
for i = 1:size(data,1)
x = data(i,1)*1e-3;y = data(i,2)*1e-3;X = data(i,3);Y = data(i,4);Z = data(i,5);
a1 = cos(fai)*cos(kalpa)-sin(fai)*sin(omega)*sin(kalpa);
a2 = -cos(fai)*sin(kalpa)-sin(fai)*sin(omega)*cos(kalpa);
a3 = -sin(fai)*cos(omega);
b1 = cos(omega)*sin(kalpa);
b2 = cos(omega)*cos(kalpa);
b3 = -sin(omega);
c1 = sin(fai)*cos(kalpa)+cos(fai)*sin(omega)*sin(kalpa);
c2 = -sin(fai)*sin(kalpa)+cos(fai)*sin(omega)*cos(kalpa);
c3 = cos(fai)*cos(omega);
RotMat = [a1 a2 a3;b1 b2 b3;c1 c2 c3];
% partial derivative
pda1_fai = -sin(fai)*cos(kalpa)-cos(fai)*sin(omega)*sin(kalpa);pda1_omega = -sin(fai)*cos(omega)*sin(kalpa);pda1_kalpa = -cos(fai)*sin(kalpa)-sin(fai)*sin(omega)*cos(kalpa);
pda2_fai = sin(fai)*sin(kalpa)-cos(fai)*sin(omega)*cos(kalpa);pda2_omega = -sin(fai)*cos(omega)*cos(kalpa);pda2_kalpa = -cos(fai)*cos(kalpa)+sin(fai)*sin(omega)*sin(kalpa);
pda3_fai = -cos(fai)*cos(omega);pda3_omega = sin(fai)*sin(omega);pda3_kalpa = 0;
pdb1_fai = 0;pdb1_omega = -sin(omega)*sin(kalpa);pdb1_kalpa = cos(omega)*cos(kalpa);
pdb2_fai = 0;pdb2_omega = -sin(omega)*cos(kalpa);pdb2_kalpa = -cos(omega)*sin(kalpa);
pdb3_fai = 0;pdb3_omega = -cos(omega);pdb3_kalpa = 0;
pdc1_fai = cos(fai)*cos(kalpa)-sin(fai)*sin(omega)*sin(kalpa);pdc1_omega = cos(fai)*cos(omega)*sin(kalpa);pdc1_kalpa = -sin(fai)*sin(kalpa)+cos(fai)*sin(omega)*cos(kalpa);
pdc2_fai = -cos(fai)*sin(kalpa)-sin(fai)*sin(omega)*cos(kalpa);pdc2_omega = cos(fai)*cos(omega)*cos(kalpa);pdc2_kalpa = -sin(fai)*cos(kalpa)-cos(fai)*sin(omega)*sin(kalpa);
pdc3_fai = -sin(fai)*cos(omega);pdc3_omega = -cos(fai)*sin(omega);pdc3_kalpa = 0;
%% Estimate and calculation
N = [X-Xs;Y-Ys;Z-Zs];
M = RotMat(:,3)'*N;% 即[a3 b3 c3]*N
x0 = -f*(RotMat(:,1)'*N)/M;%x,y的近似值
y0 = -f*(RotMat(:,2)'*N)/M;

pdX_Xs = -f*((a3*b1-a1*b3)*N(2)+(a3*c1-a1*c3)*N(3))/(M*M);
pdX_Ys = -f*((b3*a1-b1*a3)*N(1)+(b3*c1-b1*c3)*N(3))/(M*M);
pdX_Zs = -f*((c3*a1-c1*a3)*N(1)+(c3*b1-c1*b3)*N(2))/(M*M);
pdY_Xs = -f*((a3*b2-a2*b3)*N(2)+(a3*c2-a2*c3)*N(3))/(M*M);
pdY_Ys = -f*((b3*a2-b2*a3)*N(1)+(b3*c2-b2*c3)*N(3))/(M*M);
pdY_Zs = -f*((c3*a2-c2*a3)*N(1)+(c3*b2-c2*b3)*N(2))/(M*M);

pdX_fai = (-f/(M*M))*([pda1_fai pdb1_fai pdc1_fai]*N*M-[pda3_fai pdb3_fai pdc3_fai]*N*RotMat(:,1)'*N);
pdX_omega = (-f/(M*M))*([pda1_omega pdb1_omega pdc1_omega]*N*M-[pda3_omega pdb3_omega pdc3_omega]*N*RotMat(:,1)'*N);
pdX_kalpa = (-f/(M*M))*([pda1_kalpa pdb1_kalpa pdc1_kalpa]*N*M-[pda3_kalpa pdb3_kalpa pdc3_kalpa]*N*RotMat(:,1)'*N);
pdY_fai = (-f/(M*M))*([pda2_fai pdb2_fai pdc2_fai]*N*M-[pda3_fai pdb3_fai pdc3_fai]*N*RotMat(:,2)'*N);
pdY_omega = (-f/(M*M))*([pda2_omega pdb2_omega pdc2_omega]*N*M-[pda3_omega pdb3_omega pdc3_omega]*N*RotMat(:,2)'*N);
pdY_kalpa = (-f/(M*M))*([pda2_kalpa pdb2_kalpa pdc2_kalpa]*N*M-[pda3_kalpa pdb3_kalpa pdc3_kalpa]*N*RotMat(:,2)'*N);
if isempty(B)
B = [pdX_Xs pdX_Ys pdX_Zs pdX_fai pdX_omega pdX_kalpa;pdY_Xs pdY_Ys pdY_Zs pdY_fai pdY_omega pdY_kalpa];
L = [x-x0;y-y0];
else
B = [B;pdX_Xs pdX_Ys pdX_Zs pdX_fai pdX_omega pdX_kalpa;pdY_Xs pdY_Ys pdY_Zs pdY_fai pdY_omega pdY_kalpa];
L = [L;x-x0;y-y0];
end
end

v = invbc(B'*B)*(B'*L);
Ps = Ps + v';
delta_fai = v(4);delta_omega = v(5);delta_kalpa = v(6);
count = count + 1;
end
disp(['迭代次数： ',num2str(count),'次']);
disp([' X(m) Y(m) Z(m)']);
disp(['待求点坐标为： ',num2str(Ps(1:3))]);
disp([' ']);
disp([' phi(°) omega(°) kappa(°)']);
disp(['相机姿态角： ',num2str(rad2deg(Ps(4:end)))]);

本代码采用的间接平差的方法进行解算的，线性化的过程是用的比较笨的暴力求解。

——————————————————————————————————————

更新，利用新方法求解系数阵系数。数学推导这里不给出。与上面代码差异部分加粗显示

%% data_matrix
%x（mm）   y（mm）   X(m)   Y（m）   Z(m)
%-86.15   -68.99   36589.41   25273.32   2195.17
%-53.40   82.21   37631.08   31324.51   728.69
%-14.78   -76.63   39100.97   24934.98   2386.50
%10.46   64.43   40426.54   30319.81   757.31
%% Initialization
clear
clc
format shortG
load('data.txt');
f=153.24*1e-3;%unit:m
delta_fai = 1;delta_omega = 1;delta_kalpa = 1;
Ps = [mean(data(:,3)) mean(data(:,4)) mean(data(:,5))+f*40000 0 0 0];%初始值
count = 0;
while ~(abs(delta_fai*206265) < 0.1 && abs(delta_omega*206265) < 0.1 && abs(delta_kalpa*206265) < 0.1)
Xs = Ps(1);Ys = Ps(2);Zs = Ps(3);
fai = Ps(4);omega = Ps(5);kalpa = Ps(6);
B = [];
%% RotateMatrix
for i = 1:size(data,1)
x = data(i,1)*1e-3;y = data(i,2)*1e-3;X = data(i,3);Y = data(i,4);Z = data(i,5);
a1 = cos(fai)*cos(kalpa)-sin(fai)*sin(omega)*sin(kalpa);
a2 = -cos(fai)*sin(kalpa)-sin(fai)*sin(omega)*cos(kalpa);
a3 = -sin(fai)*cos(omega);
b1 = cos(omega)*sin(kalpa);
b2 = cos(omega)*cos(kalpa);
b3 = -sin(omega);
c1 = sin(fai)*cos(kalpa)+cos(fai)*sin(omega)*sin(kalpa);
c2 = -sin(fai)*sin(kalpa)+cos(fai)*sin(omega)*cos(kalpa);
c3 = cos(fai)*cos(omega);
RotMat = [a1 a2 a3;b1 b2 b3;c1 c2 c3];
N = [X-Xs;Y-Ys;Z-Zs];
A = RotMat'*N;
X_ = A(1);
Y_ = A(2);
Z_ = A(3);
x0 = -f*X_/Z_;
y0 = -f*Y_/Z_;
%% Partial derivative
pdx_Xs = (a1*f+a3*x0)/Z_;
pdx_Ys = (b1*f+b3*x0)/Z_;
pdx_Zs = (c1*f+c3*x0)/Z_;
pdy_Xs = (a2*f+a3*y0)/Z_;
pdy_Ys = (b2*f+b3*y0)/Z_;
pdy_Zs = (c2*f+c3*y0)/Z_;
pdx_phi = -b2*f-b3*y0-b2*x0*x0/f+b1*x0*y0/f;
pdx_omega = -f*sin(kalpa)-x0*(x0*sin(kalpa)+y0*cos(kalpa))/f;
pdx_kalpa = y0;
pdy_phi = b3*x0+b1*f-b2*x0*y0/f+b1*y0*y0/f;
pdy_omega = -f*cos(kalpa)-y0*(x0*sin(kalpa)+y0*cos(kalpa))/f;
pdy_kalpa = -x0;
if isempty(B)
B = [pdx_Xs pdx_Ys pdx_Zs pdx_phi pdx_omega pdx_kalpa;pdy_Xs pdy_Ys pdy_Zs pdy_phi pdy_omega pdy_kalpa];
L = [x-x0;y-y0];
else
B = [B;pdx_Xs pdx_Ys pdx_Zs pdx_phi pdx_omega pdx_kalpa;pdy_Xs pdy_Ys pdy_Zs pdy_phi pdy_omega pdy_kalpa];
L = [L;x-x0;y-y0];
end
end
%% Estimate data


v = invbc(B'*B)*(B'*L);
Ps = Ps + v';
delta_fai = v(4);delta_omega = v(5);delta_kalpa = v(6);
count = count + 1;
end
disp(['迭代次数： ',num2str(count),'次']);
disp([' X(m) Y(m) Z(m)']);
disp(['待求点坐标为： ',num2str(Ps(1:3))]);
disp([' ']);
disp([' phi(°) omega(°) kappa(°)']);
disp(['相机姿态角： ',num2str(rad2deg(Ps(4:end)))]);