From da6ff0858f4997f79f5d8cdc88b7367a5cb710b1 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期二, 07 六月 2022 10:55:26 +0800
Subject: [PATCH] Merge commit 'mqdev' into mqaddFeature
---
src/slam/core.py | 27 ++++++++++++++++++++-------
1 files changed, 20 insertions(+), 7 deletions(-)
diff --git a/src/slam/core.py b/src/slam/core.py
index 7bf6a94..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,26 @@
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():
+ """
+ 鑾峰彇姣背娉㈣澶囨暟鎹腑蹇冪浉瀵硅埖鏈哄師鐐圭殑鍧愭爣
+ """
+ pass
def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
# Notice: 鍋囧畾褰撳墠鍧愭爣绯讳负鍙虫墜绯�
@@ -120,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