156 lines
6.7 KiB
Python
156 lines
6.7 KiB
Python
|
|
|||
|
|
import os,math
|
|||
|
|
import numpy as np
|
|||
|
|
|
|||
|
|
# WGS-84经纬度转Web墨卡托
|
|||
|
|
def wgs_to_mercator(x, y):
|
|||
|
|
y = 85.0511287798 if y > 85.0511287798 else y
|
|||
|
|
y = -85.0511287798 if y < -85.0511287798 else y
|
|||
|
|
|
|||
|
|
x2 = x * 20037508.34 / 180.0
|
|||
|
|
y2 = math.log(math.tan((90.0 + y) * math.pi / 360.0)) / (math.pi / 180.0)
|
|||
|
|
|
|||
|
|
#print( ' y:',y, " before Log:",math.tan((90.0 + y) * math.pi / 360.0), ' log:' , math.log(math.tan((90.0 + y) * math.pi / 360.0)))
|
|||
|
|
y2 = y2 * 20037508.34 / 180.0
|
|||
|
|
return x2, y2
|
|||
|
|
def mercator_to_wgs(x, y):
|
|||
|
|
"""
|
|||
|
|
将墨卡托投影坐标转换为WGS-84经纬度坐标
|
|||
|
|
:param x: 墨卡托投影的X坐标
|
|||
|
|
:param y: 墨卡托投影的Y坐标
|
|||
|
|
:return: 经度（longitude）和纬度（latitude）
|
|||
|
|
"""
|
|||
|
|
# 地球半径（米）
|
|||
|
|
R = 6378137.0
|
|||
|
|
# 墨卡托投影的X坐标转换为经度
|
|||
|
|
lon = x / R * 180.0 / math.pi
|
|||
|
|
# 墨卡托投影的Y坐标转换为纬度
|
|||
|
|
lat = math.atan(math.sinh(y / R)) * 180.0 / math.pi
|
|||
|
|
return lon, lat
|
|||
|
|
|
|||
|
|
|
|||
|
|
def ImageCorToCamCor(p0,w=1920,h=1080):
|
|||
|
|
x,y=p0[0:2]
|
|||
|
|
return x-w/2.,(h-y)-h/2.
|
|||
|
|
|
|||
|
|
def wgs84_to_gcj02(lat, lon):
|
|||
|
|
"""将 WGS-84 坐标转换为 GCJ-02 坐标 (高德地图坐标)"""
|
|||
|
|
A = 6378245.0 # 长半轴
|
|||
|
|
EE = 0.00669342162296594323 # 偏心率平方
|
|||
|
|
if out_of_china(lat, lon):
|
|||
|
|
return lat, lon # 如果在中国以外，直接返回 WGS-84 坐标
|
|||
|
|
|
|||
|
|
# 坐标转换
|
|||
|
|
dlat = transform_lat(lon - 105.0, lat - 35.0)
|
|||
|
|
dlon = transform_lon(lon - 105.0, lat - 35.0)
|
|||
|
|
radlat = lat / 180.0 * math.pi
|
|||
|
|
magic = math.sin(radlat)
|
|||
|
|
magic = 1 - EE * magic * magic
|
|||
|
|
sqrt_magic = math.sqrt(magic)
|
|||
|
|
dlat = (dlat * 180.0) / (A * (1 - EE) / (magic * sqrt_magic) * math.pi)
|
|||
|
|
dlon = (dlon * 180.0) / (A / sqrt_magic * math.cos(radlat) * math.pi)
|
|||
|
|
|
|||
|
|
mg_lat = lat + dlat
|
|||
|
|
mg_lon = lon + dlon
|
|||
|
|
|
|||
|
|
return mg_lat, mg_lon
|
|||
|
|
|
|||
|
|
def out_of_china(lat, lon):
|
|||
|
|
"""检查坐标是否在中国以外"""
|
|||
|
|
return lon < 72.004 or lon > 137.8347 or lat < 0.8293 or lat > 55.8271
|
|||
|
|
|
|||
|
|
def transform_lat(lon, lat):
|
|||
|
|
"""辅助函数: 进行纬度转换"""
|
|||
|
|
ret = (-100.0 + 2.0 * lon + 3.0 * lat + 0.2 * lat * lat +
|
|||
|
|
0.1 * lon * lat + 0.2 * math.sqrt(abs(lon)))
|
|||
|
|
ret += (20.0 * math.sin(6.0 * lon * PI) + 20.0 * math.sin(2.0 * lon * PI)) * 2.0 / 3.0
|
|||
|
|
ret += (20.0 * math.sin(lat * PI) + 40.0 * math.sin(lat / 3.0 * PI)) * 2.0 / 3.0
|
|||
|
|
ret += (160.0 * math.sin(lat / 12.0 * PI) + 320.0 * math.sin(lat * PI / 30.0)) * 2.0 / 3.0
|
|||
|
|
return ret
|
|||
|
|
|
|||
|
|
def transform_lon(lon, lat):
|
|||
|
|
"""辅助函数: 进行经度转换"""
|
|||
|
|
ret = (300.0 + lon + 2.0 * lat + 0.1 * lon * lon +
|
|||
|
|
0.1 * lon * lat + 0.1 * math.sqrt(abs(lon)))
|
|||
|
|
ret += (20.0 * math.sin(6.0 * lon * PI) + 20.0 * math.sin(2.0 * lon * PI)) * 2.0 / 3.0
|
|||
|
|
ret += (20.0 * math.sin(lon * PI) + 40.0 * math.sin(lon / 3.0 * PI)) * 2.0 / 3.0
|
|||
|
|
ret += (150.0 * math.sin(lon / 12.0 * PI) + 300.0 * math.sin(lon / 30.0 * PI)) * 2.0 / 3.0
|
|||
|
|
return ret
|
|||
|
|
|
|||
|
|
def cam2word(p0,pUAV,yaw,delta=1.55e-3 * 4056.0/1920,pitch=-45,f=4.5,camH=50e3,igW=1920,igH=1080):
|
|||
|
|
|
|||
|
|
pitch = pitch/180.0*np.pi
|
|||
|
|
sinp = np.sin(pitch );cosp= np.cos(pitch)
|
|||
|
|
p0_new = ImageCorToCamCor(p0,igW,igH)
|
|||
|
|
Xc0 = p0_new[0]*delta;Zc0 = p0_new[1]*delta;
|
|||
|
|
|
|||
|
|
#(Zw0,Xw0)--相对于光心，X，Z并未校正到正东和正北。
|
|||
|
|
#f=4.5*f/24.00
|
|||
|
|
Zw0=camH*( -f*sinp + Zc0*cosp )/( f*cosp + Zc0*sinp)*1e-3
|
|||
|
|
Xw0= camH*Xc0/(f*cosp + Zc0*sinp)*1e-3
|
|||
|
|
#print(' %4.0f %4.0f %4.8f %4.8f %4.8f %4.8f f:%.2f'%( p0[0],p0[1], Xc0, Zc0,Xw0,Zw0,f ) )
|
|||
|
|
#yaw定义为拍摄方向，即图片的高方位（Z方向）偏离正北的方向，北偏东为正。
|
|||
|
|
yaw_rad = yaw/180.0*np.pi
|
|||
|
|
siny=np.sin(yaw_rad);cosy=np.cos(yaw_rad)
|
|||
|
|
Zx0_rot = Xw0*cosy + Zw0*siny + pUAV[0]
|
|||
|
|
Zw0_rot = -Xw0*siny + Zw0*cosy + pUAV[1]
|
|||
|
|
|
|||
|
|
|
|||
|
|
return Zx0_rot,Zw0_rot
|
|||
|
|
|
|||
|
|
def location( point,igW,igH,PlanWgs84,PlanH,yaw,delta,pitch,focal,outFormat='wgs84'):
|
|||
|
|
'''
|
|||
|
|
输入图像中点的X，Y坐标，及无人机相关信息、相机相关信息，输出该点的Wgs84经纬度坐标
|
|||
|
|
point--点在图像上的坐标，左上角为（0,0），X方向为宽度方向，Y方向为高度方向
|
|||
|
|
igW--图像的宽度
|
|||
|
|
igH--图像的高度
|
|||
|
|
PlanWgs84--无人机的Wgs84坐标，(lon,lat),(经度，纬度)
|
|||
|
|
PlanH--无人机拍照时的相对高度，用mm表示
|
|||
|
|
yaw--云台的yaw
|
|||
|
|
delta--单个像素的长度值，用mm表示。
|
|||
|
|
pitch--无人的pitch
|
|||
|
|
focal--真实焦距，用mm表示
|
|||
|
|
'''
|
|||
|
|
PlanX,PlanY = wgs_to_mercator(PlanWgs84[0],PlanWgs84[1])
|
|||
|
|
#print('location:',PlanX,PlanY)
|
|||
|
|
#PlanX,PlanY--东西、南北方向的墨卡托投影坐标
|
|||
|
|
#print( 'line268:',point,PlanX,PlanY,yaw, delta,pitch,focal,PlanH,igW,igH )
|
|||
|
|
cor_world = cam2word( point,(PlanX,PlanY),yaw, delta,pitch,focal,PlanH,igW,igH)
|
|||
|
|
cor_world = mercator_to_wgs(cor_world[0], cor_world[1])
|
|||
|
|
if outFormat=='GCJ02' or outFormat=='gcj02':
|
|||
|
|
cor_world = wgs84_to_gcj02(cor_world[0], cor_world[1])
|
|||
|
|
|
|||
|
|
return cor_world
|
|||
|
|
|
|||
|
|
def locate_byMqtt(box,igW,igH,camParas,outFormat='wgs84'):
|
|||
|
|
#camParas--{'lon': 3479.8250608, 'lat': 3566.7630802, 'gpssingal': 4, 'satcount': 6896, 'alt': 3.256, 'hspeed': 86.0, 'vspeed': 4.447911, 'ysingal': 0, 'tsingal': 0, 'voltage': 24.971, 'flytime': 0, 'datetime': 1739315683895, 'yaw': 70.243252, 'roll': -0.89436062, 'pitch': 0.89897547, 'armed': 'false', 'mode': 'stabilize', 'distToHome': 7.132033, 'deviceid': 'THJSQ03A2302KSPYGJ2G', 'mileage': '0', 'altasl': 21.26, 'altasl2': -20.74, 'landing_target_x': 0, 'landing_target_y': 0, 'landing_target_z': 0}
|
|||
|
|
#模型输出的点的格式是-[(486, 264), (505, 264), (505, 290), (486, 290)]
|
|||
|
|
|
|||
|
|
box_np = np.array(box);
|
|||
|
|
point = int(np.mean( box_np[:,0] )) , int(np.mean( box_np[:,1] ))
|
|||
|
|
PlanWgs84 = (float(camParas['lon']),float(camParas['lat'])) #
|
|||
|
|
PlanH = float(camParas['alt'])#
|
|||
|
|
yaw = float(camParas['camerayaw'])
|
|||
|
|
#delta = camParas['']
|
|||
|
|
delta = 1.55e-3 * 4056.0/1920
|
|||
|
|
pitch = float(camParas['camerapitch'])
|
|||
|
|
#focal = camParas['']
|
|||
|
|
focal = 3.5
|
|||
|
|
|
|||
|
|
out = location( point,igW,igH,PlanWgs84,PlanH,yaw,delta,pitch,focal,outFormat='wgs84')
|
|||
|
|
return out
|
|||
|
|
|
|||
|
|
if __name__=="__main__":
|
|||
|
|
srt="videos/DJI_20221220133918_0001_W_0.SRT"
|
|||
|
|
videoUrl = "videos/DJI_20221220133918_0001_W.MP4"
|
|||
|
|
imgOut= "videos/imgs"
|
|||
|
|
fpbeg=17273;fpend=17830
|
|||
|
|
nums = list(range(fpbeg,fpend,16))
|
|||
|
|
#generateNewSRT(srt)
|
|||
|
|
#captureImages(videoUrl,nums,imgOut)
|
|||
|
|
|
|||
|
|
process(videoUrl,srt,nums,imW=1920,imH=1080,txtDir='videos/labels' )
|
|||
|
|
#draw_results()
|
|||
|
|
|
|||
|
|
#rotate_example()
|
|||
|
|
#rotate_example3()
|