optimization for auto slam

wu_xinjun

2022-03-22 fee0c7c1872b32141612ad0b3730b865e7e4dace

 src/slam/slam.py

@@ -6,6 +6,7 @@
sys.path.insert(0, src_path)

from utils.tools import read_ply
from tqdm import tqdm



@@ -76,12 +77,33 @@
    data = rotated_ply
    return data

def transferToZero(data,x,y,z,alpha,beta,gamma,reverse=False):
    """
    转换当前坐标至另一坐标, xyz分别为当前坐标原点在目标坐标系的位置;
    alpha,beta,gamma 为当前坐标系相对目标坐标系的旋转参数。
    """

    # 平移
    moved_data = current + np.array([x,y,z])
    # 旋转
    rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
    rotated_data = np.matmul(moved_data,rotation_matrix)

    # 左右手坐标系转换, 使用z轴转换

    if reverse:
        rotated_data[:,-1] = -rotated_data.copy()[:,-1]

    return rotated_data


def compose_ply(muti_ply_with_angle: list):

    ply_nums = len(muti_ply_with_angle)
    composed_ply_array = None

    for i, ply_with_angle in enumerate( muti_ply_with_angle):
    print ("rotating the ply ...")
    for i, ply_with_angle in enumerate(tqdm(muti_ply_with_angle)):
        angle = ply_with_angle[:-1]
        ply = ply_with_angle[-1]

@@ -94,12 +116,11 @@
        # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
        rotated_ply = np.matmul(ply,rotation_matrix)
        if i == 0:
            composed_ply_array = np.array(rotated_ply)
            composed_ply_array = rotated_ply.copy()
        else:
            temp = np.array(rotated_ply)
            composed_ply_array = np.concatenate((composed_ply_array,temp),axis= 0 )
            composed_ply_array = np.concatenate((composed_ply_array,rotated_ply),axis= 0 )
    
    composed_ply_array = composed_ply_array.reshape(-1,3)
    composed_ply_array = np.array(composed_ply_array).reshape(-1,3)
    return composed_ply_array