From 44ecd09af35a006f28fedfcec49ae40e973b10c4 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期三, 09 三月 2022 10:01:38 +0800
Subject: [PATCH] add slam algorithm into dev
---
src/utils/tools.py | 56 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 56 insertions(+), 0 deletions(-)
diff --git a/src/utils/tools.py b/src/utils/tools.py
index 90adc7c..ee2f8de 100644
--- a/src/utils/tools.py
+++ b/src/utils/tools.py
@@ -1,4 +1,6 @@
import os, sys
+import numpy as np
+import time
def file_fliter(folder:str, file_ext:str, sort_format):
@@ -13,5 +15,59 @@
target_files.sort(key=sort_format)
return target_files
+
+def read_ply(path:str,start_line:int = 10):
+ count = 0
+ ply_list = []
+ with open(path,"r") as ply_file:
+ # read the line
+ for line in ply_file.readlines():
+ # strip the str of the header
+ if count < start_line:
+ pass
+ else:
+ line_str_list = line.strip().split(' ')
+ # convert the str type to float type
+ line_float_list = []
+ for value in line_str_list:
+ line_float_list.append(float(value))
+
+ ply_list.append(line_float_list)
+ count += 1
+ ply_array = np.array(ply_list,dtype=np.float16)
+
+ return ply_array
+
+
+def write_ply(data:np.array,folder:str,name:str):
+ file = os.path.join(folder,name+".ply")
+ with open (file,'w') as f:
+ f.write("ply\n")
+ f.write("format ascii 1.0\n")
+ f.write("comment write_ply generated\n")
+ f.write("element vertex %d\n" % data.shape[0])
+ f.write("property float x\n")
+ f.write("property float y\n")
+ f.write("property float z\n")
+ f.write("element face 0\n")
+ f.write("property list uchar int vertex_indices\n")
+ f.write("end_header\n")
+ n_p = 0
+
+ for p in data:
+ x = p[0]
+ y = p[1]
+ z = p[2]
+ f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z))
+ n_p += 1
+ print(time.asctime(),f"Frame {name}.ply have been saved with {n_p} points!")
+
+
+
+if __name__ == "__main__":
+ ply_array = read_ply('src\\microwave\\demo\\demoData\\2522.ply')
+
+ print(type(ply_array))
+ print(ply_array)
--
Gitblit v1.9.1