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 +++++++---
src/utils/tools.py | 58 +++++++++++++++++++++++------
2 files changed, 60 insertions(+), 19 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
diff --git a/src/utils/tools.py b/src/utils/tools.py
index ee2806a..8f9fa00 100644
--- a/src/utils/tools.py
+++ b/src/utils/tools.py
@@ -20,28 +20,62 @@
return target_files
-def read_ply(path:str,start_line:int = 10):
- ply_array = np.load(path,skiprows=start_line)
+def get_skip_rows(path):
+ with open(path,"r") as ply_file:
+ count = 0
+ # read the line
+ for line in ply_file.readlines():
+ # strip the str of the header
+ flag = line.strip()
+ if flag == "end_header":
+ return count + 1
+ else:
+ if count >= 50:
+ return None
+ count += 1
+ return None
+
+def read_ply(path:str):
+ start_line = get_skip_rows(path)
+ if start_line is not None:
+ ply_array = np.loadtxt(path,skiprows=start_line)
+ else:
+ ply_array = None
return ply_array
def write_ply(data:np.array,folder:str,name:str):
file = os.path.join(folder,name+".ply")
n_p = data.shape[0]
- header = ["ply\nformat ascii 1.0\n",
- "comment write_ply generated\n",
- f"element vertex {n_p}\n",
- "property float x\n",
- "property float y\n",
- "property float z\n",
- "element face 0\n",
- "property list uchar int vertex_indices\n",
- "end_header"]
+ n_cols = data.shape[1]
+
+ if n_cols == 3:
+ header = ["ply\nformat ascii 1.0\n",
+ "comment write_ply generated\n",
+ f"element vertex {n_p}\n",
+ "property float x\n",
+ "property float y\n",
+ "property float z\n",
+ "end_header"]
+ fmt = '%.6f','%.6f','%.6f'
+
+ elif n_cols == 6:
+ header = ["ply\nformat ascii 1.0\n",
+ "comment write_ply generated\n",
+ f"element vertex {n_p}\n",
+ "property float x\n",
+ "property float y\n",
+ "property float z\n",
+ "property uchar red\n",
+ "property uchar green\n",
+ "property uchar blue\n",
+ "end_header"]
+ fmt = '%.6f','%.6f','%.6f','%d','%d','%d'
np.savetxt(file,data,
delimiter=" ",
header="".join(header),
comments="",
- fmt='%.6f')
+ fmt=fmt)
print(f"{time.asctime()} {name}.ply have been saved with {n_p} points!")
--
Gitblit v1.9.1