import numpy as np
|
import os, sys
|
import math
|
# # add python path of src to sys.path
|
# src_path = os.path.join(__file__, *(['..'] * 2))
|
# src_path = os.path.abspath(src_path)
|
# sys.path.insert(0, src_path)
|
|
from ..utils.tools import read_ply
|
from tqdm import tqdm
|
|
|
|
# 毫米波坐标系为右手坐标系。
|
# 毫米波设备分别绕xyz轴的旋转角度分别为 alpha, beta, gamma
|
|
# 左乘旋转矩阵分别为 Rx_alpha, Ry_beta, Rz_gamma
|
|
# Rx_alpha = [[1,0,0],[0,np.cos(alpha),np.sin(alpha)],[0,-np.sin(alpha),np.cos(alpha)]]
|
# Ry_beta = [[np.cos(beta),0,-np.sin(beta)],[0,1,0],[np.sin(beta),0,np.cos(beta)]]
|
# Rz_gamma = [[np.cos(gamma),np.sin(gamma),0],[-np.sin(gamma),np.cos(gamma),0],[0,0,1]]
|
|
|
def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,order = "x_y_z",verbose=0):
|
"""generate a rotation matrix from the specific angel
|
"""
|
|
rad_list = []
|
for i, angle in enumerate([alpha_degree,beta_degree,gamma_degree]):
|
|
if np.abs(float(angle))>360:
|
raise ValueError(f"incorrect angle value: value in [-360,360] is excepted, but {angle} is offered.")
|
rad_list.append(math.radians(angle))
|
|
alpha = rad_list[0]
|
Rx_alpha = np.matrix([[1,0,0],
|
[0,np.cos(rad_list[0]),np.sin(alpha)],
|
[0,-np.sin(alpha),np.cos(alpha)]])
|
|
beta = rad_list[1]
|
Ry_beta = np.matrix([[np.cos(beta),0,-np.sin(beta)],
|
[0,1,0],
|
[np.sin(beta),0,np.cos(beta)]])
|
|
gamma = rad_list[2]
|
Rz_gamma = np.matrix([[np.cos(gamma),np.sin(gamma),0],
|
[-np.sin(gamma),np.cos(gamma),0],
|
[0,0,1]])
|
|
R_dic = {"x":Rx_alpha,
|
"y":Ry_beta,
|
"z":Rz_gamma
|
}
|
|
for i, s in enumerate(order.split("_")):
|
if i == 0:
|
rotation_matrix = R_dic[s]
|
else:
|
rotation_matrix = R_dic[s] * rotation_matrix
|
if verbose == 1:
|
print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
|
print(f"\n{beta_degree}, rad {beta}, Ry_beta \n", Ry_beta)
|
print(f"\n{gamma_degree}, rad {gamma}, Rz_gamma\n",Rz_gamma)
|
print("\n rotation matrix\n",rotation_matrix)
|
else:
|
pass
|
|
return rotation_matrix
|
|
def get_origin_coordinates():
|
"""
|
获取毫米波设备数据中心相对舵机原点的坐标
|
"""
|
pass
|
|
def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
|
# Notice: 假定当前坐标系为右手系
|
# step 1: 平移；current_coor + delta(delta_x, delta_y, delta_z) = target_coor
|
# x' = x - delta_x, y' = y - delta_y, z' = z -delta_y
|
# step 2: 旋转; 当前坐标系朝正方面给旋转到指定坐标系。
|
# step 3: 检查是否需要从右手系转换到左手系。
|
# 平移
|
ply = current - np.array([delta_x,delta_y,delta_z])
|
# 旋转
|
# z = (X * Y^T)^T = (Y * X^T)
|
# np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
|
rotation_matrix = get_rotation_matrix(alpha,beta,gamma)
|
rotated_ply = np.matmul(ply,rotation_matrix.transpose())
|
# 转为右手坐标系
|
if reverse:
|
rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]
|
|
data = rotated_ply
|
return data
|
|
def transferToZero(data,x,y,z,alpha,beta,gamma,reverse=False):
|
"""
|
转换当前坐标至另一坐标, xyz分别为当前坐标原点在目标坐标系的位置;
|
alpha,beta,gamma 为当前坐标系相对目标坐标系的旋转参数。
|
"""
|
|
# 平移
|
moved_data = data + np.array([x,y,z])
|
# 旋转
|
rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
|
rotated_data = np.matmul(moved_data,rotation_matrix.transpose())
|
|
# 左右手坐标系转换, 使用z轴转换
|
|
if reverse:
|
rotated_data[:,-1] = -rotated_data.copy()[:,-1]
|
|
return rotated_data
|
|
|
def merge_ply(muti_ply_with_angle: list):
|
|
ply_nums = len(muti_ply_with_angle)
|
merged_ply_array = None
|
|
print ("rotating the ply ...")
|
for i, ply_with_angle in enumerate(tqdm(muti_ply_with_angle)):
|
angle = ply_with_angle[:-1]
|
ply = ply_with_angle[-1]
|
|
if isinstance(ply,str):
|
ply = read_ply(ply)
|
|
rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2])
|
|
# z = (X * Y^T)^T = (Y * X^T)
|
# np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
|
rotated_ply = np.matmul(ply,rotation_matrix.transpose())
|
if i == 0:
|
merged_ply_array = rotated_ply.copy()
|
else:
|
merged_ply_array = np.concatenate((merged_ply_array,rotated_ply),axis= 0 )
|
|
merged_ply_array = np.array(merged_ply_array).reshape(-1,3)
|
return merged_ply_array
|
