diff --git a/ar_tag_pose/__init__.py b/ar_tag_pose/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/ar_tag_pose.py b/ar_tag_pose/ar_tag_pose.py
similarity index 100%
rename from ar_tag_pose.py
rename to ar_tag_pose/ar_tag_pose.py
diff --git a/ar_tag_pose_aruco_board.py b/ar_tag_pose/ar_tag_pose_aruco_board.py
similarity index 100%
rename from ar_tag_pose_aruco_board.py
rename to ar_tag_pose/ar_tag_pose_aruco_board.py
diff --git a/ar_tag_pose_charuco_board.py b/ar_tag_pose/ar_tag_pose_charuco_board.py
similarity index 100%
rename from ar_tag_pose_charuco_board.py
rename to ar_tag_pose/ar_tag_pose_charuco_board.py
diff --git a/aruco_types.py b/ar_tag_pose/aruco_types.py
similarity index 100%
rename from aruco_types.py
rename to ar_tag_pose/aruco_types.py
diff --git a/ar_tag_pose/utils.py b/ar_tag_pose/utils.py
new file mode 100644
index 0000000..b386e00
--- /dev/null
+++ b/ar_tag_pose/utils.py
@@ -0,0 +1,27 @@
+import numpy as np
+import cv2 as cv
+import math
+
+def rotation_matrix_to_euler_angles(R):
+	sy = math.sqrt(R[0,0]**2 + R[1,0]**2)
+	singular = sy < 1e-6
+
+	if not singular:
+		x = math.atan2(R[2,1] , R[2,2])
+		y = math.atan2(-R[2,0], sy)
+		z = math.atan2(R[1,0], R[0,0])
+	else:
+		# Handle singularity
+		x = math.atan2(-R[1,2], R[1,1])
+		y = math.atan2(-R[2,0], sy)
+		z = 0
+	return np.array([x, y, z])
+
+def to_board_pose_euler(r_vec, t_vec):
+	r_mat = np.ndarray((3, 3))
+	cv.Rodrigues(r_vec, r_mat)
+	r_mat = np.hstack((r_mat, t_vec))
+	r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1])))
+	r_mat = np.linalg.inv(r_mat)
+	res = rotation_matrix_to_euler_angles(r_mat)
+	return res, r_mat[:,3]
diff --git a/aruco_detect.py b/legacy/aruco_detect.py
similarity index 100%
rename from aruco_detect.py
rename to legacy/aruco_detect.py
diff --git a/aruco_gen.py b/legacy/aruco_gen.py
similarity index 100%
rename from aruco_gen.py
rename to legacy/aruco_gen.py
diff --git a/legacy/aruco_types.py b/legacy/aruco_types.py
new file mode 100644
index 0000000..bf90cc4
--- /dev/null
+++ b/legacy/aruco_types.py
@@ -0,0 +1,25 @@
+import cv2
+
+ARUCO_DICT = {
+	"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
+	"DICT_4X4_100": cv2.aruco.DICT_4X4_100,
+	"DICT_4X4_250": cv2.aruco.DICT_4X4_250,
+	"DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
+	"DICT_5X5_50": cv2.aruco.DICT_5X5_50,
+	"DICT_5X5_100": cv2.aruco.DICT_5X5_100,
+	"DICT_5X5_250": cv2.aruco.DICT_5X5_250,
+	"DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
+	"DICT_6X6_50": cv2.aruco.DICT_6X6_50,
+	"DICT_6X6_100": cv2.aruco.DICT_6X6_100,
+	"DICT_6X6_250": cv2.aruco.DICT_6X6_250,
+	"DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
+	"DICT_7X7_50": cv2.aruco.DICT_7X7_50,
+	"DICT_7X7_100": cv2.aruco.DICT_7X7_100,
+	"DICT_7X7_250": cv2.aruco.DICT_7X7_250,
+	"DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
+	"DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
+	"DICT_APRILTAG_16h5": cv2.aruco.DICT_APRILTAG_16h5,
+	"DICT_APRILTAG_25h9": cv2.aruco.DICT_APRILTAG_25h9,
+	"DICT_APRILTAG_36h10": cv2.aruco.DICT_APRILTAG_36h10,
+	"DICT_APRILTAG_36h11": cv2.aruco.DICT_APRILTAG_36h11
+}
diff --git a/cam_calib_checker.py b/legacy/cam_calib_checker.py
similarity index 100%
rename from cam_calib_checker.py
rename to legacy/cam_calib_checker.py
diff --git a/cam_pose_aruco.py b/legacy/cam_pose_aruco.py
similarity index 100%
rename from cam_pose_aruco.py
rename to legacy/cam_pose_aruco.py
diff --git a/cam_pose_aruco_board.py b/legacy/cam_pose_aruco_board.py
similarity index 100%
rename from cam_pose_aruco_board.py
rename to legacy/cam_pose_aruco_board.py
diff --git a/cam_pose_charuco_board.py b/legacy/cam_pose_charuco_board.py
similarity index 100%
rename from cam_pose_charuco_board.py
rename to legacy/cam_pose_charuco_board.py
diff --git a/cam_pose_charuco_diamond.py b/legacy/cam_pose_charuco_diamond.py
similarity index 100%
rename from cam_pose_charuco_diamond.py
rename to legacy/cam_pose_charuco_diamond.py
diff --git a/cam_pose_checker.py b/legacy/cam_pose_checker.py
similarity index 100%
rename from cam_pose_checker.py
rename to legacy/cam_pose_checker.py
diff --git a/utils.py b/legacy/utils.py
similarity index 100%
rename from utils.py
rename to legacy/utils.py
diff --git a/pyproject.toml b/pyproject.toml
index 9e1d9cf..b982c1d 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -19,6 +19,4 @@ dependencies = [
 readme = "README.md"
 
 [tool.setuptools]
-py-modules = [
-'ar_tag_pose_aruco_board', 'ar_tag_pose', 'aruco_detect', 'cam_pose_charuco_diamond', 'aruco_types', 'ar_tag_pose_charuco_board', 'cam_calib_checker', 'cam_pose_checker', 'cam_pose_aruco', 'aruco_gen', 'utils', 'cam_pose_charuco_board', 'cam_pose_aruco_board'
-]
\ No newline at end of file
+py-modules = ['ar_tag_pose/*']