auto slam

wu_xinjun

2022-03-21 e82c2a3abd0f1bd450bea46bc67b84a07017028e

 src/slam/slam.py

@@ -56,6 +56,26 @@

    return rotation_matrix

def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
    # Notice: 假定当前坐标系为右手系
    # step 1: 平移；current_coor + delta(delta_x, delta_y, delta_z) = target_coor
    #               x' = x - delta_x, y' = y - delta_y, z' = z -delta_y
    # step 2: 旋转; 当前坐标系朝正方面给旋转到指定坐标系。
    # step 3: 检查是否需要从右手系转换到左手系。
    # 平移
    ply = current - np.array([delta_x,delta_y,delta_z])
    # 旋转
    # z = (X * Y^T)^T = (Y * X^T)
    # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
    rotation_matrix = get_rotation_matrix(alpha,beta,gamma)
    rotated_ply = np.matmul(ply,rotation_matrix)
    # 转为右手坐标系
    if reverse:
        rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]

    data = rotated_ply
    return data

def compose_ply(muti_ply_with_angle: list):

    ply_nums = len(muti_ply_with_angle)
@@ -83,3 +103,10 @@
    return composed_ply_array


if __name__ == "__main__":
    current = np.array([[1,1,1],[1,3,9],[4,8,5]])
    print(current)
    target = transferTo(current,1,1,1,0,0,0,True)
    print(target)