matlab實作pnp問題，實作後方交會

一、原理流程
- 0、BLH2XYZ
- 1、求解 P
- - 1.1、坐标歸一化 T
  - 1.2、系數矩陣A 定義
  - 1.3 DLT 求解初值P0'
  - 1.4 疊代優化
  - 1.5 解除歸一化，求得P
- 2、求解C = -M^(-1)P_4
- 3、 M --QR分解得到 R&K
- 4、R(相機-世界坐标) 到RC，并求姿态(難點！！！)
- 5、利用單像求解像點對應三維點（增加輔助限制：DEM/DSM）
二、代碼
- 1、主程式
- 2、函數function .m檔案
- - 2.1、normalize.m
  - 2.2、normalize_2d.m
  - 2.3、normalize_3d.m
  - 2.4、get_plane.m
  - 2.5、geometric_error.m

一、原理流程

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個姿态角（輪轉角、偏航角、俯仰角）

matlab解決pnp問題，實作後方交會一、原理流程二、代碼

參考連結

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問題，實作後方交會

一、原理流程

0、BLH2XYZ

1、求解 P

1.1、坐标歸一化 T

1.2、系數矩陣A 定義

1.3 DLT 求解初值P0’

1.4 疊代優化

1.5 解除歸一化，求得P

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

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

4、R(相機-世界坐标) 到RC，并求姿态(難點！！！)

5、利用單像求解像點對應三維點（增加輔助限制：DEM/DSM）

二、代碼

1、主程式

2、函數function .m檔案

2.1、normalize.m

2.2、normalize_2d.m

2.3、normalize_3d.m

2.4、get_plane.m

2.5、geometric_error.m

繼續閱讀

POPUP_TO_DECIDE_LIST 彈出選擇框_SAP劉夢_新浪部落格

記錄一些Matlab用法記錄一些Matlab函數用法

matlab中cumsum函數

【重學Matlab】Note4 矩陣相關

數學模組化淺談

模組化：數學模組化

數學模組化——層次分析法（Matlab）【評價類問題】建立遞階層次結構構造判斷矩陣一緻性檢驗計算總權重并排序

數學模組化智能優化算法之神經網絡案例附Matlab代碼

simulink bus總線建立方法

拉格朗日插值 matlab

Lagrange插值函數及其Matlab代碼

使用自相關函數的紋理圖像周期計算

2018年高教社杯A題高溫作業專用服裝設計

圖像壓縮編碼碼matlab實作——行程編碼

【可靠性評估】電力系統可靠性評估matlab仿真1.軟體版本2.本算法理論知識

MATLAB:非線性規劃fmincon

matlab解決pnp問題，實作後方交會一、原理流程二、代碼

matlab實作pnp問題，實作後方交會

一、原理流程

0、BLH2XYZ

1、求解 P

1.1、坐标歸一化 T

1.2、 系數矩陣A 定義

1.3 DLT 求解初值P0’

1.4 疊代優化

1.5 解除歸一化，求得P

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

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

4、R(相機-世界坐标) 到RC，并求姿态(難點！！！)

5、利用單像求解像點對應三維點（增加輔助限制：DEM/DSM）

二、代碼

1、主程式

2、函數function .m檔案

2.1、normalize.m

2.2、normalize_2d.m

2.3、normalize_3d.m

2.4、get_plane.m

2.5、geometric_error.m

繼續閱讀

1.2、系數矩陣A 定義