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 #坐标集合，应该是二维数组