import numpy as np import cv2 from pyproj import Transformer import math from PIL import Image from pylab import * from dataclasses import dataclass def rotate_points_around_axis(points, axis, angle): # 将轴转换为单位向量 axis =axis.squeeze() axis = axis / np.linalg.norm(axis) # 将角度转换为弧度 theta = np.radians(angle) # 计算旋转矩阵 K = np.array([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]) I = np.eye(3) # 单位矩阵 R = I + np.sin(theta) * K + (1 - np.cos(theta)) * np.dot(K, K) # 将点平移到旋转轴的起点 # 旋转多个点 rotated_points = np.dot(R.T,points.T) # 矩阵乘法 # 再平移回去 return rotated_points def rotate_points(points, center, theta): """ 批量旋转二维点 :param points: 输入的二维点矩阵,形状为 (N, 2) :param center: 旋转中心 (x_c, y_c) :param theta: 旋转角度(弧度) :return: 旋转后的点矩阵,形状为 (N, 2) """ # 平移到旋转中心 translated = points - center # 平移,使得中心在原点 rotation_matrix = np.array([[np.cos(theta), np.sin(theta)], [-np.sin(theta), np.cos(theta)]]) # 应用旋转 rotated = translated @ rotation_matrix.T # 矩阵乘法 return rotated + center # 将旋转后的点移回原位置 def compute_rotation_matrix(pitch, roll1, yaw): c2w_x = np.array([[1, 0, 0], [0, math.cos(pitch), -math.sin(pitch)], [0, math.sin(pitch), math.cos(pitch)]], dtype=np.float32) c2w_y = np.array([[math.cos(roll1), 0, math.sin(roll1)], [0, 1, 0], [-math.sin(roll1), 0, math.cos(roll1)]], dtype=np.float32) c2w_z = np.array([[math.cos(yaw), -math.sin(yaw), 0], [math.sin(yaw), math.cos(yaw), 0], [0, 0, 1]], dtype=np.float32) c2w = c2w_x @ c2w_y @ c2w_z # 按 Z-Y-X 顺序组合旋转矩阵 y=np.array([[0],[0],[0]]) c2w1 = np.hstack([c2w,y]) new_row = np.array([[0, 0, 0, 1]]) c2w1 = np.vstack([c2w1, new_row]) return c2w, c2w1 def create_transformation_matrix(tvec): transform_matrix = np.eye(4) # 创建单位矩阵 transform_matrix[:3, 3] = tvec.flatten() # 平移向量 return transform_matrix def dms2dd(x,y,z): return x+y/60+z/3600 @dataclass class ProjectTrans: ''' 计算坐标 ''' def __init__(self, f_mm, sensor_width_mm, sensor_height_mm, image_width_px, image_height_px, k1, k2, k3, k4, k5): self.image_points = None self.tvec = None self.rvec = None self.object_point = None self.f_mm = f_mm # 物理焦距 (mm) self.sensor_width_mm= sensor_width_mm # 传感器宽度 (mm) self.sensor_height_mm=sensor_height_mm # 传感器高度 (mm) self.image_width_px= image_width_px # 图像宽度 (px) self.image_height_px= image_height_px # 图像高度 (px) self.Ji = np.array([[k1], [k2], [k3], [k4],[k5]], dtype=np.float32) self.camera_matrix = None def apply_transformation(self, c2w, transform_matrix): # 齐次化 homogeneous_points = np.hstack([self.object_point, np.ones((self.object_point.shape[0], 1))]) # 旋转平移 transformed_points = transform_matrix @ homogeneous_points.T transformed_points = c2w.T @ transformed_points return transformed_points[:3, :] # 返回前3列(x, y, z坐标) def compute_camera_matrix(self): #返回内参矩阵 """计算并返回相机内参矩阵.""" fx = floor((self.f_mm * self.image_width_px) / self.sensor_width_mm) fy = floor((self.f_mm * self.image_height_px) / self.sensor_height_mm) cx = self.image_width_px / 2 cy = self.image_height_px / 2 fx=3725 fy=3725 cx= 2640 cy=1978 # 构造内参矩阵 self.camera_matrix = np.array([ [fx, 0, cx], [0, fy, cy], [0, 0, 1] ], dtype=np.float32) # def transform_Y(self): # num_rows = self.image_points.shape[0] # new_column = np.ones((num_rows, 1)) # self.image_points[:,0] +=80 # self.image_points[:,1] +=100 # self.image_points = np.hstack([self.image_points, new_column]) # transform = np.array([[math.cos(radians(-84.8)), -math.sin(radians(-84.8)), 2728*(1-math.cos(radians(-84.8)))+1980.*math.sin(radians(-84.8))], # [math.sin(radians(-84.8)), math.cos(radians(-84.8)), 1980*(1-math.cos(radians(-84.8)))-2728*math.sin(radians(-84.8))], # [0, 0, 1]], dtype=np.float32) # self.image_points = (transform @ self.image_points.T).T def projectPoints(self, object_point, tvec, rvec): self.object_point = object_point self.tvec=tvec self.rvec=np.radians(rvec) # transformer = Transformer.from_crs("epsg:4326", "epsg:4544") # for index, point in enumerate(object_point): # lat,lon,z = point # self.object_poin t[index,1],self.object_point[index,0] = transformer.transform(lat, lon) # self.tvec[1] ,self.tvec[0] = transformer.transform(tvec[0], tvec[1]) self.tvec = -self.tvec # c2w = compute_rotation_matrix(np.pi - 0.1 / 180 * np.pi, 0.85 / 180 * np.pi, 0) # 计算旋转矩阵 # transform_matrix = create_transformation_matrix(self.tvec) # 创建平移矩阵 # self.object_point = self.apply_transformation(c2w, transform_matrix) # 旋转平移 # self.object_point = rotate_points_around_axis(self.object_point, np.array([0.0148148, -0.00174533, 0.999901]), -rvec[1]) c2w1, c2w = compute_rotation_matrix(np.pi-0.1/180*np.pi, -1/180*np.pi, 1/180*np.pi) #计算旋转矩阵 transform_matrix = create_transformation_matrix(self.tvec) #创建平移矩阵 self.object_point = self.apply_transformation(c2w, transform_matrix) #旋转平移 axis = c2w1.T @ np.array([[0],[0],[1]],dtype=np.float32) self.object_point = rotate_points_around_axis(self.object_point.T,axis.T,- rvec[1]) self.object_point = rotate_points_around_axis(self.object_point.T, np.array((0.9999,-0.00174533,-0.0148)), (rvec[0]+90)) self.object_point = self.object_point.T self.compute_camera_matrix() #计算相机内参矩 BUP=np.array([0,0,0],dtype=np.float32) self.rvec = np.array([0,0,0],dtype=np.float32) self.image_points, _ = cv2.projectPoints(self.object_point, self.rvec, BUP, self.camera_matrix, self.Ji) self.image_points = self.image_points.squeeze() # self.transform_Y() # self.image_points = rotate_points(self.image_points, np.array([2640, 1978]), rvec[1]/180*np.pi) return self.image_points def plot(self): x_coords=self.image_points[:,0] y_coords=self.image_points[:,1] print(self.image_points,self.rvec) # 打开图像并绘制投影点 im = Image.open('DJI_正.jpeg') imshow(im) # plot(x_coords, y_coords, 'r*') plot(2647.02,1969.38,'b*')# 绘制红色星标 show() cv2.imwrite('result1.jp eg', im)