[PyICP-SLAM] UtilsMisc.py 코드 공부

import os 
import csv
import copy
import time
import math

import numpy as np
import matplotlib.pyplot as plt

import gtsam

def getConstDigitsNumber(val, num_digits):
    return "{:.{}f}".format(val, num_digits)

def getUnixTime():
    return int(time.time())

def eulerAnglesToRotationMatrix(theta) :
     
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])
                     
    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])
                 
    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
                     
    R = np.dot(R_z, np.dot( R_y, R_x ))
 
    return R

def yawdeg2so3(yaw_deg):
    yaw_rad = np.deg2rad(yaw_deg)
    return eulerAnglesToRotationMatrix([0, 0, yaw_rad])

def yawdeg2se3(yaw_deg):
    se3 = np.eye(4)
    se3[:3, :3] = yawdeg2so3(yaw_deg)
    return se3 


def getGraphNodePose(graph, idx):

    pose = graph.atPose3(gtsam.symbol('x', idx))
    pose_trans = np.array([pose.x(), pose.y(), pose.z()])
    pose_rot = pose.rotation().matrix()

    return pose_trans, pose_rot

1. getConstDigitsNumber(val, num_digits)

이 함수는 주어진 숫자 val을 받아서, 지정된 소수점 이하 자릿수 num_digits로 포맷된 문자열을 반환한다. 이는 데이터 출력시 일관된 숫자 표현을 위해 사용된다.

2. getUnixTime()
현재 시간을 UNIX 시간 형태 (1970년 1월 1일부터의 초 단위)로 반환한다. 시간 기록이나 이벤트 타임스탬프에 사용된다.

3. eulerAnglesToRotationMatrix(theta)
오일러 각을 받아서, 해당 각도를 나타내는 회전 행렬을 반환한다.

4. yawdeg2so3(yaw_deg)
yaw 값을 해당 회전을 나타내는 SO(3) 회전 행렬을 반환한다.

5. yawdeg2se3(yaw_deg)
yaw 값을 받아서 해당 각도의 3D 회전과 함께 변환 행렬(SE(3) 행렬)을 생성한다. SE(3) 행렬은 회전과 이동을 동시에 포함하는 행렬이다.

6. getGraphNodePose(graph, idx)
그래프 구조에서 특정 인덱스의 노드 위치(포즈)를 찾은 후, 회전행렬과 이동벡터를 반환한다.