VisualMotionTrackingSystem/visual_tracker.py at master · DuraGT/VisualMotionTrackingSystem · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
import cv2
import cv2.aruco as aruco
import numpy as np

# Use the working URL
cap = cv2.VideoCapture("http://10.0.0.189:8080/videofeed")

# ArUco setup for OpenCV 4.7+
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
aruco_params = aruco.DetectorParameters()
detector = aruco.ArucoDetector(aruco_dict, aruco_params)  # New detector object

# Camera calibration parameters
focal_length = 800
center = (320, 240)
camera_matrix = np.array([[focal_length, 0, center[0]],
                          [0, focal_length, center[1]],
                          [0, 0, 1]], dtype=float)
dist_coeffs = np.zeros((4, 1))

# Marker size in cm
marker_length = 4.0

print("✅ ArUco detection started. Press 'q' to quit.")

while True:
    ret, frame = cap.read()
    if not ret:
        print("Camera stream not available.")
        break

    # Resize frame if it's too large (your stream is 1080x1920)
    frame = cv2.resize(frame, (640, 480))  # Resize to smaller for better performance

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # Detect markers - NEW API for OpenCV 4.7+
    corners, ids, rejected = detector.detectMarkers(gray)

    if ids is not None:
        # Draw all detected markers - NEW API
        frame = aruco.drawDetectedMarkers(frame, corners, ids)

        # Lists to store all marker positions
        positions = []

        # Estimate pose of each marker
        for i, corner in enumerate(corners):
            rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, dist_coeffs)

            # Draw axes on each marker
            cv2.drawFrameAxes(frame, camera_matrix, dist_coeffs, rvec, tvec, 2)  # Updated function

            # Extract translation vector (x, y, z)
            x, y, z = tvec[0][0]
            positions.append((x, y, z))

            # Display marker info
            cv2.putText(frame, f"ID:{int(ids[i])} x:{x:.1f} y:{y:.1f} z:{z:.1f}",
                        (10, 30 + 30 * i), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)

        # Compute average (centroid) of all detected markers
        if positions:
            avg_pos = np.mean(positions, axis=0)
            ax, ay, az = avg_pos
            cv2.putText(frame, f"Centroid → X:{ax:.1f} Y:{ay:.1f} Z:{az:.1f}",
                        (10, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
    else:
        cv2.putText(frame, "No markers detected", (10, 40),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)

    cv2.imshow("Aruco Marker Tracking", frame)

    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()