實現圖像上像素點與實際位置的GPS對應

作者有話說

這篇隨筆是基於我自己完成的一個項目，這個項目雖然看起來較為簡單，但是由於我本身不是學這個方向的，因此在做的過程中還是遇到了一些大大小小的問題。經過仔細研究並多次調試代碼，終於把這個問題的原理弄懂了。下面我將詳細介紹該問題的解決過程，並在隨筆末尾附上所有相關代碼，希望有興趣的可以一起交流學習。

目錄

• 問題介紹
• 相關專業術語介紹
• 解決思路及過程
• 一些值得注意的細節
• 結果
• 參考文獻
• MATLAB代碼

問題介紹  

無人機的出現給拍照錄像帶來了極大的便利。如下圖所示，無人機停留在高為2m的高空中，其所裝配的相機的拍攝方向與垂直方向的夾角為60°，利用相機參數求解所拍攝圖像上所有像素點的大地坐標，實現圖像上所有像素點與實際位置的GPS對應。另外，相機拍攝時的經緯度為（118.8675992，32.032575），相機的焦距為16mm，成像像素長和寬分別為4.65µm和3.9µm。

相關名詞介紹

• 焦距：焦距指鏡頭等效光心到相機傳感器的距離。
• 大地坐標：大地測量中以參考橢球面（本文是以地球表面）為基准面的坐標。地面點P的位置用大地經度L、大地緯度B和大地高H表示。當點在參考橢球面上時，僅用大地經度和大地緯度表示。大地經度是通過該點的大地子午面與起始大地子午面之間的夾角，大地緯度是通過該點的法線與赤道面的夾角，大地高是地面點沿法線到參考橢球面的距離。
• 高斯平面坐標系：指的是以中央子午線與赤道的交點作為坐標原點，以中央子午線的投影為縱坐標軸X，規定X軸向北為正，以赤道的投影為橫坐標軸Y，Y軸向東為正，形成的坐標系。

解決思路及過程

考慮到地球是橢球形狀的，而經緯度是大地坐標，不方便計算，所以先利用高斯投影正算公式將大地平面坐標轉換為高斯平面坐標，高斯平面坐標又和高度構成一個三維空間坐標。根據相機成像原理，把像素正中心坐標作為一個空間坐標，把相機中心坐標作為一個空間坐標，過這兩點的直線與高斯平面的交點就是這個像素點所對應的實際位置。

• 具體步驟

1. 我們將相機所在地理位置的大地坐標通過高斯正變換轉換成高斯平面坐標，高斯平面加上高度構成了三維空間，所以就實現了每個點都有一個三維坐標與其對應。
2. 我們根據焦距、相機中心的空間坐標、和角度確定出圖像上每個像素點所對應的空間坐標。
3. 根據像素點坐標和相機中心坐標求出像素點所對應的實際位置高斯平面中的坐標。
4. 將步驟3得到的實際位置的高斯坐標通過高斯逆變換得到其對應的大地坐標。

• 圖示說明

下圖反映了由圖片像素點得到對應實際位置的過程，考慮到成像屏的尺寸相對於拍攝的實物來說非常小，所以我們把這兩部分分開畫圖。圖1和圖2結合起來表示由像素點對應到實際位置的原理，即過像素點（彩色方格）與相機中心（空心圓點）的射線（彩線）與高斯平面的交點即為像素點對應的實際位置（射線終點）。

     

圖1  光線從相機中心到像素點示意圖

       

圖2  光線從實際位置到相機中心示意圖（斜視）

圖3  光線從實際位置到相機中心示意圖（俯視）

• 高斯投影說明

1. 高斯投影正算公式
2. 高斯投影反算公式

1.已知大地坐標$\left ( B,L \right )$及中央子午線經度$L_{0}$，計算高斯平面坐標$\left ( x,y \right )$，公式如下：

$ x=X+\frac{N}{2}sin\left ( B \right )cos\left ( B \right )l^{2}+\frac{N}{24}sin\left ( B \right )cos^{3}\left ( B \right )\left ( 5-t^{2}+9\eta ^{2}+4\eta ^{4} \right )l^{4}+\frac{N}{720}sin\left ( B \right )cos^{5}\left ( B \right )( 61-58t^{4}+270\eta ^{2}-330\eta ^{2}t^{2})l^{6}$

$y=Ncos\left ( B \right )l+\frac{N}{6}cos^{3}\left ( B \right )\left ( 1-t^{2}+\eta ^{2} \right )l^{3}+\frac{N}{120}cos^{5}\left ( B \right )\left ( 5-18t^{2}+t^{4}+14\eta ^{2}-58\eta ^{2}t^{2} \right )l^{5}$

其中，$B$為緯度，$l=L-L_{0}$，單位為弧度，$N=\frac{a}{\sqrt{1-e^{2}sin^{2}\left ( B \right )}}$，為卯酉圈曲率半徑，$t=tan\left ( B \right )$，$\eta ^{2}=e^{2}cos^{2}\left ( B \right )$，$e=\frac{\sqrt{a^{2}-b^{2}}}{b}$為第二偏心率，$a$為旋轉橢球長半軸，$b$為短半軸，$X$為子午線弧長。

2.已知高斯平面坐標$\left (x,y  \right )$及指定中央子午線經度$L_{0}$，計算大地坐標$\left ( B,L \right )$:

$B=B_{f}-\frac{t_{f}}{2M_{f}N_{f}}y^{2}+\frac{t_{f}}{24M_{f}N_{f}^{3}}\left (5+3t_{f}^{2}+\eta _{f}^{2}-9t_{f}^{2}\eta _{f}^{2}  \right )y^{4}+\frac{t_{f}}{720M_{f}N_{f}^{5}}\left ( 61+90t_{f}^{2}+45t_{f}^{4} \right )y^{6}$

$L=L_{0}+\frac{1}{N_{f}cos\left ( B_{f} \right )}y-\frac{1}{6N_{f}^{3}cos\left ( B_{f} \right )}\left ( 1+2t_{f}^{2}+\eta _{f}^{2} \right )y^{3}+\frac{1}{120N_{f}^{5}cos\left ( B_{f} \right )}\left ( 5+28t_{f}^{2}+24t_{f}^{4}+6\eta _{f}^{2}+8t_{f}^{2}\eta _{f}^{2} \right )y^{5}$

其中，$N_{f}=\frac{a}{\sqrt{1-e^{2}sin\left ( B_{f} \right )}}$，$M_{f}=\frac{a\left ( 1-e^{2} \right )}{\sqrt{\left (1-e^{2}sin\left ( B_{f} \right )  \right )^{3}}}$，$\eta _{f}^{2}=e^{2}cos^{2}\left ( B_{f} \right )$，$t_{f}=tan\left ( B_{f} \right )$，$B_{f}$為根據子午線弧長$X$反算的底點緯度。

一些值得注意的細節

保證一定的精度：因為像素尺寸非常小，他們所對應的實際位置的經緯度差別也是很小的，我剛處理的時候沒有考慮到這個問題，所以導致得到的結果很多都是一樣的（結果被截斷了）。所以我后來將輸出數據的位數設定為10為小數，才看出其差別。

結果

 經過編程計算，我們把得到經緯度數據分別根據像素大小畫出一張曲面來觀察結果的合理性，如下圖（圖4、圖5）所示：

                     

圖4：經度                                         圖5：緯度

根據圖4和圖5，可以看出經緯度變化的連續性和對稱性，而且數值又徘徊在相機經緯度坐標周圍，因此是合理的。

參考文獻鏈接

matlab大地測量高斯投影正反算程序設計實驗 - 百度文庫https://wenku.baidu.com/view/121aedfdee06eff9aff807a3.html#

MATLAB代碼

main.m

clear all;
clc;
tic
format long
jiaodu=(angle)/180*pi;
PixelLength=4.65/(10^6);
PixelWidth=3.9/(10^6);
ImageSize=[3376 6016];
U_0=ImageSize(1)/2;
V_0=ImageSize(2)/2;
a=6378137;%長半軸
f1=1/298.257223563;%扁率
b=a*(1-f1);%短半軸
e=(sqrt(a^2-b^2))/a;%第一偏心率
e_=(sqrt(a^2-b^2))/b;%第二偏心率
A=[120.2719052 33.1664600];
f=16/(10^3);
global c
c=num2str(floor(A(2)*1000));
c=str2num(c(end));
H=2;
X=latitude2meridian(dms2rad(A(:,2)),a,e);%X為子午線弧長，有緯度B算出
[x,y,L0]=GaussianMapDirect(dms2rad(A(:,2)),dms2degree(A(:,1)),X);
CameraCenter=[x,y,H];
TT=[cos(jiaodu) -sin(jiaodu);sin(jiaodu) cos(jiaodu)];
I=zeros(ImageSize(1),ImageSize(2)*3);
T=zeros(ImageSize(1),ImageSize(2)*2);
MeridianLongitude=L0;
for i=1:ImageSize(1)
    for j=1:ImageSize(2)
        I(i,(j-1)*3+1:j*3)=ImagePointCoordinates(ImageSize(1),ImageSize(2),i,j,PixelLength,PixelWidth,f,CameraCenter);
        P=I(i,(j-1)*3+1:j*3-1)-CameraCenter(1:2);
        P=P*TT;
        I(i,(j-1)*3+1:j*3-1)=P+CameraCenter(1:2);
        [T(i,(j-1)*2+1),T(i,j*2)]=ImagePointMapping(CameraCenter,I(i,(j-1)*3+1:j*3));
        [G(i,(j-1)*2+1),G(i,j*2)]=GaussianMapInverse(T(i,(j-1)*2+1),T(i,j*2),dms2rad(MeridianLongitude));
    end
end
for i=1:ImageSize(1)
    for j=1:1
       I(i,(j-1)*3+1:j*3)=ImagePointCoordinates(ImageSize(1),ImageSize(2),i,j,PixelLength,PixelWidth,f,CameraCenter);
        P=I(i,(j-1)*3+1:j*3-1)-CameraCenter(1:2);
        P=P*TT;
        I(i,(j-1)*3+1:j*3-1)=P+CameraCenter(1:2);
        [T(i,(j-1)*2+1),T(i,j*2)]=ImagePointMapping(CameraCenter,I(i,(j-1)*3+1:j*3));
        [G(i,(j-1)*2+1),G(i,j*2)]=GaussianMapInverse(T(i,(j-1)*2+1),T(i,j*2),dms2rad(MeridianLongitude));
    end
end
for i=2:ImageSize(1)
    for j=2:ImageSize(2)
        if j<=ImageSize(2)/2
            I(i,(j-1)*3+2)=I(1,(j-1)*3+2);
            I(i,(j-1)*3+1)=I(i,1);
            I(i,j*3)=I(i,3);
            T(i,(j-1)*2+1)=T(i,1);
            [T(i,(j-1)*2+1),T(i,j*2)]=ImagePointMapping(CameraCenter,I(i,(j-1)*3+1:j*3));
            [G(i,(j-1)*2+1),G(i,j*2)]=GaussianMapInverse(T(i,(j-1)*2+1),T(i,j*2),dms2rad(MeridianLongitude));
        else 
            T(i,(j-1)*2+1)=T(i,1);
            T(i,j*2)=2*CameraCenter(2)-T(i,(ImageSize(2)+1-j)*2);
            [G(i,(j-1)*2+1),G(i,j*2)]=GaussianMapInverse(T(i,(j-1)*2+1),T(i,j*2),dms2rad(MeridianLongitude));
        end
    end
end

toc

 GaussianTransformation.m

%首先讀取文件data1 .txt中的數據，計算其在相應六度帶高斯投影帶內的高斯平面直角坐標，並存貯在文件data2.txt 中，
%datal.txt格式為:經度( dd.mmss) 緯度( dd.mmss) 大地高
%data2.txt格式為:x(m) y(m) 中央子午線經度(dd.mmss)
clear all;
clc;
[filename,pathname]=uigetfile('*.*');%文件查找窗口
file=fullfile(pathname,filename);%合並路徑文件名.
A=importdata(file);%將生成的文件導入工作空間，變量名為A
%RefEllipsoid為橢球參數
%RefEllipsoid=[a,b,c,f,e2,e2_];
a=6378137;%WGS84橢球參數:長半軸
f=1/298.257223563;%扁率
b=a*(1-f);%WGS84橢球參數:短半軸
e=(sqrt(a^2-b^2))/a;
% [a b]=size(A.data);
% A1=[];
% for i=1:a
%     data1=A.data(i,:);
%     data1=data1(find(isnan(data1)==0));
%     A1=[A1;data1];
% end
% A.data=A1;
X=latitude2meridian(dms2rad(A.data(:,2)),a,e);%X為子午線弧長，有緯度B算出
[x,y,L0]=GaussianMapDirect(dms2rad(A.data(:,2)),dms2degree(A.data(:,1)),X);
B=[x,y,L0];
%B為重組矩陣
[filename_out,pathname_out]=uiputfile('*.txt','請輸入文件名');
%文件查找窗口
fileout=fullfile(pathname_out,filename_out);
%合並路徑文件名
fid=fopen(fileout,'wt');
%新建打開txt文件
fprintf(fid,'x(m) y(m) 中央子午線經度(dd.mms)\n');
[a b]=size(B);
for i=1:a
    fprintf(fid,'%f %f %f\n',B(i,:));
end
fclose(fid);

InverseGaussianTransform.m

%然后根據文件data2.txt中的高斯平面直角坐標及其中央子午線經度，計算其經緯度，並將計算結果按照格式存貯在文件data3.txt 中，
%data3.txt格式為:經度(dd.mmss) 緯度(dd.mmss)
clear all;
clc;
format long
[filename,pathname]=uigetfile('*.*');%文件查找窗口
file=fullfile(pathname,filename);%合並路徑文件名
A=importdata(file);%將生成的文件導入工作空間，變量名為A
A.data(:,3)=177*ones(10051632,1);
[L,B]=GaussianMapInverse(A.data(:,1),A.data(:,2),dms2rad(A.data(:,3)));
C=[L,B];%C為重組矩陣
[filename_out,pathname_out]=uiputfile('*.txt','請輸入文件名');
%文件查找窗口
fileout=fullfile(pathname_out,filename_out);%合並路徑文件名
fid=fopen(fileout,'wt');%新建打開txt文件
fprintf(fid,'經度(dd.mmss) 緯度(dd.mmss)\n');
[a b]=size(C);
for i=1:a
    fprintf(fid,'%.10f %.10f\n',C(i,:));
end
fclose(fid);

degree2dms.m

function dms=degree2dms(du) 
%度一>度分秒(ddmmss)
degree=fix(du);
min=fix((du-degree)*60);
second=(((du-degree)*60-min)*60);
dms=degree+min/100+second/10000;
end

dms2degree.m

function degree=dms2degree(jiaodu)
%度分秒(dd.mmss)->度
degree=fix(jiaodu);
mimute=fix((jiaodu-degree)*100);
second=(jiaodu-degree-mimute/100)*10000;
degree=degree+mimute/60+second/3600;
end

dms2rad.m

function rad=dms2rad(n)
deg=fix(n);%度取所給數n的整數部分
min_tem=(n-deg)*100;%去掉整數部分后小數點后移兩位
min=fix(min_tem);%分取整
sec=(min_tem-min)*100;%秒是小數點再向后移兩位的數字
rad=(deg+min/60.00+sec/3600)*pi/180.0;
end

GaussianMapDirect.m

function [x,y,L0]=GaussianMapDirect(B,L,X)
%WGS84橢球參數:
a=6378137;%長半軸
f=1/298.257223563;%扁率
b=a*(1-f);%短半軸
e=(sqrt(a^2-b^2))/a;%第一偏心率
e_=(sqrt(a^2-b^2))/b;%第二偏心率
%當地中央子午線決定於當地的直角坐標系統，首先確定您的直角坐標系統是3度帶還是6度帶投影，然后再根據如下公式推算。
Q=input('請選擇投影帶:\n');
if Q==6
    %6度帶:
    M=round((L+3)./6);
    %帶號M=round[(L+3)/6]，即對(L+3)/6的值四舍五入取整數，L為當地經度;
    L0=6.*M-3 ;
    %則中央子午線經度L0=6 X M-3
else
    %3度帶:
    M=round(L./3);%帶號M=round([L/3)，即對(L/3)的值四舍五入取整數，L為當地經度;
    L0=3.*M;
    %則中央子午線經度L0=3 X M
end
l=(L-L0).*pi/180;
N=a./(sqrt(1-(e^2).*(sin(B).^2)));
t=tan(B);
p=e_.*cos(B);%p表示yita
%計算高斯平面坐標(x，y)
x=X+(N.*(sin(B)).*(cos(B)).*(l.^2))./2+(N.*(sin(B)).*((cos(B)).^3).*(5-((t).^2)+9.*(p.^2)+4.*(p.^4)).*(l.^4))./24+(N.*sin(B).*(cos(B)).^5.*(61-58.*(t.^2)+(t.^4)+270.*(p.^2)-330.*(p.^2).*(t.^2)).*(l.^6))./720;
y=N.*cos(B).*l+(N.*(cos(B)).^3.*(1-t.^2+p.^2).*(l.^3))./6+(N.*((cos(B)).^5).*(5-18.*(t.^2)+t.^4+14.*(p.^2)-58.*(p.^2).*(t.^2)).*(l.^5))./120;
L0=degree2dms(L0);
end

GaussianMapInverse.m

function [L,B] = GaussianMapInverse(x,y,L0)
%WGS84橢球參數:
a=6378137;%長半軸
f=1/298.257223563; %扁率
b=a*(1-f);%短半軸
e=(sqrt(a^2-b^2))/a;%第- -偏心率
e_=(sqrt(a^2-b^2))/b;%第二偏心率
%根據中央子午線弧長x反算底點緯度Bf
Bf=meridian2latitude(x,a,e);
Nf=a./sqrt(1-(e.^2).*(sin(Bf).^2));
Mf=a.*(1-e.^2)./sqrt((1-(e.^2).*(sin(Bf).^2)).^3);
pf=e_.*cos(Bf);
tf=tan(Bf);
%已知高斯平面坐標(x, y)及指定中央子午線經度L0，計算大地坐標(B,L)
B=Bf-tf.*(y.^2)./(2.*Mf.*Nf)+tf.*(5+3.*(tf.^2)+pf.^2-9.*(tf.^2).*(pf.^2)).*(y.^4)./(24.*Mf.*(Nf.^3))+tf.*(61+90.*(tf.^2)+45.*(tf.^4)).*(y.^6)./(720.*Mf.*(Nf.^5));
L=L0+y./(Nf.*cos(Bf))-(1+2.*(tf.^2)+pf.^2).*y.^3./(6.*(Nf.^3).*cos(Bf))+(5+28.*tf.^2+24.*tf.^4+6.*pf.^2+8.*(tf.^2).*pf.^2).*y.^5./(120.*Nf.^5.*cos(Bf));
B=rad2dms(B);
L=rad2dms(L);
end

ImagePointCoordinates.m

function PixelCoordinates=ImagePointCoordinates(L,H,m,n,PixelLength,PixelWidth,focalLength,CameraCenter)
format long g
HH=H/2;
LL=L/2;
ImageCenter(1)=CameraCenter(1)-focalLength*sin(pi/3);
ImageCenter(2)=CameraCenter(2);
ImageCenter(3)=CameraCenter(3)+focalLength*sin(pi/3);
if m<=HH&&n<=LL
    PixelCoordinates(1)=ImageCenter(1)+((HH-m)*PixelWidth+0.5*PixelWidth)*sin(pi/6);
    PixelCoordinates(2)=ImageCenter(2)+(LL-n)*PixelLength+0.5*PixelLength;
    PixelCoordinates(3)=ImageCenter(3)+((HH-m)*PixelWidth+0.5*PixelWidth)*cos(pi/6);
end
if m<=HH&&n>LL
    PixelCoordinates(1)=ImageCenter(1)+((HH-m)*PixelWidth+0.5*PixelWidth)*sin(pi/6);
    PixelCoordinates(2)=ImageCenter(2)+(LL-n)*PixelLength-0.5*PixelLength;
    PixelCoordinates(3)=ImageCenter(3)+((HH-m)*PixelWidth+0.5*PixelWidth)*cos(pi/6);
end
if m>HH&&n<=LL
    PixelCoordinates(1)=ImageCenter(1)+((HH-m)*PixelWidth-0.5*PixelWidth)*sin(pi/6);
    PixelCoordinates(2)=ImageCenter(2)+(LL-n)*PixelLength+0.5*PixelLength;
    PixelCoordinates(3)=ImageCenter(3)+((HH-m)*PixelWidth-0.5*PixelWidth)*cos(pi/6);
end
if m>HH&&n>LL
    PixelCoordinates(1)=ImageCenter(1)+((HH-m)*PixelWidth-0.5*PixelWidth)*sin(pi/6);
    PixelCoordinates(2)=ImageCenter(2)+(LL-n)*PixelLength-0.5*PixelLength;
    PixelCoordinates(3)=ImageCenter(3)+((HH-m)*PixelWidth-0.5*PixelWidth)*cos(pi/6);
end

 ImagePointMapping.m

function [x,y]=ImagePointMapping(CameraCenter,Point)
format long
x=-CameraCenter(3)/(CameraCenter(3)-Point(3))*(CameraCenter(1)-Point(1))+CameraCenter(1);
y=-CameraCenter(3)/(CameraCenter(3)-Point(3))*(CameraCenter(2)-Point(2))+CameraCenter(2);
end

latitude2meridian.m

function x=latitude2meridian(B,a,e)
%由緯度B求對應的子午線弧長x，計算公式
m0=a*(1-e^2);
m2=(3*(e^2)*m0)/2;
m4=(5*(e^2)*m2)/4;
m6=(7*(e^2)*m4)/6;
m8=(9*(e^2)*m6)/8;
a0=m0+m2/2+3*m4/8+5*m6/16+35*m8/128;
a2=m2/2+m4/2+15*m6/32+7*m8/16;
a4=m4/8+3*m6/16+7*m8/32;
a6=m6/32+m8/16;
a8=m8/128;
x=a0*B-(a2*sin(2*B))/2+(a4*sin(4*B))/4-(a6*sin(6*B))/6+(a8*sin(8*B))/8;
end

meridian2latitude.m

function B=meridian2latitude(x,a,e)
m0=a*(1-e^2);
m2=(3*(e^2)*m0)/2;
m4=(5*(e^2)*m2)/4;
m6=(7*(e^2)*m4)/6;
m8=(9*(e^2)*m6)/8;
a0=m0+m2/2+3*m4/8+5*m6/16+35*m8/128;
a2=m2/2+m4/2+15*m6/32+7*m8/16;
a4=m4/8+3*m6/16+7*m8/32;
a6=m6/32+m8/16;
a8=m8/128;
%%緯度B的計算
B0=x./a0;%B的初始值
while 1
    F=-(a2.*sin(2.*B0))./2+(a4.*sin(4.*B0))./4-(a6.*sin(6.*B0))./6+(a8.*sin(8.*B0))./8;
    B=(x-F)./a0;
    if abs(B0-B)<10^(-6)
        break;
    end
    abs(B0-B);
    B0=B;
end
end

rad2dms.m

function dms=rad2dms(azimuth)%弧度轉度分秒
dgree_tem=azimuth*180/pi;
dgree=fix(dgree_tem);
min_tem=((dgree_tem-dgree)*60);
min=fix(min_tem);
second=((min_tem-min)*60);
dms=dgree+min/100+second/10000;
end