Axis angle(Rodrigues notation) to Rotation matrix

Rotation을 표기하는 기법 중 axis-angle, quaternion, matrix 등 여러 형태가 존재하는데 여기서 axis-angle 표현법에서 Rotation matrix로 변환하는 코드다.

 

scipy.spatial.transform 내 Rotation를 이용해 Rotation.from_rotvec(axis_angle_xyz).as_matrix()를 써도 되지만 간혹 축 2개만 쓰거나 하는 경우도 있어 내부 계산 순서를 기억해둘 겸 함수로 만들어 두었다.

 

axis-angle은 x, y, z 순서로 적혀있지만 이를 rotation matrix로 조합할 때는 z y x 순서로 조합해야 한다. 행렬 연산 순서를 생각하면 같은 순서라고 할 수도 있다. (Rodrigues formula 참고)

 

def Rx(degree=None, radian=None):
    if degree is None and radian is None:
        raise Exception("ERROR : one of degree and radian should be given.")
    elif degree is not None and radian is not None:
        raise Exception("ERROR : only one of degree and radian should be given, not both.")

    if radian is None:
        radian = degree * np.pi / 180

    Rx = np.eye(3)
    Rx[1, 1] = np.cos(radian)
    Rx[1, 2] = -np.sin(radian)
    Rx[2, 1] = np.sin(radian)
    Rx[2, 2] = np.cos(radian)
    return Rx

def Ry(degree=None, radian=None):
    if degree is None and radian is None:
        raise Exception("ERROR : one of degree and radian should be given.")
    elif degree is not None and radian is not None:
        raise Exception("ERROR : only one of degree and radian should be given, not both.")

    if radian is None:
        radian = degree * np.pi / 180

    Ry = np.eye(3)
    Ry[0, 0] = np.cos(radian)
    Ry[0, 2] = np.sin(radian)
    Ry[2, 0] = -np.sin(radian)
    Ry[2, 2] = np.cos(radian)
    return Ry

def Rz(degree=None, radian=None):
    if degree is None and radian is None:
        raise Exception("ERROR : one of degree and radian should be given.")
    elif degree is not None and radian is not None:
        raise Exception("ERROR : only one of degree and radian should be given, not both.")

    if radian is None:
        radian = degree * np.pi / 180

    Rz = np.eye(3)
    Rz[0, 0] = np.cos(radian)
    Rz[0, 1] = -np.sin(radian)
    Rz[1, 0] = np.sin(radian)
    Rz[1, 1] = np.cos(radian)
    return Rz

def make_R33(rx, ry, rz):
    rotmat_x = Rx(radian=rx)
    rotmat_y = Ry(radian=ry)
    rotmat_z = Rz(radian=rz)

    R = np.matmul(np.matmul(rotmat_z,rotmat_y),rotmat_x)
    return R

axis angle이 주어졌을 때 일단 각 axis 별 rotation matrix를 만들고 이를 최종 R matrix로 조합한다. 

 

참고로 axis 별 rotation matrix를 만드는 것도 어디가 1이었고 어디가 -가 붙었는지 헷갈리는 경우가 많은데 위에 구현된 함수를 그냥 가져다 쓰는 식으로 하면 편하더라.