583 lines
20 KiB
Python
583 lines
20 KiB
Python
|
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)
|
|||
|
|
|
|||
|
|
|