From e175b351286c34defd8c2bbe1ddd09d64d2ac862 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期四, 24 三月 2022 16:12:06 +0800
Subject: [PATCH] debug for slam core
---
src/slam/core.py | 28 +++++++++++++++++++---------
1 files changed, 19 insertions(+), 9 deletions(-)
diff --git a/src/slam/core.py b/src/slam/core.py
index c277da8..7bf6a94 100644
--- a/src/slam/core.py
+++ b/src/slam/core.py
@@ -1,5 +1,6 @@
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)
@@ -20,15 +21,16 @@
# 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):
+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(np.pi * float(angle) / 180)
+ rad_list.append(math.radians(angle))
alpha = rad_list[0]
Rx_alpha = np.matrix([[1,0,0],
@@ -44,13 +46,21 @@
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
+ 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}, {beta}, Ry_beta \n", Ry_beta)
- print(f"\n{gamma_degree}, {gamma}, Rz_gamma\n",Rz_gamma)
+ 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
@@ -69,7 +79,7 @@
# 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)
+ rotated_ply = np.matmul(ply,rotation_matrix.transpose())
# 杞负鍙虫墜鍧愭爣绯�
if reverse:
rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]
@@ -87,7 +97,7 @@
moved_data = data + np.array([x,y,z])
# 鏃嬭浆
rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
- rotated_data = np.matmul(moved_data,rotation_matrix)
+ rotated_data = np.matmul(moved_data,rotation_matrix.transpose())
# 宸﹀彸鎵嬪潗鏍囩郴杞崲, 浣跨敤z杞磋浆鎹�
@@ -114,7 +124,7 @@
# 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)
+ rotated_ply = np.matmul(ply,rotation_matrix.transpose())
if i == 0:
merged_ply_array = rotated_ply.copy()
else:
--
Gitblit v1.9.1