support for 6 columns ply

wu_xinjun

2022-05-13 13530d0ad2c32608d0b46a5c16ad4ac0c25b59f1

support for 6 columns ply

 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

@@ -20,28 +20,62 @@

    return target_files

def read_ply(path:str,start_line:int = 10):
    ply_array = np.load(path,skiprows=start_line)
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
            flag = line.strip()
            if  flag == "end_header":
                return count + 1
            else:
                if count >= 50:
                    return None
                count += 1
    return None

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")
    n_p = data.shape[0]
    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",
        "element face 0\n",
        "property list uchar int vertex_indices\n",
        "end_header"]
    n_cols = data.shape[1]

    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='%.6f')
                fmt=fmt)
    print(f"{time.asctime()} {name}.ply have been saved with {n_p} points!")