From e82c2a3abd0f1bd450bea46bc67b84a07017028e Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期一, 21 三月 2022 12:36:38 +0800
Subject: [PATCH] auto slam
---
src/slam/slam.py | 27 +++++++++++++++++++++++++++
1 files changed, 27 insertions(+), 0 deletions(-)
diff --git a/src/slam/slam.py b/src/slam/slam.py
index 2f81fa3..be8a376 100644
--- a/src/slam/slam.py
+++ b/src/slam/slam.py
@@ -56,6 +56,26 @@
return rotation_matrix
+def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
+ # Notice: 鍋囧畾褰撳墠鍧愭爣绯讳负鍙虫墜绯�
+ # step 1: 骞崇Щ锛沜urrent_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)
@@ -83,3 +103,10 @@
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)
+
+
--
Gitblit v1.9.1