support for 6 columns ply

wu_xinjun

2022-05-13 13530d0ad2c32608d0b46a5c16ad4ac0c25b59f1

 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