325 lines
10 KiB
Python
325 lines
10 KiB
Python
|
import numpy as np
|
|||
|
|
import time
|
|||
|
|
import math
|
|||
|
|
import os
|
|||
|
|
|
|||
|
|
import pyproj
|
|||
|
|
|
|||
|
|
from middleware.entity.camera_para import Camera_Para
|
|||
|
|
from touying.ImageReproject_python.img_types import Point, CamParas
|
|||
|
|
from touying.ImageReproject_python.image_reproject import ir_target_positioning, ir_red_line_reproject, \
|
|||
|
|
ir_img_reproject, ir_target_positioning_new_list
|
|||
|
|
|
|||
|
|
os.environ['GDAL_DISABLE_EXCEPTIONS'] = 'NO' # 显式启用异常处理
|
|||
|
|
# 定义常量
|
|||
|
|
pi = math.pi
|
|||
|
|
|
|||
|
|
|
|||
|
|
def target_positioning(u_pixel, v_pixel, gimbal_yawm, gimbal_pitch, gimbal_roll, height):
|
|||
|
|
"""测试目标定位功能"""
|
|||
|
|
# 输入参数
|
|||
|
|
u = 1520 # 示例图像u坐标
|
|||
|
|
v = 2512 # 示例图像v坐标
|
|||
|
|
interval = 0.05 # 高程迭代间隔
|
|||
|
|
gimbal_yaw = 66.40 * pi / 180 # 示例云台偏航角
|
|||
|
|
gimbal_pitch = -90 * pi / 180 # 示例云台俯仰角
|
|||
|
|
gimbal_roll = 0 * pi / 180 # 示例云台横滚角
|
|||
|
|
h = 590.834 # 示例相机高度
|
|||
|
|
l = (103 + 59 / 60.0 + 26.07 / 3600.0) * pi / 180 # 示例相机经度
|
|||
|
|
b = (30 + 45 / 60.0 + 46.48 / 3600.0) * pi / 180 # 示例相机纬度
|
|||
|
|
img_width = 4032 # 示例图像宽度
|
|||
|
|
img_height = 3024 # 示例图像高度
|
|||
|
|
|
|||
|
|
# 相机参数
|
|||
|
|
cam_paras = CamParas(
|
|||
|
|
fx=2795.68899,
|
|||
|
|
fy=2795.68899
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# DEM文件路径
|
|||
|
|
# dem_file = "/home/GW/ImageReproject_python/dem.tif" # 确保此文件存在
|
|||
|
|
dem_file = r"D:\project\test-touying\ImageReproject_python\dem.tif" # 确保此文件存在
|
|||
|
|
|
|||
|
|
# 测量执行时间
|
|||
|
|
start_time = time.time()
|
|||
|
|
|
|||
|
|
# 执行目标定位
|
|||
|
|
x, y, z = ir_target_positioning(
|
|||
|
|
u, v, interval, gimbal_pitch, gimbal_yaw, gimbal_roll,
|
|||
|
|
h, l, b, cam_paras, img_width, img_height, dem_file
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 计算执行时间
|
|||
|
|
elapsed_time = (time.time() - start_time) * 1000 # 转换为毫秒
|
|||
|
|
|
|||
|
|
# 输出结果
|
|||
|
|
print(f"目标定位耗时: {elapsed_time:.2f} ms")
|
|||
|
|
print(f"目标位置: 经度={x}°, 纬度={y}°, 高度={z:.2f}m")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def read_txt(filename):
|
|||
|
|
"""从文本文件读取点坐标"""
|
|||
|
|
points = []
|
|||
|
|
try:
|
|||
|
|
with open(filename, 'r') as f:
|
|||
|
|
for line in f:
|
|||
|
|
parts = line.strip().split()
|
|||
|
|
if len(parts) >= 4: # 标签 x y z
|
|||
|
|
label = parts[0]
|
|||
|
|
x = float(parts[1])
|
|||
|
|
y = float(parts[2])
|
|||
|
|
z = float(parts[3])
|
|||
|
|
point = Point(x, y, z)
|
|||
|
|
points.append(point)
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"读取文件失败: {filename}, 错误: {e}")
|
|||
|
|
|
|||
|
|
return points
|
|||
|
|
|
|||
|
|
|
|||
|
|
def red_line_reproject(gimbal_yaw, gimbal_pitch, gimbal_roll, height, cam_longtitude, cam_altitude, img_width,
|
|||
|
|
img_height, points,camera_para:Camera_Para):
|
|||
|
|
"""测试红线重投影功能"""
|
|||
|
|
# 输入参数
|
|||
|
|
# print(f"gimbal_yaw {gimbal_yaw}")
|
|||
|
|
gimbal_yaw = gimbal_yaw * pi / 180 # 示例云台偏航角
|
|||
|
|
# print(f"gimbal_pitch {gimbal_pitch}")
|
|||
|
|
gimbal_pitch = gimbal_pitch * pi / 180 # 示例云台俯仰角
|
|||
|
|
# print(f"gimbal_roll {gimbal_roll}")
|
|||
|
|
gimbal_roll = gimbal_roll * pi / 180 # 示例云台横滚角
|
|||
|
|
# print(f"height {height}")
|
|||
|
|
h = height # 示例相机高度
|
|||
|
|
|
|||
|
|
|
|||
|
|
l = cam_longtitude * pi / 180
|
|||
|
|
b = cam_altitude * pi / 180
|
|||
|
|
|
|||
|
|
# img_width = 4032 # 示例图像宽度
|
|||
|
|
# img_height = 3024 # 示例图像高度
|
|||
|
|
# # # 相机参数；校友之家 4D
|
|||
|
|
# cam_paras = CamParas(
|
|||
|
|
# fx=3659.69045,
|
|||
|
|
# fy=3659.69045,
|
|||
|
|
# cx=5280/2-40,
|
|||
|
|
# cy=3956/2-20,
|
|||
|
|
# )
|
|||
|
|
|
|||
|
|
|
|||
|
|
# # # 相机参数；哈密4TD
|
|||
|
|
# cam_paras = CamParas(
|
|||
|
|
# fx=2795.68899,
|
|||
|
|
# fy=2795.68899,
|
|||
|
|
# cx=4032/2,
|
|||
|
|
# cy=3024/2,
|
|||
|
|
# )
|
|||
|
|
cam_paras = CamParas(
|
|||
|
|
fx=camera_para.fx,
|
|||
|
|
fy=camera_para.fy,
|
|||
|
|
cx=camera_para.cx/2,
|
|||
|
|
cy=camera_para.cy/2,
|
|||
|
|
)
|
|||
|
|
# 直接使用摄像头拍照的宽度
|
|||
|
|
camera_img_width=camera_para.cx
|
|||
|
|
# 直接使用摄像头拍照的高度
|
|||
|
|
camera_img_height=camera_para.cy
|
|||
|
|
# 图像文件路径
|
|||
|
|
# img_file = r"D:\project\test-touying\ImageReproject_python\test_image.jpeg" # 确保此文件存在
|
|||
|
|
# img_save_file = r"D:\project\test-touying\ImageReproject_python\test_image_reprojected.jpg"
|
|||
|
|
|
|||
|
|
# # 读取点坐标
|
|||
|
|
# points = [] # 如果有点坐标文件，可以使用read_txt函数读取
|
|||
|
|
# points = read_txt(r"D:\project\test-touying\ImageReproject_python\points.txt")
|
|||
|
|
# 测量执行时间
|
|||
|
|
start_time = time.time()
|
|||
|
|
|
|||
|
|
# # 输入参数
|
|||
|
|
# # gimbal_yaw = 66.40 * pi / 180 # 示例云台偏航角
|
|||
|
|
# gimbal_pitch = -90 * pi / 180 # 示例云台俯仰角
|
|||
|
|
# gimbal_roll = 0 * pi / 180 # 示例云台横滚角
|
|||
|
|
# h = 590.834 # 示例相机高度
|
|||
|
|
# l = (103 + 59 / 60.0 + 26.07 / 3600.0) * pi / 180 # 示例相机经度
|
|||
|
|
# b = (30 + 45 / 60.0 + 46.48 / 3600.0) * pi / 180 # 示例相机纬度
|
|||
|
|
# img_width = 4032 # 示例图像宽度
|
|||
|
|
# img_height = 3024 # 示例图像高度
|
|||
|
|
|
|||
|
|
# 执行红线重投影
|
|||
|
|
# print(f"gimbal_pitch {gimbal_pitch}")
|
|||
|
|
# print(f"gimbal_yaw {gimbal_yaw}")
|
|||
|
|
# print(f"gimbal_roll {gimbal_roll}")
|
|||
|
|
# print(f"h {h}")
|
|||
|
|
# print(f"l {l}")
|
|||
|
|
# print(f"b {b}")
|
|||
|
|
# print(f"cam_paras {cam_paras}")
|
|||
|
|
# print(f"img_width {img_width}")
|
|||
|
|
# print(f"img_height {img_height}")
|
|||
|
|
# print(f"points {points}")
|
|||
|
|
# utm_points = []
|
|||
|
|
utm_points=convert_points_to_utm(points)
|
|||
|
|
pixel_list = ir_red_line_reproject(
|
|||
|
|
gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b,
|
|||
|
|
cam_paras, img_width, img_height,camera_img_width,camera_img_height, utm_points
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 计算执行时间
|
|||
|
|
elapsed_time = (time.time() - start_time) * 1000 # 转换为毫秒
|
|||
|
|
|
|||
|
|
# 输出结果
|
|||
|
|
# print(f"红线重投影耗时: {elapsed_time:.2f} ms")
|
|||
|
|
return pixel_list
|
|||
|
|
|
|||
|
|
#
|
|||
|
|
# def convert_points_to_utm(points, utm_points):
|
|||
|
|
# # 创建UTM投影
|
|||
|
|
# wgs84 = pyproj.CRS('EPSG:4326')
|
|||
|
|
#
|
|||
|
|
# # 计算所有点的中心点
|
|||
|
|
# sum_lat = 0.0
|
|||
|
|
# sum_lon = 0.0
|
|||
|
|
# count = len(points)
|
|||
|
|
#
|
|||
|
|
# for point in points:
|
|||
|
|
# sum_lat += point.X # 纬度
|
|||
|
|
# sum_lon += point.Y # 经度
|
|||
|
|
#
|
|||
|
|
# if count > 0:
|
|||
|
|
# center_lat = sum_lat / count
|
|||
|
|
# center_lon = sum_lon / count
|
|||
|
|
#
|
|||
|
|
# # 计算中心点对应的UTM带号
|
|||
|
|
# utm_zone = int(center_lon / 6) + 31
|
|||
|
|
#
|
|||
|
|
# # 使用计算出的带号创建UTM投影
|
|||
|
|
# utm_proj = pyproj.CRS(f'EPSG:{32600 + utm_zone}')
|
|||
|
|
# else:
|
|||
|
|
# # 如果没有点，使用默认带号
|
|||
|
|
# utm_proj = pyproj.CRS(f'EPSG:{32600 + 48}')
|
|||
|
|
#
|
|||
|
|
# transformer = pyproj.Transformer.from_crs(wgs84, utm_proj, always_xy=True)
|
|||
|
|
# # 将点坐标转换为UTM坐标
|
|||
|
|
# for point in points:
|
|||
|
|
# # 将经纬度转换为UTM坐标
|
|||
|
|
# x, y = transformer.transform(point.Y, point.X)
|
|||
|
|
# utm_point = Point(x, y, point.Z,point.id)
|
|||
|
|
# utm_points.append(utm_point)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def convert_points_to_utm(points):
|
|||
|
|
# 创建UTM投影
|
|||
|
|
wgs84 = pyproj.CRS('EPSG:4326')
|
|||
|
|
|
|||
|
|
# 计算所有点的中心点
|
|||
|
|
sum_lat = 0.0
|
|||
|
|
sum_lon = 0.0
|
|||
|
|
count = len(points)
|
|||
|
|
|
|||
|
|
for point in points:
|
|||
|
|
sum_lat += point.X # 纬度
|
|||
|
|
sum_lon += point.Y # 经度
|
|||
|
|
|
|||
|
|
if count > 0:
|
|||
|
|
center_lat = sum_lat / count
|
|||
|
|
center_lon = sum_lon / count
|
|||
|
|
|
|||
|
|
# 计算中心点对应的UTM带号
|
|||
|
|
utm_zone = int(center_lon / 6) + 31
|
|||
|
|
|
|||
|
|
# 使用计算出的带号创建UTM投影
|
|||
|
|
utm_proj = pyproj.CRS(f'EPSG:{32600 + utm_zone}')
|
|||
|
|
else:
|
|||
|
|
# 如果没有点，使用默认带号
|
|||
|
|
utm_proj = pyproj.CRS(f'EPSG:{32600 + 48}')
|
|||
|
|
|
|||
|
|
transformer = pyproj.Transformer.from_crs(wgs84, utm_proj, always_xy=True)
|
|||
|
|
|
|||
|
|
utm_points = []
|
|||
|
|
# 将点坐标转换为UTM坐标
|
|||
|
|
for point in points:
|
|||
|
|
# 将经纬度转换为UTM坐标
|
|||
|
|
x, y = transformer.transform(point.Y, point.X)
|
|||
|
|
utm_point = Point(x, y, point.Z,point.id) ############
|
|||
|
|
utm_points.append(utm_point)
|
|||
|
|
return utm_points
|
|||
|
|
|
|||
|
|
def img_reproject():
|
|||
|
|
"""测试图像重投影功能"""
|
|||
|
|
# 输入参数
|
|||
|
|
obj_longitude = 103.990109 # 目标点经度(度)
|
|||
|
|
obj_latitude = 30.762915 # 目标点纬度(度)
|
|||
|
|
obj_height = 481.64 # 目标点高度
|
|||
|
|
|
|||
|
|
gimbal_yaw = 66.40 * pi / 180 # 示例云台偏航角
|
|||
|
|
gimbal_pitch = -90 * pi / 180 # 示例云台俯仰角
|
|||
|
|
gimbal_roll = 0 * pi / 180 # 示例云台横滚角
|
|||
|
|
h = 590.834 # 示例相机高度
|
|||
|
|
l = (103 + 59 / 60.0 + 26.07 / 3600.0) * pi / 180 # 示例相机经度
|
|||
|
|
b = (30 + 45 / 60.0 + 46.48 / 3600.0) * pi / 180 # 示例相机纬度
|
|||
|
|
img_width = 4032 # 示例图像宽度
|
|||
|
|
img_height = 3024 # 示例图像高度
|
|||
|
|
|
|||
|
|
# 相机参数
|
|||
|
|
cam_paras = CamParas(
|
|||
|
|
fx=2795.68899,
|
|||
|
|
fy=2795.68899
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 测量执行时间
|
|||
|
|
start_time = time.time()
|
|||
|
|
|
|||
|
|
# 执行图像重投影
|
|||
|
|
pixel_x, pixel_y = ir_img_reproject(
|
|||
|
|
gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b,
|
|||
|
|
cam_paras, img_width, img_height, obj_longitude, obj_latitude, obj_height
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 计算执行时间
|
|||
|
|
elapsed_time = (time.time() - start_time) * 1000 # 转换为毫秒
|
|||
|
|
|
|||
|
|
# 输出结果
|
|||
|
|
print(f"图像重投影耗时: {elapsed_time:.2f} ms")
|
|||
|
|
print(f"目标点在图像上的坐标: x={pixel_x:.2f}, y={pixel_y:.2f}")
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 通过相机姿态、经纬度、相机目标中心点高程、图片目标坐标数组，计算目标经纬度
|
|||
|
|
def cal_canv_location_by_osd( us, vs, gimbal_pitch, gimbal_yaw, gimbal_roll,
|
|||
|
|
height,l, b, img_width, img_height, heights):
|
|||
|
|
"""测试目标定位功能"""
|
|||
|
|
# 输入参数
|
|||
|
|
# u = 1520 # 示例图像u坐标
|
|||
|
|
# v = 2512 # 示例图像v坐标
|
|||
|
|
# interval = 0.05 # 高程迭代间隔
|
|||
|
|
gimbal_yaw = gimbal_yaw * pi / 180 # 示例云台偏航角
|
|||
|
|
gimbal_pitch = gimbal_pitch * pi / 180 # 示例云台俯仰角
|
|||
|
|
gimbal_roll = gimbal_roll * pi / 180 # 示例云台横滚角
|
|||
|
|
|
|||
|
|
l = l * pi / 180 # 示例相机经度
|
|||
|
|
b = b * pi / 180 # 示例相机纬度
|
|||
|
|
img_width = img_width # 示例图像宽度
|
|||
|
|
img_height = img_height # 示例图像高度
|
|||
|
|
|
|||
|
|
# 相机参数；哈密4TD
|
|||
|
|
cam_paras = CamParas(
|
|||
|
|
fx=2795.68899,
|
|||
|
|
fy=2795.68899,
|
|||
|
|
cx=4032/2-40,
|
|||
|
|
cy=3024/2-20,
|
|||
|
|
)
|
|||
|
|
# 测量执行时间
|
|||
|
|
start_time = time.time()
|
|||
|
|
|
|||
|
|
# 执行目标定位
|
|||
|
|
results = ir_target_positioning_new_list(
|
|||
|
|
us, vs, gimbal_pitch, gimbal_yaw, gimbal_roll,
|
|||
|
|
height,l, b, cam_paras, img_width, img_height, heights
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 计算执行时间
|
|||
|
|
elapsed_time = (time.time() - start_time) * 1000 # 转换为毫秒
|
|||
|
|
|
|||
|
|
# 输出结果
|
|||
|
|
print(f"像素坐标反算经纬度耗时: {elapsed_time:.2f} ms")
|
|||
|
|
|
|||
|
|
return results #坐标集合，应该是二维数组
|
|||
|
|
|