slam algorithm

wu_xinjun

2022-03-09 7e23498bb27c683b74b62f1495bc610ccc04bad2

 src/utils/tools.py

@@ -1,5 +1,6 @@
import os, sys
import numpy as np
import time

def file_fliter(folder:str, file_ext:str, sort_format):

@@ -38,6 +39,31 @@
    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')