import numpy as np
|
import os, sys
|
# 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
|
|
|
|
# 毫米波坐标系为右手坐标系。
|
# 毫米波设备分别绕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,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(np.pi * float(angle) / 180)
|
|
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]])
|
|
rotation_matrix = Rx_alpha * Ry_beta * Rz_gamma
|
|
if verbose == 1:
|
print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
|
print(f"\n{beta_degree}, {beta}, Ry_beta \n", Ry_beta)
|
print(f"\n{gamma_degree}, {gamma}, Rz_gamma\n",Rz_gamma)
|
print("\n rotation matrix\n",rotation_matrix)
|
else:
|
pass
|
|
return rotation_matrix
|
|
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)
|
# 转为右手坐标系
|
if reverse:
|
rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]
|
|
data = rotated_ply
|
return data
|
|
def compose_ply(muti_ply_with_angle: list):
|
|
ply_nums = len(muti_ply_with_angle)
|
composed_ply_array = None
|
|
for i, ply_with_angle in enumerate( 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)
|
if i == 0:
|
composed_ply_array = np.array(rotated_ply)
|
else:
|
temp = np.array(rotated_ply)
|
composed_ply_array = np.concatenate((composed_ply_array,temp),axis= 0 )
|
|
composed_ply_array = composed_ply_array.reshape(-1,3)
|
return composed_ply_array
|
|
|
if __name__ == "__main__":
|
current = np.array([[1,1,1],[1,3,9],[4,8,5]])
|
print(current)
|
target = transferTo(current,1,1,1,0,0,0,True)
|
print(target)
|
