From fee0c7c1872b32141612ad0b3730b865e7e4dace Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期二, 22 三月 2022 13:00:18 +0800
Subject: [PATCH] optimization for auto slam
---
src/slam/slam.py | 31 ++++++++++++++++++++++++++-----
1 files changed, 26 insertions(+), 5 deletions(-)
diff --git a/src/slam/slam.py b/src/slam/slam.py
index be8a376..de3904d 100644
--- a/src/slam/slam.py
+++ b/src/slam/slam.py
@@ -6,6 +6,7 @@
sys.path.insert(0, src_path)
from utils.tools import read_ply
+from tqdm import tqdm
@@ -76,12 +77,33 @@
data = rotated_ply
return data
+def transferToZero(data,x,y,z,alpha,beta,gamma,reverse=False):
+ """
+ 杞崲褰撳墠鍧愭爣鑷冲彟涓�鍧愭爣, xyz鍒嗗埆涓哄綋鍓嶅潗鏍囧師鐐瑰湪鐩爣鍧愭爣绯荤殑浣嶇疆;
+ alpha,beta,gamma 涓哄綋鍓嶅潗鏍囩郴鐩稿鐩爣鍧愭爣绯荤殑鏃嬭浆鍙傛暟銆�
+ """
+
+ # 骞崇Щ
+ moved_data = current + np.array([x,y,z])
+ # 鏃嬭浆
+ rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
+ rotated_data = np.matmul(moved_data,rotation_matrix)
+
+ # 宸﹀彸鎵嬪潗鏍囩郴杞崲, 浣跨敤z杞磋浆鎹�
+
+ if reverse:
+ rotated_data[:,-1] = -rotated_data.copy()[:,-1]
+
+ return rotated_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):
+ print ("rotating the ply ...")
+ for i, ply_with_angle in enumerate(tqdm(muti_ply_with_angle)):
angle = ply_with_angle[:-1]
ply = ply_with_angle[-1]
@@ -94,12 +116,11 @@
# 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)
+ composed_ply_array = rotated_ply.copy()
else:
- temp = np.array(rotated_ply)
- composed_ply_array = np.concatenate((composed_ply_array,temp),axis= 0 )
+ composed_ply_array = np.concatenate((composed_ply_array,rotated_ply),axis= 0 )
- composed_ply_array = composed_ply_array.reshape(-1,3)
+ composed_ply_array = np.array(composed_ply_array).reshape(-1,3)
return composed_ply_array
--
Gitblit v1.9.1