167 lines
7.2 KiB
Python
167 lines
7.2 KiB
Python
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) |