debug for slam core

wu_xinjun

2022-03-24 e175b351286c34defd8c2bbe1ddd09d64d2ac862

 src/slam/core.py

@@ -1,5 +1,6 @@
import numpy as np
import os, sys
import math
# # add python path of src to sys.path
# src_path = os.path.join(__file__, *(['..'] * 2))
# src_path = os.path.abspath(src_path)
@@ -20,15 +21,16 @@
# 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,verbose=0):
def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,order = "x_y_z",verbose=0):
    """generate a rotation matrix from the specific angel
    """

    rad_list = []
    for i, angle in enumerate([alpha_degree,beta_degree,gamma_degree]):

        if np.abs(float(angle))>360:
            raise ValueError(f"incorrect angle value: value in [-360,360] is excepted, but {angle} is offered.")
        rad_list.append(np.pi * float(angle) / 180)
        rad_list.append(math.radians(angle))

    alpha = rad_list[0]
    Rx_alpha = np.matrix([[1,0,0],
@@ -44,13 +46,21 @@
    Rz_gamma = np.matrix([[np.cos(gamma),np.sin(gamma),0],
                        [-np.sin(gamma),np.cos(gamma),0],
                        [0,0,1]])
         
    rotation_matrix = Rx_alpha * Ry_beta * Rz_gamma

    R_dic = {"x":Rx_alpha,
            "y":Ry_beta,
            "z":Rz_gamma
            }

    for i, s in enumerate(order.split("_")):
        if i == 0:
            rotation_matrix =  R_dic[s]
        else:
            rotation_matrix = R_dic[s] * rotation_matrix 
    if verbose == 1:
        print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
        print(f"\n{beta_degree}, {beta}, Ry_beta \n", Ry_beta)
        print(f"\n{gamma_degree}, {gamma}, Rz_gamma\n",Rz_gamma)
        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)
    else:
        pass
@@ -69,7 +79,7 @@
    # 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)
    rotated_ply = np.matmul(ply,rotation_matrix.transpose())
    # 转为右手坐标系
    if reverse:
        rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]
@@ -87,7 +97,7 @@
    moved_data = data + np.array([x,y,z])
    # 旋转
    rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
    rotated_data = np.matmul(moved_data,rotation_matrix)
    rotated_data = np.matmul(moved_data,rotation_matrix.transpose())

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

@@ -114,7 +124,7 @@

        # z = (X * Y^T)^T = (Y * X^T)
        # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
        rotated_ply = np.matmul(ply,rotation_matrix)
        rotated_ply = np.matmul(ply,rotation_matrix.transpose())
        if i == 0:
            merged_ply_array = rotated_ply.copy()
        else: