From 13530d0ad2c32608d0b46a5c16ad4ac0c25b59f1 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期五, 13 五月 2022 14:08:50 +0800
Subject: [PATCH] support for 6 columns ply
---
src/slam/core.py | 21 ++++++++++++++-------
1 files changed, 14 insertions(+), 7 deletions(-)
diff --git a/src/slam/core.py b/src/slam/core.py
index 7401954..eea9e94 100644
--- a/src/slam/core.py
+++ b/src/slam/core.py
@@ -21,7 +21,7 @@
# 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):
+def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,rank,order = "x_y_z",verbose=0):
"""generate a rotation matrix from the specific angel
"""
@@ -56,16 +56,20 @@
if i == 0:
rotation_matrix = R_dic[s]
else:
- rotation_matrix = R_dic[s] * rotation_matrix
+ rotation_matrix = R_dic[s] * rotation_matrix
+
+ m = np.identity(rank)
+ m[:3,:3] = 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)
+ print("\n rotation matrix\n",m)
else:
pass
- return rotation_matrix
+ return m
def get_origin_coordinates():
"""
@@ -126,17 +130,20 @@
if isinstance(ply,str):
ply = read_ply(ply)
- rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2])
+ N_rows = ply.shape[0]
+ N_cols = ply.shape[1]
+
+ rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2],rank=N_cols)
# z = (X * Y^T)^T = (Y * X^T)
- # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
+ # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix.transpose())
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)
+ merged_ply_array = np.array(merged_ply_array).reshape(-1,N_cols)
return merged_ply_array
--
Gitblit v1.9.1