一、原理流程

0、BLH2XYZ

经纬度坐标转换空间之间坐标

1、求解 P

1.1、坐标归一化 T

质心为原点，到原点距离的均值为sqrt(2)
fsolve函数：用此函数解算满足要求的T矩阵的各非零元素，函数fsolve中的function参数采用.m文件来定义

1.2、 系数矩阵A 定义

由xi,Xi的数值构成

1.3 DLT 求解初值P0’

A矩阵SVD分解后，右奇异矩阵最小特征值对应的特征向量，为AP=0 的线性解，以此作为P的初值P0

1.4 迭代优化

算法-levenberg marquardt 最小化几何（投影）误差，优化P
lsqnonlin:采用该函数实现迭代优化，函数lsqnonlin中的function参数采用.m文件来定义

1.5 解除归一化，求得P

2、求解C = -M^(-1)P_4

M是P 3x3的子矩阵

3、 M --QR分解得到 R&K

针对分解得到的R&K，结合射影的实际空间情形，进行简单变换，参考链接
make diagonal of K positive：

T = diag(sign(diag(K)));
K = K * T;
R = T * R; % (T is its own inverse)

如果图像y轴和摄像机y轴指向相反方向，将K第二列以及R的第二行取反：

K = K.*[1,-1,1;1,-1,1;1,-1,1];
R = R.* [1,1,1;-1,-1,-1;1,1,1];

4、R(相机-世界坐标) 到RC，并求姿态(难点！！！)

（相机姿态变换，世界坐标-相机坐标系 R_c）R = R_cT（参考链接）
矩阵的分解针对于R_c

旋转矩阵的分解：
坐标系1-坐标系2的旋转矩阵为R12，则针对R12的以X轴为主轴的分解得到的角度为坐标系2相对于坐标系1的3个姿态角（轮转角、偏航角、俯仰角）

参考链接

5、利用单像求解像点对应三维点（增加辅助约束：DEM/DSM）

平面拟合：采用fsolve函数，解算平面参数，function参数设置同上
采用xi = P *Xi 以及约束条件 S（Xi） =0 求解Xi: 采用inv处理求解由约束条件构造的系数阵，进而求得Xi的解

二、代码

1、主程序

point_count = 6;
m = [xx,xx,xx,xx,xx,xx];% x坐标矩阵
n = [xx,xx,xx,xx,xx,xx];% y坐标矩阵
M = [xx,xx,xx,xx,xx,xx];% X坐标矩阵
N = [xx,xx,xx,xx,xx,xx];% Y坐标矩阵
V = [xx,xx,xx,xx,xx,xx];%Z 坐标矩阵
% %0 坐标转换BLH 2 XYZ
% 
% lat = [xx,xx,xx,xx,xx,xx];% 
% lon = [xx,xx,xx,xx,xx,xx];% 
% H1 = [xx,xx,xx,xx,xx,xx];%Z 坐标矩阵
% f = 1 / 298.257223563;
% a= 6378137;
% b = a * (1 - f);
% e = sqrt(a * a - b * b) / a;
% 
% for i = 1:point_count
%     N(i) = a / sqrt(1 - e * e * sin(lat(i) * pi / 180) * sin(lat(i) * pi/ 180));
%     WGS84_X(i) = (N(i) + H1(i)) * cos(lat(i) * pi / 180) * cos(lon(i) * pi / 180);
%     WGS84_Y(i) = (N(i) + H1(i)) * cos(lat(i) * pi / 180) * sin(lon(i) * pi / 180);
%     WGS84_Z(i) = (N(i)* (1 - (e * e)) + H1(i)) * sin(lat(i) * pi / 180);
%     
% end

% M = WGS84_X% X坐标矩阵
% N = WGS84_Y% Y坐标矩阵
% V = WGS84_Z%Z 坐标矩阵


% 1、求解 P


%1.1坐标归一化 T:质心为原点，到原点距离的均值为sqrt(2)
%2d

T_2d = normalize([m;n])

mn = T_2d * [m;n;ones(1,point_count)];
m = mn(1,:);
n = mn(2,:);


%3d
T_3d = normalize([M;N;V])


MNV = T_3d*[M;N;V;ones(1,point_count)];

M = MNV(1,:);
N = MNV(2,:);
V = MNV(3,:);



% 1.2 系数矩阵A 定义

A = [];
for i = 1:point_count
    
     A = [A;[0,0,0,0,-M(i),-N(i),-V(i),-1,n(i)*M(i),n(i)*N(i),n(i)*V(i),n(i);
         M(i),N(i),V(i),1,0,0,0,0,-m(i)*M(i),-m(i)*N(i),-m(i)*V(i),-m(i)]];
end

%1.3 DLT 求解初值P0'
[U,S,V]=svd(A);
P0 = V(:,end);
% size(P0)

sv = P0.*  P0;     %the vector with elements 
                % as square of v's elements
dp = sum(sv);    % sum of squares -- the dot product
mag = sqrt(dp)  % magnitude

% aa = norm(P0)

%1.4 迭代算法-levenberg marquardt 最小化几何误差

% options = optimoptions(@lsqnonlin,'Algorithm','Levenberg-Marquardt','Display','iter');
options = optimoptions(@lsqnonlin,'Algorithm','Levenberg-Marquardt');
[x,resnorm,residual,exitflag,output] = lsqnonlin(@geometric_error,P0,[],[],options);
output;
P= reshape(x,3,4);
% norm(P)
sv = x.*  x;     %the vector with elements 
                % as square of v's elements
dp = sum(sv);    % sum of squares -- the dot product
mag = sqrt(dp);  % magnitude

%1.5 解除归一化，求得P

P = inv(T_2d)*P*T_3d;



% 2、求解C = -M^(-1)P4

M= P(:,1:3);
p4 = P(:,end);
C = -inv(M)*p4


%3、 M QR分解得到 R&K

[Q,R]=qr(M);
K = Q;
%make diagonal of K positive
T = diag(sign(diag(K)));
K = K * T;
R = T * R; % (T is its own inverse)

%如果图像y轴和摄像机y轴指向相反方向，将K第二列以及R的第二行取反

K = K.*[1,-1,1;1,-1,1;1,-1,1];
R = R.* [1,1,1;-1,-1,-1;1,1,1];

% 4、R(相机-世界坐标) 到RC（相机姿态变换，世界坐标-相机坐标系）R = RCT
Rc = R';

% 分解Rc,以Z 为主轴的转角系统

theta_z = atan2(Rc(2,1),Rc(1,1))/pi*180
theta_y = atan2(-Rc(3,1),sqrt(Rc(1,1).^2+Rc(2,1).^2))/pi*180
theta_x = atan2(Rc(3,2),Rc(3,3))/pi*180

% 5、求解成像最远点

% 5.1 求解四角点三维坐标

%5.1.1 平面拟合


plane0 = [1,1,1];
options = optimoptions('fsolve','Display','none','Algorithm','Levenberg-Marquardt');
plane = fsolve(@get_plane,plane0,options);


height = 1080;
width = 1920;
corner_point_2d = [0,0,1;0,height,1;width,0,1;width,height,1]';

corner_point_3d = inv([P;[plane,1]])*[corner_point_2d;zeros(1,4)];
corner_point_3d = corner_point_3d /diag(corner_point_3d(end,:));
corner_point_3d = corner_point_3d(1:3,:)%转为非齐次坐标
vector_3d = corner_point_3d-C;

for i = 1:4
    distance(i) = norm(vector_3d(1:2,i));
end

distance_max = max( distance)

2、函数function .m文件

2.1、normalize.m

function T = normalize(cors)
[height,width] = size(cors);
x0 = [1,zeros(1,height)];
% options = optimoptions('fsolve','Display','none','PlotFcn',@optimplotfirstorderopt);
options = optimoptions('fsolve','Display','none');
if (height ==2)
    
    t = fsolve(@normalize_2d,x0,options);% T矩阵的三个参数
    
else
    
    t = fsolve(@normalize_3d,x0,options);% T矩阵的三个参数
end

T = [[diag(t(1)*ones(1,height));zeros(1,height)],[t(:,2:end)';1]];

2.2、normalize_2d.m

function F = normalize_2d(x)

point_count = 6;
m = [292,671,1444,1500,1265,902];% x坐标矩阵
n = [748,606,404,275,506,599];% y坐标矩阵
sum_m = sum(m);
sum_n = sum(n);
eqn_dis =0;
for i  = 1:point_count
    eqn_dis = eqn_dis + sqrt((x(1)*m(i)+x(2)).^2+(x(1)*n(i)+x(3)).^2);
end

F(1) = sum_m*x(1)+point_count*x(2);
F(2) = sum_n*x(1)+point_count*x(3);
F(3) = eqn_dis-sqrt(2)*point_count;

2.3、normalize_3d.m

function F = normalize_3d(x)

point_count = 6;
M = [xx,xx,xx,xx,xx,xx];% X坐标矩阵
N = [xx,xx,xx,xx,xx,xx];% Y坐标矩阵
V = [xx,xx,xx,xx,xx,xx];%Z 坐标矩阵
sum_M = sum(M);
sum_N = sum(N);
sum_V = sum(V);
eqn_dis =0;
for i  = 1:point_count
    eqn_dis = eqn_dis + sqrt((x(1)*M(i)+x(2)).^2+(x(1)*N(i)+x(3)).^2+(x(1)*V(i)+x(4)).^2);
end

F(1) = sum_M*x(1)+point_count*x(2);
F(2) = sum_N*x(1)+point_count*x(3);
F(3) = sum_V*x(1)+point_count*x(4);
F(4) = eqn_dis-sqrt(2)*point_count;

2.4、get_plane.m

function F = get_plane(x)

M = [xx,xx,xx,xx,xx,xx];% X坐标矩阵
N = [xx,xx,xx,xx,xx,xx];% Y坐标矩阵
V = [xx,xx,xx,xx,xx,xx];%Z 坐标矩阵
for i = 1 : 6
    
    F(i) = x(1)* M(i)+x(2)*N(i)+ x(3)* V(i)+1;
end

2.5、geometric_error.m

function F = geometric_error(P)


m = [xx,xx,xx,xx,xx,xx];% x坐标矩阵
n = [xx,xx,xx,xx,xx,xx];% y坐标矩阵

M = [xx,xx,xx,xx,xx,xx];% X坐标矩阵
N = [xx,xx,xx,xx,xx,xx];% Y坐标矩阵
V = [xx,xx,xx,xx,xx,xx];%Z 坐标矩阵


%坐标归一化
%2d


x0 = [1,0,0];

% options = optimset('Display','iter','TolFun','1e-8');
% options = optimoptions('fsolve','Display','none','PlotFcn',@optimplotfirstorderopt);
fun = @normalize_2d;
x_result= fsolve(fun,x0);% T矩阵的三个参数

T_2d = [x_result(1),0,x_result(2);0,x_result(1),x_result(3);0,0,1];

m = m*x_result(1)+x_result(2);
n = n*x_result(1)+x_result(3);

%3d
xx_0 = [1,0,0,0];

fun_3d = @normalize_3d;
xx_result= fsolve(fun_3d,xx_0);% T矩阵的三个参数

T_3d = [xx_result(1),0,0,xx_result(2);0,xx_result(1),0,xx_result(3);0,0,xx_result(1),xx_result(4);0,0,0,1];

%归一化
M = M *xx_result(1)+xx_result(2);
N = N *xx_result(1)+ xx_result(3);
V = V*xx_result(1)+ xx_result(4);
F = 0;
point_count = 6;
for i = 1:point_count
    x = [m(i),n(i),1]';
    X = [M(i),N(i),V(i),1]';
    P = reshape(P,3,4);
    F = F + norm(x-P*X).^2;
end    

最后

以上就是飞快外套为你收集整理的matlab解决pnp问题，实现后方交会一、原理流程二、代码的全部内容，希望文章能够帮你解决matlab解决pnp问题，实现后方交会一、原理流程二、代码所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。