Commit for Gitea
This commit is contained in:
DuOtto
@@ -0,0 +1,260 @@
|
||||
import numpy as np
|
||||
import cv2
|
||||
from PySide6.QtCore import QObject, QTimer
|
||||
import mediapipe as mp
|
||||
from Tbd.helper import Hand2D
|
||||
from collections import defaultdict
|
||||
from Helpers.HandUdpSender import HandUdpSender
|
||||
|
||||
import struct
|
||||
import time
|
||||
|
||||
class TrackingController(QObject):
|
||||
def __init__(self, cameraList, P_mats, logger=None, tick_ms=30):
|
||||
super().__init__()
|
||||
|
||||
self.cameraList = cameraList
|
||||
self.P_mats = P_mats
|
||||
self.logger = logger
|
||||
|
||||
self.udp = HandUdpSender(host="127.0.0.1", port=9000)
|
||||
|
||||
self.timer = QTimer(self)
|
||||
self.timer.timeout.connect(self._tick)
|
||||
self.tick_ms = tick_ms
|
||||
|
||||
self.running = False
|
||||
|
||||
# init mediapipe here (or inject it)
|
||||
|
||||
self.mp_hands = mp.solutions.hands
|
||||
self.hands = self.mp_hands.Hands(
|
||||
static_image_mode=False,
|
||||
max_num_hands=4,
|
||||
model_complexity=1,
|
||||
min_detection_confidence=0.5,
|
||||
min_tracking_confidence=0.5,
|
||||
)
|
||||
|
||||
def start(self):
|
||||
# Guard: need projections
|
||||
self.logger.log("Amount of P_mats:" + str(len(self.P_mats)))
|
||||
if not self.P_mats or len(self.P_mats) < 2:
|
||||
if self.logger: self.logger.log("[Tracking] Missing P_mats (need >=2 cameras calibrated)")
|
||||
return
|
||||
|
||||
if self.running:
|
||||
return
|
||||
self.running = True
|
||||
self.timer.start(self.tick_ms)
|
||||
if self.logger: self.logger.log("[Tracking] Started")
|
||||
|
||||
def stop(self):
|
||||
if not self.running:
|
||||
return
|
||||
self.running = False
|
||||
self.timer.stop()
|
||||
if self.logger: self.logger.log("[Tracking] Stopped")
|
||||
|
||||
def _tick(self):
|
||||
hands2d = self._detectHands_all_cameras()
|
||||
if not hands2d:
|
||||
return
|
||||
|
||||
# Group hands across cameras (you already have _allocateDetectedHands logic)
|
||||
hand_groups = self._allocateDetectedHands(hands2d)
|
||||
|
||||
# Triangulate (your existing triangulation approach)
|
||||
hands3d = self._triangulateHands(hand_groups)
|
||||
|
||||
if len(hands3d) > 0:
|
||||
self.udp.send_hands3d(hands3d)
|
||||
|
||||
print(
|
||||
f"hands2d={len(hands2d)} groups={len(hand_groups)} hands3d={len(hands3d)}",
|
||||
flush=True
|
||||
)
|
||||
|
||||
if len(hands2d) > 0:
|
||||
print("cam_ids:", sorted(set(h.camera_id for h in hands2d)), flush=True)
|
||||
|
||||
def _detectHands_all_cameras(self):
|
||||
out = []
|
||||
for camera in self.cameraList:
|
||||
ok, frameBGR = camera.capture.read()
|
||||
if not ok or frameBGR is None:
|
||||
continue
|
||||
|
||||
h, w = frameBGR.shape[:2]
|
||||
frameRGB = cv2.cvtColor(frameBGR, cv2.COLOR_BGR2RGB)
|
||||
frameRGB.flags.writeable = False
|
||||
|
||||
hand_tracking_result = self.hands.process(frameRGB)
|
||||
if not hand_tracking_result.multi_hand_landmarks:
|
||||
continue
|
||||
|
||||
handed = hand_tracking_result.multi_handedness
|
||||
for hi, lm_list in enumerate(hand_tracking_result.multi_hand_landmarks):
|
||||
landmarks_px = np.array(
|
||||
[[lm.x * w, lm.y * h] for lm in lm_list.landmark],
|
||||
dtype=np.float32
|
||||
)
|
||||
|
||||
label, score = "Unknown", 0.0
|
||||
if hi < len(handed) and handed[hi].classification:
|
||||
label = handed[hi].classification[0].label
|
||||
score = handed[hi].classification[0].score
|
||||
|
||||
out.append(Hand2D(
|
||||
camera_id=camera.index,
|
||||
handedness=label,
|
||||
score=score,
|
||||
landmarks_px=landmarks_px
|
||||
))
|
||||
return out
|
||||
|
||||
def _allocateDetectedHands(self, hands2d):
|
||||
frames_by_camera = defaultdict(list)
|
||||
for hand in hands2d:
|
||||
frames_by_camera[hand.camera_id].append(hand)
|
||||
|
||||
used = set()
|
||||
hand_groups = []
|
||||
reproj_threshold = 50.0
|
||||
|
||||
camera_ids = sorted(frames_by_camera.keys())
|
||||
|
||||
for cam_a in camera_ids:
|
||||
if cam_a not in self.P_mats:
|
||||
continue
|
||||
for hand_a in frames_by_camera[cam_a]:
|
||||
if id(hand_a) in used:
|
||||
continue
|
||||
|
||||
group = {cam_a: hand_a}
|
||||
used.add(id(hand_a))
|
||||
|
||||
P_a = self.P_mats[cam_a]
|
||||
|
||||
for cam_b in camera_ids:
|
||||
if cam_b == cam_a:
|
||||
continue
|
||||
if cam_b not in self.P_mats:
|
||||
continue
|
||||
|
||||
P_b = self.P_mats[cam_b]
|
||||
best_hand = None
|
||||
best_err = np.inf
|
||||
|
||||
for hand_b in frames_by_camera[cam_b]:
|
||||
if id(hand_b) in used:
|
||||
continue
|
||||
|
||||
err = self.pair_reprojection_error(
|
||||
hand_a, hand_b, P_a, P_b, key_idxs=[0, 9]
|
||||
)
|
||||
if err < best_err:
|
||||
best_err = err
|
||||
best_hand = hand_b
|
||||
|
||||
if best_hand is not None and best_err < reproj_threshold:
|
||||
group[cam_b] = best_hand
|
||||
used.add(id(best_hand))
|
||||
|
||||
hand_groups.append(group)
|
||||
|
||||
return hand_groups
|
||||
|
||||
def triangulate_hands_with_metadata(self, hand_groups):
|
||||
hands3d = []
|
||||
for group in hand_groups:
|
||||
if len(group) < 2:
|
||||
continue
|
||||
|
||||
handedness, confidence = self.choose_metadata_from_group(group)
|
||||
|
||||
cams = list(group.keys())
|
||||
P1 = self.P_mats[cams[0]]
|
||||
P2 = self.P_mats[cams[1]]
|
||||
lm1 = group[cams[0]].landmarks_px
|
||||
lm2 = group[cams[1]].landmarks_px
|
||||
|
||||
pts3d = []
|
||||
for i in range(lm1.shape[0]):
|
||||
X = self.triangulate_point(P1, P2, lm1[i], lm2[i])
|
||||
pts3d.append(X)
|
||||
|
||||
hands3d.append({
|
||||
"handedness": handedness, # "Left"/"Right"/"Unknown"
|
||||
"confidence": confidence, # 0..1
|
||||
"landmarks": np.asarray(pts3d, dtype=np.float32) # (21,3)
|
||||
})
|
||||
return hands3d
|
||||
|
||||
# --- math helpers (copy from your existing code) ---
|
||||
def triangulate_point(self, P1, P2, x1, x2):
|
||||
A = np.zeros((4, 4), dtype=np.float32)
|
||||
A[0] = x1[0] * P1[2] - P1[0]
|
||||
A[1] = x1[1] * P1[2] - P1[1]
|
||||
A[2] = x2[0] * P2[2] - P2[0]
|
||||
A[3] = x2[1] * P2[2] - P2[1]
|
||||
_, _, Vt = np.linalg.svd(A)
|
||||
X_h = Vt[-1]
|
||||
X_h /= X_h[3]
|
||||
|
||||
return X_h[:3]
|
||||
|
||||
def project_point(self, P, X):
|
||||
X_h = np.array([X[0], X[1], X[2], 1.0], dtype=np.float32)
|
||||
x = P @ X_h
|
||||
return np.array([x[0] / x[2], x[1] / x[2]], dtype=np.float32)
|
||||
|
||||
def pair_reprojection_error(self, hand_a, hand_b, P_a, P_b, key_idxs):
|
||||
errors = []
|
||||
for idx in key_idxs:
|
||||
x1 = hand_a.landmarks_px[idx]
|
||||
x2 = hand_b.landmarks_px[idx]
|
||||
X = self.triangulate_point(P_a, P_b, x1, x2)
|
||||
x1_hat = self.project_point(P_a, X)
|
||||
x2_hat = self.project_point(P_b, X)
|
||||
errors.append(np.linalg.norm(x1_hat - x1))
|
||||
errors.append(np.linalg.norm(x2_hat - x2))
|
||||
return float(np.mean(errors))
|
||||
|
||||
def choose_metadata_from_group(group: dict):
|
||||
# group: {cam_id: Hand2D, ...}
|
||||
best = max(group.values(), key=lambda h: float(getattr(h, "score", 0.0)))
|
||||
handedness = getattr(best, "handedness", "Unknown")
|
||||
confidence = float(getattr(best, "score", 0.0))
|
||||
return handedness, confidence
|
||||
|
||||
def _triangulateHands(self, hand_groups):
|
||||
hands3d = []
|
||||
|
||||
for group in hand_groups:
|
||||
cams = list(group.keys())
|
||||
if len(cams) < 2:
|
||||
continue
|
||||
|
||||
# Pick metadata from best 2D view
|
||||
best = max(group.values(), key=lambda hh: float(getattr(hh, "score", 0.0)))
|
||||
handedness = getattr(best, "handedness", "Unknown")
|
||||
confidence = float(getattr(best, "score", 0.0))
|
||||
|
||||
P1 = self.P_mats[cams[0]]
|
||||
P2 = self.P_mats[cams[1]]
|
||||
lm1 = group[cams[0]].landmarks_px
|
||||
lm2 = group[cams[1]].landmarks_px
|
||||
|
||||
pts3d = []
|
||||
for i in range(lm1.shape[0]):
|
||||
X = self.triangulate_point(P1, P2, lm1[i], lm2[i])
|
||||
pts3d.append(X)
|
||||
|
||||
hands3d.append({
|
||||
"landmarks": np.asarray(pts3d, dtype=np.float32),
|
||||
"handedness": handedness,
|
||||
"confidence": confidence,
|
||||
})
|
||||
|
||||
return hands3d
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,32 @@
|
||||
import cv2, numpy as np
|
||||
|
||||
from helper import compute_distance
|
||||
|
||||
cal_circle = None # reference circle (x,y,r)
|
||||
|
||||
def object_calibration(frame_vis, fingertip_idx, CIRCLE_TOUCH_THRESHOLD):
|
||||
global cal_circle
|
||||
hsv = cv2.cvtColor(frame_vis, cv2.COLOR_BGR2HSV)
|
||||
mask = cv2.inRange(hsv, np.array([10,100,100]), np.array([25,255,255]))
|
||||
masked = cv2.bitwise_and(frame_vis, frame_vis, mask=mask)
|
||||
gray = cv2.cvtColor(masked, cv2.COLOR_BGR2GRAY)
|
||||
gray = cv2.medianBlur(gray,5)
|
||||
circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1.2, 100,
|
||||
param1=50, param2=30, minRadius=10, maxRadius=300)
|
||||
if circles is not None:
|
||||
circles = np.round(circles[0]).astype(int)
|
||||
touched = [(x,y,r) for x,y,r in circles
|
||||
if abs(compute_distance((x,y), fingertip_idx)-r) < CIRCLE_TOUCH_THRESHOLD]
|
||||
if touched:
|
||||
touched.sort(key=lambda c: abs(compute_distance((c[0],c[1]), fingertip_idx)-c[2]))
|
||||
x,y,r = touched[0]
|
||||
cal_circle = (x,y,r)
|
||||
PIXELS_PER_INCH = 2 * r
|
||||
print(f"Circle calib: {PIXELS_PER_INCH:.2f} px/inch")
|
||||
|
||||
def draw_cal_circle(frame_vis):
|
||||
global cal_circle
|
||||
cx,cy,cr = cal_circle
|
||||
cv2.circle(frame_vis,(cx,cy),cr,(0,0,255),2)
|
||||
cv2.drawMarker(frame_vis,(cx,cy),(0,0,255),cv2.MARKER_TILTED_CROSS,15,1)
|
||||
cv2.putText(frame_vis,f"Ref r={cr} px",(cx-cr,cy+cr+20),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,0,255),1)
|
||||
@@ -0,0 +1,30 @@
|
||||
import numpy as np
|
||||
|
||||
# ----- Configuration -----
|
||||
CAMERA_INDICES_TO_CHECK = 10
|
||||
FRAME_WIDTH = 3980
|
||||
FRAME_HEIGHT = 2560
|
||||
PIXELS_PER_INCH = 38 # will be set by calibration
|
||||
PINCH_THRESHOLD = 40 # px to start touch
|
||||
RELEASE_THRESHOLD = 60 # px to end touch
|
||||
CIRCLE_TOUCH_THRESHOLD = 20 # px tolerance for circle touch
|
||||
|
||||
# HSV range for shape color (tune for your arrow: now tailored for orange)
|
||||
LOWER_SHAPE = np.array([10, 100, 100]) # hue from 10° (orange) to
|
||||
UPPER_SHAPE = np.array([30, 255, 255]) # hue up to 30°, full sat/val range
|
||||
|
||||
DEBOUNCE_TIME = 1.0 # seconds
|
||||
|
||||
# ——— Constants for Tabs & Buttons ———
|
||||
TAB_W, TAB_H = 40, 60
|
||||
TAB_X = 600 # adjust this to your projector output width
|
||||
TAB_Y0 = 50
|
||||
TAB_GAP = 10
|
||||
# left/right arrow buttons
|
||||
LEFT_BTN_TOP = (TAB_X, TAB_Y0+2*(TAB_H+TAB_GAP)+20)
|
||||
RIGHT_BTN_TOP = (TAB_X, LEFT_BTN_TOP[1]+TAB_H+TAB_GAP)
|
||||
|
||||
DEBUG = True
|
||||
|
||||
TARGET_FPS = 40
|
||||
FRAME_INTERVAL = 1.0 / TARGET_FPS
|
||||
@@ -0,0 +1,188 @@
|
||||
import mediapipe as mp
|
||||
import cv2, numpy as np, time, fitz
|
||||
from helper import compute_distance
|
||||
from config import PINCH_THRESHOLD, RELEASE_THRESHOLD
|
||||
|
||||
# hand_state.py
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Tuple, Dict, Optional
|
||||
|
||||
from helper import finger_straight
|
||||
|
||||
# ----- Initialize Hand Detector -----
|
||||
mp_hands = mp.solutions.hands
|
||||
mp_draw = mp.solutions.drawing_utils
|
||||
hands = mp_hands.Hands(
|
||||
static_image_mode=False,
|
||||
max_num_hands=8,
|
||||
min_detection_confidence=0.7,
|
||||
min_tracking_confidence=0.5
|
||||
)
|
||||
|
||||
start_pt = None # measurement start point
|
||||
measuring = False # measurement in progress
|
||||
|
||||
@dataclass
|
||||
class HandState:
|
||||
id: int # a persistent identifier for this hand
|
||||
landmarks: Dict[str, Tuple[int,int]] = field(default_factory=dict)
|
||||
finger_straightness: Dict[str, float] = field(default_factory=dict)
|
||||
gesture: Optional[str] = None # e.g. "pinch", "fist", "open"
|
||||
gesture_persistence: int = 0 # how many frames the current gesture has held
|
||||
|
||||
#handList = Dict[int, HandState] = {}
|
||||
next_id = 0
|
||||
|
||||
def update(mp_results, frame_vis):
|
||||
h, w, _ = frame_vis.shape
|
||||
detected_centroids = []
|
||||
landmarks_list = []
|
||||
|
||||
# 1) pull out centroids & raw landmarks
|
||||
if mp_results.multi_hand_landmarks:
|
||||
for hand in mp_results.multi_hand_landmarks:
|
||||
pts = []
|
||||
for lm in hand.landmark:
|
||||
pts.append((int(lm.x*w), int(lm.y*h)))
|
||||
centroid = np.mean(pts, axis=0)
|
||||
detected_centroids.append(tuple(centroid.astype(int)))
|
||||
landmarks_list.append((hand, pts))
|
||||
|
||||
# 2) match to existing by nearest centroid
|
||||
new_hands = {}
|
||||
used_ids = set()
|
||||
for (hand, pts), centroid in zip(landmarks_list, detected_centroids):
|
||||
# find best existing hand
|
||||
best_id, best_dist = None, 1e9
|
||||
for hid, state in handList.items():
|
||||
dx, dy = np.array(state.landmarks['centroid']) - centroid
|
||||
d = np.hypot(dx, dy)
|
||||
if d < best_dist and d < 100: # 100px max match distance
|
||||
best_dist, best_id = d, hid
|
||||
|
||||
if best_id is None:
|
||||
hid = next_id
|
||||
next_id += 1
|
||||
state = HandState(id=hid)
|
||||
else:
|
||||
hid = best_id
|
||||
state = handList[hid]
|
||||
|
||||
used_ids.add(hid)
|
||||
|
||||
# 3) update state
|
||||
state.persistence += 1
|
||||
state.landmarks['centroid'] = centroid
|
||||
|
||||
# compute fingertip positions
|
||||
idx_tip = pts[mp.solutions.hands.HandLandmark.INDEX_FINGER_TIP]
|
||||
mid_tip = pts[mp.solutions.hands.HandLandmark.MIDDLE_FINGER_TIP]
|
||||
state.landmarks['index_tip'] = idx_tip
|
||||
state.landmarks['middle_tip'] = mid_tip
|
||||
|
||||
for name, tip_i, pip_i, mcp_i in [
|
||||
('index', mp.solutions.hands.HandLandmark.INDEX_FINGER_TIP,
|
||||
mp.solutions.hands.HandLandmark.INDEX_FINGER_PIP,
|
||||
mp.solutions.hands.HandLandmark.INDEX_FINGER_MCP),
|
||||
('middle', mp.solutions.hands.HandLandmark.MIDDLE_FINGER_TIP,
|
||||
mp.solutions.hands.HandLandmark.MIDDLE_FINGER_PIP,
|
||||
mp.solutions.hands.HandLandmark.MIDDLE_FINGER_MCP),
|
||||
# add ring, pinky, thumb similarly...
|
||||
]:
|
||||
p_tip = pts[tip_i]
|
||||
p_pip = pts[pip_i]
|
||||
p_mcp = pts[mcp_i]
|
||||
state.finger_straightness[name] = finger_straight(p_tip, p_pip, p_mcp)
|
||||
|
||||
# 5) simple gesture detection
|
||||
if state.finger_straightness['index'] < 0.2 and \
|
||||
state.finger_straightness['middle'] < 0.2:
|
||||
gesture = 'fist'
|
||||
elif state.finger_straightness['index'] > 0.8 and \
|
||||
state.finger_straightness['middle'] > 0.8:
|
||||
gesture = 'open'
|
||||
else:
|
||||
gesture = None
|
||||
|
||||
if gesture == state.gesture:
|
||||
state.gesture_persistence += 1
|
||||
else:
|
||||
state.gesture = gesture
|
||||
state.gesture_persistence = 0
|
||||
|
||||
new_hands[hid] = state
|
||||
|
||||
# 6) drop hands not seen this frame
|
||||
handList = new_hands
|
||||
return list(hands.values())
|
||||
|
||||
|
||||
|
||||
def detect_hands(frame):
|
||||
rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
|
||||
rgb.flags.writeable = False
|
||||
results = hands.process(rgb)
|
||||
|
||||
return results
|
||||
|
||||
|
||||
def check_hand(hand, frame_vis, object_calibrating):
|
||||
h, w, _ = frame_vis.shape
|
||||
|
||||
fingertip_idx = None
|
||||
fingertip_mid = None
|
||||
|
||||
mp_draw.draw_landmarks(frame_vis, hand, mp_hands.HAND_CONNECTIONS)
|
||||
# get index and middle finger tips and PIP joints
|
||||
idx_tip = hand.landmark[mp_hands.HandLandmark.INDEX_FINGER_TIP]
|
||||
mid_tip = hand.landmark[mp_hands.HandLandmark.MIDDLE_FINGER_TIP]
|
||||
ix, iy = int(idx_tip.x * w), int(idx_tip.y * h)
|
||||
mx, my = int(mid_tip.x * w), int(mid_tip.y * h)
|
||||
fingertip_idx = (ix, iy)
|
||||
fingertip_mid = (mx, my)
|
||||
|
||||
# draw fingertips
|
||||
cv2.circle(frame_vis, fingertip_idx, 8, (255,255,0), -1)
|
||||
cv2.circle(frame_vis, fingertip_mid, 8, (0,255,0), -1)
|
||||
|
||||
measuring, start_pt = is_measuring(frame_vis, hand, object_calibrating)
|
||||
|
||||
return fingertip_idx, measuring, start_pt
|
||||
|
||||
|
||||
|
||||
def is_measuring(frame_vis, hand, object_calibrating):
|
||||
global measuring, start_pt
|
||||
h, w, _ = frame_vis.shape
|
||||
|
||||
idx_tip = hand.landmark[mp_hands.HandLandmark.INDEX_FINGER_TIP]
|
||||
idx_pip = hand.landmark[mp_hands.HandLandmark.INDEX_FINGER_PIP]
|
||||
mid_tip = hand.landmark[mp_hands.HandLandmark.MIDDLE_FINGER_TIP]
|
||||
mid_pip = hand.landmark[mp_hands.HandLandmark.MIDDLE_FINGER_PIP]
|
||||
|
||||
ix, iy = int(idx_tip.x * w), int(idx_tip.y * h)
|
||||
mx, my = int(mid_tip.x * w), int(mid_tip.y * h)
|
||||
fingertip_idx = (ix, iy)
|
||||
fingertip_mid = (mx, my)
|
||||
|
||||
# check extension
|
||||
index_ext = idx_tip.y < idx_pip.y
|
||||
middle_ext = mid_tip.y < mid_pip.y
|
||||
|
||||
# if measurement in progress but fingers no longer both extended, stop measuring
|
||||
if measuring and not (index_ext and middle_ext):
|
||||
measuring = False
|
||||
|
||||
# pinch distance
|
||||
pinch = compute_distance(fingertip_idx, fingertip_mid)
|
||||
cv2.putText(frame_vis, f"Pinch: {int(pinch)} px", (10,30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0),2)
|
||||
|
||||
# measurement gesture
|
||||
if not object_calibrating and index_ext and middle_ext:
|
||||
if pinch < PINCH_THRESHOLD and not measuring:
|
||||
measuring = True
|
||||
start_pt = fingertip_idx
|
||||
elif pinch > RELEASE_THRESHOLD and measuring:
|
||||
measuring = False
|
||||
|
||||
return measuring, start_pt
|
||||
@@ -0,0 +1,66 @@
|
||||
import numpy as np
|
||||
import json
|
||||
import cv2
|
||||
|
||||
from dataclasses import dataclass, field, asdict
|
||||
from typing import List, Optional, Tuple, Dict
|
||||
|
||||
|
||||
# ----- Helper Functions -----
|
||||
def compute_distance(p1, p2):
|
||||
return np.hypot(p2[0] - p1[0], p2[1] - p1[1])
|
||||
|
||||
# 4) compute straightness per finger
|
||||
def finger_straight(i_tip, i_pip, i_mcp):
|
||||
d1 = np.hypot(*(np.array(i_tip)-np.array(i_pip)))
|
||||
d2 = np.hypot(*(np.array(i_pip)-np.array(i_mcp)))
|
||||
return float(np.clip(d1/(d2+1e-3), 0, 1))
|
||||
|
||||
|
||||
def default_profile():
|
||||
return []
|
||||
|
||||
@dataclass
|
||||
class CameraObject():
|
||||
capture: Optional[cv2.VideoCapture]
|
||||
index: int
|
||||
pxHeight: int = 1536
|
||||
pxWidth: int = 2048
|
||||
|
||||
# Calibration
|
||||
camera_matrix: Optional[np.ndarray] = None # 3x3
|
||||
distortion_coefficients: Optional[np.ndarray] = None # 1x5 / 1x8
|
||||
rotation_matrix_world_to_camera: Optional[np.ndarray] = None # 3x3 (world->cam)
|
||||
translation_vector_world_to_camera: Optional[np.ndarray] = None # 3x1 (world->cam)
|
||||
camera_projection_matrix: Optional[np.ndarray] = None
|
||||
|
||||
def load_video_capture(self):
|
||||
#self.capture = cv2.VideoCapture(self.index, cv2.CAP_MSMF)
|
||||
self.capture = cv2.VideoCapture(self.index, cv2.CAP_ANY)
|
||||
#self.capture = cv2.VideoCapture(self.index, cv2.CAP_DSHOW)
|
||||
self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, 2048.0)
|
||||
self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1536.0)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Hand2D():
|
||||
camera_id: str # e.g. "overhead_left"
|
||||
handedness: str # "Left" / "Right"
|
||||
score: float # detection/tracking confidence
|
||||
landmarks_px: np.ndarray # shape (21,2) in pixels
|
||||
|
||||
|
||||
@dataclass
|
||||
class CharucoDetection:
|
||||
camera_id: int
|
||||
|
||||
# Output of Charuco detection
|
||||
charuco_corners: np.ndarray # (N, 1, 2) float32
|
||||
charuco_ids: np.ndarray # (N, 1) int32
|
||||
|
||||
gray_frame: Optional[np.ndarray] = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class CalibrationSample:
|
||||
detections: dict[int, CharucoDetection] # camera_id → detection
|
||||
@@ -0,0 +1,194 @@
|
||||
|
||||
import cv2
|
||||
import mediapipe as mp
|
||||
import numpy as np
|
||||
|
||||
# ----- Helper Functions -----
|
||||
def compute_distance(p1, p2):
|
||||
"""Compute Euclidean distance between two points p1 and p2"""
|
||||
return np.hypot(p2[0] - p1[0], p2[1] - p1[1])
|
||||
|
||||
# ----- Configuration -----
|
||||
CAMERA_INDEX = 0 # Change if multiple cameras
|
||||
FRAME_WIDTH = 1280
|
||||
FRAME_HEIGHT = 720
|
||||
# Initial calibration: approximate pixels per inch
|
||||
PIXELS_PER_INCH = 20
|
||||
# Gesture thresholds
|
||||
PINCH_THRESHOLD = 40 # px distance index-middle to start action
|
||||
RELEASE_THRESHOLD = 60 # px distance to end action
|
||||
# Circle touch threshold for object calibration
|
||||
CIRCLE_TOUCH_THRESHOLD = 20 # px tolerance to detect finger on circle
|
||||
|
||||
# ----- Initialize Hand Detector -----
|
||||
mp_hands = mp.solutions.hands
|
||||
mp_draw = mp.solutions.drawing_utils
|
||||
hands = mp_hands.Hands(
|
||||
static_image_mode=False,
|
||||
max_num_hands=1,
|
||||
min_detection_confidence=0.7,
|
||||
min_tracking_confidence=0.5
|
||||
)
|
||||
|
||||
# ----- State Variables -----
|
||||
measuring = False # Flag for measurement gesture
|
||||
start_pt = None
|
||||
calibrating = False # Flag for pinch-based calibration mode
|
||||
cal_start = None
|
||||
object_calibrating = False # Flag for object-based calibration mode
|
||||
cal_circle = None # Stores calibrated circle (x, y, r)
|
||||
|
||||
# ----- Main Loop -----
|
||||
def main():
|
||||
global measuring, start_pt, calibrating, cal_start, object_calibrating, PIXELS_PER_INCH
|
||||
|
||||
cap = cv2.VideoCapture(CAMERA_INDEX)
|
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
|
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
|
||||
|
||||
if not cap.isOpened():
|
||||
print(f"Error: cannot open camera {CAMERA_INDEX}")
|
||||
return
|
||||
|
||||
print("Press 'c' for pinch calibration, 'o' for object circle calibration, 'q' to quit.")
|
||||
|
||||
while True:
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
break
|
||||
|
||||
frame = cv2.flip(frame, 1)
|
||||
h, w, _ = frame.shape
|
||||
|
||||
# Hand detection
|
||||
img_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
|
||||
img_rgb.flags.writeable = False
|
||||
results = hands.process(img_rgb)
|
||||
img_rgb.flags.writeable = True
|
||||
frame = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR)
|
||||
|
||||
fingertip_idx = None
|
||||
fingertip_mid = None
|
||||
index_extended = False
|
||||
middle_extended = False
|
||||
|
||||
if results.multi_hand_landmarks:
|
||||
hand = results.multi_hand_landmarks[0]
|
||||
mp_draw.draw_landmarks(frame, hand, mp_hands.HAND_CONNECTIONS)
|
||||
# get index and middle finger tips and PIP to check extension
|
||||
idx_tip = hand.landmark[mp_hands.HandLandmark.INDEX_FINGER_TIP]
|
||||
idx_pip = hand.landmark[mp_hands.HandLandmark.INDEX_FINGER_PIP]
|
||||
mid_tip = hand.landmark[mp_hands.HandLandmark.MIDDLE_FINGER_TIP]
|
||||
mid_pip = hand.landmark[mp_hands.HandLandmark.MIDDLE_FINGER_PIP]
|
||||
ix, iy = int(idx_tip.x * w), int(idx_tip.y * h)
|
||||
mx, my = int(mid_tip.x * w), int(mid_tip.y * h)
|
||||
fingertip_idx = (ix, iy)
|
||||
fingertip_mid = (mx, my)
|
||||
# draw fingertips
|
||||
cv2.circle(frame, fingertip_idx, 8, (0,255,0), -1)
|
||||
cv2.circle(frame, fingertip_mid, 8, (0,255,0), -1)
|
||||
# determine if fingers are extended (tip above PIP)
|
||||
index_extended = idx_tip.y < idx_pip.y
|
||||
middle_extended = mid_tip.y < mid_pip.y
|
||||
# pinch distance between index and middle
|
||||
pinch_dist = compute_distance(fingertip_idx, fingertip_mid)
|
||||
cv2.putText(frame, f"Pinch: {int(pinch_dist)}px", (10,30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2)
|
||||
|
||||
# Pinch-based calibration
|
||||
if calibrating:
|
||||
if pinch_dist < PINCH_THRESHOLD and cal_start is None:
|
||||
cal_start = fingertip_idx
|
||||
print("Pinch calibration start point set.")
|
||||
elif pinch_dist > RELEASE_THRESHOLD and cal_start is not None:
|
||||
cal_end = fingertip_idx
|
||||
px_dist = compute_distance(cal_start, cal_end)
|
||||
inches = float(input("Enter actual distance between points in inches: "))
|
||||
PIXELS_PER_INCH = px_dist / inches
|
||||
print(f"Pinch calibration done: {PIXELS_PER_INCH:.2f} pixels/inch")
|
||||
calibrating = False
|
||||
cal_start = None
|
||||
|
||||
# Measurement gesture (only when not calibrating)
|
||||
elif not object_calibrating and index_extended and middle_extended:
|
||||
if pinch_dist < PINCH_THRESHOLD and not measuring:
|
||||
measuring = True
|
||||
start_pt = fingertip_idx
|
||||
elif pinch_dist > RELEASE_THRESHOLD and measuring:
|
||||
measuring = False
|
||||
|
||||
# Object-based calibration
|
||||
if object_calibrating:
|
||||
# Mask for orange color to find printed reference circle
|
||||
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
|
||||
# HSV range for orange (tune as needed)
|
||||
lower_orange = np.array([10, 100, 100])
|
||||
upper_orange = np.array([25, 255, 255])
|
||||
color_mask = cv2.inRange(hsv, lower_orange, upper_orange)
|
||||
masked_frame = cv2.bitwise_and(frame, frame, mask=color_mask)
|
||||
# Convert masked area to grayscale for Hough
|
||||
gray = cv2.cvtColor(masked_frame, cv2.COLOR_BGR2GRAY)
|
||||
gray = cv2.medianBlur(gray, 5)
|
||||
circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, dp=1.2, minDist=100,
|
||||
param1=50, param2=30, minRadius=10, maxRadius=300)
|
||||
if circles is not None and fingertip_idx is not None:
|
||||
circles = np.round(circles[0, :]).astype(int)
|
||||
# Filter circles by proximity of fingertip to circumference
|
||||
touched = []
|
||||
for x, y, r in circles:
|
||||
dist_c = compute_distance((x, y), fingertip_idx)
|
||||
if abs(dist_c - r) < CIRCLE_TOUCH_THRESHOLD:
|
||||
touched.append((x, y, r))
|
||||
if touched:
|
||||
# choose circle closest to exact touch point
|
||||
touched.sort(key=lambda c: abs(compute_distance((c[0], c[1]), fingertip_idx) - c[2]))
|
||||
x, y, r = touched[0]
|
||||
# store calibrated circle permanently
|
||||
cal_circle = (x, y, r)
|
||||
# draw selected calibration circle
|
||||
cv2.circle(frame, (x, y), r, (0, 0, 255), 3)
|
||||
cv2.drawMarker(frame, (x, y), (0, 0, 255), markerType=cv2.MARKER_CROSS, markerSize=20, thickness=2)
|
||||
cv2.line(frame, (x - r, y), (x + r, y), (0, 0, 255), 2)
|
||||
cv2.putText(frame, f"Cal Circle r={r}px", (x - r, y - r - 10),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2)
|
||||
# compute pixels per inch from diameter
|
||||
PIXELS_PER_INCH = (2 * r) / 1.0
|
||||
print(f"Object calibration done: {PIXELS_PER_INCH:.2f} pixels/inch")
|
||||
object_calibrating = False
|
||||
|
||||
# Overlay mode text
|
||||
if calibrating:
|
||||
cv2.putText(frame, "PINCH CALIBRATING...", (10,60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,255), 2)
|
||||
if object_calibrating:
|
||||
cv2.putText(frame, 'PLACE 1" CIRCLE & POINT AT IT', (10,90),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,255), 2)
|
||||
|
||||
# Draw measurement line and values
|
||||
if measuring and start_pt and fingertip_idx:
|
||||
cv2.line(frame, start_pt, fingertip_idx, (255,0,0), 2)
|
||||
px = compute_distance(start_pt, fingertip_idx)
|
||||
inch = px / PIXELS_PER_INCH
|
||||
cm = inch * 2.54
|
||||
midpt = ((start_pt[0] + fingertip_idx[0])//2,
|
||||
(start_pt[1] + fingertip_idx[1])//2)
|
||||
cv2.putText(frame, f"{inch:.2f} in / {cm:.1f} cm", (midpt[0]+10, midpt[1]-10),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,0), 2)
|
||||
|
||||
# Display
|
||||
cv2.imshow('Hand Measure', frame)
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord('q'):
|
||||
break
|
||||
elif key == ord('c'):
|
||||
calibrating = True
|
||||
cal_start = None
|
||||
print("Entered pinch calibration mode.")
|
||||
elif key == ord('o'):
|
||||
object_calibrating = True
|
||||
print("Entered object calibration mode. Present a 1-inch circle & point at it.")
|
||||
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Reference in New Issue
Block a user