add slam algorithm into dev

wu_xinjun

2022-03-09 44ecd09af35a006f28fedfcec49ae40e973b10c4

 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)