固定攝像頭檢測行人投射到gps座標

場景是在2樓我們固定了一個攝像頭，然後實現yolo檢測行人，並把行人的gps座標試試顯示出來

下面附上部分代碼：

import math,os


m_nScreenResolution_l = 640;
m_nScreenResolution_h = 480;
m_dScreenResolution_angle = 90/640

m_dPlane_kappa = 0;
m_dPlane_phi = 0;
m_dPlane_omega = 0;
# 攝像頭高度
m_dFixed_elevation = 30;
m_dTarget_elevation = 0;

location =(31.253190,121.495568)

# 攝像頭gps位置
m_dFixed_longitude = 119.4461939288;
m_dFixed_latitude = 32.3946498603;
# 偏航角
m_dCamera_kappa = 177;
# 俯仰角
m_dCamera_phi = 25;

m_nPic_x = 800
m_nPic_y = 556



PI = math.pi

path1 = '/data/drone_detect/detection_track/yolo_drone/190809/txt/'
# path2 = './correction_txt/'
path3 = '/data/drone_detect/detection_track/yolo_drone/190809/weizhi/'
txtfiles = os.listdir(path1)
txtfiles = sorted(txtfiles)
for file in txtfiles:
	file1 = file
	print(file1)
	with open(path3+file1,"w") as fgps:
		with open(path1+file,"r") as fxy:
			xy = fxy.readlines()
			for cor in xy:
				print(cor)
				m_nPic_x = float(cor.strip('\n').split(' ')[-2])
				m_nPic_y =float(cor.strip('\n').split(' ')[-1])
				C30 = (m_nPic_x - m_nScreenResolution_l/2) * m_dScreenResolution_angle + m_dCamera_kappa
				C31 = m_dCamera_phi - ((m_nScreenResolution_h/2) - m_nPic_y) * m_dScreenResolution_angle
				C32 = math.sqrt((pow(C30, 2) + pow(C31, 2)))
				C34 = math.atan(C31 / C30) + (PI * m_dPlane_omega / 180)
				C36 = C32 * math.sin(C34) * abs(C30) / C30
				C37 = C32 * math.cos(C34) * abs(C30) / C30 + m_dPlane_kappa
				C38 = C36 - m_dPlane_phi
				C39 = (m_dFixed_elevation - m_dTarget_elevation)/math.tan(PI * C38 / 180)
				C41 =   C39 * math.sin(PI * C37 / 180)
				longitude = m_dFixed_longitude + C41/111000/math.cos(PI * m_dFixed_latitude / 180)
				C40 =   C39 * math.cos(PI * C37 / 180)
				latitude = m_dFixed_latitude + C40 / 111000
				#print(str(latitude)+','+str(longitude),C30,C31,C32,C34,C36,C37,C38,C40,C41)

				fgps.write(cor.strip('\n').split(' ')[0]+','+str(latitude)+','+str(longitude)+'\n')








'''
double m_dFov_x;                        //FOV視場水平角 C1
double m_dFov_y;                        //FOV視場垂直角 C2
int    m_nScreenResolution_l;           //畫面分辨率L   C3
int    m_nScreenResolution_h;           //畫面分辨率H   C4
double m_dScreenResolution_angle;     //畫面角度分辨率   C5
double m_dPlane_kappa;                   //飛機 偏航角  C8
double m_dPlane_phi;                     //飛機 俯仰角  C9
double m_dPlane_omega;                   //飛機 橫滾角  C10
double m_dFixed_longitude;               //定位 經度    C11
double m_dFixed_latitude;                //定位 緯度    C12
double m_dFixed_elevation;               //定位 海拔    C13
double m_dCamera_kappa;                  //攝像頭 偏航角  C14
double m_dCamera_phi;                    //攝像頭 俯仰角  C15
int    m_nPic_x;                         //畫面   x     C16
int    m_nPic_y;                         //畫面   y     C17
double m_dTarget_elevation;              //目的點 海拔    C18
'''