Merge commit '13530d0ad2c32608d0b46a5c16ad4ac0c25b59f1' into dev

wu_xinjun

2022-05-13 33224139e89b8c971b8b2442a2aae9dbd92063f5

Merge commit '13530d0ad2c32608d0b46a5c16ad4ac0c25b59f1' into dev

 .gitignore

@@ -4,6 +4,11 @@
# proto
src/protos/aerial_pb2.py

# projects
.vscode
.vs
.idea

# env
env/aerial_deploy
.env/

 .vscode/launch.json

File was deleted

 .vscode/settings.json

File was deleted

 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):

 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

 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



 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!")