import numpy as np import cv2 import math import os import pyproj # import gdal from osgeo import gdal, osr from touying.ImageReproject_python.img_types import ImgExif gdal.UseExceptions() # 显式启用 GDAL 异常 # from img_types import Point, POS, ImgEO, CamParas, ImgExif # from dpal import DPAL class ImageReproject: """图像重投影类，实现目标定位、红线重投影和图像重投影功能""" def __init__(self): """初始化""" pass def ir_target_positioning_new_list(self, us, vs, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, Height_Terras): """目标定位（多组坐标版本） Args: us: 图像x坐标列表 vs: 图像y坐标列表 gimbal_pitch: 云台俯仰角(弧度) gimbal_yaw: 云台偏航角(弧度) gimbal_roll: 云台横滚角(弧度) h: 高度 l: 经度(弧度) b: 纬度(弧度) cam_paras: 相机参数 img_width: 图像宽度 img_height: 图像高度 Height_Terras: 对应的地面高度列表 Returns: list of tuples: 每个元组包含(经度, 纬度, 高度) """ # 验证输入长度是否一致 if len(us) != len(vs) or len(us) != len(Height_Terras): raise ValueError("us, vs 和 Height_Terras 的长度必须相同") # 创建ImgExif对象并设置参数 img_exif = ImgExif() img_exif.width = img_width img_exif.height = img_height img_exif.B = b img_exif.L = l img_exif.H = h img_exif.GimbalYaw = gimbal_yaw img_exif.GimbalPitch = gimbal_pitch img_exif.GimbalRoll = gimbal_roll img_exif.camPara = cam_paras # 计算投影矩阵 img_exif.com_zone() img_exif.computing_p_mat() # 创建UTM投影 wgs84 = pyproj.CRS('EPSG:4326') utm_proj = pyproj.CRS(f'EPSG:{32600 + img_exif.UTMzone}') transformer = pyproj.Transformer.from_crs(utm_proj, wgs84, always_xy=True) results = [] for u, v, Height_Terra in zip(us, vs, Height_Terras): # 去畸变处理 u_undistort, v_undistort = img_exif.undistortion_point(u, v) plane = [0.0, 0.0, 1.0, -Height_Terra] new_row = np.array([plane], dtype=np.float64) P_ = np.vstack([img_exif.P, new_row]) mat_inv = np.linalg.inv(P_) img_pt = np.array([[u_undistort], [v_undistort], [1], [0]], dtype=np.float64) target_ptr = np.dot(mat_inv, img_pt) point = self.normalized(target_ptr) # utm转经纬度 point = transformer.transform(point[0], point[1], point[2]) results.append((point[0], point[1], point[2])) # 添加经度、纬度和高度到结果列表 return results def target_positioning_new(self, u, v, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, Height_Terra): """目标定位 Args: u: 图像x坐标 v: 图像y坐标 gimbal_pitch: 云台俯仰角(弧度) gimbal_yaw: 云台偏航角(弧度) gimbal_roll: 云台横滚角(弧度) h: 高度 l: 经度(弧度) b: 纬度(弧度) cam_paras: 相机参数 img_width: 图像宽度 img_height: 图像高度 Height_Terra: 当前地面高度 x: 输出经度 y: 输出纬度 z: 输出高度 Returns: (x, y, z): 目标点的经度、纬度和高度 """ # 创建ImgExif对象并设置参数 img_exif = ImgExif() img_exif.width = img_width img_exif.height = img_height img_exif.B = b img_exif.L = l img_exif.H = h img_exif.GimbalYaw = gimbal_yaw img_exif.GimbalPitch = gimbal_pitch img_exif.GimbalRoll = gimbal_roll img_exif.camPara = cam_paras # 计算投影矩阵 img_exif.com_zone() img_exif.computing_p_mat() # 去畸变处理 u_undistort, v_undistort = img_exif.undistortion_point(u, v) # 创建UTM投影 wgs84 = pyproj.CRS('EPSG:4326') utm_proj = pyproj.CRS(f'EPSG:{32600 + img_exif.UTMzone}') transformer = pyproj.Transformer.from_crs(utm_proj, wgs84, always_xy=True) plane = [0.0, 0.0, 1.0, -Height_Terra] new_row = np.array([plane], dtype=np.float64) P_ = np.vstack([img_exif.P, new_row]) mat_inv = np.linalg.inv(P_) img_pt = np.array([[u_undistort], [v_undistort], [1], [0]], dtype=np.float64) target_ptr = np.dot(mat_inv, img_pt) point = self.normalized(target_ptr) # utm转经纬度 point = transformer.transform(point[0], point[1], point[2]) return point[0], point[1], point[2] # 返回经度、纬度和高度 def target_positioning(self, u, v, interval, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, dem_file, x=None, y=None, z=None): """目标定位 Args: u: 图像x坐标 v: 图像y坐标 interval: 间隔 gimbal_pitch: 云台俯仰角(弧度) gimbal_yaw: 云台偏航角(弧度) gimbal_roll: 云台横滚角(弧度) h: 高度 l: 经度(弧度) b: 纬度(弧度) cam_paras: 相机参数 img_width: 图像宽度 img_height: 图像高度 dem_file: DEM文件路径 x: 输出经度 y: 输出纬度 z: 输出高度 Returns: (x, y, z): 目标点的经度、纬度和高度 """ # 创建ImgExif对象并设置参数 img_exif = ImgExif() img_exif.width = img_width img_exif.height = img_height img_exif.B = b img_exif.L = l img_exif.H = h img_exif.GimbalYaw = gimbal_yaw img_exif.GimbalPitch = gimbal_pitch img_exif.GimbalRoll = gimbal_roll img_exif.camPara = cam_paras # 计算投影矩阵 img_exif.com_zone() img_exif.computing_p_mat() # 去畸变处理 u_undistort, v_undistort = img_exif.undistortion_point(u, v) # 创建UTM投影 wgs84 = pyproj.CRS('EPSG:4326') utm_proj = pyproj.CRS(f'EPSG:{32600 + img_exif.UTMzone}') transformer = pyproj.Transformer.from_crs(utm_proj, wgs84, always_xy=True) # 获取DEM的最小和最大高程 minZ, maxZ = self.get_dem_min_max(dem_file) planes = [] h = minZ while h <= maxZ: planes.append([0.0, 0.0, 1.0, -h]) h += interval points = [] for plane in planes: new_row = np.array([plane], dtype=np.float64) P_ = np.vstack([img_exif.P, new_row]) mat_inv = np.linalg.inv(P_) img_pt = np.array([[u_undistort], [v_undistort], [1], [0]], dtype=np.float64) target_ptr = np.dot(mat_inv, img_pt) point = self.normalized(target_ptr) # utm转经纬度 point = transformer.transform(point[0], point[1], point[2]) points.append(point) # 过滤不符合高程的点 filtered_points = [] for pt in points: projectZ = pt[2] demZ = self.get_dem_z(dem_file, pt[0], pt[1]) diff = projectZ - demZ if abs(diff) <= interval: filtered_points.append(pt) if len(filtered_points) == 0: print("未得到采样点！") return False, None if len(filtered_points) == 1: return filtered_points[0][0], filtered_points[0][1], filtered_points[0][2] else: cam_center = np.array([img_exif.EO.Xs, img_exif.EO.Ys, img_exif.EO.Zs]) dists = [np.linalg.norm(np.array(pt) - cam_center) for pt in filtered_points] min_idx = int(np.argmin(dists)) return filtered_points[min_idx][0], filtered_points[min_idx][1], filtered_points[min_idx][2] def red_line_reproject(self, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, camera_img_width, camera_img_height, points): """红线重投影 Args: gimbal_pitch: 云台俯仰角(弧度) gimbal_yaw: 云台偏航角(弧度) gimbal_roll: 云台横滚角(弧度) h: 高度 l: 经度(弧度) b: 纬度(弧度) cam_paras: 相机参数 img_width: 图像宽度 img_height: 图像高度 camera_img_width: 直接使用摄像头拍照的宽度 camera_img_height: 直接使用摄像头拍照的高度 points: 点坐标列表 img_file: 输入图像文件路径 img_save_file: 输出图像文件路径 Returns: None """ # 创建ImgExif对象并设置参数 img_exif = ImgExif() img_exif.width = img_width img_exif.height = img_height img_exif.B = b img_exif.L = l img_exif.H = h img_exif.GimbalYaw = gimbal_yaw img_exif.GimbalPitch = gimbal_pitch img_exif.GimbalRoll = gimbal_roll img_exif.camPara = cam_paras # 计算投影矩阵 img_exif.com_zone() img_exif.computing_p_mat() # # 读取图像 # img = cv2.imread(img_file) # if img is None: # print(f"无法读取图像文件: {img_file}") # return pixel_list=[] # 投影点到图像上 for point in points: # 构建三维点的齐次坐标 point_3d = np.array([point.X, point.Y, point.Z, 1.0]) # 投影到图像平面 point_2d = np.matmul(img_exif.P, point_3d) # 约束：只处理相机前方的点 if point_2d[2] <= 0: continue # 归一化 point_2d = point_2d / point_2d[2] # 应用畸变 u, v = img_exif.distortion_point(point_2d[0], point_2d[1],img_width,camera_img_width) # # # 转换到视频上：校友之家4D # u = u - (5280 / 2 - img_width / 2) # v = v - (3956 / 2 - img_height / 2) # # 转换到视频上：哈密4TD u = u - (camera_img_width / 2 - img_width / 2) v = v - (camera_img_height / 2 - img_height / 2) pixel_list.append({ "u": int(u), "v": int(v), "id":point.id }) # # 检查点是否在图像范围内 # if 0 <= u < img_width and 0 <= v < img_height: # # 在图像上绘制点 # cv2.circle(img, (int(u), int(v)), 15, (0, 0, 255), -1) # # # 保存图像 # cv2.imwrite(img_save_file, img) return pixel_list def img_reproject(self, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, obj_longitude, obj_latitude, obj_height, pixel_x=None, pixel_y=None): """图像重投影 Args: gimbal_pitch: 云台俯仰角(弧度) gimbal_yaw: 云台偏航角(弧度) gimbal_roll: 云台横滚角(弧度) h: 高度 l: 经度(弧度) b: 纬度(弧度) cam_paras: 相机参数 img_width: 图像宽度 img_height: 图像高度 obj_longitude: 目标点经度(度) obj_latitude: 目标点纬度(度) obj_height: 目标点高度 pixel_x: 输出像素x坐标 pixel_y: 输出像素y坐标 Returns: (pixel_x, pixel_y): 目标点在图像上的像素坐标 """ # 创建ImgExif对象并设置参数 img_exif = ImgExif() img_exif.width = img_width img_exif.height = img_height img_exif.B = b img_exif.L = l img_exif.H = h img_exif.GimbalYaw = gimbal_yaw img_exif.GimbalPitch = gimbal_pitch img_exif.GimbalRoll = gimbal_roll img_exif.camPara = cam_paras # 计算投影矩阵 img_exif.com_zone() img_exif.computing_p_mat() # 创建UTM投影 wgs84 = pyproj.CRS('EPSG:4326') utm_proj = pyproj.CRS(f'EPSG:{32600 + img_exif.UTMzone}') transformer = pyproj.Transformer.from_crs(wgs84, utm_proj, always_xy=True) # 将目标点经纬度转换为UTM坐标 obj_x, obj_y = transformer.transform(obj_longitude, obj_latitude) # 构建三维点的齐次坐标 point_3d = np.array([obj_x, obj_y, obj_height, 1.0]) # 投影到图像平面 point_2d = np.matmul(img_exif.P, point_3d) # 归一化 point_2d = point_2d / point_2d[2] # 应用畸变 u, v = img_exif.distortion_point(point_2d[0], point_2d[1],img_width) # 设置输出结果 if pixel_x is not None: pixel_x = u if pixel_y is not None: pixel_y = v return u, v def get_dem_min_max(self, dem_file): """获取DEM文件的最小和最大高程 Args: dem_file: DEM文件路径 Returns: (min_z, max_z): 最小和最大高程 """ # 打开DEM文件 dataset = gdal.Open(dem_file, gdal.GA_ReadOnly) if dataset is None: print(f"无法打开DEM文件: {dem_file}") return 0, 1000 # 返回默认值 # 获取波段 band = dataset.GetRasterBand(1) # 获取统计信息 min_z, max_z, mean, std_dev = band.GetStatistics(True, True) # 关闭数据集 dataset = None return min_z, max_z def get_dem_z(self, dem_file, x, y): """从DEM文件中获取指定点的高程 Args: dem_file: DEM文件路径 x: UTM X坐标 y: UTM Y坐标 Returns: z: 高程值，如果无法获取则返回None """ # 打开DEM文件 dataset = gdal.Open(dem_file, gdal.GA_ReadOnly) if dataset is None: print(f"无法打开DEM文件: {dem_file}") return None # 获取地理变换参数 geotransform = dataset.GetGeoTransform() # 获取波段 band = dataset.GetRasterBand(1) # 计算像素坐标 px = int((x - geotransform[0]) / geotransform[1]) py = int((y - geotransform[3]) / geotransform[5]) # 检查像素坐标是否在范围内 if px < 0 or px >= dataset.RasterXSize or py < 0 or py >= dataset.RasterYSize: dataset = None return None # 读取高程值 data = band.ReadAsArray(px, py, 1, 1) if data is None: dataset = None return None # 获取高程值 z = data[0, 0] # 检查是否为NoData值 no_data_value = band.GetNoDataValue() if no_data_value is not None and z == no_data_value: dataset = None return None # 关闭数据集 dataset = None return z def normalized(self, mat): # 归一化 return [mat[0, 0] / mat[3, 0], mat[1, 0] / mat[3, 0], mat[2, 0] / mat[3, 0]] # C风格接口 def ir_target_positioning(u, v, interval, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, dem_file, x=None, y=None, z=None): """目标定位的C风格接口""" reproject = ImageReproject() return reproject.target_positioning(u, v, interval, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, dem_file, x, y, z) def 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, points): """红线重投影的C风格接口""" reproject = ImageReproject() pixel_list=reproject.red_line_reproject(gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, camera_img_width, camera_img_height, points) return pixel_list def ir_img_reproject(gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, obj_longitude, obj_latitude, obj_height, pixel_x=None, pixel_y=None): """图像重投影的C风格接口""" reproject = ImageReproject() return reproject.img_reproject(gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, obj_longitude, obj_latitude, obj_height, pixel_x, pixel_y) def ir_target_positioning_new(u, v, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, Height_Terra): """目标定位的C风格接口""" reproject = ImageReproject() return reproject.target_positioning_new(u, v, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, Height_Terra) def ir_target_positioning_new_list(self, us, vs, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, Height_Terras): """目标定位（多组坐标版本） Args: us: 图像x坐标列表 vs: 图像y坐标列表 gimbal_pitch: 云台俯仰角(弧度) gimbal_yaw: 云台偏航角(弧度) gimbal_roll: 云台横滚角(弧度) h: 高度 l: 经度(弧度) b: 纬度(弧度) cam_paras: 相机参数 img_width: 图像宽度 img_height: 图像高度 Height_Terras: 对应的地面高度列表 Returns: list of tuples: 每个元组包含(经度, 纬度, 高度) """ # 验证输入长度是否一致 if len(us) != len(vs) or len(us) != len(Height_Terras): raise ValueError("us, vs 和 Height_Terras 的长度必须相同") # 创建ImgExif对象并设置参数 img_exif = ImgExif() img_exif.width = img_width img_exif.height = img_height img_exif.B = b img_exif.L = l img_exif.H = h img_exif.GimbalYaw = gimbal_yaw img_exif.GimbalPitch = gimbal_pitch img_exif.GimbalRoll = gimbal_roll img_exif.camPara = cam_paras # 计算投影矩阵 img_exif.com_zone() img_exif.computing_p_mat() # 创建UTM投影 wgs84 = pyproj.CRS('EPSG:4326') utm_proj = pyproj.CRS(f'EPSG:{32600 + img_exif.UTMzone}') transformer = pyproj.Transformer.from_crs(utm_proj, wgs84, always_xy=True) results = [] for u, v, Height_Terra in zip(us, vs, Height_Terras): # 去畸变处理 u_undistort, v_undistort = img_exif.undistortion_point(u, v) plane = [0.0, 0.0, 1.0, -Height_Terra] new_row = np.array([plane], dtype=np.float64) P_ = np.vstack([img_exif.P, new_row]) mat_inv = np.linalg.inv(P_) img_pt = np.array([[u_undistort], [v_undistort], [1], [0]], dtype=np.float64) target_ptr = np.dot(mat_inv, img_pt) point = self.normalized(target_ptr) # utm转经纬度 point = transformer.transform(point[0], point[1], point[2]) results.append((point[0], point[1], point[2])) # 添加经度、纬度和高度到结果列表 return results # 对应的C风格接口也需要修改 def ir_target_positioning_new_list(us, vs, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, Height_Terras): """目标定位的C风格接口（多组坐标版本）""" reproject = ImageReproject() return reproject.ir_target_positioning_new_list(us, vs, gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b, cam_paras, img_width, img_height, Height_Terras)