From 33224139e89b8c971b8b2442a2aae9dbd92063f5 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期五, 13 五月 2022 14:10:39 +0800
Subject: [PATCH] Merge commit '13530d0ad2c32608d0b46a5c16ad4ac0c25b59f1' into dev
---
/dev/null | 13 ----
.gitignore | 5 +
src/microwave/parsers.py | 2
src/microwave/deviceTools.py | 53 ++++++++---------
src/slam/core.py | 21 ++++--
src/utils/tools.py | 79 +++++++++++++++-----------
6 files changed, 90 insertions(+), 83 deletions(-)
diff --git a/.gitignore b/.gitignore
index e9715de..3487ac0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,6 +4,11 @@
# proto
src/protos/aerial_pb2.py
+# projects
+.vscode
+.vs
+.idea
+
# env
env/aerial_deploy
.env/
diff --git a/.vscode/launch.json b/.vscode/launch.json
deleted file mode 100644
index 9692457..0000000
--- a/.vscode/launch.json
+++ /dev/null
@@ -1,132 +0,0 @@
-{
- // 浣跨敤 IntelliSense 浜嗚В鐩稿叧灞炴�с��
- // 鎮仠浠ユ煡鐪嬬幇鏈夊睘鎬х殑鎻忚堪銆�
- // 娆蹭簡瑙ｆ洿澶氫俊鎭紝璇疯闂�: https://go.microsoft.com/fwlink/?linkid=830387
- "version": "0.2.0",
- "configurations": [
- {
- "name": "test auto slam",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/slamAutoMode.py",
- "console": "integratedTerminal",
- "args": [
- "COM11",
- "100",
- "5",
- "5",
- "0",
- "20",
- "auto",
- "192.168.2.65",
- "1000",
- "testDevice",
- "../../04_Data/20220316test"]
- },
- {
- "name": "test_slam",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/slamManualMode.py",
- "console": "integratedTerminal",
- "args": [
- "../../04_Data/LinChiLi220325-1",
- "0",
- "20",
- "auto",
- "data"
- ],
- "justMyCode": true
- },
- {
- "name": "Client_test",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/src/Client.py",
- "args": [
- "192.168.1.224",
- "chai",
- "password123",
- "aerial_rpc",
- "pod1",
- "http://192.168.1.224/api",
- "localhost"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "Client_production",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/src/Client.py",
- "args": [
- "172.17.17.206",
- "chai",
- "password123",
- "aerial_rpc",
- "pod1",
- "http://172.17.17.206/api",
- "192.168.2.65"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "Server_test",
- "type": "python",
- "python": "${workspaceFolder}/.env/python.exe",
- "request": "launch",
- "program": "${workspaceFolder}/src/Server.py",
- "args": [
- "localhost",
- "chai",
- "password123",
- "aerial_rpc",
- "5000",
- "./Images"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "Server_production",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/src/Server.py",
- "args": [
- "172.17.17.206",
- "chai",
- "password123",
- "aerial_rpc",
- "5000",
- "./Images"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "udp2bin",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/udp2bin.py",
- "args": [
- "./src/microwave/demo/demoData"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "bin2ply",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/bin2ply.py",
- "args": [
- "../../00_Docs/mutiFrameBin"
- ],
- "console": "integratedTerminal"
- }
- ]
-}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
deleted file mode 100644
index 95a74cc..0000000
--- a/.vscode/settings.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
-
- "python.poetryPath": "${workspaceFolder}/.env/python.exe",
- "jupyter.kernels.filter": [
- {
- "type": "jupyterKernelspec",
- "path": "${workspaceFolder}/.env/python.exe"
- }
- ],
- "python.envFile": "",
- "python.terminal.activateEnvironment": true,
- "python.defaultInterpreterPath": "${workspaceFolder}/.env/python.exe"
- }
\ No newline at end of file
diff --git a/src/microwave/deviceTools.py b/src/microwave/deviceTools.py
index b26422e..7031f4f 100644
--- a/src/microwave/deviceTools.py
+++ b/src/microwave/deviceTools.py
@@ -3,6 +3,8 @@
import time
import os, sys
import open3d as o3d
+from multiprocessing import Pool
+import numpy as np
from ..utils import file_fliter, write_ply
from .servers import MicroWaveReceiver
@@ -109,41 +111,34 @@
return bin_list
+def bin2ply_f(folder,file):
+
+ with open(os.path.join(folder,file), "rb") as bin_file:
+ print(f"Extracting the data in {file} ...")
+ buf = bin_file.read()
+ buf_data = MicroWaveParser.ParserFrame(buf)
+ for frame in buf_data:
+
+ Frame_id = frame[0]
+ FrameNumber = frame[1]
+ point_cloud = np.array(frame[2])
+ if len(point_cloud) == 0 :
+ print(f"Notice: Frame {FrameNumber} is empty!")
+ else:
+ write_ply(point_cloud,folder,str(FrameNumber))
+
# bin2ply
-def bin2ply_tool(folder):
+def bin2ply_tool(folder,nums_worker = 4):
+ pool = Pool(processes = nums_worker)
+
bin_files = file_fliter(folder,'bin')
for file in bin_files:
+ pool.apply_async(func=bin2ply_f,args=(folder,file,))
- filename = file
- # (fn, ext) = os.path.splitext(filename)
- with open(os.path.join(folder,file), "rb") as bin_file:
- buf = bin_file.read()
- buf_data = MicroWaveParser.ParserFrame(buf)
+ pool.close()
+ pool.join()
-
- for frame in buf_data:
- Frame_id = frame[0]
- FrameNumber = frame[1]
- point_cloud = frame[2]
- if len(point_cloud) != 0 :
- with open(os.path.join(folder,str(FrameNumber) +".ply"), "w") as f:
- f.write("ply\n")
- f.write("format ascii 1.0\n")
- f.write("comment MicroWave2Ply generated\n")
- f.write("element vertex %d\n" % len(point_cloud))
- 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 point_cloud:
- x, y, z = p
- f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z))
- n_p += 1
- print(time.asctime(),f"File {file} Frame {FrameNumber}.ply have been saved with {n_p} points!")
def plot_ply(path):
if isinstance(path,str):
diff --git a/src/microwave/parsers.py b/src/microwave/parsers.py
index ee14151..ccd2fb9 100644
--- a/src/microwave/parsers.py
+++ b/src/microwave/parsers.py
@@ -45,7 +45,7 @@
point_data = frame[i * 8 + sizeof(HeaderBlock): (i + 1) * 8 + sizeof(HeaderBlock)]
detection = DetectionBlock(header, footer, point_data)
- point = (detection.X, detection.Y, detection.Z)
+ point = [detection.X, detection.Y, detection.Z]
point_cloud.append(point)
# append the point cloud data to the list
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 80b55d1..8f9fa00 100644
--- a/src/utils/tools.py
+++ b/src/utils/tools.py
@@ -1,3 +1,4 @@
+from email import header
import os, sys
import numpy as np
import time
@@ -19,51 +20,63 @@
return target_files
-def read_ply(path:str,start_line:int = 10):
- count = 0
- ply_list = []
+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
- if count < start_line:
- pass
+ flag = line.strip()
+ if flag == "end_header":
+ return count + 1
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))
+ if count >= 50:
+ return None
+ count += 1
+ return None
- ply_list.append(line_float_list)
- count += 1
- ply_array = np.array(ply_list,dtype=np.float16)
-
+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")
- 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
+ n_p = data.shape[0]
+ n_cols = data.shape[1]
- for p in tqdm(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 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=fmt)
+ print(f"{time.asctime()} {name}.ply have been saved with {n_p} points!")
--
Gitblit v1.9.1