...

Author: CursedPrograms

.gitignore

@@ -3,6 +3,8 @@ __pycache__/
 *.py[cod]
 *$py.class
 
+psdenv/
+
 # C extensions
 *.so
 

README.md

@@ -25,6 +25,9 @@ To make the setup script executable, run the following command in your terminal:
 chmod +x setup.sh
 ```
 
+pip install opencv-contrib-python==4.6.0.66
+
+
 <br>
 
 <div align="center">

main.py

@@ -14,14 +14,14 @@ def main():
 
     scripts = {
         "1": {
-            "name": "Run 'Script00'",
-            "description": "This is Script01",
+            "name": "Run 'marker.py'",
+            "description": "This is marker.py",
             "file_name": "scripts/marker.py"
         },
         "2": {
-            "name": "Run 'Script01",
-            "description": "This is Script01",
-            "file_name": "scripts/marker.py"
+            "name": "Run 'pose_estimation.py",
+            "description": "This is pose_estimation.py",
+            "file_name": "scripts/pose_estimation.py"
         },
         "00": {
             "name": "Run 'install_dependencies.py'",

requirements.txt

@@ -1 +1,4 @@
 opencv-python==1.26.2
+opencv-contrib-python>=4.5.0
+matplotlib>=3.0.0
+numpy>=1.19.0

scripts/pose_estimation.py

@@ -0,0 +1,122 @@
+import numpy as np
+import cv2
+import sys
+import time
+
+
+
+
+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
+}
+
+def aruco_display(corners, ids, rejected, image):
+    
+	if len(corners) > 0:
+		
+		ids = ids.flatten()
+		
+		for (markerCorner, markerID) in zip(corners, ids):
+			
+			corners = markerCorner.reshape((4, 2))
+			(topLeft, topRight, bottomRight, bottomLeft) = corners
+			
+			topRight = (int(topRight[0]), int(topRight[1]))
+			bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
+			bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
+			topLeft = (int(topLeft[0]), int(topLeft[1]))
+
+			cv2.line(image, topLeft, topRight, (0, 255, 0), 2)
+			cv2.line(image, topRight, bottomRight, (0, 255, 0), 2)
+			cv2.line(image, bottomRight, bottomLeft, (0, 255, 0), 2)
+			cv2.line(image, bottomLeft, topLeft, (0, 255, 0), 2)
+			
+			cX = int((topLeft[0] + bottomRight[0]) / 2.0)
+			cY = int((topLeft[1] + bottomRight[1]) / 2.0)
+			cv2.circle(image, (cX, cY), 4, (0, 0, 255), -1)
+			
+			cv2.putText(image, str(markerID),(topLeft[0], topLeft[1] - 10), cv2.FONT_HERSHEY_SIMPLEX,
+				0.5, (0, 255, 0), 2)
+			print("[Inference] ArUco marker ID: {}".format(markerID))
+			
+	return image
+
+
+
+def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coefficients):
+    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_type)  # Changed to use local variable
+    parameters = cv2.aruco.DetectorParameters_create()
+
+    corners, ids, rejected_img_points = cv2.aruco.detectMarkers(
+        gray, aruco_dict, parameters=parameters,
+        cameraMatrix=matrix_coefficients,
+        distCoeff=distortion_coefficients
+    )
+
+    if len(corners) > 0:
+        # Draw detected markers
+        cv2.aruco.drawDetectedMarkers(frame, corners, ids)
+
+        for i in range(len(ids)):
+            rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners[i], 0.02, matrix_coefficients, distortion_coefficients)
+
+            # Draw the axis for each marker
+            cv2.aruco.drawAxis(frame, matrix_coefficients, distortion_coefficients, rvec, tvec, 0.01)
+
+    return frame
+
+    
+
+aruco_type = "DICT_5X5_100"
+
+arucoDict = cv2.aruco.Dictionary_get(ARUCO_DICT[aruco_type])
+
+arucoParams = cv2.aruco.DetectorParameters_create()
+
+
+intrinsic_camera = np.array(((933.15867, 0, 657.59),(0,933.1586, 400.36993),(0,0,1)))
+distortion = np.array((-0.43948,0.18514,0,0))
+
+
+cap = cv2.VideoCapture(0)
+
+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+
+
+
+while cap.isOpened():
+    
+    ret, img = cap.read()
+    
+    output = pose_estimation(img, ARUCO_DICT[aruco_type], intrinsic_camera, distortion)
+
+    cv2.imshow('Estimated Pose', output)
+
+    key = cv2.waitKey(1) & 0xFF
+    if key == ord('q'):
+        break
+
+cap.release()
+cv2.destroyAllWindows()