[ref] overlayのリファクタリングとテストを追加

src-python/models/overlay/overlay_utils.py

@@ -1,11 +1,29 @@
 import numpy as np
+from typing import Sequence
 
-def toHomogeneous(matrix):
+
+def toHomogeneous(matrix: np.ndarray) -> np.ndarray:
+    """Convert a 3x4 base matrix to a 4x4 homogeneous matrix.
+
+    Args:
+        matrix: 3x4 numpy array
+
+    Returns:
+        4x4 numpy array with last row [0, 0, 0, 1]
+    """
     homogeneous_matrix = np.vstack([matrix, [0, 0, 0, 1]])
     return homogeneous_matrix
 
 # 移動行列を生成する関数
-def calcTranslationMatrix(translation):
+def calcTranslationMatrix(translation: Sequence[float]) -> np.ndarray:
+    """Create a 4x4 translation matrix from a 3-element translation.
+
+    Args:
+        translation: (tx, ty, tz)
+
+    Returns:
+        4x4 numpy translation matrix
+    """
     tx, ty, tz = translation
     return np.array([
         [1, 0, 0, tx],
@@ -15,9 +33,10 @@ def calcTranslationMatrix(translation):
     ])
 
 # X軸周りの回転行列を生成する関数
-def calcRotationMatrixX(angle):
-    c = np.cos(np.pi/180*angle)
-    s = np.sin(np.pi/180*angle)
+def calcRotationMatrixX(angle: float) -> np.ndarray:
+    """Rotation matrix around X axis for given angle in degrees."""
+    c = np.cos(np.pi / 180 * angle)
+    s = np.sin(np.pi / 180 * angle)
     return np.array([
         [1, 0, 0, 0],
         [0, c, -s, 0],
@@ -26,9 +45,10 @@ def calcRotationMatrixX(angle):
     ])
 
 # Y軸周りの回転行列を生成する関数
-def calcRotationMatrixY(angle):
-    c = np.cos(np.pi/180*angle)
-    s = np.sin(np.pi/180*angle)
+def calcRotationMatrixY(angle: float) -> np.ndarray:
+    """Rotation matrix around Y axis for given angle in degrees."""
+    c = np.cos(np.pi / 180 * angle)
+    s = np.sin(np.pi / 180 * angle)
     return np.array([
         [c, 0, s, 0],
         [0, 1, 0, 0],
@@ -37,9 +57,10 @@ def calcRotationMatrixY(angle):
     ])
 
 # Z軸周りの回転行列を生成する関数
-def calcRotationMatrixZ(angle):
-    c = np.cos(np.pi/180*angle)
-    s = np.sin(np.pi/180*angle)
+def calcRotationMatrixZ(angle: float) -> np.ndarray:
+    """Rotation matrix around Z axis for given angle in degrees."""
+    c = np.cos(np.pi / 180 * angle)
+    s = np.sin(np.pi / 180 * angle)
     return np.array([
         [c, -s, 0, 0],
         [s, c, 0, 0],
@@ -48,7 +69,17 @@ def calcRotationMatrixZ(angle):
     ])
 
 # 3x4行列の座標を基準として回転や移動を行う関数
-def transform_matrix(base_matrix, translation, rotation):
+def transform_matrix(base_matrix: np.ndarray, translation: Sequence[float], rotation: Sequence[float]) -> np.ndarray:
+    """Apply translation and Euler rotations to a 3x4 base matrix.
+
+    Args:
+        base_matrix: 3x4 base transform matrix
+        translation: (tx, ty, tz)
+        rotation: (x_deg, y_deg, z_deg)
+
+    Returns:
+        Transformed 3x4 matrix (numpy.ndarray)
+    """
     homogeneous_base_matrix = toHomogeneous(base_matrix)
     translation_matrix = calcTranslationMatrix(translation)
     rotation_matrix_x = calcRotationMatrixX(rotation[0])
@@ -60,10 +91,18 @@ def transform_matrix(base_matrix, translation, rotation):
     result_matrix = np.dot(homogeneous_base_matrix, transformation_matrix)
     return result_matrix[:3, :]
 
-def euler_to_rotation_matrix(angles):
+def euler_to_rotation_matrix(angles: Sequence[float]) -> np.ndarray:
+    """Convert Euler angles in degrees to a 3x3 rotation matrix.
+
+    Args:
+        angles: (x_deg, y_deg, z_deg)
+
+    Returns:
+        3x3 rotation matrix
+    """
     phi = angles[0] * np.pi / 180
     theta = angles[1] * np.pi / 180
-    psi = angles[2]* np.pi / 180
+    psi = angles[2] * np.pi / 180
     R_x = np.array([[1, 0, 0],
                     [0, np.cos(phi), -np.sin(phi)],
                     [0, np.sin(phi), np.cos(phi)]])