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)
|